Add functionality to access device descriptors

6 files changed, 518 insertions, 46 deletions

diff --git a/TODO b/TODO
index 2e20edb..9956176 100644
--- a/TODO
+++ b/TODO
@@ -4,3 +4,5 @@ API docs
 notifications of hotplugged/unplugged devices
 thread safety
 signalfd emulation through pipes and sigaction() for older kernels
+signalfd not needed for usbfs? can we poll on the fd?
+use poll() rather than select()?
diff --git a/libfpusb/Makefile.am b/libfpusb/Makefile.am
index 85ee8a8..9e9bee2 100644
--- a/libfpusb/Makefile.am
+++ b/libfpusb/Makefile.am
@@ -1,7 +1,7 @@
 lib_LTLIBRARIES = libfpusb.la
 
 libfpusb_la_CFLAGS = -fvisibility=hidden $(AM_CFLAGS)
-libfpusb_la_SOURCES = signalfd.h fpusbi.h usbfs.h core.c io.c
+libfpusb_la_SOURCES = signalfd.h fpusbi.h usbfs.h core.c descriptor.c io.c
 libfpusb_la_LIBADD = -lrt
 
 pkginclude_HEADERS = fpusb.h
diff --git a/libfpusb/core.c b/libfpusb/core.c
index b2e8d18..7342c2d 100644
--- a/libfpusb/core.c
+++ b/libfpusb/core.c
@@ -41,72 +41,107 @@
 static struct list_head usb_devs;
 struct list_head open_devs;
 
-static int parse_descriptor(unsigned char *source, char *descriptor, void *dest)
-{
-	unsigned char *sp = source, *dp = dest;
-	uint16_t w;
-	uint32_t d;
-	char *cp;
-
-	for (cp = descriptor; *cp; cp++) {
-		switch (*cp) {
-			case 'b':	/* 8-bit byte */
-				*dp++ = *sp++;
-				break;
-			case 'w':	/* 16-bit word, convert from little endian to CPU */
-				w = (sp[1] << 8) | sp[0]; sp += 2;
-				dp += ((unsigned long)dp & 1);	/* Align to word boundary */
-				*((uint16_t *)dp) = w; dp += 2;
-				break;
-			case 'd':	/* 32-bit dword, convert from little endian to CPU */
-				d = (sp[3] << 24) | (sp[2] << 16) | (sp[1] << 8) | sp[0]; sp += 4;
-				dp += ((unsigned long)dp & 2);	/* Align to dword boundary */
-				*((uint32_t *)dp) = d; dp += 4;
-				break;
-			case 'W':	/* 16-bit word, keep CPU endianess */
-				dp += ((unsigned long)dp & 1);	/* Align to word boundary */
-				memcpy(dp, sp, 2); sp += 2; dp += 2;
-				break;
-			case 'D':	/* 32-bit dword, keep CPU endianess */
-				dp += ((unsigned long)dp & 2);	/* Align to dword boundary */
-				memcpy(dp, sp, 4); sp += 4; dp += 4;
-				break;
-		}
-	}
-
-	return sp - source;
-}
-
 static int scan_device(char *busdir, const char *devnum)
 {
 	char path[PATH_MAX + 1];
-    unsigned char raw_desc[DEVICE_DESC_LENGTH];
+	unsigned char raw_desc[DEVICE_DESC_LENGTH];
 	struct fpusb_dev *dev = malloc(sizeof(*dev));
-	int fd;
+	int fd = 0;
+	int i;
 	int r;
+	int tmp;
+
+	if (!dev)
+		return -1;
 
 	snprintf(path, PATH_MAX, "%s/%s", busdir, devnum);
 	fp_dbg("%s", path);
 	fd = open(path, O_RDWR);
 	if (!fd) {
 		fp_dbg("open '%s' failed, ret=%d errno=%d", path, fd, errno);
-		return -1;
+		r = -1;
+		goto err;
 	}
 
-    r = read(fd, raw_desc, DEVICE_DESC_LENGTH);
+	r = read(fd, raw_desc, DEVICE_DESC_LENGTH);
 	if (r < 0) {
 		fp_err("read failed ret=%d errno=%d", r, errno);
-		return r;
+		goto err;
 	}
 	/* FIXME: short read handling? */
 
-    parse_descriptor(raw_desc, "bbWbbbbWWWbbbb", &dev->desc);
-	fp_dbg("found device %04x:%04x", dev->desc.idVendor, dev->desc.idProduct);
+	fpi_parse_descriptor(raw_desc, "bbWbbbbWWWbbbb", &dev->desc);
+
+	/* Now try to fetch the rest of the descriptors */
+	if (dev->desc.bNumConfigurations > USB_MAXCONFIG) {
+		fp_err("too many configurations");
+		r = -1;
+		goto err;
+	}
+
+	if (dev->desc.bNumConfigurations < 1) {
+		fp_dbg("no configurations?");
+		r = -1;
+		goto err;
+	}
+
+	tmp = dev->desc.bNumConfigurations * sizeof(struct usb_config_descriptor);
+	dev->config = malloc(tmp);
+	if (!dev->config) {
+		r = -1;
+		goto err;
+	}
+
+	memset(dev->config, 0, tmp);
+
+	for (i = 0; i < dev->desc.bNumConfigurations; i++) {
+		unsigned char buffer[8], *bigbuffer;
+		struct usb_config_descriptor config;
+
+		/* Get the first 8 bytes to figure out what the total length is */
+		r = read(fd, buffer, sizeof(buffer));
+		if (r < sizeof(buffer)) {
+			fp_err("short descriptor read (%d/%d)", r, sizeof(buffer));
+			goto err;
+		}
+
+		fpi_parse_descriptor(buffer, "bbw", &config);
+
+		bigbuffer = malloc(config.wTotalLength);
+		if (!bigbuffer)
+			goto err;
+
+		/* Read the rest of the config descriptor */
+		memcpy(bigbuffer, buffer, sizeof(buffer));
+
+		tmp = config.wTotalLength - 8;
+		r = read(fd, bigbuffer + 8, tmp);
+		if (r < tmp) {
+			fp_err("short descriptor read (%d/%d)", r, tmp);
+			free(bigbuffer);
+			goto err;
+		}
+
+		r = fpi_parse_configuration(&dev->config[i], bigbuffer);
+		if (r > 0)
+			fp_warn("descriptor data still left\n");
+		free(bigbuffer);
+	}
+
 	dev->nodepath = strdup(path);
+	if (!dev->nodepath)
+		goto err;
+
+	fp_dbg("found device %04x:%04x", dev->desc.idVendor, dev->desc.idProduct);
 	list_add(&dev->list, &usb_devs);
+	r = 0;
 
-	close(fd);
-	return 0;
+err:
+	if (fd)
+		close(fd);
+	if (r < 0 && dev)
+		free(dev);
+	return r;
 }
 
 static int scan_busdir(const char *busnum)
@@ -177,6 +212,12 @@ API_EXPORTED struct usb_dev_descriptor *fpusb_dev_get_descriptor(
 	return &dev->desc;
 }
 
+API_EXPORTED struct usb_config_descriptor *fpusb_dev_get_config(
+	struct fpusb_dev *dev)
+{
+	return dev->config;
+}
+
 API_EXPORTED struct fpusb_dev_handle *fpusb_devh_open(struct fpusb_dev *dev)
 {
 	struct fpusb_dev_handle *devh;
@@ -217,6 +258,11 @@ API_EXPORTED void fpusb_devh_close(struct fpusb_dev_handle *devh)
 	free(devh);
 }
 
+API_EXPORTED struct fpusb_dev *fpusb_devh_get_dev(struct fpusb_dev_handle *devh)
+{
+	return devh->dev;
+}
+
 API_EXPORTED int fpusb_devh_claim_intf(struct fpusb_dev_handle *dev,
 	int iface)
 {
diff --git a/libfpusb/descriptor.c b/libfpusb/descriptor.c
new file mode 100644
index 0000000..1f2333d
--- /dev/null
+++ b/libfpusb/descriptor.c
@@ -0,0 +1,355 @@
+/*
+ * USB descriptor handling functions for libfpusb
+ * Copyright (C) 2007 Daniel Drake <dsd@gentoo.org>
+ *
+ * Portions based on libusb-0.1
+ * Copyright (c) 2001 Johannes Erdfelt <johannes@erdfelt.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "fpusbi.h"
+
+#define DESC_HEADER_LENGTH		2
+#define DEVICE_DESC_LENGTH		18
+#define CONFIG_DESC_LENGTH		9
+#define INTERFACE_DESC_LENGTH		9
+#define ENDPOINT_DESC_LENGTH		7
+#define ENDPOINT_AUDIO_DESC_LENGTH	9
+
+int fpi_parse_descriptor(unsigned char *source, char *descriptor, void *dest)
+{
+	unsigned char *sp = source, *dp = dest;
+	uint16_t w;
+	uint32_t d;
+	char *cp;
+
+	for (cp = descriptor; *cp; cp++) {
+		switch (*cp) {
+			case 'b':	/* 8-bit byte */
+				*dp++ = *sp++;
+				break;
+			case 'w':	/* 16-bit word, convert from little endian to CPU */
+				w = (sp[1] << 8) | sp[0]; sp += 2;
+				dp += ((unsigned long)dp & 1);	/* Align to word boundary */
+				*((uint16_t *)dp) = w; dp += 2;
+				break;
+			case 'd':	/* 32-bit dword, convert from little endian to CPU */
+				d = (sp[3] << 24) | (sp[2] << 16) | (sp[1] << 8) | sp[0]; sp += 4;
+				dp += ((unsigned long)dp & 2);	/* Align to dword boundary */
+				*((uint32_t *)dp) = d; dp += 4;
+				break;
+			case 'W':	/* 16-bit word, keep CPU endianess */
+				dp += ((unsigned long)dp & 1);	/* Align to word boundary */
+				memcpy(dp, sp, 2); sp += 2; dp += 2;
+				break;
+			case 'D':	/* 32-bit dword, keep CPU endianess */
+				dp += ((unsigned long)dp & 2);	/* Align to dword boundary */
+				memcpy(dp, sp, 4); sp += 4; dp += 4;
+				break;
+		}
+	}
+
+	return sp - source;
+}
+
+static int parse_endpoint(struct usb_endpoint_descriptor *endpoint,
+	unsigned char *buffer, int size)
+{
+	struct usb_descriptor_header header;
+	unsigned char *begin;
+	int parsed = 0;
+	int len;
+
+	fpi_parse_descriptor(buffer, "bb", &header);
+
+	/* Everything should be fine being passed into here, but we sanity */
+	/*  check JIC */
+	if (header.bLength > size) {
+		fp_err("ran out of descriptors parsing");
+		return -1;
+	}
+
+	if (header.bDescriptorType != USB_DT_ENDPOINT) {
+		fp_err("unexpected descriptor %x (expected %x)",
+			header.bDescriptorType, USB_DT_ENDPOINT);
+		return parsed;
+	}
+
+	if (header.bLength >= ENDPOINT_AUDIO_DESC_LENGTH)
+		fpi_parse_descriptor(buffer, "bbbbwbbb", endpoint);
+	else if (header.bLength >= ENDPOINT_DESC_LENGTH)
+		fpi_parse_descriptor(buffer, "bbbbwb", endpoint);
+
+	buffer += header.bLength;
+	size -= header.bLength;
+	parsed += header.bLength;
+
+	/* Skip over the rest of the Class Specific or Vendor Specific */
+	/*  descriptors */
+	begin = buffer;
+	while (size >= DESC_HEADER_LENGTH) {
+		fpi_parse_descriptor(buffer, "bb", &header);
+
+		if (header.bLength < 2) {
+			fp_err("invalid descriptor length %d", header.bLength);
+			return -1;
+		}
+
+		/* If we find another "proper" descriptor then we're done  */
+		if ((header.bDescriptorType == USB_DT_ENDPOINT) ||
+				(header.bDescriptorType == USB_DT_INTERFACE) ||
+				(header.bDescriptorType == USB_DT_CONFIG) ||
+				(header.bDescriptorType == USB_DT_DEVICE))
+			break;
+
+		fp_dbg("skipping descriptor %x", header.bDescriptorType);
+		buffer += header.bLength;
+		size -= header.bLength;
+		parsed += header.bLength;
+	}
+
+	/* Copy any unknown descriptors into a storage area for drivers */
+	/*  to later parse */
+	len = (int)(buffer - begin);
+	if (!len) {
+		endpoint->extra = NULL;
+		endpoint->extralen = 0;
+		return parsed;
+	}
+
+	endpoint->extra = malloc(len);
+	if (!endpoint->extra) {
+		endpoint->extralen = 0;
+		return parsed;
+	}
+
+	memcpy(endpoint->extra, begin, len);
+	endpoint->extralen = len;
+
+	return parsed;
+}
+
+static int parse_interface(struct usb_interface *interface,
+	unsigned char *buffer, int size)
+{
+	int i;
+	int len;
+	int r;
+	int parsed = 0;
+	int tmp;
+	struct usb_descriptor_header header;
+	struct usb_interface_descriptor *ifp;
+	unsigned char *begin;
+
+	interface->num_altsetting = 0;
+
+	while (size >= INTERFACE_DESC_LENGTH) {
+		interface->altsetting = realloc(interface->altsetting,
+			sizeof(struct usb_interface_descriptor) *
+			(interface->num_altsetting + 1));
+		if (!interface->altsetting)
+			return -1;
+
+		ifp = interface->altsetting + interface->num_altsetting;
+		interface->num_altsetting++;
+		fpi_parse_descriptor(buffer, "bbbbbbbbb", ifp);
+
+		/* Skip over the interface */
+		buffer += ifp->bLength;
+		parsed += ifp->bLength;
+		size -= ifp->bLength;
+
+		begin = buffer;
+
+		/* Skip over any interface, class or vendor descriptors */
+		while (size >= DESC_HEADER_LENGTH) {
+			fpi_parse_descriptor(buffer, "bb", &header);
+			if (header.bLength < 2) {
+				fp_err("invalid descriptor of length %d", header.bLength);
+				return -1;
+			}
+
+			/* If we find another "proper" descriptor then we're done */
+			if ((header.bDescriptorType == USB_DT_INTERFACE) ||
+					(header.bDescriptorType == USB_DT_ENDPOINT) ||
+					(header.bDescriptorType == USB_DT_CONFIG) ||
+					(header.bDescriptorType == USB_DT_DEVICE))
+				break;
+
+			buffer += header.bLength;
+			parsed += header.bLength;
+			size -= header.bLength;
+		}
+
+		/* Copy any unknown descriptors into a storage area for */
+		/*  drivers to later parse */
+		len = (int)(buffer - begin);
+		if (!len) {
+			ifp->extra = NULL;
+			ifp->extralen = 0;
+		} else {
+			ifp->extra = malloc(len);
+			if (!ifp->extra) {
+				ifp->extralen = 0;
+				/* FIXME will leak memory */
+				return -1;
+			}
+			memcpy(ifp->extra, begin, len);
+			ifp->extralen = len;
+		}
+
+		/* Did we hit an unexpected descriptor? */
+		fpi_parse_descriptor(buffer, "bb", &header);
+		if ((size >= DESC_HEADER_LENGTH) &&
+				((header.bDescriptorType == USB_DT_CONFIG) ||
+				 (header.bDescriptorType == USB_DT_DEVICE)))
+			return parsed;
+
+		if (ifp->bNumEndpoints > USB_MAXENDPOINTS) {
+			fp_err("too many endpoints (%d)", ifp->bNumEndpoints);
+			/* FIXME will leak memory */
+			return -1;
+		}
+
+		if (ifp->bNumEndpoints > 0) {
+			tmp = ifp->bNumEndpoints * sizeof(struct usb_endpoint_descriptor);
+			ifp->endpoint = malloc(tmp);
+			if (!ifp->endpoint)
+				/* FIXME will leak memory? */
+				return -1;      
+
+			memset(ifp->endpoint, 0, tmp);
+			for (i = 0; i < ifp->bNumEndpoints; i++) {
+				fpi_parse_descriptor(buffer, "bb", &header);
+
+				if (header.bLength > size) {
+					fp_err("ran out of descriptors parsing");
+					/* FIXME will leak memory */
+					return -1;
+				}
+
+				r = parse_endpoint(ifp->endpoint + i, buffer, size);
+				if (r < 0)
+					/* FIXME will leak memory */
+					return r;
+
+				buffer += r;
+				parsed += r;
+				size -= r;
+			}
+		} else
+			ifp->endpoint = NULL;
+
+		/* We check to see if it's an alternate to this one */
+		ifp = (struct usb_interface_descriptor *) buffer;
+		if (size < USB_DT_INTERFACE_SIZE ||
+				ifp->bDescriptorType != USB_DT_INTERFACE ||
+				!ifp->bAlternateSetting)
+			return parsed;
+	}
+
+	return parsed;
+}
+
+int fpi_parse_configuration(struct usb_config_descriptor *config,
+	unsigned char *buffer)
+{
+	int i;
+	int r;
+	int size;
+	int tmp;
+	struct usb_descriptor_header header;
+
+	fpi_parse_descriptor(buffer, "bbwbbbbb", config);
+	size = config->wTotalLength;
+
+	if (config->bNumInterfaces > USB_MAXINTERFACES) {
+		fp_err("too many interfaces (%d)", config->bNumInterfaces);
+		return -1;
+	}
+
+	tmp = config->bNumInterfaces * sizeof(struct usb_interface);
+	config->interface = malloc(tmp);
+	if (!config->interface)
+		return -1;      
+
+	memset(config->interface, 0, tmp);
+	buffer += config->bLength;
+	size -= config->bLength;
+
+	config->extra = NULL;
+	config->extralen = 0;
+
+	for (i = 0; i < config->bNumInterfaces; i++) {
+		int len;
+		unsigned char *begin;
+
+		/* Skip over the rest of the Class Specific or Vendor */
+		/*  Specific descriptors */
+		begin = buffer;
+		while (size >= DESC_HEADER_LENGTH) {
+			fpi_parse_descriptor(buffer, "bb", &header);
+
+			if ((header.bLength > size) ||
+					(header.bLength < DESC_HEADER_LENGTH)) {
+				fp_err("invalid descriptor length of %d", header.bLength);
+				return -1;
+			}
+
+			/* If we find another "proper" descriptor then we're done */
+			if ((header.bDescriptorType == USB_DT_ENDPOINT) ||
+					(header.bDescriptorType == USB_DT_INTERFACE) ||
+					(header.bDescriptorType == USB_DT_CONFIG) ||
+					(header.bDescriptorType == USB_DT_DEVICE))
+				break;
+
+			fp_dbg("skipping descriptor 0x%x\n", header.bDescriptorType);
+			buffer += header.bLength;
+			size -= header.bLength;
+		}
+
+		/* Copy any unknown descriptors into a storage area for */
+		/*  drivers to later parse */
+		len = (int)(buffer - begin);
+		if (len) {
+			/* FIXME: We should realloc and append here */
+			if (!config->extralen) {
+				config->extra = malloc(len);
+				if (!config->extra) {
+					config->extralen = 0;
+					/* FIXME will leak memory */
+					return -1;
+				}
+
+				memcpy(config->extra, begin, len);
+				config->extralen = len;
+			}
+		}
+
+		r = parse_interface(config->interface + i, buffer, size);
+		if (r < 0)
+			return r;
+
+		buffer += r;
+		size -= r;
+	}
+
+	return size;
+}
+
diff --git a/libfpusb/fpusb.h b/libfpusb/fpusb.h
index 22f3536..0d54be5 100644
--- a/libfpusb/fpusb.h
+++ b/libfpusb/fpusb.h
@@ -113,6 +113,58 @@ struct usb_dev_descriptor {
 	uint8_t  bNumConfigurations;
 };
 
+struct usb_endpoint_descriptor {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+	uint8_t  bEndpointAddress;
+	uint8_t  bmAttributes;
+	uint16_t wMaxPacketSize;
+	uint8_t  bInterval;
+	uint8_t  bRefresh;
+	uint8_t  bSynchAddress;
+
+	unsigned char *extra;	/* Extra descriptors */
+	int extralen;
+};
+
+struct usb_interface_descriptor {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+	uint8_t  bInterfaceNumber;
+	uint8_t  bAlternateSetting;
+	uint8_t  bNumEndpoints;
+	uint8_t  bInterfaceClass;
+	uint8_t  bInterfaceSubClass;
+	uint8_t  bInterfaceProtocol;
+	uint8_t  iInterface;
+
+	struct usb_endpoint_descriptor *endpoint;
+
+	unsigned char *extra;	/* Extra descriptors */
+	int extralen;
+};
+
+struct usb_interface {
+	struct usb_interface_descriptor *altsetting;
+	int num_altsetting;
+};
+
+struct usb_config_descriptor {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+	uint16_t wTotalLength;
+	uint8_t  bNumInterfaces;
+	uint8_t  bConfigurationValue;
+	uint8_t  iConfiguration;
+	uint8_t  bmAttributes;
+	uint8_t  MaxPower;
+
+	struct usb_interface *interface;
+
+	unsigned char *extra;	/* Extra descriptors */
+	int extralen;
+};
+
 /* fpusb */
 
 struct fpusb_dev;
@@ -160,10 +212,12 @@ void fpusb_exit(void);
 int fpusb_find_devices(void);
 fpusb_dev *fpusb_get_devices(void);
 struct usb_dev_descriptor *fpusb_dev_get_descriptor(fpusb_dev *dev);
+struct usb_config_descriptor *fpusb_dev_get_config(fpusb_dev *dev);
 fpusb_dev *fpusb_dev_next(fpusb_dev *dev);
 
 fpusb_dev_handle *fpusb_devh_open(fpusb_dev *dev);
 void fpusb_devh_close(fpusb_dev_handle *devh);
+struct fpusb_dev *fpusb_devh_get_dev(fpusb_dev_handle *devh);
 int fpusb_devh_claim_intf(fpusb_dev_handle *dev, int iface);
 int fpusb_devh_release_intf(fpusb_dev_handle *dev, int iface);
 
diff --git a/libfpusb/fpusbi.h b/libfpusb/fpusbi.h
index b4287f4..ea5a24a 100644
--- a/libfpusb/fpusbi.h
+++ b/libfpusb/fpusbi.h
@@ -36,6 +36,10 @@
 #define USBFS_PATH "/dev/bus/usb"
 #define DEVICE_DESC_LENGTH		18
 
+#define USB_MAXENDPOINTS	32
+#define USB_MAXINTERFACES	32
+#define USB_MAXCONFIG		8
+
 struct list_head {
 	struct list_head *prev, *next;
 };
@@ -145,6 +149,7 @@ struct fpusb_dev {
 	struct list_head list;
 	char *nodepath;
 	struct usb_dev_descriptor desc;
+	struct usb_config_descriptor *config;
 };
 
 struct fpusb_dev_handle {
@@ -189,6 +194,12 @@ struct usb_ctrl_setup {
 	uint16_t wLength;
 } __attribute__((packed));
 
+/* All standard descriptors have these 2 fields in common */
+struct usb_descriptor_header {
+	uint8_t  bLength;
+	uint8_t  bDescriptorType;
+};
+
 /* shared data and functions */
 
 extern struct list_head open_devs;
@@ -196,5 +207,9 @@ extern struct list_head open_devs;
 int fpi_io_init(int _signum);
 void fpi_io_exit(void);
 
+int fpi_parse_descriptor(unsigned char *source, char *descriptor, void *dest);
+int fpi_parse_configuration(struct usb_config_descriptor *config,
+		unsigned char *buffer);
+
 #endif