3 files changed, 73 insertions, 50 deletions

diff --git a/drivers/mtd/chips/cfi_probe.c b/drivers/mtd/chips/cfi_probe.c
index c418e92e1d92..e706be2ad0cb 100644
--- a/drivers/mtd/chips/cfi_probe.c
+++ b/drivers/mtd/chips/cfi_probe.c
@@ -44,17 +44,14 @@ do { \
 
 #define xip_enable(base, map, cfi) \
 do { \
-        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); \
         xip_allowed(base, map); \
 } while (0)
 
 #define xip_disable_qry(base, map, cfi) \
 do { \
         xip_disable(); \
-        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); \
+        qry_mode_on(base, map, cfi); \
 } while (0)
 
 #else
@@ -70,32 +67,6 @@ do { \
    in: interleave,type,mode
    ret: table index, <0 for error
  */
-static 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
-}
 
 static int __xipram cfi_probe_chip(struct map_info *map, __u32 base,
                                    unsigned long *chip_map, struct cfi_private *cfi)
@@ -116,11 +87,7 @@ static int __xipram cfi_probe_chip(struct map_info *map, __u32 base,
         }
 
         xip_disable();
-        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)) {
+        if (!qry_mode_on(base, map, cfi)) {
                 xip_enable(base, map, cfi);
                 return 0;
         }
@@ -144,8 +111,7 @@ static int __xipram cfi_probe_chip(struct map_info *map, __u32 base,
                 if (qry_present(map, start, cfi)) {
                         /* Eep. This chip also had the QRY marker.
                          * Is it an alias for the new one? */
-                        cfi_send_gen_cmd(0xF0, 0, start, map, cfi, cfi->device_type, NULL);
-                        cfi_send_gen_cmd(0xFF, 0, start, map, cfi, cfi->device_type, NULL);
+                        qry_mode_off(start, map, cfi);
 
                         /* If the QRY marker goes away, it's an alias */
                         if (!qry_present(map, start, cfi)) {
@@ -158,8 +124,7 @@ static int __xipram cfi_probe_chip(struct map_info *map, __u32 base,
                          * unfortunate. Stick the new chip in read mode
                          * too and if it's the same, assume it's an alias. */
                         /* FIXME: Use other modes to do a proper check */
-                        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
-                        cfi_send_gen_cmd(0xFF, 0, start, map, cfi, cfi->device_type, NULL);
+                        qry_mode_off(base, map, cfi);
 
                         if (qry_present(map, base, cfi)) {
                                 xip_allowed(base, map);
@@ -176,8 +141,7 @@ static int __xipram cfi_probe_chip(struct map_info *map, __u32 base,
         cfi->numchips++;
 
         /* Put it back into 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);
         xip_allowed(base, map);
 
         printk(KERN_INFO "%s: Found %d x%d devices at 0x%x in %d-bit bank\n",
@@ -237,9 +201,7 @@ static int __xipram cfi_chip_setup(struct map_info *map,
                           cfi_read_query(map, base + 0xf * ofs_factor);
 
         /* Put it back into Read Mode */
-        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
-        /* ... even if it's an Intel chip */
-        cfi_send_gen_cmd(0xFF, 0, base, map, cfi, cfi->device_type, NULL);
+        qry_mode_off(base, map, cfi);
         xip_allowed(base, map);
 
         /* Do any necessary byteswapping */
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);
diff --git a/include/linux/mtd/cfi.h b/include/linux/mtd/cfi.h
index d6fb115f5a07..3058917d7b92 100644
--- a/include/linux/mtd/cfi.h
+++ b/include/linux/mtd/cfi.h
@@ -12,6 +12,7 @@
 #include <linux/mtd/flashchip.h>
 #include <linux/mtd/map.h>
 #include <linux/mtd/cfi_endian.h>
+#include <linux/mtd/xip.h>
 
 #ifdef CONFIG_MTD_CFI_I1
 #define cfi_interleave(cfi) 1
@@ -430,7 +431,6 @@ static inline uint32_t cfi_send_gen_cmd(u_char cmd, uint32_t cmd_addr, uint32_t
 {
         map_word val;
         uint32_t addr = base + cfi_build_cmd_addr(cmd_addr, cfi_interleave(cfi), type);
-
         val = cfi_build_cmd(cmd, map, cfi);
 
         if (prev_val)
@@ -483,6 +483,13 @@ static inline void cfi_udelay(int us)
         }
 }
 
+int __xipram qry_present(struct map_info *map, __u32 base,
+                                struct cfi_private *cfi);
+int __xipram qry_mode_on(uint32_t base, struct map_info *map,
+                                struct cfi_private *cfi);
+void __xipram qry_mode_off(uint32_t base, struct map_info *map,
+                                struct cfi_private *cfi);
+
 struct cfi_extquery *cfi_read_pri(struct map_info *map, uint16_t adr, uint16_t size,
                              const char* name);
 struct cfi_fixup {