proxy.c

February 23, 2024 · View on GitHub

#define _POSIX_C_SOURCE 200809L #include <assert.h> #include <errno.h> #include <signal.h> #include <stdarg.h> #include <stdbool.h> #include <stdio.h> #include <stdlib.h> #include <string.h> #include <fcntl.h> #include <pthread.h> #include <sys/ioctl.h> #include <linux/usb/raw_gadget.h> #include <linux/usb/ch9.h> #include <libusb.h> #include "intpack.h"

struct config { struct libusb_config_descriptor *desc; struct interface *iface; size_t numiface; int index; };

struct interface { const struct libusb_interface *desc; const struct libusb_interface_descriptor *alt; struct endpoint *ep; size_t numep; };

struct endpoint { const struct libusb_endpoint_descriptor *desc; int addr; int handle; int running; pthread_t thread; };

static int rawfd; static libusb_device_handle *dev; static struct config config;

static void fatal(const char *msg, ...) { va_list ap;

va_start(ap, msg);
vfprintf(stderr, msg, ap);
va_end(ap);
if (!msg)
	fprintf(stderr, "%s\n", strerror(errno));
else if (*msg && msg[strlen(msg) - 1] == ':')
	fprintf(stderr, " %s\n", strerror(errno));
else
	fputc('\n', stderr);
exit(1);

}

static void dump(const char *name, const unsigned char *buf, size_t len) { size_t i;

assert(len <= 1024);
if (name)
	fputs(name, stdout);
if (len > 0) {
	if (name)
		putchar('\t');
	printf("%.2x", buf[0]);
}
for (i = 1; i < len; ++i)
	printf(" %.2x", buf[i]);
putchar('\n');

}

static void rawconnect(void) { }

static void controldone(struct libusb_transfer *transfer) { *(int *)transfer->user_data = 1; }

static void * bulkreader(void *arg) { struct endpoint *ep; struct usb_raw_ep_io *io; int ret, err, len;

ep = arg;
fprintf(stderr, "bulkreader %#.2x\n", ep->desc->bEndpointAddress);
io = malloc(sizeof *io + 1024);
if (!io)
	fatal(NULL);
io->ep = ep->handle;
io->flags = 0;
for (;;) {
	err = libusb_bulk_transfer(dev, ep->addr, io->data, 1024, &len, 0);
	if (err)
		fatal("libusb_bulk_transfer: %s", libusb_strerror(err));
	io->length = len;
	//dump("bulk <-", io->data, len);
	ret = ioctl(rawfd, USB_RAW_IOCTL_EP_WRITE, io);
	if (ret < 0)
		fatal("usb_raw_ep_read %#.2x:", ep->desc->bEndpointAddress);
}
free(io);
return NULL;

}

static void * bulkwriter(void *arg) { struct endpoint *ep; struct usb_raw_ep_io *io; int ret, len, err; unsigned char *buf;

ep = arg;
fprintf(stderr, "bulkwriter %#.2x\n", ep->desc->bEndpointAddress);
io = malloc(sizeof *io + 1024);
if (!io)
	fatal(NULL);
io->ep = ep->handle;
io->flags = 0;
io->length = 1024;
for (;;) {
	ret = ioctl(rawfd, USB_RAW_IOCTL_EP_READ, io);
	if (ret < 0)
		fatal("usb_raw_ep_read %#.2x:", ep->desc->bEndpointAddress);
	//dump("bulk ->", io->data, ret);
	buf = io->data;
	while (ret > 0) {
		err = libusb_bulk_transfer(dev, ep->addr, buf, ret, &len, 0);
		if (err)
			fatal("libusb_bulk_transfer: %s", libusb_strerror(err));
		ret -= len;
		buf += len;
	}
}
free(io);
return NULL;

}

static void setinterface(int idx, int alt) { int i, err; struct interface *iface; struct endpoint *ep; void *(*func)(void *); struct usb_endpoint_descriptor epd;

fprintf(stderr, "SET_INTERFACE %d %d\n", idx, alt);
for (i = 0; i < config.numiface; ++i) {
	iface = &config.iface[i];
	if (iface->desc->altsetting[0].bInterfaceNumber == idx)
		break;
}
if (i == config.numiface)
	fatal("invalid interface %d", idx);
for (i = 0; i < iface->numep; ++i) {
	ep = &iface->ep[i];
	if (ep->running) {
		err = pthread_kill(ep->thread, SIGUSR1);
		if (err)
			fatal("pthread_kill: %s", strerror(err));
		err = pthread_join(ep->thread, NULL);
		if (err)
			fatal("pthread_join: %s", strerror(err));
	}
	if (ioctl(rawfd, USB_RAW_IOCTL_EP_DISABLE, ep->handle) != 0)
		fatal("usb_raw_ep_disable %d:", ep->handle);
}
for (i = 0; i < iface->desc->num_altsetting; ++i) {
	iface->alt = &iface->desc->altsetting[i];
	if (iface->alt->bAlternateSetting == alt)
		break;
}
if (i == iface->desc->num_altsetting)
	fatal("invalid interface %d alt. setting %d", idx, alt);
iface->numep = iface->alt->bNumEndpoints;
iface->ep = realloc(iface->ep, iface->numep * sizeof *iface->ep);
if (!iface->ep && iface->numep > 0)
	fatal(NULL);
for (i = 0; i < iface->numep; ++i) {
	ep = &iface->ep[i];
	ep->desc = &iface->alt->endpoint[i];
	ep->addr = ep->desc->bEndpointAddress;
	fprintf(stderr, "ep enable %#.2x\n", ep->addr);
	epd.bLength = ep->desc->bLength;
	epd.bDescriptorType = ep->desc->bDescriptorType;
	epd.bEndpointAddress = ep->desc->bEndpointAddress;
	epd.bmAttributes = ep->desc->bmAttributes;
	epd.wMaxPacketSize = ep->desc->wMaxPacketSize;
	epd.bInterval = ep->desc->bInterval;
	ep->handle = ioctl(rawfd, USB_RAW_IOCTL_EP_ENABLE, &epd);
	if (ep->handle < 0)
		fatal("usb_raw_ep_enable %#.2x\n", epd.bEndpointAddress);
	func = NULL;
	switch (usb_endpoint_type((const struct usb_endpoint_descriptor *)ep->desc)) {
	case USB_ENDPOINT_XFER_BULK:
		func = usb_endpoint_dir_in((const struct usb_endpoint_descriptor *)ep->desc) ? bulkreader : bulkwriter;
		break;
	}
	if (func) {
		ep->running = true;
		err = pthread_create(&ep->thread, NULL, func, ep);
	} else {
		ep->running = false;
	}
}

}

static void controlin(const unsigned char *data) { int err, index, alt, i;

switch (data[1]) {
case USB_REQ_SET_CONFIGURATION:
	if (ioctl(rawfd, USB_RAW_IOCTL_CONFIGURE) != 0)
		fatal("usb_raw_configure:");
	index = getle16(data + 4);
	fprintf(stderr, "SET_CONFIGURATION %d\n", index);
	if (config.index != -1) {
		libusb_free_config_descriptor(config.desc);
	}
	config.index = index;
	err = libusb_get_config_descriptor(libusb_get_device(dev), index, &config.desc);
	if (err != 0)
		fatal("libusb_get_config_descriptor %d: %s", index, libusb_strerror(err));
	config.numiface = config.desc->bNumInterfaces;
	config.iface = realloc(config.iface, config.numiface * sizeof *config.iface);
	if (!config.iface)
		fatal(NULL);
	for (i = 0; i < config.numiface; ++i) {
		config.iface[i].desc = &config.desc->interface[i];
		config.iface[i].numep = 0;
		libusb_claim_interface(dev, config.desc->interface[i].altsetting[0].bInterfaceNumber);
		setinterface(config.desc->interface[i].altsetting[0].bInterfaceNumber, 0);
	}
	break;
case USB_REQ_SET_INTERFACE:
	index = getle16(data + 4);
	alt = getle16(data + 2);
	setinterface(index, alt);
	break;
default:
	fprintf(stderr, "control in %d\n", data[1]);
}

}

static void rawcontrol(const unsigned char *data, size_t len) { unsigned char *databuf; struct usb_raw_ep_io *io; struct libusb_transfer *transfer; int err, ret, done;

if (len != 8)
	fatal("control transfer has unexpected size");
transfer = libusb_alloc_transfer(0);
if (!transfer)
	fatal("libusb_alloc_transfer failed");
if (!(data[0] & USB_DIR_IN)) {
	len = getle16(data + 6);
	io = malloc(sizeof io + len);
	if (!io)
		fatal(NULL);
	io->ep = 0;
	io->flags = 0;
	io->length = len;
	ret = ioctl(rawfd, USB_RAW_IOCTL_EP0_READ, io);
	if (ret < 0)
		fatal("usb_raw_ep0_read:");
	dump("h<-", io->data, io->length);
	len = 8 + ret;
	databuf = malloc(len);
	memcpy(databuf, data, 8);
	memcpy(databuf + 8, io->data, ret);
	free(io);
	controlin(databuf);
}
done = 0;
libusb_fill_control_transfer(transfer, dev, (unsigned char *)data, controldone, &done, 0);
err = libusb_submit_transfer(transfer);
if (err != 0)
	fatal("libusb_submit_transfer: %s", libusb_strerror(err));
while (!done) {
	err = libusb_handle_events_completed(NULL, &done);
	if (err < 0)
		fatal("libusb_handle_events_completed: %s", libusb_strerror(err));
}
if (transfer->status != LIBUSB_TRANSFER_COMPLETED)
	fatal("control transfer unsuccessful: %d", transfer->status);
if (data[0] & USB_DIR_IN) {
	io = malloc(sizeof io + transfer->actual_length);
	if (!io)
		fatal(NULL);
	io->ep = 0;
	io->flags = 0;
	io->length = transfer->actual_length;
	dump("d<-", libusb_control_transfer_get_data(transfer), transfer->actual_length);
	memcpy(io->data, libusb_control_transfer_get_data(transfer), transfer->actual_length);
	ret = ioctl(rawfd, USB_RAW_IOCTL_EP0_WRITE, io);
	if (ret < 0)
		fatal("usb_raw_ep0_write:");
	free(io);
}
libusb_free_transfer(transfer);

}

static void sighandler(int sig) { }

int main(int argc, char *argv[]) { struct sigaction sa; struct usb_raw_init init; struct usb_raw_event *event; int err;

memset(&sa, 0, sizeof sa);
sa.sa_handler = sighandler;
if (sigaction(SIGUSR1, &sa, NULL) != 0)
	fatal("sigaction SIGUSR1");

err = libusb_init(NULL);
if (err)
	fatal("libusb_init: %s", libusb_strerror(err));
dev = libusb_open_device_with_vid_pid(NULL, 0x2a39, 0x3fd9);
if (!dev)
	fatal("libusb_open failed");
err = libusb_set_auto_detach_kernel_driver(dev, 1);
if (err)
	fatal("libusb_set_auto_detach_kernel_driver: %s", libusb_strerror(err));

rawfd = open("/dev/raw-gadget", O_RDWR);
if (rawfd < 0)
	fatal("open /dev/raw-gadget:");
strcpy((char *)init.driver_name, "fe980000.usb");
strcpy((char *)init.device_name, "fe980000.usb");
init.speed = USB_SPEED_HIGH;

if (ioctl(rawfd, USB_RAW_IOCTL_INIT, &init) != 0)
	fatal("usb_raw_init:");
if (ioctl(rawfd, USB_RAW_IOCTL_RUN, NULL) != 0)
	fatal("usb_raw_run:");

event = malloc(sizeof event + 1024);
for (;;) {
	event->length = 1024;
	if (ioctl(rawfd, USB_RAW_IOCTL_EVENT_FETCH, event) != 0)
		fatal("usb_raw_event_fetch:");
	switch (event->type) {
	case USB_RAW_EVENT_CONNECT:
		rawconnect();
		break;
	case USB_RAW_EVENT_CONTROL:
		rawcontrol(event->data, event->length);
		break;
	}
}

}