Add pinctrl from device tree to mts-io

1 files changed, 69 insertions, 2 deletions

diff --git a/io-module/mts-io.c b/io-module/mts-io.c
index 8f7739e..bfaf726 100644
--- a/io-module/mts-io.c
+++ b/io-module/mts-io.c
@@ -52,6 +52,61 @@
 
 #define LED_LS_CONTROLLABLE		0
 
+/* To use AT91 pinctrl to set pull-up
+ * and pull-down, we use device tree.
+ * This seems the easiest way.  Most
+ * drivers implement pinctrl in the
+ * probe function, which causes
+ * device tree to be read, and ultimately
+ * causes the at91 pinctrl code to
+ * set the pull-up/pulldown registers
+ * as specified in the mts-io
+ * pinctrl section of device tree.
+ * .compatible lets us find our device
+ * tree entries.  Probe and remove
+ * are mandatory even though our code
+ * is a no-op.  This probe code was based
+ * on the i2c-gpio driver.
+ */
+static const struct of_device_id mts_io_dt_ids[] = {
+    { .compatible = "mts,mts-io", },
+    { /* sentinel */ }
+};
+MODULE_DEVICE_TABLE(of, mts_io_dt_ids);
+
+/*
+ * We must call platform_set_drvdata, or else the
+ * devres_head for the driver has junk in it, and
+ * this causes warning stack dumps in dd.c,
+ * really_probe() function.
+ *
+ * priv is the private data structure for the driver,
+ * and could be used by our driver, but for now,
+ * it is unused.
+ */
+static int mts_io_probe(struct platform_device *pdev)
+{
+    return 0;
+}
+    
+static int mts_io_remove(struct platform_device *pdev)
+{
+    return 0;
+}
+
+/*
+ * minimal required functions for the platform driver.
+ */
+static struct platform_driver mts_io_driver = {
+    .driver         = {
+        .name   = "mts-io",
+        .of_match_table = of_match_ptr(mts_io_dt_ids),
+    },
+    .probe          = mts_io_probe,
+    .remove         = mts_io_remove,
+};
+
+
 /* on-board EEPROM */
 static struct mts_id_eeprom_layout id_eeprom;
 static uint8_t mts_hw_version;
@@ -863,14 +918,26 @@ static int __init mts_io_init(void)
 
 	log_info("init: " DRIVER_VERSION);
 
+	/* We do a platform_driver_register to do a probe
+	 * of device tree and set the pinctrl.  We then
+	 * unregister to remove
+	 * the probe function.  If we don't remove the
+	 * probe function, we will do a 2nd probe in
+	 * platform_device_add, which will result in a
+	 * stack trace in the log.
+        ret = platform_driver_register(&mts_io_driver);
+        if (ret)
+            printk(KERN_ERR "mts-io: probe failed: %d\n", ret);
+        platform_driver_unregister(&mts_io_driver);
+        
 	mts_io_platform_device = platform_device_alloc(PLATFORM_NAME, -1);
 	if (!mts_io_platform_device) {
 		cleanup();
 		return -ENOMEM;
 	}
 
-  /* request_firmware() requires a device, so call after device allocated */
-  ret = mts_id_eeprom_load();
+        /* request_firmware() requires a device, so call after device allocated */
+        ret = mts_id_eeprom_load();
 	if (ret) {
 		cleanup();
 		return ret;