/*
    Common Flash Interface probe code.
    (C) 2000 Red Hat. GPL'd.
-   $Id: cfi_probe.c,v 1.84 2005/11/07 11:14:23 gleixner Exp $
+   $Id: cfi_probe.c,v 1.85 2005/11/15 23:28:17 tpoynor Exp $
 */
 
 #include <linux/config.h>
        cfi_send_gen_cmd(0xaa, 0x555, base, map, cfi, cfi->device_type, NULL);
        cfi_send_gen_cmd(0x55, 0x2aa, base, map, cfi, cfi->device_type, NULL);
        cfi_send_gen_cmd(0x90, 0x555, base, map, cfi, cfi->device_type, NULL);
-       cfi->mfr = cfi_read_query(map, base);
-       cfi->id = cfi_read_query(map, base + ofs_factor);
+       cfi->mfr = cfi_read_query16(map, base);
+       cfi->id = cfi_read_query16(map, base + ofs_factor);
 
        /* Put it back into Read Mode */
        cfi_send_gen_cmd(0xF0, 0, base, map, cfi, cfi->device_type, NULL);
 
 
 /* Common Flash Interface structures
  * See http://support.intel.com/design/flash/technote/index.htm
- * $Id: cfi.h,v 1.56 2005/11/07 11:14:54 gleixner Exp $
+ * $Id: cfi.h,v 1.57 2005/11/15 23:28:17 tpoynor Exp $
  */
 
 #ifndef __MTD_CFI_H__
        }
 }
 
+static inline uint16_t cfi_read_query16(struct map_info *map, uint32_t addr)
+{
+       map_word val = map_read(map, addr);
+
+       if (map_bankwidth_is_1(map)) {
+               return val.x[0] & 0xff;
+       } else if (map_bankwidth_is_2(map)) {
+               return cfi16_to_cpu(val.x[0]);
+       } else {
+               /* No point in a 64-bit byteswap since that would just be
+                  swapping the responses from different chips, and we are
+                  only interested in one chip (a representative sample) */
+               return cfi32_to_cpu(val.x[0]);
+       }
+}
+
 static inline void cfi_udelay(int us)
 {
        if (us >= 1000) {