Fix CFI query responses for NOR flash

This change fixes the CFI query responses to handle NOR device
widths that are different from the bank width.  Support is also
added for multi-width devices in a x8 configuration.  This is
typically x8/x16 devices, but the CFI specification mentions
x8/x32 devices so those should be supported as well if they
exist.
The query response data is now replicated per-device in the bank,
and is adjusted for x16 or x32 parts configured in x8 mode.

The existing code is left in place for boards that have not
been updated to specify an explicit device_width.  The VExpress
board has been updated in an earlier patch in this series so
this is the only board currently affected.
Signed-off-by: NRoy Franz <roy.franz@linaro.org>
Message-id: 1386279359-32286-7-git-send-email-roy.franz@linaro.org
[PMM: fixed a few formatting nits]
Signed-off-by: NPeter Maydell <peter.maydell@linaro.org>

hw/block/pflash_cfi01.c

@@ -119,6 +119,67 @@ static void pflash_timer (void *opaque)
     pfl->cmd = 0;
 }
 
+/* Perform a CFI query based on the bank width of the flash.
+ * If this code is called we know we have a device_width set for
+ * this flash.
+ */
+static uint32_t pflash_cfi_query(pflash_t *pfl, hwaddr offset)
+{
+    int i;
+    uint32_t resp = 0;
+    hwaddr boff;
+
+    /* Adjust incoming offset to match expected device-width
+     * addressing. CFI query addresses are always specified in terms of
+     * the maximum supported width of the device.  This means that x8
+     * devices and x8/x16 devices in x8 mode behave differently.  For
+     * devices that are not used at their max width, we will be
+     * provided with addresses that use higher address bits than
+     * expected (based on the max width), so we will shift them lower
+     * so that they will match the addresses used when
+     * device_width==max_device_width.
+     */
+    boff = offset >> (ctz32(pfl->bank_width) +
+                      ctz32(pfl->max_device_width) - ctz32(pfl->device_width));
+
+    if (boff > pfl->cfi_len) {
+        return 0;
+    }
+    /* Now we will construct the CFI response generated by a single
+     * device, then replicate that for all devices that make up the
+     * bus.  For wide parts used in x8 mode, CFI query responses
+     * are different than native byte-wide parts.
+     */
+    resp = pfl->cfi_table[boff];
+    if (pfl->device_width != pfl->max_device_width) {
+        /* The only case currently supported is x8 mode for a
+         * wider part.
+         */
+        if (pfl->device_width != 1 || pfl->bank_width > 4) {
+            DPRINTF("%s: Unsupported device configuration: "
+                    "device_width=%d, max_device_width=%d\n",
+                    __func__, pfl->device_width,
+                    pfl->max_device_width);
+            return 0;
+        }
+        /* CFI query data is repeated, rather than zero padded for
+         * wide devices used in x8 mode.
+         */
+        for (i = 1; i < pfl->max_device_width; i++) {
+            resp = deposit32(resp, 8 * i, 8, pfl->cfi_table[boff]);
+        }
+    }
+    /* Replicate responses for each device in bank. */
+    if (pfl->device_width < pfl->bank_width) {
+        for (i = pfl->device_width;
+             i < pfl->bank_width; i += pfl->device_width) {
+            resp = deposit32(resp, 8 * i, 8 * pfl->device_width, resp);
+        }
+    }
+
+    return resp;
+}
+
 static uint32_t pflash_read (pflash_t *pfl, hwaddr offset,
                              int width, int be)
 {
@@ -127,13 +188,6 @@ static uint32_t pflash_read (pflash_t *pfl, hwaddr offset,
     uint8_t *p;
 
     ret = -1;
-    boff = offset & 0xFF; /* why this here ?? */
-
-    if (pfl->bank_width == 2) {
-        boff = boff >> 1;
-    } else if (pfl->bank_width == 4) {
-        boff = boff >> 2;
-    }
 
 #if 0
     DPRINTF("%s: reading offset " TARGET_FMT_plx " under cmd %02x width %d\n",
@@ -213,6 +267,13 @@ static uint32_t pflash_read (pflash_t *pfl, hwaddr offset,
         DPRINTF("%s: status %x\n", __func__, ret);
         break;
     case 0x90:
+        boff = offset & 0xFF;
+        if (pfl->bank_width == 2) {
+            boff = boff >> 1;
+        } else if (pfl->bank_width == 4) {
+            boff = boff >> 2;
+        }
+
         switch (boff) {
         case 0:
             ret = pfl->ident0 << 8 | pfl->ident1;
@@ -230,10 +291,32 @@ static uint32_t pflash_read (pflash_t *pfl, hwaddr offset,
         }
         break;
     case 0x98: /* Query mode */
-        if (boff > pfl->cfi_len)
-            ret = 0;
-        else
-            ret = pfl->cfi_table[boff];
+        if (!pfl->device_width) {
+            /* Preserve old behavior if device width not specified */
+            boff = offset & 0xFF;
+            if (pfl->bank_width == 2) {
+                boff = boff >> 1;
+            } else if (pfl->bank_width == 4) {
+                boff = boff >> 2;
+            }
+
+            if (boff > pfl->cfi_len) {
+                ret = 0;
+            } else {
+                ret = pfl->cfi_table[boff];
+            }
+        } else {
+            /* If we have a read larger than the bank_width, combine multiple
+             * CFI queries into a single response.
+             */
+            int i;
+            for (i = 0; i < width; i += pfl->bank_width) {
+                ret = deposit32(ret, i * 8, pfl->bank_width * 8,
+                                pflash_cfi_query(pfl,
+                                                 offset + i * pfl->bank_width));
+            }
+        }
+
         break;
     }
     return ret;