define __GNU_SOURCE ifndef RTLD_NEXT define RTLD_NEXT void -1l endif i

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
#define __GNU_SOURCE 1
#ifndef RTLD_NEXT
#define RTLD_NEXT ((void *) -1l)
#endif
#include <stdlib.h>
#include <stdarg.h>
#include <stdio.h>
#include <dlfcn.h>
#include <asm/termbits.h>
#include <linux/serial.h>
#include <asm/ioctls.h>
static int speed_hack = 0;
static int enable_speed_hack = 0;
int ioctl(int fd, unsigned long request, ...) {
static int (*func_ioctl) (int, unsigned long, void *) = NULL;
va_list args;
void *argp;
if (!func_ioctl)
func_ioctl = (int (*) (int, unsigned long, void *)) dlsym(RTLD_NEXT, "ioctl");
va_start(args, request);
argp = va_arg (args, void *);
va_end(args);
if (request == TIOCSSERIAL || request == TIOCGSERIAL) {
struct serial_struct *nuts = (struct serial_struct *) argp;
if (request == TIOCGSERIAL) {
int ret = func_ioctl(fd, request, argp);
nuts->baud_base = 768000000; // LCM(1600000, 1500000, 1228800)
return ret;
} else {
speed_hack = nuts->baud_base / nuts->custom_divisor;
printf("HOOK TIOCSSERIAL: baud_base=%d, custom_divisor=%d, speed=%d, fd=%d\n", nuts->baud_base, nuts->custom_divisor, nuts->baud_base / nuts->custom_divisor, fd);
return 0;
}
}
return func_ioctl(fd, request, argp);
}
unsigned cfgetospeed(const struct termios *termios_p) {
static int (*func_cfgetospeed) (const struct termios *termios_p) = NULL;
if (!func_cfgetospeed)
func_cfgetospeed = (int (*) (const struct termios *termios_p)) dlsym(RTLD_NEXT, "cfgetospeed");
unsigned speed = func_cfgetospeed(termios_p);
if (speed == 0x1000) {
printf("cfgetospeed = %x\n", speed);
return B0;
}
return speed;
}
int tcsetattr(int fd, int optional_actions, const struct termios *termios_p) {
static int (*func_tcsetattr) (int fd, int optional_actions, const struct termios *termios_p) = NULL;
if (enable_speed_hack) {
struct termios2 tio;
if (ioctl(fd, TCGETS2, &tio) != 0)
perror("TCGETS2 hack");
tio.c_cflag &= ~CBAUD;
tio.c_cflag |= CBAUDEX;
tio.c_ispeed = speed_hack;
tio.c_ospeed = speed_hack;
if (ioctl(fd, TCSETS2, &tio) != 0)
perror("TCSETS2 hack");
return 0;
}
if (!func_tcsetattr)
func_tcsetattr = (int (*) (int fd, int optional_actions, const struct termios *termios_p)) dlsym(RTLD_NEXT, "tcsetattr");
return func_tcsetattr(fd, optional_actions, termios_p);
}
int cfsetospeed(struct termios *termios_p, unsigned speed) {
static int (*func_cfsetospeed) (struct termios *termios_p, unsigned speed) = NULL;
if (!func_cfsetospeed)
func_cfsetospeed = (int (*) (struct termios *termios_p, unsigned speed)) dlsym(RTLD_NEXT, "cfsetospeed");
enable_speed_hack = speed == B38400;
return func_cfsetospeed(termios_p, speed);
}