1 files changed, 58 insertions, 4 deletions

diff --git a/drivers/mtd/chips/cfi_util.c b/drivers/mtd/chips/cfi_util.c
index 0ee457018016..8d7553670526 100644
--- a/drivers/mtd/chips/cfi_util.c
+++ b/drivers/mtd/chips/cfi_util.c
@@ -24,6 +24,62 @@
 #include <linux/mtd/cfi.h>
 #include <linux/mtd/compatmac.h>
 
+int __xipram qry_present(struct map_info *map, __u32 base,
+                                struct cfi_private *cfi)
+{
+        int osf = cfi->interleave * cfi->device_type;   /* scale factor */
+        map_word val[3];
+        map_word qry[3];
+
+        qry[0] = cfi_build_cmd('Q', map, cfi);
+        qry[1] = cfi_build_cmd('R', map, cfi);
+        qry[2] = cfi_build_cmd('Y', map, cfi);
+
+        val[0] = map_read(map, base + osf*0x10);
+        val[1] = map_read(map, base + osf*0x11);
+        val[2] = map_read(map, base + osf*0x12);
+
+        if (!map_word_equal(map, qry[0], val[0]))
+                return 0;
+
+        if (!map_word_equal(map, qry[1], val[1]))
+                return 0;
+
+        if (!map_word_equal(map, qry[2], val[2]))
+                return 0;
+
+        return 1;       /* "QRY" found */
+}
+
+int __xipram qry_mode_on(uint32_t base, struct map_info *map,
+                                struct cfi_private *cfi)
+{
+        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
+        cfi_send_gen_cmd(0x98, 0x55, base, map, cfi, cfi->device_type, NULL);
+        if (qry_present(map, base, cfi))
+                return 1;
+        /* QRY not found probably we deal with some odd CFI chips */
+        /* Some revisions of some old Intel chips? */
+        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
+        cfi_send_gen_cmd(0xFF, 0, base, map, cfi, cfi->device_type, NULL);
+        cfi_send_gen_cmd(0x98, 0x55, base, map, cfi, cfi->device_type, NULL);
+        if (qry_present(map, base, cfi))
+                return 1;
+        /* ST M29DW chips */
+        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
+        cfi_send_gen_cmd(0x98, 0x555, base, map, cfi, cfi->device_type, NULL);
+        if (qry_present(map, base, cfi))
+                return 1;
+        /* QRY not found */
+        return 0;
+}
+void __xipram qry_mode_off(uint32_t base, struct map_info *map,
+                                struct cfi_private *cfi)
+{
+        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
+        cfi_send_gen_cmd(0xFF, 0, base, map, cfi, cfi->device_type, NULL);
+}
+
 struct cfi_extquery *
 __xipram cfi_read_pri(struct map_info *map, __u16 adr, __u16 size, const char* name)
 {
@@ -48,8 +104,7 @@ __xipram cfi_read_pri(struct map_info *map, __u16 adr, __u16 size, const char* n
 #endif
 
         /* Switch it into Query Mode */
-        cfi_send_gen_cmd(0x98, 0x55, base, map, cfi, cfi->device_type, NULL);
-
+        qry_mode_on(base, map, cfi);
         /* Read in the Extended Query Table */
         for (i=0; i<size; i++) {
                 ((unsigned char *)extp)[i] =
@@ -57,8 +112,7 @@ __xipram cfi_read_pri(struct map_info *map, __u16 adr, __u16 size, const char* n
         }
 
         /* Make sure it returns to read mode */
-        cfi_send_gen_cmd(0xf0, 0, base, map, cfi, cfi->device_type, NULL);
-        cfi_send_gen_cmd(0xff, 0, base, map, cfi, cfi->device_type, NULL);
+        qry_mode_off(base, map, cfi);
 
 #ifdef CONFIG_MTD_XIP
         (void) map_read(map, base);