1 files changed, 37 insertions, 17 deletions

diff --git a/drivers/mfd/wm8350-core.c b/drivers/mfd/wm8350-core.c
index 764bf15..2188d75 100644
--- a/drivers/mfd/wm8350-core.c
+++ b/drivers/mfd/wm8350-core.c
@@ -1227,52 +1227,72 @@ int wm8350_device_init(struct wm8350 *wm8350, int irq,
 		       struct wm8350_platform_data *pdata)
 {
 	int ret = -EINVAL;
-	u16 id1, id2, mask, mode;
+	u16 id1, id2, mask_rev;
+	u16 cust_id, mode, chip_rev;
 
 	/* get WM8350 revision and config mode */
 	wm8350->read_dev(wm8350, WM8350_RESET_ID, sizeof(id1), &id1);
 	wm8350->read_dev(wm8350, WM8350_ID, sizeof(id2), &id2);
+	wm8350->read_dev(wm8350, WM8350_REVISION, sizeof(mask_rev), &mask_rev);
 
 	id1 = be16_to_cpu(id1);
 	id2 = be16_to_cpu(id2);
+	mask_rev = be16_to_cpu(mask_rev);
 
-	if (id1 == 0x6143) {
-		switch ((id2 & WM8350_CHIP_REV_MASK) >> 12) {
+	if (id1 != 0x6143) {
+		dev_err(wm8350->dev,
+			"Device with ID %x is not a WM8350\n", id1);
+		ret = -ENODEV;
+		goto err;
+	}
+
+	mode = id2 & WM8350_CONF_STS_MASK >> 10;
+	cust_id = id2 & WM8350_CUST_ID_MASK;
+	chip_rev = (id2 & WM8350_CHIP_REV_MASK) >> 12;
+	dev_info(wm8350->dev,
+		 "CONF_STS %d, CUST_ID %d, MASK_REV %d, CHIP_REV %d\n",
+		 mode, cust_id, mask_rev, chip_rev);
+
+	if (cust_id != 0) {
+		dev_err(wm8350->dev, "Unsupported CUST_ID\n");
+		ret = -ENODEV;
+		goto err;
+	}
+
+	switch (mask_rev) {
+	case 0:
+		switch (chip_rev) {
 		case WM8350_REV_E:
-			dev_info(wm8350->dev, "Found Rev E device\n");
+			dev_info(wm8350->dev, "WM8350 Rev E\n");
 			break;
 		case WM8350_REV_F:
-			dev_info(wm8350->dev, "Found Rev F device\n");
+			dev_info(wm8350->dev, "WM8350 Rev F\n");
 			break;
 		case WM8350_REV_G:
-			dev_info(wm8350->dev, "Found Rev G device\n");
+			dev_info(wm8350->dev, "WM8350 Rev G\n");
 			wm8350->power.rev_g_coeff = 1;
 			break;
 		case WM8350_REV_H:
-			dev_info(wm8350->dev, "Found Rev H device\n");
+			dev_info(wm8350->dev, "WM8350 Rev H\n");
 			wm8350->power.rev_g_coeff = 1;
 			break;
 		default:
 			/* For safety we refuse to run on unknown hardware */
-			dev_info(wm8350->dev, "Found unknown rev %x\n",
-				 (id2 & WM8350_CHIP_REV_MASK) >> 12);
+			dev_err(wm8350->dev, "Unknown WM8350 CHIP_REV\n");
 			ret = -ENODEV;
 			goto err;
 		}
-	} else {
-		dev_info(wm8350->dev, "Device with ID %x is not a WM8350\n",
-			 id1);
+		break;
+
+	default:
+		dev_err(wm8350->dev, "Unknown MASK_REV\n");
 		ret = -ENODEV;
 		goto err;
 	}
 
-	mode = id2 & WM8350_CONF_STS_MASK >> 10;
-	mask = id2 & WM8350_CUST_ID_MASK;
-	dev_info(wm8350->dev, "Config mode %d, ROM mask %d\n", mode, mask);
-
 	ret = wm8350_create_cache(wm8350, mode);
 	if (ret < 0) {
-		printk(KERN_ERR "wm8350: failed to create register cache\n");
+		dev_err(wm8350->dev, "Failed to create register cache\n");
 		return ret;
 	}