1 files changed, 71 insertions, 61 deletions

diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 5b26fffc353..5b96f621f7c 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -2819,6 +2819,72 @@ static u16 onfi_crc16(u16 crc, u8 const *p, size_t len)
 }
 
 /*
+ * Check if the NAND chip is ONFI compliant, returns 1 if it is, 0 otherwise
+ */
+static int nand_flash_detect_onfi(struct mtd_info *mtd, struct nand_chip *chip,
+                                        int busw)
+{
+        struct nand_onfi_params *p = &chip->onfi_params;
+        int i;
+        int val;
+
+        /* try ONFI for unknow chip or LP */
+        chip->cmdfunc(mtd, NAND_CMD_READID, 0x20, -1);
+        if (chip->read_byte(mtd) != 'O' || chip->read_byte(mtd) != 'N' ||
+                chip->read_byte(mtd) != 'F' || chip->read_byte(mtd) != 'I')
+                return 0;
+
+        printk(KERN_INFO "ONFI flash detected\n");
+        chip->cmdfunc(mtd, NAND_CMD_PARAM, 0, -1);
+        for (i = 0; i < 3; i++) {
+                chip->read_buf(mtd, (uint8_t *)p, sizeof(*p));
+                if (onfi_crc16(ONFI_CRC_BASE, (uint8_t *)p, 254) ==
+                                le16_to_cpu(p->crc)) {
+                        printk(KERN_INFO "ONFI param page %d valid\n", i);
+                        break;
+                }
+        }
+
+        if (i == 3)
+                return 0;
+
+        /* check version */
+        val = le16_to_cpu(p->revision);
+        if (val == 1 || val > (1 << 4)) {
+                printk(KERN_INFO "%s: unsupported ONFI version: %d\n",
+                                                                __func__, val);
+                return 0;
+        }
+
+        if (val & (1 << 4))
+                chip->onfi_version = 22;
+        else if (val & (1 << 3))
+                chip->onfi_version = 21;
+        else if (val & (1 << 2))
+                chip->onfi_version = 20;
+        else
+                chip->onfi_version = 10;
+
+        sanitize_string(p->manufacturer, sizeof(p->manufacturer));
+        sanitize_string(p->model, sizeof(p->model));
+        if (!mtd->name)
+                mtd->name = p->model;
+        mtd->writesize = le32_to_cpu(p->byte_per_page);
+        mtd->erasesize = le32_to_cpu(p->pages_per_block) * mtd->writesize;
+        mtd->oobsize = le16_to_cpu(p->spare_bytes_per_page);
+        chip->chipsize = le32_to_cpu(p->blocks_per_lun) * mtd->erasesize;
+        busw = 0;
+        if (le16_to_cpu(p->features) & 1)
+                busw = NAND_BUSWIDTH_16;
+
+        chip->options &= ~NAND_CHIPOPTIONS_MSK;
+        chip->options |= (NAND_NO_READRDY |
+                        NAND_NO_AUTOINCR) & NAND_CHIPOPTIONS_MSK;
+
+        return 1;
+}
+
+/*
  * Get the flash and manufacturer id and lookup if the type is supported
  */
 static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
@@ -2828,6 +2894,7 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 {
         int i, maf_idx;
         u8 id_data[8];
+        int ret;
 
         /* Select the device */
         chip->select_chip(mtd, 0);
@@ -2872,67 +2939,10 @@ static struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 
         chip->onfi_version = 0;
         if (!type->name || !type->pagesize) {
-                /* try ONFI for unknow chip or LP */
-                chip->cmdfunc(mtd, NAND_CMD_READID, 0x20, -1);
-                if (chip->read_byte(mtd) == 'O' &&
-                        chip->read_byte(mtd) == 'N' &&
-                        chip->read_byte(mtd) == 'F' &&
-                        chip->read_byte(mtd) == 'I') {
-
-                        struct nand_onfi_params *p = &chip->onfi_params;
-                        int i;
-
-                        printk(KERN_INFO "ONFI flash detected\n");
-                        chip->cmdfunc(mtd, NAND_CMD_PARAM, 0, -1);
-                        for (i = 0; i < 3; i++) {
-                                chip->read_buf(mtd, (uint8_t *)p, sizeof(*p));
-                                if (onfi_crc16(ONFI_CRC_BASE, (uint8_t *)p, 254) ==
-                                                                le16_to_cpu(p->crc))
-                                {
-                                         printk(KERN_INFO "ONFI param page %d valid\n", i);
-                                         break;
-                                }
-                        }
-
-                        if (i < 3) {
-                                /* check version */
-                                int val = le16_to_cpu(p->revision);
-                                if (val == 1 || val > (1 << 4))
-                                        printk(KERN_INFO "%s: unsupported ONFI version: %d\n",
-                                                __func__, val);
-                                else {
-                                        if (val & (1 << 4))
-                                                chip->onfi_version = 22;
-                                        else if (val & (1 << 3))
-                                                chip->onfi_version = 21;
-                                        else if (val & (1 << 2))
-                                                chip->onfi_version = 20;
-                                        else
-                                                chip->onfi_version = 10;
-                                }
-                        }
-
-                        if (chip->onfi_version) {
-                                sanitize_string(p->manufacturer, sizeof(p->manufacturer));
-                                sanitize_string(p->model, sizeof(p->model));
-                                if (!mtd->name)
-                                        mtd->name = p->model;
-                                mtd->writesize = le32_to_cpu(p->byte_per_page);
-                                mtd->erasesize = le32_to_cpu(p->pages_per_block)*mtd->writesize;
-                                mtd->oobsize = le16_to_cpu(p->spare_bytes_per_page);
-                                chip->chipsize = le32_to_cpu(p->blocks_per_lun) * mtd->erasesize;
-                                busw = 0;
-                                if (le16_to_cpu(p->features) & 1)
-                                        busw = NAND_BUSWIDTH_16;
-
-                                chip->options &= ~NAND_CHIPOPTIONS_MSK;
-                                chip->options |= (NAND_NO_READRDY |
-                                                NAND_NO_AUTOINCR) & NAND_CHIPOPTIONS_MSK;
-
-                                goto ident_done;
-
-                        }
-                }
+                /* Check is chip is ONFI compliant */
+                ret = nand_flash_detect_onfi(mtd, chip, busw);
+                if (ret)
+                        goto ident_done;
         }
 
         chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);