Staging: ipack: Read the ID space during device registration.

We keep a copy of the ID space for later use.

Signed-off-by: Jens Taprogge <jens.taprogge@taprogge.org>
Signed-off-by: Samuel Iglesias Gonsálvez <siglesias@igalia.com>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>

diff --git a/drivers/staging/ipack/ipack.c b/drivers/staging/ipack/ipack.c
index c1cd97a..da9e7bd 100644 (file)
--- a/drivers/staging/ipack/ipack.c
+++ b/drivers/staging/ipack/ipack.c
@@ -22,6 +22,7 @@ static DEFINE_IDA(ipack_ida);
 static void ipack_device_release(struct device *dev)
 {
        struct ipack_device *device = to_ipack_dev(dev);
+       kfree(device->id);
        kfree(device);
 }
 
@@ -117,6 +118,77 @@ void ipack_driver_unregister(struct ipack_driver *edrv)
 }
 EXPORT_SYMBOL_GPL(ipack_driver_unregister);
 
+static int ipack_device_read_id(struct ipack_device *dev)
+{
+       u8 __iomem *idmem;
+       int i;
+       int ret = 0;
+
+       ret = dev->bus->ops->map_space(dev, 0, IPACK_ID_SPACE);
+       if (ret) {
+               dev_err(&dev->dev, "error mapping memory\n");
+               return ret;
+       }
+       idmem = dev->id_space.address;
+
+       /* Determine ID PROM Data Format.  If we find the ids "IPAC" or "IPAH"
+        * we are dealing with a IndustryPack  format 1 device.  If we detect
+        * "VITA4 " (16 bit big endian formatted) we are dealing with a
+        * IndustryPack format 2 device */
+       if ((ioread8(idmem + 1) == 'I') &&
+                       (ioread8(idmem + 3) == 'P') &&
+                       (ioread8(idmem + 5) == 'A') &&
+                       ((ioread8(idmem + 7) == 'C') ||
+                        (ioread8(idmem + 7) == 'H'))) {
+               dev->id_format = IPACK_ID_VERSION_1;
+               dev->id_avail = ioread8(idmem + 0x15);
+               if ((dev->id_avail < 0x0c) || (dev->id_avail > 0x40)) {
+                       dev_warn(&dev->dev, "invalid id size");
+                       dev->id_avail = 0x0c;
+               }
+       } else if ((ioread8(idmem + 0) == 'I') &&
+                       (ioread8(idmem + 1) == 'V') &&
+                       (ioread8(idmem + 2) == 'A') &&
+                       (ioread8(idmem + 3) == 'T') &&
+                       (ioread8(idmem + 4) == ' ') &&
+                       (ioread8(idmem + 5) == '4')) {
+               dev->id_format = IPACK_ID_VERSION_2;
+               dev->id_avail = ioread16be(idmem + 0x16);
+               if ((dev->id_avail < 0x1a) || (dev->id_avail > 0x40)) {
+                       dev_warn(&dev->dev, "invalid id size");
+                       dev->id_avail = 0x1a;
+               }
+       } else {
+               dev->id_format = IPACK_ID_VERSION_INVALID;
+               dev->id_avail = 0;
+       }
+
+       if (!dev->id_avail) {
+               ret = -ENODEV;
+               goto out;
+       }
+
+       /* Obtain the amount of memory required to store a copy of the complete
+        * ID ROM contents */
+       dev->id = kmalloc(dev->id_avail, GFP_KERNEL);
+       if (!dev->id) {
+               dev_err(&dev->dev, "dev->id alloc failed.\n");
+               ret = -ENOMEM;
+               goto out;
+       }
+       for (i = 0; i < dev->id_avail; i++) {
+               if (dev->id_format == IPACK_ID_VERSION_1)
+                       dev->id[i] = ioread8(idmem + (i << 1) + 1);
+               else
+                       dev->id[i] = ioread8(idmem + i);
+       }
+
+out:
+       dev->bus->ops->unmap_space(dev, IPACK_ID_SPACE);
+
+       return ret;
+}
+
 struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
                                           int slot, int irqv)
 {
@@ -137,8 +209,16 @@ struct ipack_device *ipack_device_register(struct ipack_bus_device *bus,
        dev_set_name(&dev->dev,
                     "ipack-dev.%u.%u", dev->bus_nr, dev->slot);
 
+       ret = ipack_device_read_id(dev);
+       if (ret < 0) {
+               dev_err(&dev->dev, "error reading device id section.\n");
+               kfree(dev);
+               return NULL;
+       }
+
        ret = device_register(&dev->dev);
        if (ret < 0) {
+               kfree(dev->id);
                kfree(dev);
                return NULL;
        }

diff --git a/drivers/staging/ipack/ipack.h b/drivers/staging/ipack/ipack.h
index 703142d..2851e33 100644 (file)
--- a/drivers/staging/ipack/ipack.h
+++ b/drivers/staging/ipack/ipack.h
@@ -12,6 +12,8 @@
 #include <linux/mod_devicetable.h>
 #include <linux/device.h>
 
+#include "ipack_ids.h"
+
 #define IPACK_IDPROM_OFFSET_I                  0x01
 #define IPACK_IDPROM_OFFSET_P                  0x03
 #define IPACK_IDPROM_OFFSET_A                  0x05
@@ -72,6 +74,9 @@ struct ipack_device {
        struct ipack_addr_space io_space;
        struct ipack_addr_space mem_space;
        struct device dev;
+       unsigned char           *id;
+       size_t                   id_avail;
+       u8                       id_format;
 };
 
 /**