Redirects USB communication from livesuit to a TCP server

hook_usb_communication.c

/*
gcc -fPIC -shared -o valetudo-sunxi-livesuit/hook_usb_communication.so hook_usb_communication.c




*/
#include <dlfcn.h>
#include <linux/netlink.h>
#include <memory.h>
#include <pthread.h>
#include <semaphore.h>
#include <stdarg.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include <linux/ioctl.h>

#define recv __recv
#include <netinet/ip.h>
#include <sys/epoll.h>
#include <sys/socket.h>
#undef recv

#define MOCK_SERVER_PORT 1337

/* Data packet sent to the TCP client. Represents one usb transaction. */
typedef struct mocked_data_packet {
  int value; // seems to be unused
  int length;
  char data[0];
} mocked_data_packet_t;

///// AWUSB IOCTLs

#define AWUSB_IOC_MAGIC 's'

/* by Cesc */
struct usb_param {
  unsigned test_num;
  unsigned p1; /* parameter 1 */
  unsigned p2;
  unsigned p3;
};

struct aw_command {
  int value;
  int length;
  void *buffer; //? by Cesc
};

#define AWUSB_IOCRESET _IO(AWUSB_IOC_MAGIC, 0)
#define AWUSB_IOCSET _IOW(AWUSB_IOC_MAGIC, 1, struct usb_param)
#define AWUSB_IOCGET _IOR(AWUSB_IOC_MAGIC, 2, struct usb_param)
#define AWUSB_IOCSEND _IOW(AWUSB_IOC_MAGIC, 3, struct aw_command)
#define AWUSB_IOCRECV _IOR(AWUSB_IOC_MAGIC, 4, struct aw_command)
#define AWUSB_IOCSEND_RECV _IOWR(AWUSB_IOC_MAGIC, 5, struct aw_command)

// Function pointers for the original functions (not hooked)
int (*orig_socket)(int, int, int);
int (*orig_bind)(int, const struct sockaddr *, socklen_t);
int (*orig_setsockopt)(int, int, int, const void *, socklen_t);
int (*orig_recv)(int, void *, size_t, int);
int (*orig_open)(const char *, int, ...);
int (*orig_ioctl)(int, unsigned long, ...);

int dummy_uevent_fd = -1;
int dummy_aw_efex_fd = -1;

int tcp_client_fd = -1;

sem_t usp_device_detect_sem;

void *server_thread_func(void *arg) {
  int server_socket;
  struct sockaddr_in server_addr;

  server_socket = orig_socket(AF_INET, SOCK_STREAM, 0);
  if (server_socket == -1) {
    perror("socket");
    return NULL;
  }

  server_addr.sin_family = AF_INET;
  server_addr.sin_addr.s_addr = INADDR_ANY;
  server_addr.sin_port = htons(MOCK_SERVER_PORT);

  if (orig_bind(server_socket, (struct sockaddr *)&server_addr,
                sizeof(server_addr)) == -1) {
    perror("bind");
    close(server_socket);
    exit(1);
    return NULL;
  }

  if (listen(server_socket, 5) == -1) {
    perror("listen");
    close(server_socket);
    return NULL;
  }

  printf("TCP server listening on port %d\n", MOCK_SERVER_PORT);

  int epoll_fd = epoll_create1(0);
  struct epoll_event ev;
  struct epoll_event events[10];
  ev.events = EPOLLIN;
  ev.data.fd = server_socket;
  if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, server_socket, &ev) == -1) {
    perror("epoll_ctl");
    close(server_socket);
    return NULL;
  }

  while (1) {
    int nfds = epoll_wait(epoll_fd, events, 10, -1);
    if (nfds == -1) {
      perror("epoll_wait");
      close(server_socket);
      return NULL;
    }
    for (int i = 0; i < nfds; i++) {
      if (events[i].data.fd == server_socket) {
        int new_client_socket = accept(server_socket, NULL, NULL);
        if (new_client_socket == -1) {
          perror("accept");
          close(server_socket);
          return NULL;
        }
        // Remove old client socket
        if (tcp_client_fd != -1) {
          epoll_ctl(epoll_fd, EPOLL_CTL_DEL, tcp_client_fd, NULL);
          close(tcp_client_fd);
        }
        tcp_client_fd = new_client_socket;
        printf("Accepted connection from client\n");
        ev.events = EPOLLIN;
        ev.data.fd = tcp_client_fd;
        if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, tcp_client_fd, &ev) == -1) {
          perror("epoll_ctl");
          close(server_socket);
          close(tcp_client_fd);
          return NULL;
        }

        sem_post(&usp_device_detect_sem);
      } else {
        char buf[1024];
        int len = orig_recv(events[i].data.fd, buf, sizeof(buf), 0);
        if (len == -1) {
          perror("recv");
          close(server_socket);
          close(tcp_client_fd);
          return NULL;
        }
        if (len == 0) {
          printf("Client disconnected\n");
          close(tcp_client_fd);
          epoll_ctl(epoll_fd, EPOLL_CTL_DEL, tcp_client_fd, NULL);
        } else {
          buf[len] = '\0';
          printf("Received message from client: %s\n", buf);
        }
      }
    }
  }

  close(server_socket);
  return NULL;
}

void __attribute__((constructor)) init_hook() {
  // Get address of original socket function
  orig_socket = dlsym(RTLD_NEXT, "socket");
  orig_bind = dlsym(RTLD_NEXT, "bind");
  orig_setsockopt = dlsym(RTLD_NEXT, "setsockopt");
  orig_recv = dlsym(RTLD_NEXT, "recv");
  orig_open = dlsym(RTLD_NEXT, "open");
  orig_ioctl = dlsym(RTLD_NEXT, "ioctl");

  sem_init(&usp_device_detect_sem, 0, 0);
  pthread_t server_thread;
  pthread_create(&server_thread, NULL, server_thread_func, NULL);
}

int open(const char *pathname, int flags, ...) {
  if (strcmp(pathname, "/dev/aw_efex0") == 0) {
    printf("Intercepted open call on /dev/aw_efex0\n");
    dummy_aw_efex_fd = orig_open("/dev/null", flags);
    return dummy_aw_efex_fd;
  }
  return orig_open(pathname, flags);
}

int ioctl(int fd, unsigned long request, ...) {
  va_list args;
  void *argp;
  if (fd == dummy_aw_efex_fd) {

    struct aw_command cmd;
    switch (request) {
    case AWUSB_IOCRESET:
      printf("Intercepted ioctl call on /dev/aw_efex0: AWUSB_IOCRESET\n");
      break;
    case AWUSB_IOCSET:
      printf("Intercepted ioctl call on /dev/aw_efex0: AWUSB_IOCSET\n");
      break;
    case AWUSB_IOCGET:
      printf("Intercepted ioctl call on /dev/aw_efex0: AWUSB_IOCGET\n");
      break;
    case AWUSB_IOCSEND:
      va_start(args, request);
      argp = va_arg(args, void *);
      va_end(args);
      cmd = *(struct aw_command *)argp;
      printf("send: %d\n", cmd.length);

      mocked_data_packet_t *data_packet =
          malloc(sizeof(mocked_data_packet_t) + cmd.length);
      data_packet->value = cmd.value;
      data_packet->length = cmd.length;
      memcpy(data_packet->data, cmd.buffer, cmd.length);

      if (tcp_client_fd != -1) {
        send(tcp_client_fd, data_packet,
             sizeof(mocked_data_packet_t) + cmd.length, 0);
      }
      free(data_packet);

      break;
    case AWUSB_IOCRECV:
      va_start(args, request);
      argp = va_arg(args, void *);
      va_end(args);
      cmd = *(struct aw_command *)argp;
      printf("recv: %d\n", cmd.length);

      if (tcp_client_fd != -1) {
        
        int actual = orig_recv(tcp_client_fd, cmd.buffer, cmd.length, 0);
        printf("actual: %d\n", actual);
      }

      break;
    case AWUSB_IOCSEND_RECV:
      printf("Intercepted ioctl call on /dev/aw_efex0: AWUSB_IOCSEND_RECV\n");
      break;
    default:
      printf("Intercepted ioctl call on /dev/aw_efex0: unknown request\n");
      break;
    }
    return 0;
  }

  va_start(args, request);
  argp = va_arg(args, void *);
  va_end(args);
  return orig_ioctl(fd, request, argp);
}

int socket(int domain, int type, int protocol) {

  printf("socket() call intercepted! (domain: %d, type: %d, protocol: %d)\n",
         domain, type, protocol);
  if (domain == AF_NETLINK && type == SOCK_DGRAM &&
      protocol == NETLINK_KOBJECT_UEVENT) {
    printf("Intercepted NETLINK_KOBJECT_UEVENT socket call\n");
    // create dummy file descriptor
    dummy_uevent_fd = orig_socket(domain, type, protocol);
    return dummy_uevent_fd;
  }
  // Call the original socket function
  return orig_socket(domain, type, protocol);
}

int bind(int sockfd, const struct sockaddr *addr, socklen_t addrlen) {

  if (sockfd == dummy_uevent_fd) {
    printf("Intercepted bind call on dummy socket\n");
    return 0;
  }
  return orig_bind(sockfd, addr, addrlen);
}

int setsockopt(int sockfd, int level, int optname, const void *optval,
               socklen_t optlen) {
  if (sockfd == dummy_uevent_fd) {
    printf("Intercepted setsockopt call on dummy socket\n");
    return 0;
  }
  return orig_setsockopt(sockfd, level, optname, optval, optlen);
}

int recv(int sockfd, void *buf, size_t len, int flags) {
  if (sockfd == dummy_uevent_fd) {
    // printf("Intercepted recv call on dummy socket\n");
    // wait 100ms for the semaphore
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    ts.tv_nsec += 100000000 * 1;
    int ret = sem_timedwait(&usp_device_detect_sem, &ts);
    if (ret == 0) {
      printf("Injecting hotplug event\n");
      char data_to_send[] =
          "ACTION=add\0"
          "DEVPATH=/devices/pci0000:00/0000:00:1d.0/usb2/2-1/2-1.1/2-1.1:1.0\0"
          "DEVTYPE=aaaa\0"
          "PRODUCT=1f3a/efe8/2b3\0"
          "MODALIAS=usb:v1F3ApEFE8d02B3dc00dsc00dp00icFFiscFFipFF\0"
          "DEVNAME=aw_efex0\0";
      memcpy(buf, data_to_send, sizeof(data_to_send));
      return sizeof(data_to_send);

    } else {

      // because of a bug in livesuit, we need to send an event that it is not
      // looking for, so that it clears a variable. This prevents it from
      // detecting the one device we want multiple times.
      char data_to_send[] =
          "ACTION=add\0"
          "DEVPATH=/devices/pci0000:00/0000:00:1d.0/usb2/2-1/2-1.1/2-1.1:1.0\0"
          "DEVTYPE=some_other_dev\0"
          "PRODUCT=some_other_dev\0"
          "MODALIAS=some_other_dev\0"
          "DEVNAME=some_other_dev\0";
      memcpy(buf, data_to_send, sizeof(data_to_send));
      return sizeof(data_to_send);
    }
  }
  // Call the original recv function
  return orig_recv(sockfd, buf, len, flags);
}

LiveSuit.sh

#!/bin/bash

APP=LiveSuit
TOP_DIR=`pwd`

MACHINE=$(uname -m)
if [ ${MACHINE} == 'x86_64' ]; then
	BIN_DIR="x86-64"	
elif [ ${MACHINE} == 'i686' ]; then
	BIN_DIR="x86"
else
	echo "Error: unknown architecture ${MACHINE}"
	exit
fi

echo "Starting ${BIN_DIR}/${APP}."
echo ""

LD_PRELOAD="$TOP_DIR/hook_usb_communication.so" LD_LIBRARY_PATH=${TOP_DIR}/${BIN_DIR}/ ${BIN_DIR}/${APP}