Remove FSF address from GPL notices
[fw/openocd] / src / flash / nor / cfi.c
index 398dd61a1b6e49139b9419e925d9308df9862e99..248f76005e59ae2de2b8fc62bf58b478f02b4bee 100644 (file)
@@ -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.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
 #define AT49BV6416      0x00d6
 #define AT49BV6416T     0x00d2
 
-static struct cfi_unlock_addresses cfi_unlock_addresses[] = {
+static const struct cfi_unlock_addresses cfi_unlock_addresses[] = {
        [CFI_UNLOCK_555_2AA] = { .unlock1 = 0x555, .unlock2 = 0x2aa },
        [CFI_UNLOCK_5555_2AAA] = { .unlock1 = 0x5555, .unlock2 = 0x2aaa },
 };
 
-/* CFI fixups foward declarations */
-static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param);
-static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param);
-static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param);
-static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param);
+static const int cfi_status_poll_mask_dq6_dq7 = CFI_STATUS_POLL_MASK_DQ6_DQ7;
+
+/* CFI fixups forward declarations */
+static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, const void *param);
+static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, const void *param);
+static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, const void *param);
+static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, const void *param);
+static void cfi_fixup_0002_polling_bits(struct flash_bank *bank, const void *param);
 
 /* fixup after reading cmdset 0002 primary query table */
 static const struct cfi_fixup cfi_0002_fixups[] = {
@@ -71,6 +72,8 @@ static const struct cfi_fixup cfi_0002_fixups[] = {
         &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
        {CFI_MFR_SST, 0x274b, cfi_fixup_0002_unlock_addresses,
         &cfi_unlock_addresses[CFI_UNLOCK_5555_2AAA]},
+       {CFI_MFR_SST, 0x235f, cfi_fixup_0002_polling_bits,      /* 39VF3201C */
+        &cfi_status_poll_mask_dq6_dq7},
        {CFI_MFR_SST, 0x236d, cfi_fixup_0002_unlock_addresses,
         &cfi_unlock_addresses[CFI_UNLOCK_555_2AA]},
        {CFI_MFR_ATMEL, 0x00C8, cfi_fixup_reversed_erase_regions, NULL},
@@ -109,7 +112,6 @@ static void cfi_fixup(struct flash_bank *bank, const struct cfi_fixup *fixups)
        }
 }
 
-/* inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset) */
 static inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32_t offset)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -132,6 +134,7 @@ static inline uint32_t flash_address(struct flash_bank *bank, int sector, uint32
 static void cfi_command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
 {
        int i;
+       struct cfi_flash_bank *cfi_info = bank->driver_priv;
 
        /* clear whole buffer, to ensure bits that exceed the bus_width
         * are set to zero
@@ -139,7 +142,7 @@ static void cfi_command(struct flash_bank *bank, uint8_t cmd, uint8_t *cmd_buf)
        for (i = 0; i < CFI_MAX_BUS_WIDTH; i++)
                cmd_buf[i] = 0;
 
-       if (bank->target->endianness == TARGET_LITTLE_ENDIAN) {
+       if (cfi_info->endianness == TARGET_LITTLE_ENDIAN) {
                for (i = bank->bus_width; i > 0; i--)
                        *cmd_buf++ = (i & (bank->chip_width - 1)) ? 0x0 : cmd;
        } else {
@@ -163,6 +166,7 @@ static int cfi_send_command(struct flash_bank *bank, uint8_t cmd, uint32_t addre
 static int cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 {
        struct target *target = bank->target;
+       struct cfi_flash_bank *cfi_info = bank->driver_priv;
        uint8_t data[CFI_MAX_BUS_WIDTH];
 
        int retval;
@@ -171,7 +175,7 @@ static int cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset, ui
        if (retval != ERROR_OK)
                return retval;
 
-       if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+       if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
                *val = data[0];
        else
                *val = data[bank->bus_width - 1];
@@ -186,6 +190,7 @@ static int cfi_query_u8(struct flash_bank *bank, int sector, uint32_t offset, ui
 static int cfi_get_u8(struct flash_bank *bank, int sector, uint32_t offset, uint8_t *val)
 {
        struct target *target = bank->target;
+       struct cfi_flash_bank *cfi_info = bank->driver_priv;
        uint8_t data[CFI_MAX_BUS_WIDTH];
        int i;
 
@@ -195,7 +200,7 @@ static int cfi_get_u8(struct flash_bank *bank, int sector, uint32_t offset, uint
        if (retval != ERROR_OK)
                return retval;
 
-       if (bank->target->endianness == TARGET_LITTLE_ENDIAN) {
+       if (cfi_info->endianness == TARGET_LITTLE_ENDIAN) {
                for (i = 0; i < bank->bus_width / bank->chip_width; i++)
                        data[0] |= data[i];
 
@@ -232,7 +237,7 @@ static int cfi_query_u16(struct flash_bank *bank, int sector, uint32_t offset, u
                        return retval;
        }
 
-       if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+       if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
                *val = data[0] | data[bank->bus_width] << 8;
        else
                *val = data[bank->bus_width - 1] | data[(2 * bank->bus_width) - 1] << 8;
@@ -262,7 +267,7 @@ static int cfi_query_u32(struct flash_bank *bank, int sector, uint32_t offset, u
                        return retval;
        }
 
-       if (bank->target->endianness == TARGET_LITTLE_ENDIAN)
+       if (cfi_info->endianness == TARGET_LITTLE_ENDIAN)
                *val = data[0] | data[bank->bus_width] << 8 |
                        data[bank->bus_width * 2] << 16 | data[bank->bus_width * 3] << 24;
        else
@@ -523,6 +528,11 @@ static int cfi_read_spansion_pri_ext(struct flash_bank *bank)
        if (retval != ERROR_OK)
                return retval;
 
+       /* default values for implementation specific workarounds */
+       pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
+       pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
+       pri_ext->_reversed_geometry = 0;
+
        if ((pri_ext->pri[0] != 'P') || (pri_ext->pri[1] != 'R') || (pri_ext->pri[2] != 'I')) {
                retval = cfi_send_command(bank, 0xf0, flash_address(bank, 0, 0x0));
                if (retval != ERROR_OK)
@@ -591,11 +601,6 @@ static int cfi_read_spansion_pri_ext(struct flash_bank *bank)
 
        LOG_DEBUG("WP# protection 0x%x", pri_ext->TopBottom);
 
-       /* default values for implementation specific workarounds */
-       pri_ext->_unlock1 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock1;
-       pri_ext->_unlock2 = cfi_unlock_addresses[CFI_UNLOCK_555_2AA].unlock2;
-       pri_ext->_reversed_geometry = 0;
-
        return ERROR_OK;
 }
 
@@ -799,6 +804,7 @@ static int cfi_intel_info(struct flash_bank *bank, char *buf, int buf_size)
 FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
 {
        struct cfi_flash_bank *cfi_info;
+       int bus_swap = 0;
 
        if (CMD_ARGC < 6)
                return ERROR_COMMAND_SYNTAX_ERROR;
@@ -807,7 +813,7 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
         * - not exceed max value;
         * - not be null;
         * - be equal to a power of 2.
-        * bus must be wide enought to hold one chip */
+        * bus must be wide enough to hold one chip */
        if ((bank->chip_width > CFI_MAX_CHIP_WIDTH)
                        || (bank->bus_width > CFI_MAX_BUS_WIDTH)
                        || (bank->chip_width == 0)
@@ -825,8 +831,6 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
        cfi_info->pri_ext = NULL;
        bank->driver_priv = cfi_info;
 
-       cfi_info->write_algorithm = NULL;
-
        cfi_info->x16_as_x8 = 0;
        cfi_info->jedec_probe = 0;
        cfi_info->not_cfi = 0;
@@ -834,11 +838,18 @@ FLASH_BANK_COMMAND_HANDLER(cfi_flash_bank_command)
        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], "bus_swap") == 0)
+                       bus_swap = 1;
                else if (strcmp(CMD_ARGV[i], "jedec_probe") == 0)
                        cfi_info->jedec_probe = 1;
        }
 
-       cfi_info->write_algorithm = NULL;
+       if (bus_swap)
+               cfi_info->endianness =
+                       bank->target->endianness == TARGET_LITTLE_ENDIAN ?
+                       TARGET_BIG_ENDIAN : TARGET_LITTLE_ENDIAN;
+       else
+               cfi_info->endianness = bank->target->endianness;
 
        /* bank wasn't probed yet */
        cfi_info->qry[0] = 0xff;
@@ -875,9 +886,8 @@ static int cfi_intel_erase(struct flash_bank *bank, int first, int last)
                        if (retval != ERROR_OK)
                                return retval;
 
-                       LOG_ERROR("couldn't erase block %i of flash bank at base 0x%" PRIx32,
-                               i,
-                               bank->base);
+                       LOG_ERROR("couldn't erase block %i of flash bank at base 0x%"
+                                       PRIx32, i, bank->base);
                        return ERROR_FLASH_OPERATION_FAILED;
                }
        }
@@ -1104,19 +1114,6 @@ static int cfi_protect(struct flash_bank *bank, int set, int first, int last)
        }
 }
 
-/* Convert code image to target endian
- * FIXME create general block conversion fcts in target.c?) */
-static void cfi_fix_code_endian(struct target *target, uint8_t *dest,
-       const uint32_t *src, uint32_t count)
-{
-       uint32_t i;
-       for (i = 0; i < count; i++) {
-               target_buffer_set_u32(target, dest, *src);
-               dest += 4;
-               src++;
-       }
-}
-
 static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
 {
        struct target *target = bank->target;
@@ -1140,13 +1137,13 @@ static uint32_t cfi_command_val(struct flash_bank *bank, uint8_t cmd)
        }
 }
 
-static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
+static int cfi_intel_write_block(struct flash_bank *bank, const uint8_t *buffer,
        uint32_t address, uint32_t count)
 {
-       struct cfi_flash_bank *cfi_info = bank->driver_priv;
        struct target *target = bank->target;
        struct reg_param reg_params[7];
        struct arm_algorithm arm_algo;
+       struct working_area *write_algorithm;
        struct working_area *source = NULL;
        uint32_t buffer_size = 32768;
        uint32_t write_command_val, busy_pattern_val, error_pattern_val;
@@ -1163,80 +1160,80 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 
        /* see contib/loaders/flash/armv4_5_cfi_intel_32.s for src */
        static const uint32_t word_32_code[] = {
-               0xe4904004,     /* loop:        ldr r4, [r0], #4 */
-               0xe5813000,     /*              str r3, [r1] */
-               0xe5814000,     /*              str r4, [r1] */
-               0xe5914000,     /* busy:  ldr r4, [r1] */
-               0xe0047005,     /*              and r7, r4, r5 */
-               0xe1570005,     /*              cmp r7, r5 */
-               0x1afffffb,     /*              bne busy */
-               0xe1140006,     /*              tst r4, r6 */
-               0x1a000003,     /*              bne done */
-               0xe2522001,     /*              subs r2, r2, #1 */
-               0x0a000001,     /*              beq done */
-               0xe2811004,     /*              add r1, r1 #4 */
-               0xeafffff2,     /*              b loop */
-               0xeafffffe      /* done:        b -2 */
+               0xe4904004,     /* loop: ldr r4, [r0], #4 */
+               0xe5813000,     /*       str r3, [r1] */
+               0xe5814000,     /*       str r4, [r1] */
+               0xe5914000,     /* busy: ldr r4, [r1] */
+               0xe0047005,     /*        and r7, r4, r5 */
+               0xe1570005,     /*       cmp r7, r5 */
+               0x1afffffb,     /*       bne busy */
+               0xe1140006,     /*       tst r4, r6 */
+               0x1a000003,     /*       bne done */
+               0xe2522001,     /*       subs r2, r2, #1 */
+               0x0a000001,     /*       beq done */
+               0xe2811004,     /*       add r1, r1 #4 */
+               0xeafffff2,     /*       b loop */
+               0xeafffffe      /* done: b -2 */
        };
 
        /* see contib/loaders/flash/armv4_5_cfi_intel_16.s for src */
        static const uint32_t word_16_code[] = {
-               0xe0d040b2,     /* loop:        ldrh r4, [r0], #2 */
-               0xe1c130b0,     /*              strh r3, [r1] */
-               0xe1c140b0,     /*              strh r4, [r1] */
-               0xe1d140b0,     /* busy ldrh r4, [r1] */
-               0xe0047005,     /*              and r7, r4, r5 */
-               0xe1570005,     /*              cmp r7, r5 */
-               0x1afffffb,     /*              bne busy */
-               0xe1140006,     /*              tst r4, r6 */
-               0x1a000003,     /*              bne done */
-               0xe2522001,     /*              subs r2, r2, #1 */
-               0x0a000001,     /*              beq done */
-               0xe2811002,     /*              add r1, r1 #2 */
-               0xeafffff2,     /*              b loop */
+               0xe0d040b2,     /* loop: ldrh r4, [r0], #2 */
+               0xe1c130b0,     /*       strh r3, [r1] */
+               0xe1c140b0,     /*       strh r4, [r1] */
+               0xe1d140b0,     /* busy  ldrh r4, [r1] */
+               0xe0047005,     /*       and r7, r4, r5 */
+               0xe1570005,     /*       cmp r7, r5 */
+               0x1afffffb,     /*       bne busy */
+               0xe1140006,     /*       tst r4, r6 */
+               0x1a000003,     /*       bne done */
+               0xe2522001,     /*       subs r2, r2, #1 */
+               0x0a000001,     /*       beq done */
+               0xe2811002,     /*       add r1, r1 #2 */
+               0xeafffff2,     /*       b loop */
                0xeafffffe      /* done:        b -2 */
        };
 
        /* see contib/loaders/flash/armv4_5_cfi_intel_8.s for src */
        static const uint32_t word_8_code[] = {
-               0xe4d04001,     /* loop:        ldrb r4, [r0], #1 */
-               0xe5c13000,     /*              strb r3, [r1] */
-               0xe5c14000,     /*              strb r4, [r1] */
-               0xe5d14000,     /* busy ldrb r4, [r1] */
-               0xe0047005,     /*              and r7, r4, r5 */
-               0xe1570005,     /*              cmp r7, r5 */
-               0x1afffffb,     /*              bne busy */
-               0xe1140006,     /*              tst r4, r6 */
-               0x1a000003,     /*              bne done */
-               0xe2522001,     /*              subs r2, r2, #1 */
-               0x0a000001,     /*              beq done */
-               0xe2811001,     /*              add r1, r1 #1 */
-               0xeafffff2,     /*              b loop */
-               0xeafffffe      /* done:        b -2 */
+               0xe4d04001,     /* loop: ldrb r4, [r0], #1 */
+               0xe5c13000,     /*       strb r3, [r1] */
+               0xe5c14000,     /*       strb r4, [r1] */
+               0xe5d14000,     /* busy  ldrb r4, [r1] */
+               0xe0047005,     /*       and r7, r4, r5 */
+               0xe1570005,     /*       cmp r7, r5 */
+               0x1afffffb,     /*       bne busy */
+               0xe1140006,     /*       tst r4, r6 */
+               0x1a000003,     /*       bne done */
+               0xe2522001,     /*       subs r2, r2, #1 */
+               0x0a000001,     /*       beq done */
+               0xe2811001,     /*       add r1, r1 #1 */
+               0xeafffff2,     /*       b loop */
+               0xeafffffe      /* done: b -2 */
        };
        uint8_t target_code[4*CFI_MAX_INTEL_CODESIZE];
        const uint32_t *target_code_src;
        uint32_t target_code_size;
        int retval = ERROR_OK;
 
-       /*  todo:  if ( (!is_armv7m(target_to_armv7m(target)) && (!is_arm(target_to_arm(target)) )
-        **/
-       if (strncmp(target_type_name(target), "mips_m4k", 8) == 0) {
-               LOG_ERROR("Your target has no flash block write support yet.");
+       /* check we have a supported arch */
+       if (is_arm(target_to_arm(target))) {
+               /* All other ARM CPUs have 32 bit instructions */
+               arm_algo.common_magic = ARM_COMMON_MAGIC;
+               arm_algo.core_mode = ARM_MODE_SVC;
+               arm_algo.core_state = ARM_STATE_ARM;
+       } else {
+               LOG_ERROR("Unknown architecture");
                return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
 
        cfi_intel_clear_status_register(bank);
 
-       arm_algo.common_magic = ARM_COMMON_MAGIC;
-       arm_algo.core_mode = ARM_MODE_SVC;
-       arm_algo.core_state = ARM_STATE_ARM;
-
        /* If we are setting up the write_algorith, we need target_code_src
         * if not we only need target_code_size. */
 
        /* However, we don't want to create multiple code paths, so we
-        * do the unecessary evaluation of target_code_src, which the
+        * do the unnecessary evaluation of target_code_src, which the
         * compiler will probably nicely optimize away if not needed */
 
        /* prepare algorithm code for target endian */
@@ -1260,35 +1257,33 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
        }
 
        /* flash write code */
-       if (!cfi_info->write_algorithm) {
-               if (target_code_size > sizeof(target_code)) {
-                       LOG_WARNING("Internal error - target code buffer to small. "
+       if (target_code_size > sizeof(target_code)) {
+               LOG_WARNING("Internal error - target code buffer to small. "
                                "Increase CFI_MAX_INTEL_CODESIZE and recompile.");
-                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-               }
-               cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+       }
 
-               /* Get memory for block write handler */
-               retval = target_alloc_working_area(target,
-                               target_code_size,
-                               &cfi_info->write_algorithm);
-               if (retval != ERROR_OK) {
-                       LOG_WARNING("No working area available, can't do block memory writes");
-                       return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
-               }
-               ;
+       target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
 
-               /* write algorithm code to working area */
-               retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-                               target_code_size, target_code);
-               if (retval != ERROR_OK) {
-                       LOG_ERROR("Unable to write block write code to target");
-                       goto cleanup;
-               }
+       /* Get memory for block write handler */
+       retval = target_alloc_working_area(target,
+                       target_code_size,
+                       &write_algorithm);
+       if (retval != ERROR_OK) {
+               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,
+                       target_code_size, target_code);
+       if (retval != ERROR_OK) {
+               LOG_ERROR("Unable to write block write code to target");
+               goto cleanup;
        }
 
        /* Get a workspace buffer for the data to flash starting with 32k size.
-          Half size until buffer would be smaller 256 Bytem then fail back */
+        * Half size until buffer would be smaller 256 Bytes then fail back */
        /* FIXME Why 256 bytes, why not 32 bytes (smallest flash write page */
        while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
                buffer_size /= 2;
@@ -1299,7 +1294,6 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
                        goto cleanup;
                }
        }
-       ;
 
        /* setup algo registers */
        init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
@@ -1340,8 +1334,8 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
 
                /* Execute algorithm, assume breakpoint for last instruction */
                retval = target_run_algorithm(target, 0, NULL, 7, reg_params,
-                               cfi_info->write_algorithm->address,
-                               cfi_info->write_algorithm->address + target_code_size -
+                               write_algorithm->address,
+                               write_algorithm->address + target_code_size -
                                sizeof(uint32_t),
                                10000,  /* 10s should be enough for max. 32k of data */
                                &arm_algo);
@@ -1361,7 +1355,7 @@ static int cfi_intel_write_block(struct flash_bank *bank, uint8_t *buffer,
                /* Check return value from algo code */
                wsm_error = buf_get_u32(reg_params[4].value, 0, 32) & error_pattern_val;
                if (wsm_error) {
-                       /* read status register (outputs debug inforation) */
+                       /* read status register (outputs debug information) */
                        uint8_t status;
                        cfi_intel_wait_status_busy(bank, 100, &status);
                        cfi_intel_clear_status_register(bank);
@@ -1381,10 +1375,7 @@ cleanup:
        if (source)
                target_free_working_area(target, source);
 
-       if (cfi_info->write_algorithm) {
-               target_free_working_area(target, cfi_info->write_algorithm);
-               cfi_info->write_algorithm = NULL;
-       }
+       target_free_working_area(target, write_algorithm);
 
        destroy_reg_param(&reg_params[0]);
        destroy_reg_param(&reg_params[1]);
@@ -1397,7 +1388,7 @@ cleanup:
        return retval;
 }
 
-static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffer,
+static int cfi_spansion_write_block_mips(struct flash_bank *bank, const uint8_t *buffer,
        uint32_t address, uint32_t count)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -1405,6 +1396,7 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
        struct target *target = bank->target;
        struct reg_param reg_params[10];
        struct mips32_algorithm mips32_info;
+       struct working_area *write_algorithm;
        struct working_area *source;
        uint32_t buffer_size = 32768;
        uint32_t status;
@@ -1429,67 +1421,50 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
 
        static const uint32_t mips_word_16_code[] = {
                /* start:       */
-               MIPS32_LHU(9, 0, 4),            /* lhu $t1, ($a0)               ; out = &saddr                          */
-               MIPS32_ADDI(4, 4, 2),           /* addi $a0, $a0, 2             ; saddr += 2                            */
-               MIPS32_SH(13, 0, 12),           /* sh $t5, ($t4)                ; *fl_unl_addr1 =
-                                                *fl_unl_cmd1           */
-               MIPS32_SH(15, 0, 14),           /* sh $t7, ($t6)                ; *fl_unl_addr2 =
-                                                *fl_unl_cmd2           */
-               MIPS32_SH(7, 0, 12),            /* sh $a3, ($t4)                ; *fl_unl_addr1 =
-                                                *fl_write_cmd          */
-               MIPS32_SH(9, 0, 5),             /* sh $t1, ($a1)                ; *daddr = out                          */
-               MIPS32_NOP,                     /* nop                                                                  */
+               MIPS32_LHU(9, 0, 4),                    /* lhu $t1, ($a0)               ; out = &saddr */
+               MIPS32_ADDI(4, 4, 2),                   /* addi $a0, $a0, 2             ; saddr += 2 */
+               MIPS32_SH(13, 0, 12),                   /* sh $t5, ($t4)                ; *fl_unl_addr1 = fl_unl_cmd1 */
+               MIPS32_SH(15, 0, 14),                   /* sh $t7, ($t6)                ; *fl_unl_addr2 = fl_unl_cmd2 */
+               MIPS32_SH(7, 0, 12),                    /* sh $a3, ($t4)                ; *fl_unl_addr1 = fl_write_cmd */
+               MIPS32_SH(9, 0, 5),                             /* sh $t1, ($a1)                ; *daddr = out */
+               MIPS32_NOP,                                             /* nop */
                /* busy:        */
-               MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
-               MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
-                                                *temp1;                        */
-               MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
-                                                *DQ7mask               */
-               MIPS32_BNE(11, 8, 13),          /* bne $t3, $t0, cont           ; if (temp2 !=
-                                                *DQ7mask) goto cont    */
-               MIPS32_NOP,                     /* nop                                                                  */
-
-               MIPS32_SRL(10, 8, 2),           /* srl $t2,$t0,2                ; temp1 = DQ7mask >>
-                                                *2                     */
-               MIPS32_AND(11, 10, 11),         /* and $t3, $t2, $t3            ; temp2 = temp2 &
-                                                *temp1                 */
-               MIPS32_BNE(11, 10, NEG16(8)),   /* bne $t3, $t2, busy           ; if (temp2 !=
-                                                *temp1) goto busy              */
-               MIPS32_NOP,                     /* nop                                                                  */
-
-               MIPS32_LHU(10, 0, 5),           /* lhu $t2, ($a1)               ; temp1 = *daddr                        */
-               MIPS32_XOR(11, 9, 10),          /* xor $t3, $a0, $t2            ; temp2 = out ^
-                                                *temp1;                        */
-               MIPS32_AND(11, 8, 11),          /* and $t3, $t0, $t3            ; temp2 = temp2 &
-                                                *DQ7mask               */
-               MIPS32_BNE(11, 8, 4),           /* bne $t3, $t0, cont           ; if (temp2 !=
-                                                *DQ7mask) goto cont    */
-               MIPS32_NOP,                     /* nop                                                                  */
-
-               MIPS32_XOR(9, 9, 9),            /* xor $t1, $t1, $t1            ; out = 0                               */
-               MIPS32_BEQ(9, 0, 11),           /* beq $t1, $zero, done         ; if (out == 0) goto
-                                                *done          */
-               MIPS32_NOP,                     /* nop                                                                  */
+               MIPS32_LHU(10, 0, 5),                   /* lhu $t2, ($a1)               ; temp1 = *daddr */
+               MIPS32_XOR(11, 9, 10),                  /* xor $t3, $a0, $t2    ; temp2 = out ^ temp1; */
+               MIPS32_AND(11, 8, 11),                  /* and $t3, $t0, $t3    ; temp2 = temp2 & DQ7mask */
+               MIPS32_BNE(11, 8, 13),                  /* bne $t3, $t0, cont   ; if (temp2 != DQ7mask) goto cont */
+               MIPS32_NOP,                                             /* nop                                                                  */
+
+               MIPS32_SRL(10, 8, 2),                   /* srl $t2,$t0,2                ; temp1 = DQ7mask >> 2 */
+               MIPS32_AND(11, 10, 11),                 /* and $t3, $t2, $t3    ; temp2 = temp2 & temp1 */
+               MIPS32_BNE(11, 10, NEG16(8)),   /* bne $t3, $t2, busy   ; if (temp2 != temp1) goto busy */
+               MIPS32_NOP,                                             /* nop                                                                  */
+
+               MIPS32_LHU(10, 0, 5),                   /* lhu $t2, ($a1)               ; temp1 = *daddr */
+               MIPS32_XOR(11, 9, 10),                  /* xor $t3, $a0, $t2    ; temp2 = out ^ temp1; */
+               MIPS32_AND(11, 8, 11),                  /* and $t3, $t0, $t3    ; temp2 = temp2 & DQ7mask */
+               MIPS32_BNE(11, 8, 4),                   /* bne $t3, $t0, cont   ; if (temp2 != DQ7mask) goto cont */
+               MIPS32_NOP,                                             /* nop */
+
+               MIPS32_XOR(9, 9, 9),                    /* xor $t1, $t1, $t1    ; out = 0 */
+               MIPS32_BEQ(9, 0, 11),                   /* beq $t1, $zero, done ; if (out == 0) goto done */
+               MIPS32_NOP,                                             /* nop */
                /* cont:        */
-               MIPS32_ADDI(6, 6, NEG16(1)),    /* addi, $a2, $a2, -1           ; numwrites--                           */
-               MIPS32_BNE(6, 0, 5),            /* bne $a2, $zero, cont2        ; if (numwrite != 0)
-                                                *goto cont2            */
-               MIPS32_NOP,                     /* nop                                                                  */
+               MIPS32_ADDI(6, 6, NEG16(1)),    /* addi, $a2, $a2, -1   ; numwrites-- */
+               MIPS32_BNE(6, 0, 5),                    /* bne $a2, $zero, cont2        ; if (numwrite != 0) goto cont2 */
+               MIPS32_NOP,                                             /* nop */
 
-               MIPS32_LUI(9, 0),               /* lui $t1, 0                                                           */
-               MIPS32_ORI(9, 9, 0x80),         /* ori $t1, $t1, 0x80           ; out = 0x80                            */
+               MIPS32_LUI(9, 0),                               /* lui $t1, 0 */
+               MIPS32_ORI(9, 9, 0x80),                 /* ori $t1, $t1, 0x80   ; out = 0x80 */
 
-               MIPS32_B(4),                    /* b done                       ; goto done                             */
-               MIPS32_NOP,                     /* nop                                                                  */
+               MIPS32_B(4),                                    /* b done                       ; goto done */
+               MIPS32_NOP,                                             /* nop */
                /* cont2:       */
-               MIPS32_ADDI(5, 5, 2),           /* addi $a0, $a0, 2             ; daddr += 2                            */
-               MIPS32_B(NEG16(33)),            /* b start                      ; goto start                            */
-               MIPS32_NOP,                     /* nop                                                                  */
-               /* done:
-                *MIPS32_B(NEG16(1)),   */      /* b done                       ; goto done                             */
-               MIPS32_SDBBP,                   /* sdbbp                        ; break();                              */
-               /*MIPS32_B(NEG16(33)),  */      /* b start                      ; goto start
-                * MIPS32_NOP, */
+               MIPS32_ADDI(5, 5, 2),                   /* addi $a0, $a0, 2     ; daddr += 2 */
+               MIPS32_B(NEG16(33)),                    /* b start                      ; goto start */
+               MIPS32_NOP,                                             /* nop */
+               /* done: */
+               MIPS32_SDBBP,                                   /* sdbbp                        ; break(); */
        };
 
        mips32_info.common_magic = MIPS32_COMMON_MAGIC;
@@ -1518,62 +1493,60 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
        }
 
        /* flash write code */
-       if (!cfi_info->write_algorithm) {
-               uint8_t *target_code;
+       uint8_t *target_code;
 
-               /* convert bus-width dependent algorithm code to correct endiannes */
-               target_code = malloc(target_code_size);
-               if (target_code == NULL) {
-                       LOG_ERROR("Out of memory");
-                       return ERROR_FAIL;
-               }
-               cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+       /* convert bus-width dependent algorithm code to correct endianness */
+       target_code = malloc(target_code_size);
+       if (target_code == NULL) {
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
 
-               /* allocate working area */
-               retval = target_alloc_working_area(target, target_code_size,
-                               &cfi_info->write_algorithm);
-               if (retval != ERROR_OK) {
-                       free(target_code);
-                       return retval;
-               }
+       target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
 
-               /* write algorithm code to working area */
-               retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-                               target_code_size, target_code);
-               if (retval != ERROR_OK) {
-                       free(target_code);
-                       return retval;
-               }
+       /* allocate working area */
+       retval = target_alloc_working_area(target, target_code_size,
+                       &write_algorithm);
+       if (retval != ERROR_OK) {
+               free(target_code);
+               return retval;
+       }
 
+       /* write algorithm code to working area */
+       retval = target_write_buffer(target, write_algorithm->address,
+                       target_code_size, target_code);
+       if (retval != ERROR_OK) {
                free(target_code);
+               return retval;
        }
+
+       free(target_code);
+
        /* the following code still assumes target code is fixed 24*4 bytes */
 
        while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
                buffer_size /= 2;
                if (buffer_size <= 256) {
-                       /* if we already allocated the writing code, but failed to get a
+                       /* we already allocated the writing code, but failed to get a
                         * buffer, free the algorithm */
-                       if (cfi_info->write_algorithm)
-                               target_free_working_area(target, cfi_info->write_algorithm);
+                       target_free_working_area(target, write_algorithm);
 
                        LOG_WARNING(
                                "not enough working area available, can't do block memory writes");
                        return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
                }
        }
-       ;
 
-       init_reg_param(&reg_params[0], "a0", 32, PARAM_OUT);
-       init_reg_param(&reg_params[1], "a1", 32, PARAM_OUT);
-       init_reg_param(&reg_params[2], "a2", 32, PARAM_OUT);
-       init_reg_param(&reg_params[3], "a3", 32, PARAM_OUT);
-       init_reg_param(&reg_params[4], "t0", 32, PARAM_OUT);
-       init_reg_param(&reg_params[5], "t1", 32, PARAM_IN);
-       init_reg_param(&reg_params[6], "t4", 32, PARAM_OUT);
-       init_reg_param(&reg_params[7], "t5", 32, PARAM_OUT);
-       init_reg_param(&reg_params[8], "t6", 32, PARAM_OUT);
-       init_reg_param(&reg_params[9], "t7", 32, PARAM_OUT);
+       init_reg_param(&reg_params[0], "r4", 32, PARAM_OUT);
+       init_reg_param(&reg_params[1], "r5", 32, PARAM_OUT);
+       init_reg_param(&reg_params[2], "r6", 32, PARAM_OUT);
+       init_reg_param(&reg_params[3], "r7", 32, PARAM_OUT);
+       init_reg_param(&reg_params[4], "r8", 32, PARAM_OUT);
+       init_reg_param(&reg_params[5], "r9", 32, PARAM_IN);
+       init_reg_param(&reg_params[6], "r12", 32, PARAM_OUT);
+       init_reg_param(&reg_params[7], "r13", 32, PARAM_OUT);
+       init_reg_param(&reg_params[8], "r14", 32, PARAM_OUT);
+       init_reg_param(&reg_params[9], "r15", 32, PARAM_OUT);
 
        while (count > 0) {
                uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
@@ -1593,8 +1566,8 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
                buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
 
                retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
-                               cfi_info->write_algorithm->address,
-                               cfi_info->write_algorithm->address + ((target_code_size) - 4),
+                               write_algorithm->address,
+                               write_algorithm->address + ((target_code_size) - 4),
                                10000, &mips32_info);
                if (retval != ERROR_OK)
                        break;
@@ -1627,14 +1600,17 @@ static int cfi_spansion_write_block_mips(struct flash_bank *bank, uint8_t *buffe
        return retval;
 }
 
-static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
+static int cfi_spansion_write_block(struct flash_bank *bank, const uint8_t *buffer,
        uint32_t address, uint32_t count)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
        struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
        struct target *target = bank->target;
        struct reg_param reg_params[10];
-       struct arm_algorithm arm_algo;
+       void *arm_algo;
+       struct arm_algorithm armv4_5_algo;
+       struct armv7m_algorithm armv7m_algo;
+       struct working_area *write_algorithm;
        struct working_area *source;
        uint32_t buffer_size = 32768;
        uint32_t status;
@@ -1666,8 +1642,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0xe5883000,             /* str  r3, [r8]                                */
                0xe5815000,             /* str  r5, [r1]                                */
                0xe1a00000,             /* nop                                                  */
-               /*
-                * 00008110 <sp_32_busy>:               */
+               /* 00008110 <sp_32_busy>:               */
                0xe5916000,             /* ldr  r6, [r1]                                */
                0xe0257006,             /* eor  r7, r5, r6                              */
                0xe0147007,             /* ands r7, r4, r7                              */
@@ -1680,15 +1655,13 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0x0a000001,             /* beq  8140 <sp_32_cont> ; b if DQ7 == Data7 */
                0xe3a05000,             /* mov  r5, #0  ; 0x0 - return 0x00, error */
                0x1a000004,             /* bne  8154 <sp_32_done>               */
-               /*
-                * 00008140 <sp_32_cont>:               */
+               /* 00008140 <sp_32_cont>:               */
                0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
                0x03a05080,             /* moveq        r5, #128        ; 0x80  */
                0x0a000001,             /* beq  8154 <sp_32_done>               */
                0xe2811004,             /* add  r1, r1, #4      ; 0x4           */
                0xeaffffe8,             /* b    8100 <sp_32_code>               */
-               /*
-                * 00008154 <sp_32_done>:               */
+               /* 00008154 <sp_32_done>:               */
                0xeafffffe              /* b    8154 <sp_32_done>               */
        };
 
@@ -1701,8 +1674,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0xe1c830b0,             /* strh r3, [r8]                                */
                0xe1c150b0,             /* strh r5, [r1]                                */
                0xe1a00000,             /* nop                  (mov r0,r0)             */
-               /*
-                * 00008168 <sp_16_busy>:               */
+               /* 00008168 <sp_16_busy>:               */
                0xe1d160b0,             /* ldrh r6, [r1]                                */
                0xe0257006,             /* eor  r7, r5, r6                              */
                0xe0147007,             /* ands r7, r4, r7                              */
@@ -1715,19 +1687,17 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0x0a000001,             /* beq  8198 <sp_16_cont>               */
                0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
                0x1a000004,             /* bne  81ac <sp_16_done>               */
-               /*
-                * 00008198 <sp_16_cont>:               */
+               /* 00008198 <sp_16_cont>:               */
                0xe2522001,     /* subs r2, r2, #1      ; 0x1           */
                0x03a05080,     /* moveq        r5, #128        ; 0x80  */
                0x0a000001,     /* beq  81ac <sp_16_done>               */
                0xe2811002,     /* add  r1, r1, #2      ; 0x2           */
                0xeaffffe8,     /* b    8158 <sp_16_code>               */
-               /*
-                * 000081ac <sp_16_done>:               */
+               /* 000081ac <sp_16_done>:               */
                0xeafffffe              /* b    81ac <sp_16_done>               */
        };
 
-       /* see contib/loaders/flash/armv7m_cfi_span_16.s for src */
+       /* see contrib/loaders/flash/armv7m_cfi_span_16.s for src */
        static const uint32_t armv7m_word_16_code[] = {
                0x5B02F830,
                0x9000F8A8,
@@ -1749,7 +1719,36 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0x0000BE00
        };
 
-       /* see contib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
+       /* see contrib/loaders/flash/armv7m_cfi_span_16_dq7.s for src */
+       static const uint32_t armv7m_word_16_code_dq7only[] = {
+               /* 00000000 <code>: */
+               0x5B02F830,             /* ldrh.w       r5, [r0], #2    */
+               0x9000F8A8,             /* strh.w       r9, [r8]                */
+               0xB000F8AA,             /* strh.w       fp, [sl]                */
+               0x3000F8A8,             /* strh.w       r3, [r8]                */
+               0xBF00800D,             /* strh r5, [r1, #0]            */
+                                               /* nop                                          */
+
+               /* 00000014 <busy>: */
+               0xEA85880E,             /* ldrh r6, [r1, #0]            */
+                                               /* eor.w        r7, r5, r6              */
+               0x40270706,             /* ands         r7, r4                  */
+               0x3A01D1FA,             /* bne.n        14 <busy>               */
+                                               /* subs r2, #1                          */
+               0xF101D002,             /* beq.n        28 <success>    */
+               0xE7EB0102,             /* add.w        r1, r1, #2              */
+                                               /* b.n  0 <code>                        */
+
+               /* 00000028 <success>: */
+               0x0580F04F,             /* mov.w        r5, #128                */
+               0xBF00E7FF,             /* b.n  30 <done>                       */
+                                               /* nop (for alignment purposes) */
+
+               /* 00000030 <done>: */
+               0x0000BE00              /* bkpt 0x0000                          */
+       };
+
+       /* see contrib/loaders/flash/armv4_5_cfi_span_16_dq7.s for src */
        static const uint32_t armv4_5_word_16_code_dq7only[] = {
                /* <sp_16_code>:                                */
                0xe0d050b2,             /* ldrh r5, [r0], #2                    */
@@ -1758,8 +1757,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0xe1c830b0,             /* strh r3, [r8]                                */
                0xe1c150b0,             /* strh r5, [r1]                                */
                0xe1a00000,             /* nop                  (mov r0,r0)             */
-               /*
-                * <sp_16_busy>:                                */
+               /* <sp_16_busy>:                                */
                0xe1d160b0,             /* ldrh r6, [r1]                                */
                0xe0257006,             /* eor  r7, r5, r6                              */
                0xe2177080,             /* ands r7, #0x80                               */
@@ -1770,12 +1768,11 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0x0a000001,             /* beq  81ac <sp_16_done>               */
                0xe2811002,             /* add  r1, r1, #2      ; 0x2           */
                0xeafffff0,             /* b    8158 <sp_16_code>               */
-               /*
-                * 000081ac <sp_16_done>:               */
+               /* 000081ac <sp_16_done>:               */
                0xeafffffe              /* b    81ac <sp_16_done>               */
        };
 
-       /* see contib/loaders/flash/armv4_5_cfi_span_8.s for src */
+       /* see contrib/loaders/flash/armv4_5_cfi_span_8.s for src */
        static const uint32_t armv4_5_word_8_code[] = {
                /* 000081b0 <sp_16_code_end>:   */
                0xe4d05001,             /* ldrb r5, [r0], #1                    */
@@ -1784,8 +1781,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0xe5c83000,             /* strb r3, [r8]                                */
                0xe5c15000,             /* strb r5, [r1]                                */
                0xe1a00000,             /* nop                  (mov r0,r0)             */
-               /*
-                * 000081c0 <sp_8_busy>:                */
+               /* 000081c0 <sp_8_busy>:                */
                0xe5d16000,             /* ldrb r6, [r1]                                */
                0xe0257006,             /* eor  r7, r5, r6                              */
                0xe0147007,             /* ands r7, r4, r7                              */
@@ -1798,33 +1794,32 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                0x0a000001,             /* beq  81f0 <sp_8_cont>                */
                0xe3a05000,             /* mov  r5, #0  ; 0x0                   */
                0x1a000004,             /* bne  8204 <sp_8_done>                */
-               /*
-                * 000081f0 <sp_8_cont>:                */
+               /* 000081f0 <sp_8_cont>:                */
                0xe2522001,             /* subs r2, r2, #1      ; 0x1           */
                0x03a05080,             /* moveq        r5, #128        ; 0x80  */
                0x0a000001,             /* beq  8204 <sp_8_done>                */
                0xe2811001,             /* add  r1, r1, #1      ; 0x1           */
                0xeaffffe8,             /* b    81b0 <sp_16_code_end>   */
-               /*
-                * 00008204 <sp_8_done>:                */
+               /* 00008204 <sp_8_done>:                */
                0xeafffffe              /* b    8204 <sp_8_done>                */
        };
 
        if (strncmp(target_type_name(target), "mips_m4k", 8) == 0)
                return cfi_spansion_write_block_mips(bank, buffer, address, count);
 
-       if (is_armv7m(target_to_armv7m(target))) {      /* Cortex-M3 target */
-               arm_algo.common_magic = ARMV7M_COMMON_MAGIC;
-               arm_algo.core_mode = ARMV7M_MODE_HANDLER;
-               arm_algo.core_state = ARM_STATE_ARM;
-       } else if (is_arm7_9(target_to_arm7_9(target))) {
+       if (is_armv7m(target_to_armv7m(target))) {      /* armv7m target */
+               armv7m_algo.common_magic = ARMV7M_COMMON_MAGIC;
+               armv7m_algo.core_mode = ARM_MODE_THREAD;
+               arm_algo = &armv7m_algo;
+       } else if (is_arm(target_to_arm(target))) {
                /* All other ARM CPUs have 32 bit instructions */
-               arm_algo.common_magic = ARM_COMMON_MAGIC;
-               arm_algo.core_mode = ARM_MODE_SVC;
-               arm_algo.core_state = ARM_STATE_ARM;
+               armv4_5_algo.common_magic = ARM_COMMON_MAGIC;
+               armv4_5_algo.core_mode = ARM_MODE_SVC;
+               armv4_5_algo.core_state = ARM_STATE_ARM;
+               arm_algo = &armv4_5_algo;
        } else {
                LOG_ERROR("Unknown architecture");
-               return ERROR_FAIL;
+               return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
        }
 
        int target_code_size = 0;
@@ -1832,7 +1827,7 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
 
        switch (bank->bus_width) {
                case 1:
-                       if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
+                       if (is_armv7m(target_to_armv7m(target))) {
                                LOG_ERROR("Unknown ARM architecture");
                                return ERROR_FAIL;
                        }
@@ -1842,28 +1837,28 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                case 2:
                        /* Check for DQ5 support */
                        if (cfi_info->status_poll_mask & (1 << 5)) {
-                               if (arm_algo.common_magic == ARM_COMMON_MAGIC) {/* armv4_5 target */
-                                       target_code_src = armv4_5_word_16_code;
-                                       target_code_size = sizeof(armv4_5_word_16_code);
-                               } else if (arm_algo.common_magic == ARMV7M_COMMON_MAGIC) {      /*
-                                                                                                *cortex-m3
-                                                                                                *target
-                                                                                                **/
+                               if (is_armv7m(target_to_armv7m(target))) {
+                                       /* armv7m target */
                                        target_code_src = armv7m_word_16_code;
                                        target_code_size = sizeof(armv7m_word_16_code);
+                               } else { /* armv4_5 target */
+                                       target_code_src = armv4_5_word_16_code;
+                                       target_code_size = sizeof(armv4_5_word_16_code);
                                }
                        } else {
                                /* No DQ5 support. Use DQ7 DATA# polling only. */
-                               if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
-                                       LOG_ERROR("Unknown ARM architecture");
-                                       return ERROR_FAIL;
+                               if (is_armv7m(target_to_armv7m(target))) {
+                                       /* armv7m target */
+                                       target_code_src = armv7m_word_16_code_dq7only;
+                                       target_code_size = sizeof(armv7m_word_16_code_dq7only);
+                               } else { /* armv4_5 target */
+                                       target_code_src = armv4_5_word_16_code_dq7only;
+                                       target_code_size = sizeof(armv4_5_word_16_code_dq7only);
                                }
-                               target_code_src = armv4_5_word_16_code_dq7only;
-                               target_code_size = sizeof(armv4_5_word_16_code_dq7only);
                        }
                        break;
                case 4:
-                       if (arm_algo.common_magic != ARM_COMMON_MAGIC) {
+                       if (is_armv7m(target_to_armv7m(target))) {
                                LOG_ERROR("Unknown ARM architecture");
                                return ERROR_FAIL;
                        }
@@ -1877,51 +1872,49 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
        }
 
        /* flash write code */
-       if (!cfi_info->write_algorithm) {
-               uint8_t *target_code;
+       uint8_t *target_code;
 
-               /* convert bus-width dependent algorithm code to correct endiannes */
-               target_code = malloc(target_code_size);
-               if (target_code == NULL) {
-                       LOG_ERROR("Out of memory");
-                       return ERROR_FAIL;
-               }
-               cfi_fix_code_endian(target, target_code, target_code_src, target_code_size / 4);
+       /* convert bus-width dependent algorithm code to correct endianness */
+       target_code = malloc(target_code_size);
+       if (target_code == NULL) {
+               LOG_ERROR("Out of memory");
+               return ERROR_FAIL;
+       }
 
-               /* allocate working area */
-               retval = target_alloc_working_area(target, target_code_size,
-                               &cfi_info->write_algorithm);
-               if (retval != ERROR_OK) {
-                       free(target_code);
-                       return retval;
-               }
+       target_buffer_set_u32_array(target, target_code, target_code_size / 4, target_code_src);
 
-               /* write algorithm code to working area */
-               retval = target_write_buffer(target, cfi_info->write_algorithm->address,
-                               target_code_size, target_code);
-               if (retval != ERROR_OK) {
-                       free(target_code);
-                       return retval;
-               }
+       /* allocate working area */
+       retval = target_alloc_working_area(target, target_code_size,
+                       &write_algorithm);
+       if (retval != ERROR_OK) {
+               free(target_code);
+               return retval;
+       }
 
+       /* write algorithm code to working area */
+       retval = target_write_buffer(target, write_algorithm->address,
+                       target_code_size, target_code);
+       if (retval != ERROR_OK) {
                free(target_code);
+               return retval;
        }
+
+       free(target_code);
+
        /* the following code still assumes target code is fixed 24*4 bytes */
 
        while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
                buffer_size /= 2;
                if (buffer_size <= 256) {
-                       /* if we already allocated the writing code, but failed to get a
+                       /* we already allocated the writing code, but failed to get a
                         * buffer, free the algorithm */
-                       if (cfi_info->write_algorithm)
-                               target_free_working_area(target, cfi_info->write_algorithm);
+                       target_free_working_area(target, write_algorithm);
 
                        LOG_WARNING(
                                "not enough working area available, can't do block memory writes");
                        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);
@@ -1952,9 +1945,9 @@ static int cfi_spansion_write_block(struct flash_bank *bank, uint8_t *buffer,
                buf_set_u32(reg_params[9].value, 0, 32, 0x55555555);
 
                retval = target_run_algorithm(target, 0, NULL, 10, reg_params,
-                               cfi_info->write_algorithm->address,
-                               cfi_info->write_algorithm->address + ((target_code_size) - 4),
-                               10000, &arm_algo);
+                               write_algorithm->address,
+                               write_algorithm->address + ((target_code_size) - 4),
+                               10000, arm_algo);
                if (retval != ERROR_OK)
                        break;
 
@@ -2003,7 +1996,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;
@@ -2016,7 +2011,7 @@ static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t
        return ERROR_OK;
 }
 
-static int cfi_intel_write_words(struct flash_bank *bank, uint8_t *word,
+static int cfi_intel_write_words(struct flash_bank *bank, const uint8_t *word,
        uint32_t wordcount, uint32_t address)
 {
        int retval;
@@ -2136,7 +2131,7 @@ static int cfi_spansion_write_word(struct flash_bank *bank, uint8_t *word, uint3
        return ERROR_OK;
 }
 
-static int cfi_spansion_write_words(struct flash_bank *bank, uint8_t *word,
+static int cfi_spansion_write_words(struct flash_bank *bank, const uint8_t *word,
        uint32_t wordcount, uint32_t address)
 {
        int retval;
@@ -2229,7 +2224,7 @@ static int cfi_write_word(struct flash_bank *bank, uint8_t *word, uint32_t addre
        return ERROR_FLASH_OPERATION_FAILED;
 }
 
-static int cfi_write_words(struct flash_bank *bank, uint8_t *word,
+static int cfi_write_words(struct flash_bank *bank, const uint8_t *word,
        uint32_t wordcount, uint32_t address)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -2311,7 +2306,7 @@ static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, u
        }
 
        if (count) {
-               LOG_INFO("Fixup %d unaligned read tail bytes", count);
+               LOG_INFO("Fixup %" PRIu32 " unaligned read tail bytes", count);
 
                /* read a complete word from flash */
                retval = target_read_memory(target, read_p, bank->bus_width, 1, current_word);
@@ -2326,7 +2321,7 @@ static int cfi_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, u
        return ERROR_OK;
 }
 
-static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+static int cfi_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset, uint32_t count)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
        struct target *target = bank->target;
@@ -2467,7 +2462,7 @@ static int cfi_write(struct flash_bank *bank, uint8_t *buffer, uint32_t offset,
        return cfi_reset(bank);
 }
 
-static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *param)
+static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, const void *param)
 {
        (void) param;
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -2476,7 +2471,7 @@ static void cfi_fixup_reversed_erase_regions(struct flash_bank *bank, void *para
        pri_ext->_reversed_geometry = 1;
 }
 
-static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
+static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, const void *param)
 {
        int i;
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
@@ -2497,16 +2492,24 @@ static void cfi_fixup_0002_erase_regions(struct flash_bank *bank, void *param)
        }
 }
 
-static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, void *param)
+static void cfi_fixup_0002_unlock_addresses(struct flash_bank *bank, const void *param)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;
        struct cfi_spansion_pri_ext *pri_ext = cfi_info->pri_ext;
-       struct cfi_unlock_addresses *unlock_addresses = param;
+       const struct cfi_unlock_addresses *unlock_addresses = param;
 
        pri_ext->_unlock1 = unlock_addresses->unlock1;
        pri_ext->_unlock2 = unlock_addresses->unlock2;
 }
 
+static void cfi_fixup_0002_polling_bits(struct flash_bank *bank, const void *param)
+{
+       struct cfi_flash_bank *cfi_info = bank->driver_priv;
+       const int *status_poll_mask = param;
+
+       cfi_info->status_poll_mask = *status_poll_mask;
+}
+
 
 static int cfi_query_string(struct flash_bank *bank, int address)
 {
@@ -2634,7 +2637,7 @@ static int cfi_probe(struct flash_bank *bank)
                 * a single bus sequence with address = 0x55, data = 0x98 should put
                 * the device into CFI query mode.
                 *
-                * SST flashes clearly violate this, and we will consider them incompatbile for now
+                * SST flashes clearly violate this, and we will consider them incompatible for now
                 */
 
                retval = cfi_query_string(bank, 0x55);
@@ -3068,7 +3071,7 @@ static int get_cfi_info(struct flash_bank *bank, char *buf, int buf_size)
        return ERROR_OK;
 }
 
-static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, void *param)
+static void cfi_fixup_0002_write_buffer(struct flash_bank *bank, const void *param)
 {
        struct cfi_flash_bank *cfi_info = bank->driver_priv;