greybus: start parsing descriptor fields

4 files changed, 126 insertions, 40 deletions

diff --git a/drivers/staging/greybus/core.c b/drivers/staging/greybus/core.c
index f4b562c709d9..9e9ffa0420e4 100644
--- a/drivers/staging/greybus/core.c
+++ b/drivers/staging/greybus/core.c
@@ -195,6 +195,76 @@ error_i2c:
 static const struct greybus_module_id fake_gb_id =
 	{ GREYBUS_DEVICE(0x42, 0x42) };
 
+static int create_function(struct greybus_device *gdev,
+			   struct greybus_descriptor *desc, int desc_size)
+{
+	int header_size = sizeof(struct greybus_descriptor_function);
+
+	if (desc_size != header_size) {
+		pr_err("invalid function header size %d\n", desc_size);
+		return -EINVAL;
+	}
+	memcpy(&gdev->function, &desc->function, header_size);
+	return 0;
+}
+
+static int create_module_id(struct greybus_device *gdev,
+			    struct greybus_descriptor *desc, int desc_size)
+{
+	int header_size = sizeof(struct greybus_descriptor_module_id);
+
+	if (desc_size != header_size) {
+		pr_err("invalid module header size %d\n", desc_size);
+		return -EINVAL;
+	}
+	memcpy(&gdev->module_id, &desc->module_id, header_size);
+	return 0;
+}
+
+static int create_serial_number(struct greybus_device *gdev,
+				struct greybus_descriptor *desc, int desc_size)
+{
+	int header_size = sizeof(struct greybus_descriptor_serial_number);
+
+	if (desc_size != header_size) {
+		pr_err("invalid serial number header size %d\n", desc_size);
+		return -EINVAL;
+	}
+	memcpy(&gdev->serial_number, &desc->serial_number, header_size);
+	return 0;
+}
+
+static int create_string(struct greybus_device *gdev,
+			 struct greybus_descriptor *desc, int desc_size)
+{
+	int string_size;
+	struct gdev_string *string;
+	int header_size  = sizeof(struct greybus_descriptor_string);
+
+	if ((gdev->num_strings + 1) >= MAX_STRINGS_PER_MODULE) {
+		pr_err("too many strings for this module!\n");
+		return -EINVAL;
+	}
+
+	if (desc_size < header_size) {
+		pr_err("invalid string header size %d\n", desc_size);
+		return -EINVAL;
+	}
+
+	string_size = le16_to_cpu(desc->string.length);
+	string = kzalloc(sizeof(*string) + string_size + 1, GFP_KERNEL);
+	if (!string)
+		return -ENOMEM;
+
+	string->length = string_size;
+	string->id = desc->string.id;
+	memcpy(&string->string, &desc->string.string, string_size);
+	gdev->string[gdev->num_strings] = string;
+	gdev->num_strings++;
+
+	return 0;
+}
+
 /**
  * greybus_new_device:
  *
@@ -210,6 +280,7 @@ struct greybus_device *greybus_new_device(int module_number, u8 *data, int size)
 	int overall_size;
 	int header_size;
 	int desc_size;
+	int i;
 	u8 version_major;
 	u8 version_minor;
 
@@ -244,62 +315,64 @@ struct greybus_device *greybus_new_device(int module_number, u8 *data, int size)
 
 		switch (desc->header.type) {
 		case GREYBUS_TYPE_FUNCTION:
-			header_size =
-				sizeof(struct greybus_descriptor_function);
-			if (desc_size != header_size) {
-				pr_err("invalid function header size %d\n",
-				       desc_size);
-				goto error;
-			}
-			memcpy(&gdev->function, &desc->function, header_size);
-			size -= header_size;
-			data += header_size;
+			retval = create_function(gdev, desc, desc_size);
 			break;
 
 		case GREYBUS_TYPE_MODULE_ID:
-			header_size =
-				sizeof(struct greybus_descriptor_module_id);
-			if (desc_size != header_size) {
-				pr_err("invalid module header size %d\n",
-				       desc_size);
-				goto error;
-			}
-			memcpy(&gdev->module_id, &desc->module_id, header_size);
-			size -= header_size;
-			data += header_size;
+			retval = create_module_id(gdev, desc, desc_size);
 			break;
 
 		case GREYBUS_TYPE_SERIAL_NUMBER:
-			header_size =
-				sizeof(struct greybus_descriptor_serial_number);
+			retval = create_serial_number(gdev, desc, desc_size);
+			break;
+
+		case GREYBUS_TYPE_STRING:
+			retval = create_string(gdev, desc, desc_size);
+			break;
+
+		case GREYBUS_TYPE_CPORT: {
+			struct gdev_cport *cport;
+
+			header_size = sizeof(struct greybus_descriptor_cport);
 			if (desc_size != header_size) {
 				pr_err("invalid serial number header size %d\n",
 				       desc_size);
 				goto error;
 			}
-			memcpy(&gdev->serial_number, &desc->serial_number,
-			       header_size);
-			size -= header_size;
-			data += header_size;
+			cport = kzalloc(sizeof(*cport), GFP_KERNEL);
+			if (!cport)
+				goto error;
+			cport->number = le16_to_cpu(desc->cport.number);
+			cport->size = le16_to_cpu(desc->cport.size);
+			cport->speed = desc->cport.speed;
+			gdev->cport[gdev->num_cports] = cport;
+			gdev->num_cports++;
+			// FIXME - check for too many cports...
+
+			size -= desc_size;
+			data += desc_size;
 			break;
-
-		case GREYBUS_TYPE_DEVICE_STRING:
-		case GREYBUS_TYPE_CPORT:
+			}
 		case GREYBUS_TYPE_INVALID:
 		default:
 			pr_err("invalid descriptor type %d\n", desc->header.type);
 			goto error;
 		}
-#if 0
-		struct greybus_descriptor_string	string;
-		struct greybus_descriptor_cport		cport;
-#endif
+		if (retval)
+			goto error;
+		size -= desc_size;
+		data += desc_size;
 	}
+
 	retval = gb_init_subdevs(gdev, &fake_gb_id);
 	if (retval)
 		goto error;
 	return gdev;
 error:
+	for (i = 0; i < gdev->num_strings; ++i)
+		kfree(gdev->string[i]);
+	for (i = 0; i < gdev->num_cports; ++i)
+		kfree(gdev->cport[i]);
 	kfree(gdev);
 	return NULL;
 }
diff --git a/drivers/staging/greybus/gbuf.c b/drivers/staging/greybus/gbuf.c
index de31da8165de..9f37fc6e31cf 100644
--- a/drivers/staging/greybus/gbuf.c
+++ b/drivers/staging/greybus/gbuf.c
@@ -18,7 +18,7 @@
 
 
 struct gbuf *greybus_alloc_gbuf(struct greybus_device *gdev,
-				struct cport *cport,
+				struct gdev_cport *cport,
 				gfp_t mem_flags)
 {
 	return NULL;
diff --git a/drivers/staging/greybus/greybus.h b/drivers/staging/greybus/greybus.h
index 93d7dae61012..6d82806e3e00 100644
--- a/drivers/staging/greybus/greybus.h
+++ b/drivers/staging/greybus/greybus.h
@@ -35,10 +35,17 @@
 
 struct gbuf;
 
-struct cport {
+struct gdev_cport {
 	u16	number;
 	u16	size;
 	// FIXME, what else?
+	u8	speed;	// valid???
+};
+
+struct gdev_string {
+	u16	length;
+	u8	id;
+	u8	string[0];
 };
 
 typedef void (*gbuf_complete_t)(struct gbuf *gbuf);
@@ -51,7 +58,7 @@ struct gbuf {
 	struct gbuf_anchor *anchor;	// FIXME do we need?
 
 	struct greybus_device *gdev;
-	struct cport *cport;
+	struct gdev_cport *cport;
 	int status;
 	void *transfer_buffer;
 	u32 transfer_flags;		/* flags for the transfer buffer */
@@ -86,14 +93,20 @@ struct gb_sdio_host;
 struct gb_tty;
 struct gb_usb_device;
 
+/* Increase these values if needed */
+#define MAX_CPORTS_PER_MODULE	10
+#define MAX_STRINGS_PER_MODULE	10
+
 struct greybus_device {
 	struct device dev;
 	u16 module_number;
 	struct greybus_descriptor_function function;
 	struct greybus_descriptor_module_id module_id;
 	struct greybus_descriptor_serial_number serial_number;
-	int num_cport;
-	struct cport *cport[10];		// FIXME - no more than 10 cports per device...
+	int num_cports;
+	int num_strings;
+	struct gdev_cport *cport[MAX_CPORTS_PER_MODULE];
+	struct gdev_string *string[MAX_STRINGS_PER_MODULE];
 
 	struct gb_i2c_device *gb_i2c_dev;
 	struct gb_gpio_device *gb_gpio_dev;
@@ -104,7 +117,7 @@ struct greybus_device {
 #define to_greybus_device(d) container_of(d, struct greybus_device, dev)
 
 struct gbuf *greybus_alloc_gbuf(struct greybus_device *gdev,
-				struct cport *cport,
+				struct gdev_cport *cport,
 				gfp_t mem_flags);
 void greybus_free_gbuf(struct gbuf *gbuf);
 
diff --git a/drivers/staging/greybus/greybus_desc.h b/drivers/staging/greybus/greybus_desc.h
index e7355a365c92..37cc99ace3bf 100644
--- a/drivers/staging/greybus/greybus_desc.h
+++ b/drivers/staging/greybus/greybus_desc.h
@@ -21,7 +21,7 @@ enum greybus_descriptor_type {
 	GREYBUS_TYPE_FUNCTION		= 0x0001,
 	GREYBUS_TYPE_MODULE_ID		= 0x0002,
 	GREYBUS_TYPE_SERIAL_NUMBER	= 0x0003,
-	GREYBUS_TYPE_DEVICE_STRING	= 0x0004,
+	GREYBUS_TYPE_STRING		= 0x0004,
 	GREYBUS_TYPE_CPORT		= 0x0005,
 };