path: root/libthor/thor_usb.c

#include <sys/types.h>
#include <stdio.h>
#include <endian.h>
#include <errno.h>
#include <string.h>
#include <assert.h>
#ifdef __linux__
#include <linux/usb/cdc.h>
#else
#define USB_CDC_SUBCLASS_ACM			0x02

#define USB_CDC_PROTO_NONE			0
#define USB_CDC_ACM_PROTO_AT_V25TER		1
#endif
#ifdef __linux__
#include <linux/usb/ch9.h>
#else
#include <stdint.h>

#define USB_DT_INTERFACE_ASSOCIATION	0x0b

#define USB_CLASS_COMM			2
#define USB_CLASS_CDC_DATA		0x0a

struct usb_descriptor_header {
	uint8_t  bLength;
	uint8_t  bDescriptorType;
} __attribute__ ((packed));

struct usb_interface_assoc_descriptor {
	uint8_t  bLength;
	uint8_t  bDescriptorType;

	uint8_t  bFirstInterface;
	uint8_t  bInterfaceCount;
	uint8_t  bFunctionClass;
	uint8_t  bFunctionSubClass;
	uint8_t  bFunctionProtocol;
	uint8_t  iFunction;
} __attribute__ ((packed));
#endif
#include <libusb-1.0/libusb.h>

#include "thor.h"
#include "thor_internal.h"

#define MAX_SERIAL_LEN 256

struct hotplug_helper {
	struct thor_device_handle *th;
	struct thor_device_id *dev_id;
	int completed;
};

static int check_busid_match(const char *expected, libusb_device *dev)
{
	/* Max USB depth is 7 */
	uint8_t dev_port[8];
	int nports;
	uint8_t bus_number;
	int val;
	int i;
	int ret;

	bus_number = libusb_get_bus_number(dev);
	ret = sscanf(expected, "%d", &val);
	if (ret < 1)
		return -EINVAL;

	if (val != bus_number)
		return 0;

	expected = strchr(expected, '-');
	if (!expected)
		return -EINVAL;

	nports = libusb_get_port_numbers(dev, (uint8_t *)dev_port, sizeof(dev_port));
	if (nports < 0)
		return nports;


	for (i = 0; i < nports; ++i) {
		ret = sscanf(expected, "%d", &val);
		if (ret < 1)
			return -EINVAL;

		if (val != dev_port[i])
			return 0;

		expected = strchr(expected, '.');
		if (!expected) {
			if (i + 1 == nports)
				return 1;
			else
				break;
		}
	}

	return 0;
}

static int check_vid_pid_match(int vid, int pid, libusb_device *dev)
{
	struct libusb_device_descriptor desc;
	int ret;

	ret = libusb_get_device_descriptor(dev, &desc);
	if (ret < 0)
		return ret;

	if (vid >= 0 && vid != desc.idVendor)
		return 0;

	if (pid >= 0 && pid != desc.idProduct)
		return 0;

	return 1;
}

static int check_serial_match(const char *serial, libusb_device *dev,
		       libusb_device_handle **devh)
{
	char buf[MAX_SERIAL_LEN];
	struct libusb_device_descriptor desc;
	libusb_device_handle *handle;
	int ret;

	ret = libusb_get_device_descriptor(dev, &desc);
	if (ret < 0)
		return ret;

	ret = libusb_open(dev, &handle);
	if (ret < 0)
		return ret;

	ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber,
						 (unsigned char*)buf,
						 sizeof(buf));
	if (ret < 0)
		return ret;

	if (strcmp(serial, buf))
		return 0;

	*devh = handle;
	return 1;
}

static inline int
is_data_interface(const struct libusb_interface_descriptor *idesc)
{
	return idesc->bInterfaceClass == USB_CLASS_CDC_DATA;
}

static inline int
is_control_interface(const struct libusb_interface_descriptor *idesc)
{
	return idesc->bInterfaceClass == USB_CLASS_COMM
		&& idesc->bInterfaceSubClass == USB_CDC_SUBCLASS_ACM
		&& idesc->bInterfaceProtocol == USB_CDC_ACM_PROTO_AT_V25TER;
}

static int find_idesc_by_id(struct libusb_config_descriptor *cdesc, int id)
{
	int i;

	for (i = 0; i < cdesc->bNumInterfaces; ++i)
		if (cdesc->interface[i].altsetting[0].bInterfaceNumber == id)
			return i;

	return -ENODEV;
}

static int check_assoc(struct libusb_config_descriptor *cdesc,
		       struct usb_interface_assoc_descriptor *assoc_desc,
		       struct thor_device_handle *th)
{
	int intf_a, intf_b;

	if (assoc_desc->bInterfaceCount != 2
	    || assoc_desc->bFunctionClass != USB_CLASS_COMM
	    || assoc_desc->bFunctionSubClass != USB_CDC_SUBCLASS_ACM
	    || assoc_desc->bFunctionProtocol != USB_CDC_PROTO_NONE)
		return -EINVAL;

	intf_a = find_idesc_by_id(cdesc, assoc_desc->bFirstInterface);
	intf_b = find_idesc_by_id(cdesc, assoc_desc->bFirstInterface + 1);

	if (is_data_interface(cdesc->interface[intf_a].altsetting + 0)
	    && is_control_interface(cdesc->interface[intf_b].altsetting + 0)) {
		th->data_interface = intf_a;
		th->data_interface_id = assoc_desc->bFirstInterface;
		th->control_interface = intf_b;
		th->control_interface_id = assoc_desc->bFirstInterface + 1;
	} else if (is_control_interface(cdesc->interface[intf_a].altsetting + 0)
		   && is_data_interface(cdesc->interface[intf_b].altsetting + 0)) {
		th->data_interface = intf_b;
		th->data_interface_id = assoc_desc->bFirstInterface + 1;
		th->control_interface = intf_a;
		th->control_interface_id = assoc_desc->bFirstInterface;
	} else {
		return -ENODEV;
	}

	return 0;
}

static int find_interfaces(struct libusb_config_descriptor *cdesc,
			   struct thor_device_handle *th)
{
	struct usb_descriptor_header *header;
	struct usb_interface_assoc_descriptor *assoc_desc = NULL;
	int assoc_valid = 0;
	int pos;
	int ret;

	/* Try to find IAD and use it */
	pos = 0;
	for (; pos < cdesc->extra_length; pos += header->bLength) {
		header = (struct usb_descriptor_header *)(cdesc->extra + pos);
		if (header->bDescriptorType != USB_DT_INTERFACE_ASSOCIATION)
			continue;

		if (pos + sizeof(assoc_desc) > cdesc->extra_length)
			break;

		assoc_desc = (struct usb_interface_assoc_descriptor *)header;
		ret = check_assoc(cdesc, assoc_desc, th);
		if (!ret) {
			assoc_valid = 1;
			break;
		}
	}

	/*
	 * If we were unable to find IAD let's
	 * just try to manually find interfaces
	 */
	if (!assoc_valid) {
		int i;
#define get_intf_desc(_intf) (&(cdesc->interface[_intf].altsetting[0]))
		th->data_interface = -1;
		th->control_interface = -1;

		for (i = 0; i < cdesc->bNumInterfaces; ++i) {
			if (!is_data_interface(get_intf_desc(i)))
				continue;

			th->data_interface = i;
			th->data_interface_id =
				get_intf_desc(i)->bInterfaceNumber;
			break;
		}

		if (th->data_interface < 0)
			return -ENODEV;

		for (i = 0; i < cdesc->bNumInterfaces; ++i) {
			if (!is_control_interface(get_intf_desc(i)))
				continue;
			th->control_interface = i;
			th->control_interface_id =
				get_intf_desc(i)->bInterfaceNumber;
		}

		if (th->control_interface < 0)
			return -ENODEV;
#undef get_intf_desc
	}

	return 0;
}

static int find_data_eps(struct libusb_config_descriptor *cdesc,
			 struct thor_device_handle *th)
{
	const struct libusb_interface_descriptor *idesc;
	int i;

	idesc = cdesc->interface[th->data_interface_id].altsetting + 0;

	if (idesc->bNumEndpoints != 2)
		return -EINVAL;

	th->data_ep_in = -1;
	th->data_ep_out = -1;

	for (i = 0; i < idesc->bNumEndpoints; ++i) {
		if ((idesc->endpoint[i].bmAttributes & 0x03) !=
		    LIBUSB_TRANSFER_TYPE_BULK)
			return -1;
		if ((idesc->endpoint[i].bEndpointAddress & (1 << 7))
		    == LIBUSB_ENDPOINT_IN)
			th->data_ep_in = idesc->endpoint[i].bEndpointAddress;
		else
			th->data_ep_out = idesc->endpoint[i].bEndpointAddress;
	}

	if (th->data_ep_in < 0 || th->data_ep_out < 0)
		return -EINVAL;

	return 0;
}

static int find_intf_and_eps(libusb_device *dev,
			     struct thor_device_handle *th)
{
	struct libusb_config_descriptor *cdesc;
	int ret;

	ret = libusb_get_active_config_descriptor(dev, &cdesc);
	if (ret < 0)
		return ret;

	ret = find_interfaces(cdesc, th);
	if (ret) {
		ret = -ENODEV;
		goto cleanup_desc;
	}

	ret = find_data_eps(cdesc, th);
	if (ret) {
		ret = -ENODEV;
		goto cleanup_desc;
	}

	ret = 0;
cleanup_desc:
	libusb_free_config_descriptor(cdesc);
	return ret;
}

static int claim_intf(struct thor_device_handle *th)
{
	int ret;

	/*
	 * Check if our OS allows us to detach kernel driver.
	 * If yes then we mark this device as auto-detach and try to claim
	 * our interfaces. libusb will detach kernel driver, if any when we
	 * will try to claim interface.
	 * If our os doesn't support detaching kernel driver we simply try
	 * to claim our interfaces. If we fail it means that probably there
	 * is some kernel driver bound to this device but we cannot do anything
	 * with this.
	 */
	ret = libusb_has_capability(LIBUSB_CAP_SUPPORTS_DETACH_KERNEL_DRIVER);
	if (ret) {
		ret = libusb_set_auto_detach_kernel_driver(th->devh, 1);
		if (ret < 0)
			goto out;
	}

	ret = libusb_claim_interface(th->devh, th->data_interface_id);
	if (ret < 0)
		goto out;

	ret = libusb_claim_interface(th->devh, th->control_interface_id);
	if (ret < 0)
		goto release_data;

	return 0;

release_data:
	libusb_release_interface(th->devh, th->data_interface);
out:
	return ret;
}

static int check_device_match(struct thor_device_id *dev_id,
		       libusb_device *dev, struct thor_device_handle *th)
{
	int ret;

	if (dev_id->busid) {
		ret = check_busid_match(dev_id->busid, dev);
		if (ret <= 0)
			goto no_match;
	}

	if (dev_id->vid >= 0 || dev_id->pid >= 0) {
		ret = check_vid_pid_match(dev_id->vid, dev_id->pid, dev);
		if (ret <= 0)
			goto no_match;
	}

	if (dev_id->serial) {
		ret = check_serial_match(dev_id->serial, dev, &th->devh);
		if (ret <= 0)
			goto no_match;
	} else {
		ret = libusb_open(dev, &th->devh);
		if (ret < 0)
			goto no_match;
	}

	ret = find_intf_and_eps(dev, th);
	if (ret < 0)
		goto err;

	ret = claim_intf(th);
	if (ret < 0)
		goto err;

	return 1;
err:
	libusb_close(th->devh);
no_match:
	return 0;
}

static int find_existing_device(struct thor_device_id *dev_id,
			    struct thor_device_handle *th)
{
	libusb_device **dev_list;
	int i, ndevices;
	int ret = 0;

	ndevices = libusb_get_device_list(NULL, &dev_list);
	if (ndevices < 0)
		return ndevices;

	for (i = 0; i < ndevices; ++i) {
		ret = check_device_match(dev_id, dev_list[i], th);
		if (ret > 0)
			/* device match and opened */
			break;
	}

	libusb_free_device_list(dev_list, 1);

	return ret > 0 ? 1 : 0;

}

static int hotplug_device_arrived(libusb_context *ctx, libusb_device *device,
		   libusb_hotplug_event event, void *user_data)
{
	struct hotplug_helper *helper = user_data;

	if (check_device_match(helper->dev_id, device, helper->th) > 0) {
		helper->completed = 1;
		return 1;
	}

	return 0;
}


int t_usb_find_device(struct thor_device_id *dev_id, int wait,
		      struct thor_device_handle *th)
{
	struct hotplug_helper helper = {
		.th = th,
		.dev_id = dev_id,
		.completed = 0,
	};
	int found;

	found = find_existing_device(dev_id, th);
	if (found <= 0) {
		if (!wait)
			return found;

		libusb_hotplug_register_callback(NULL,
						 LIBUSB_HOTPLUG_EVENT_DEVICE_ARRIVED,
						 0,
						 dev_id->vid >= 0 ? dev_id->vid
						 : LIBUSB_HOTPLUG_MATCH_ANY,
						 dev_id->pid >= 0 ? dev_id->pid
						 : LIBUSB_HOTPLUG_MATCH_ANY,
						 LIBUSB_HOTPLUG_MATCH_ANY,
						 hotplug_device_arrived,
						 &helper,
						 NULL);

		while (!helper.completed)
			libusb_handle_events_completed(NULL, &helper.completed);
	}

	return 1;
}

void t_usb_close_device(struct thor_device_handle *th)
{
	if (th->devh)
		libusb_close(th->devh);
}

int t_usb_send(struct thor_device_handle *th, unsigned char *buf,
	       size_t count, int timeout)
{
	int ret;
	int transferred = 0;

	ret = libusb_bulk_transfer(th->devh,
				   th->data_ep_out,
				   (unsigned char *)buf,
				   count,
				   &transferred,
				   timeout);

	if (ret < 0)
		return ret;
	if (transferred < count)
		return -EIO;

	return 0;
}

int t_usb_recv(struct thor_device_handle *th, unsigned char *buf,
	       size_t count, int timeout)
{
	int ret;
	int transferred = 0;

	ret = libusb_bulk_transfer(th->devh,
				   th->data_ep_in,
				   (unsigned char *)buf,
				   count,
				   &transferred,
				   timeout);

	if (ret < 0)
		return ret;
	if (transferred < count)
		return -EIO;

	return 0;
}

int t_usb_send_req(struct thor_device_handle *th, request_type req_id,
		   int req_sub_id, int *idata, int icnt, char **sdata, int scnt)
{
	struct rqt_pkt req;
	int i;
	int ret;

	assert(icnt <= sizeof(req.int_data)/sizeof(req.int_data[0]));
	assert(icnt >= 0);
	assert(scnt <= sizeof(req.str_data)/sizeof(req.str_data[0]));
	assert(scnt >= 0);

	memset(&req, 0, sizeof(req));

	req.id = req_id;
	req.sub_id = req_sub_id;

	if (idata) {
		for (i = 0; i < icnt; i++)
			req.int_data[i] = idata[i];
	}

	if (sdata) {
		for (i = 0; i < scnt; i++)
			strcpy(req.str_data[i],sdata[i]);
	}

	ret = t_usb_send(th, (unsigned char *)&req, RQT_PKT_SIZE, DEFAULT_TIMEOUT);

	return ret;
}

int t_usb_recv_req(struct thor_device_handle *th, struct res_pkt *resp)
{
	int ret;

	ret = t_usb_recv(th, (unsigned char *)resp, sizeof(*resp),
			 DEFAULT_TIMEOUT);

	return ret;
}

static void t_usb_transfer_finished(struct libusb_transfer *ltransfer)
{
	struct t_usb_transfer *t = ltransfer->user_data;

	t->cancelled = 0;
	t->ret = 0;
	switch (ltransfer->status) {
	case LIBUSB_TRANSFER_COMPLETED:
		if (ltransfer->actual_length != t->size)
			t->ret = -EIO;
		break;
	case LIBUSB_TRANSFER_CANCELLED:
		t->cancelled = 1;
		break;
	default:
		t->ret = -EIO;
	}

	if (t->transfer_finished)
		t->transfer_finished(t);
}

int t_usb_init_transfer(struct t_usb_transfer *t,
			libusb_device_handle *devh,
			unsigned char ep,
			unsigned char *buf, size_t size,
			t_usb_transfer_cb transfer_finished,
			unsigned int timeout)
{
	t->ltransfer = libusb_alloc_transfer(0);
	if (!t->ltransfer)
		return -ENOMEM;

	t->transfer_finished = transfer_finished;
	t->size = size;
	libusb_fill_bulk_transfer(t->ltransfer, devh, ep,
				  buf, size, t_usb_transfer_finished, t,
				  0);

	return 0;
}

int t_usb_handle_events_completed(int *completed)
{
	struct timeval tv = {0, 0};
	int ret = 0;

	while (!*completed) {
		ret = libusb_handle_events_timeout_completed(NULL,
							     &tv,
							     completed);
		if (ret < 0 && ret != LIBUSB_ERROR_BUSY
		    && ret != LIBUSB_ERROR_TIMEOUT
		    && ret != LIBUSB_ERROR_OVERFLOW
		    && ret != LIBUSB_ERROR_INTERRUPTED)
			break;
		else
			ret = 0;
	}

	return ret;
}