sdio: add device id table and matching

Signed-off-by: Pierre Ossman <drzeus@drzeus.cx>

drivers/mmc/core/sdio_bus.c

@@ -21,14 +21,47 @@
 #include "sdio_bus.h"
 
 #define dev_to_sdio_func(d)	container_of(d, struct sdio_func, dev)
+#define to_sdio_driver(d)      container_of(d, struct sdio_driver, drv)
+
+static const struct sdio_device_id *sdio_match_one(struct sdio_func *func,
+	const struct sdio_device_id *id)
+{
+	if (id->class != (__u8)SDIO_ANY_ID && id->class != func->class)
+		return NULL;
+	if (id->vendor != (__u16)SDIO_ANY_ID && id->vendor != func->vendor)
+		return NULL;
+	if (id->device != (__u16)SDIO_ANY_ID && id->device != func->device)
+		return NULL;
+	return id;
+}
+
+static const struct sdio_device_id *sdio_match_device(struct sdio_func *func,
+	struct sdio_driver *sdrv)
+{
+	const struct sdio_device_id *ids;
+
+	ids = sdrv->id_table;
+
+	if (ids) {
+		while (ids->class || ids->vendor || ids->device) {
+			if (sdio_match_one(func, ids))
+				return ids;
+			ids++;
+		}
+	}
+
+	return NULL;
+}
 
-/*
- * This currently matches any SDIO function to any driver in order
- * to help initial development and testing.
- */
 static int sdio_bus_match(struct device *dev, struct device_driver *drv)
 {
-	return 1;
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	struct sdio_driver *sdrv = to_sdio_driver(drv);
+
+	if (sdio_match_device(func, sdrv))
+		return 1;
+
+	return 0;
 }
 
 static int
@@ -42,11 +75,24 @@ sdio_bus_uevent(struct device *dev, char **envp, int num_envp, char *buf,
 
 static int sdio_bus_probe(struct device *dev)
 {
-	return -ENODEV;
+	struct sdio_driver *drv = to_sdio_driver(dev->driver);
+	struct sdio_func *func = dev_to_sdio_func(dev);
+	const struct sdio_device_id *id;
+
+	id = sdio_match_device(func, drv);
+	if (!id)
+		return -ENODEV;
+
+	return drv->probe(func, id);
 }
 
 static int sdio_bus_remove(struct device *dev)
 {
+	struct sdio_driver *drv = to_sdio_driver(dev->driver);
+	struct sdio_func *func = dev_to_sdio_func(dev);
+
+	drv->remove(func);
+
 	return 0;
 }
 

include/linux/mmc/sdio_func.h

@@ -12,6 +12,9 @@
 #ifndef MMC_SDIO_FUNC_H
 #define MMC_SDIO_FUNC_H
 
+#include <linux/device.h>
+#include <linux/mod_devicetable.h>
+
 struct mmc_card;
 
 /*
@@ -58,13 +61,38 @@ struct sdio_func {
  */
 struct sdio_driver {
 	char *name;
+	const struct sdio_device_id *id_table;
 
-	int (*probe)(struct sdio_func *);
+	int (*probe)(struct sdio_func *, const struct sdio_device_id *);
 	void (*remove)(struct sdio_func *);
 
 	struct device_driver drv;
 };
 
+/**
+ * SDIO_DEVICE - macro used to describe a specific SDIO device
+ * @vend: the 16 bit manufacturer code
+ * @dev: the 16 bit function id
+ *
+ * This macro is used to create a struct sdio_device_id that matches a
+ * specific device. The class field will be set to SDIO_ANY_ID.
+ */
+#define SDIO_DEVICE(vend,dev) \
+	.class = SDIO_ANY_ID, \
+	.vendor = (vend), .device = (dev)
+
+/**
+ * SDIO_DEVICE_CLASS - macro used to describe a specific SDIO device class
+ * @dev_class: the 8 bit standard interface code
+ *
+ * This macro is used to create a struct sdio_device_id that matches a
+ * specific standard SDIO function type.  The vendor and device fields will
+ * be set to SDIO_ANY_ID.
+ */
+#define SDIO_DEVICE_CLASS(dev_class) \
+	.class = (dev_class), \
+	.vendor = SDIO_ANY_ID, .device = SDIO_ANY_ID
+
 extern int sdio_register_driver(struct sdio_driver *);
 extern void sdio_unregister_driver(struct sdio_driver *);
 

include/linux/mod_devicetable.h

@@ -340,4 +340,15 @@ struct parisc_device_id {
 #define PA_HVERSION_ANY_ID	0xffff
 #define PA_SVERSION_ANY_ID	0xffffffff
 
+/* SDIO */
+
+#define SDIO_ANY_ID (~0)
+
+struct sdio_device_id {
+	__u8	class;			/* Standard interface or SDIO_ANY_ID */
+	__u16	vendor;			/* Vendor or SDIO_ANY_ID */
+	__u16	device;			/* Device ID or SDIO_ANY_ID */
+	kernel_ulong_t driver_data;	/* Data private to the driver */
+};
+
 #endif /* LINUX_MOD_DEVICETABLE_H */