Add libusb backend for thor protocol

Add implementation of functions used for communication
with tharget on USB level via libusb.

Change-Id: I59603b3bb05570e8f13c8ca3e4f48fabf2fc03c0
Signed-off-by: Krzysztof Opasiak <k.opasiak@samsung.com>

2 files changed, 590 insertions, 0 deletions

diff --git a/libthor/thor_internal.h b/libthor/thor_internal.h
index 05f7671..20085b4 100644
--- a/libthor/thor_internal.h
+++ b/libthor/thor_internal.h
@@ -48,5 +48,22 @@ int t_file_get_data_src(const char *path, struct thor_data_src **data);
 
 int t_tar_get_data_src(const char *path, struct thor_data_src **data);
 
+int t_usb_send(struct thor_device_handle *th, unsigned char *buf,
+	       size_t count, int timeout);
+
+int t_usb_recv(struct thor_device_handle *th, unsigned char *buf,
+	       size_t count, int timeout);
+
+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);
+
+int t_usb_recv_req(struct thor_device_handle *th, struct res_pkt *resp);
+
+int t_usb_find_device(struct thor_device_id *dev_id, int wait,
+		      struct thor_device_handle *th);
+
+void t_usb_close_device(struct thor_device_handle *th);
+
 #endif /* THOR_INTERNAL_H__ */
 
diff --git a/libthor/thor_usb.c b/libthor/thor_usb.c
new file mode 100644
index 0000000..791826f
--- /dev/null
+++ b/libthor/thor_usb.c
@@ -0,0 +1,573 @@
+#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;
+}
+