greybus: start parsing descriptor fields

diff --git a/drivers/staging/greybus/core.c b/drivers/staging/greybus/core.c
index f4b562c..9e9ffa0 100644 (file)
--- 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 de31da8..9f37fc6 100644 (file)
--- 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 93d7dae..6d82806 100644 (file)
--- 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 e7355a3..37cc99a 100644 (file)
--- 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,
 };