SM501: Check SM501 ID register on initialisation

When binding the driver, check the ID register for a valid identity, in case
the SM501 is not functioning correctly.

Signed-off-by: Ben Dooks <ben-linux@fluff.org>
Signed-off-by: Andrew Morton <akpm@linux-foundation.org>
Signed-off-by: Linus Torvalds <torvalds@linux-foundation.org>

2 files changed, 13 insertions, 4 deletions

diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index e14d70e07418..8135e4c3bf47 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -893,6 +893,7 @@ static int sm501_init_dev(struct sm501_devdata *sm)
 {
         resource_size_t mem_avail;
         unsigned long dramctrl;
+        unsigned long devid;
         int ret;
 
         mutex_init(&sm->clock_lock);
@@ -900,13 +901,18 @@ static int sm501_init_dev(struct sm501_devdata *sm)
 
         INIT_LIST_HEAD(&sm->devices);
 
-        dramctrl = readl(sm->regs + SM501_DRAM_CONTROL);
+        devid = readl(sm->regs + SM501_DEVICEID);
 
+        if ((devid & SM501_DEVICEID_IDMASK) != SM501_DEVICEID_SM501) {
+                dev_err(sm->dev, "incorrect device id %08lx\n", devid);
+                return -EINVAL;
+        }
+
+        dramctrl = readl(sm->regs + SM501_DRAM_CONTROL);
         mem_avail = sm501_mem_local[(dramctrl >> 13) & 0x7];
 
-        dev_info(sm->dev, "SM501 At %p: Version %08x, %ld Mb, IRQ %d\n",
-                 sm->regs, readl(sm->regs + SM501_DEVICEID),
-                 (unsigned long)mem_avail >> 20, sm->irq);
+        dev_info(sm->dev, "SM501 At %p: Version %08lx, %ld Mb, IRQ %d\n",
+                 sm->regs, devid, (unsigned long)mem_avail >> 20, sm->irq);
 
         sm501_dump_gate(sm);
 
diff --git a/include/linux/sm501-regs.h b/include/linux/sm501-regs.h
index 8b4ecf02f9dc..014e73b31fc0 100644
--- a/include/linux/sm501-regs.h
+++ b/include/linux/sm501-regs.h
@@ -109,6 +109,9 @@
 #define SM501_DEVICEID                  (0x000060)
 /* 0x050100A0 */
 
+#define SM501_DEVICEID_SM501            (0x05010000)
+#define SM501_DEVICEID_IDMASK           (0xffff0000)
+
 #define SM501_PLLCLOCK_COUNT            (0x000064)
 #define SM501_MISC_TIMING               (0x000068)
 #define SM501_CURRENT_SDRAM_CLOCK       (0x00006C)