cfi: Add support for strangely endianness broken SoC implementations

[openocd.git] / src / flash / nor / cfi.c

diff --git a/src/flash/nor/cfi.c b/src/flash/nor/cfi.c
index b808830010250e1731cc3ced3d22021a53207925..f7d8a90f181303e76873746bd4c56b7882e24936 100644 (file)
--- a/src/flash/nor/cfi.c
+++ b/src/flash/nor/cfi.c
@@ -17,9 +17,7 @@
  *   GNU General Public License for more details.                          *
  *                                                                         *
  *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.           *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -836,10 +834,13 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
        cfi_info->x16_as_x8 = 0;
        cfi_info->jedec_probe = 0;
        cfi_info->not_cfi = 0;
+       cfi_info->data_swap = 0;
 
        for (unsigned i = 6; i < CMD_ARGC; i++) {
                if (strcmp(CMD_ARGV[i], "x16_as_x8") == 0)
                        cfi_info->x16_as_x8 = 1;
+               else if (strcmp(CMD_ARGV[i], "data_swap") == 0)
+                       cfi_info->data_swap = 1;
                else if (strcmp(CMD_ARGV[i], "bus_swap") == 0)
                        bus_swap = 1;
                else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
@@ -1275,7 +1276,6 @@ static int cfi_intel_write_block(struct flash_bank *bank, const uint8_t *buffer,
                LOG_WARNING("No working area available, can't do block memory writes");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
-       ;
 
        /* write algorithm code to working area */
        retval = target_write_buffer(target, write_algorithm->address,
@@ -1297,7 +1297,6 @@ static int cfi_intel_write_block(struct flash_bank *bank, const uint8_t *buffer,
                        goto cleanup;
                }
        }
-       ;
 
        /* setup algo registers */
        init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
@@ -1540,7 +1539,6 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, const uint8_t
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
        }
-       ;
 
        init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
        init_reg_param(&reg_params[1], "r5", 32, PARAM_OUT);
@@ -1920,7 +1918,6 @@ static int cfi_spansion_write_block(struct flash_bank *bank, const uint8_t *buff
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
        }
-       ;
 
        init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
        init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
@@ -2002,7 +1999,9 @@ static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t
 
        uint8_t status;
        retval = cfi_intel_wait_status_busy(bank, cfi_info->word_write_timeout, &status);
-       if (retval != 0x80) {
+       if (retval != ERROR_OK)
+               return retval;
+       if (status != 0x80) {
                retval = cfi_send_command(bank, 0xff, flash_address(bank, 0, 0x0));
                if (retval != ERROR_OK)
                        return retval;
@@ -2335,6 +2334,8 @@ static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t of
        int blk_count;  /* number of bus_width bytes for block copy */
        uint8_t current_word[CFI_MAX_BUS_WIDTH * 4];    /* word (bus_width size) currently being
                                                         *programmed */
+       uint8_t *swapped_buffer = NULL;
+       const uint8_t *real_buffer = NULL;
        int i;
        int retval;
 
@@ -2361,8 +2362,14 @@ static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t of
                        return retval;
 
                /* replace only bytes that must be written */
-               for (i = align; (i < bank->bus_width) && (count > 0); i++, count--)
-                       current_word[i] = *buffer++;
+               for (i = align;
+                    (i < bank->bus_width) && (count > 0);
+                    i++, count--)
+                       if (cfi_info->data_swap)
+                               /* data bytes are swapped (reverse endianness) */
+                               current_word[bank->bus_width - i] = *buffer++;
+                       else
+                               current_word[i] = *buffer++;
 
                retval = cfi_write_word(bank, current_word, write_p);
                if (retval != ERROR_OK)
@@ -2370,6 +2377,22 @@ static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t of
                write_p += bank->bus_width;
        }
 
+       if (cfi_info->data_swap && count) {
+               swapped_buffer = malloc(count & ~(bank->bus_width - 1));
+               switch (bank->bus_width) {
+               case 2:
+                       buf_bswap16(swapped_buffer, buffer,
+                                   count & ~(bank->bus_width - 1));
+                       break;
+               case 4:
+                       buf_bswap32(swapped_buffer, buffer,
+                                   count & ~(bank->bus_width - 1));
+                       break;
+               }
+               real_buffer = buffer;
+               buffer = swapped_buffer;
+       }
+
        /* handle blocks of bus_size aligned bytes */
        blk_count = count & ~(bank->bus_width - 1);     /* round down, leave tail bytes */
        switch (cfi_info->pri_id) {
@@ -2439,6 +2462,11 @@ static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t of
                        return retval;
        }
 
+       if (swapped_buffer) {
+               buffer = real_buffer + (buffer - swapped_buffer);
+               free(swapped_buffer);
+       }
+
        /* return to read array mode, so we can read from flash again for padding */
        retval = cfi_reset(bank);
        if (retval != ERROR_OK)
@@ -2455,7 +2483,11 @@ static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t of
 
                /* replace only bytes that must be written */
                for (i = 0; (i < bank->bus_width) && (count > 0); i++, count--)
-                       current_word[i] = *buffer++;
+                       if (cfi_info->data_swap)
+                               /* data bytes are swapped (reverse endianness) */
+                               current_word[bank->bus_width - i] = *buffer++;
+                       else
+                               current_word[i] = *buffer++;
 
                retval = cfi_write_word(bank, current_word, write_p);
                if (retval != ERROR_OK)