3 files changed, 23 insertions, 22 deletions

diff --git a/drivers/mtd/chips/cfi_cmdset_0002.c b/drivers/mtd/chips/cfi_cmdset_0002.c
index 3e6f5d8609e8..d74ec46aa032 100644
--- a/drivers/mtd/chips/cfi_cmdset_0002.c
+++ b/drivers/mtd/chips/cfi_cmdset_0002.c
@@ -406,19 +406,6 @@ struct mtd_info *cfi_cmdset_0002(struct map_info *map, int primary)
                 /* Set the default CFI lock/unlock addresses */
                 cfi->addr_unlock1 = 0x555;
                 cfi->addr_unlock2 = 0x2aa;
-                /* Modify the unlock address if we are in compatibility mode */
-                if (    /* x16 in x8 mode */
-                        ((cfi->device_type == CFI_DEVICETYPE_X8) &&
-                                (cfi->cfiq->InterfaceDesc ==
-                                        CFI_INTERFACE_X8_BY_X16_ASYNC)) ||
-                        /* x32 in x16 mode */
-                        ((cfi->device_type == CFI_DEVICETYPE_X16) &&
-                                (cfi->cfiq->InterfaceDesc ==
-                                        CFI_INTERFACE_X16_BY_X32_ASYNC)))
-                {
-                        cfi->addr_unlock1 = 0xaaa;
-                        cfi->addr_unlock2 = 0x555;
-                }
 
         } /* CFI mode */
         else if (cfi->cfi_mode == CFI_MODE_JEDEC) {
diff --git a/drivers/mtd/chips/jedec_probe.c b/drivers/mtd/chips/jedec_probe.c
index f84ab6182148..2f3f2f719ba4 100644
--- a/drivers/mtd/chips/jedec_probe.c
+++ b/drivers/mtd/chips/jedec_probe.c
@@ -1808,9 +1808,7 @@ static inline u32 jedec_read_mfr(struct map_info *map, uint32_t base,
          * several first banks can contain 0x7f instead of actual ID
          */
         do {
-                uint32_t ofs = cfi_build_cmd_addr(0 + (bank << 8),
-                                                  cfi_interleave(cfi),
-                                                  cfi->device_type);
+                uint32_t ofs = cfi_build_cmd_addr(0 + (bank << 8), map, cfi);
                 mask = (1 << (cfi->device_type * 8)) - 1;
                 result = map_read(map, base + ofs);
                 bank++;
@@ -1824,7 +1822,7 @@ static inline u32 jedec_read_id(struct map_info *map, uint32_t base,
 {
         map_word result;
         unsigned long mask;
-        u32 ofs = cfi_build_cmd_addr(1, cfi_interleave(cfi), cfi->device_type);
+        u32 ofs = cfi_build_cmd_addr(1, map, cfi);
         mask = (1 << (cfi->device_type * 8)) -1;
         result = map_read(map, base + ofs);
         return result.x[0] & mask;
@@ -2067,8 +2065,8 @@ static int jedec_probe_chip(struct map_info *map, __u32 base,
 
         }
         /* Ensure the unlock addresses we try stay inside the map */
-        probe_offset1 = cfi_build_cmd_addr(cfi->addr_unlock1, cfi_interleave(cfi), cfi->device_type);
-        probe_offset2 = cfi_build_cmd_addr(cfi->addr_unlock2, cfi_interleave(cfi), cfi->device_type);
+        probe_offset1 = cfi_build_cmd_addr(cfi->addr_unlock1, map, cfi);
+        probe_offset2 = cfi_build_cmd_addr(cfi->addr_unlock2, map, cfi);
         if (    ((base + probe_offset1 + map_bankwidth(map)) >= map->size) ||
                 ((base + probe_offset2 + map_bankwidth(map)) >= map->size))
                 goto retry;
diff --git a/include/linux/mtd/cfi.h b/include/linux/mtd/cfi.h
index ee5124ec319e..00e2b575021f 100644
--- a/include/linux/mtd/cfi.h
+++ b/include/linux/mtd/cfi.h
@@ -282,9 +282,25 @@ struct cfi_private {
 /*
  * Returns the command address according to the given geometry.
  */
-static inline uint32_t cfi_build_cmd_addr(uint32_t cmd_ofs, int interleave, int type)
+static inline uint32_t cfi_build_cmd_addr(uint32_t cmd_ofs,
+                                struct map_info *map, struct cfi_private *cfi)
 {
-        return (cmd_ofs * type) * interleave;
+        unsigned bankwidth = map_bankwidth(map);
+        unsigned interleave = cfi_interleave(cfi);
+        unsigned type = cfi->device_type;
+        uint32_t addr;
+        
+        addr = (cmd_ofs * type) * interleave;
+
+        /* Modify the unlock address if we are in compatiblity mode.
+         * For 16bit devices on 8 bit busses
+         * and 32bit devices on 16 bit busses
+         * set the low bit of the alternating bit sequence of the address.
+         */
+        if (((type * interleave) > bankwidth) && ((uint8_t)cmd_ofs == 0xaa))
+                addr |= (type >> 1)*interleave;
+
+        return  addr;
 }
 
 /*
@@ -430,7 +446,7 @@ static inline uint32_t cfi_send_gen_cmd(u_char cmd, uint32_t cmd_addr, uint32_t
                                 int type, map_word *prev_val)
 {
         map_word val;
-        uint32_t addr = base + cfi_build_cmd_addr(cmd_addr, cfi_interleave(cfi), type);
+        uint32_t addr = base + cfi_build_cmd_addr(cmd_addr, map, cfi);
         val = cfi_build_cmd(cmd, map, cfi);
 
         if (prev_val)