+ struct reg_param reg_params[10];
+ struct arm_algorithm armv4_5_info;
+ struct working_area *source;
+ uint32_t buffer_size = 32768;
+ uint32_t status;
+ int retval = ERROR_OK;
+
+ /* input parameters - */
+ /* R0 = source address */
+ /* R1 = destination address */
+ /* R2 = number of writes */
+ /* R3 = flash write command */
+ /* R4 = constant to mask DQ7 bits (also used for Dq5 with shift) */
+ /* output parameters - */
+ /* R5 = 0x80 ok 0x00 bad */
+ /* temp registers - */
+ /* R6 = value read from flash to test status */
+ /* R7 = holding register */
+ /* unlock registers - */
+ /* R8 = unlock1_addr */
+ /* R9 = unlock1_cmd */
+ /* R10 = unlock2_addr */
+ /* R11 = unlock2_cmd */
+
+ /* see contib/loaders/flash/armv4_5_cfi_span_32.s for src */
+ static const uint32_t armv4_5_word_32_code[] = {
+ /* 00008100 <sp_32_code>: */
+ 0xe4905004, /* ldr r5, [r0], #4 */
+ 0xe5889000, /* str r9, [r8] */
+ 0xe58ab000, /* str r11, [r10] */
+ 0xe5883000, /* str r3, [r8] */
+ 0xe5815000, /* str r5, [r1] */
+ 0xe1a00000, /* nop */
+ /* */
+ /* 00008110 <sp_32_busy>: */
+ 0xe5916000, /* ldr r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 0x0a000007, /* beq 8140 <sp_32_cont> ; b if DQ7 == Data7 */
+ 0xe0166124, /* ands r6, r6, r4, lsr #2 */
+ 0x0afffff9, /* beq 8110 <sp_32_busy> ; b if DQ5 low */
+ 0xe5916000, /* ldr r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 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>: */
+ 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>: */
+ 0xeafffffe /* b 8154 <sp_32_done> */
+ };
+
+ /* see contib/loaders/flash/armv4_5_cfi_span_16.s for src */
+ static const uint32_t armv4_5_word_16_code[] = {
+ /* 00008158 <sp_16_code>: */
+ 0xe0d050b2, /* ldrh r5, [r0], #2 */
+ 0xe1c890b0, /* strh r9, [r8] */
+ 0xe1cab0b0, /* strh r11, [r10] */
+ 0xe1c830b0, /* strh r3, [r8] */
+ 0xe1c150b0, /* strh r5, [r1] */
+ 0xe1a00000, /* nop (mov r0,r0) */
+ /* */
+ /* 00008168 <sp_16_busy>: */
+ 0xe1d160b0, /* ldrh r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 0x0a000007, /* beq 8198 <sp_16_cont> */
+ 0xe0166124, /* ands r6, r6, r4, lsr #2 */
+ 0x0afffff9, /* beq 8168 <sp_16_busy> */
+ 0xe1d160b0, /* ldrh r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 0x0a000001, /* beq 8198 <sp_16_cont> */
+ 0xe3a05000, /* mov r5, #0 ; 0x0 */
+ 0x1a000004, /* bne 81ac <sp_16_done> */
+ /* */
+ /* 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>: */
+ 0xeafffffe /* b 81ac <sp_16_done> */
+ };
+
+ /* see contib/loaders/flash/armv7m_cfi_span_16.s for src */
+ static const uint32_t armv7m_word_16_code[] = {
+ 0x5B02F830,
+ 0x9000F8A8,
+ 0xB000F8AA,
+ 0x3000F8A8,
+ 0xBF00800D,
+ 0xEA85880E,
+ 0x40270706,
+ 0xEA16D00A,
+ 0xD0F70694,
+ 0xEA85880E,
+ 0x40270706,
+ 0xF04FD002,
+ 0xD1070500,
+ 0xD0023A01,
+ 0x0102F101,
+ 0xF04FE7E0,
+ 0xE7FF0580,
+ 0x0000BE00
+ };
+
+ /* see contib/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 */
+ 0xe1c890b0, /* strh r9, [r8] */
+ 0xe1cab0b0, /* strh r11, [r10] */
+ 0xe1c830b0, /* strh r3, [r8] */
+ 0xe1c150b0, /* strh r5, [r1] */
+ 0xe1a00000, /* nop (mov r0,r0) */
+ /* */
+ /* <sp_16_busy>: */
+ 0xe1d160b0, /* ldrh r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe2177080, /* ands r7, #0x80 */
+ 0x1afffffb, /* bne 8168 <sp_16_busy> */
+ /* */
+ 0xe2522001, /* subs r2, r2, #1 ; 0x1 */
+ 0x03a05080, /* moveq r5, #128 ; 0x80 */
+ 0x0a000001, /* beq 81ac <sp_16_done> */
+ 0xe2811002, /* add r1, r1, #2 ; 0x2 */
+ 0xeafffff0, /* b 8158 <sp_16_code> */
+ /* */
+ /* 000081ac <sp_16_done>: */
+ 0xeafffffe /* b 81ac <sp_16_done> */
+ };
+
+ /* see contib/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 */
+ 0xe5c89000, /* strb r9, [r8] */
+ 0xe5cab000, /* strb r11, [r10] */
+ 0xe5c83000, /* strb r3, [r8] */
+ 0xe5c15000, /* strb r5, [r1] */
+ 0xe1a00000, /* nop (mov r0,r0) */
+ /* */
+ /* 000081c0 <sp_8_busy>: */
+ 0xe5d16000, /* ldrb r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 0x0a000007, /* beq 81f0 <sp_8_cont> */
+ 0xe0166124, /* ands r6, r6, r4, lsr #2 */
+ 0x0afffff9, /* beq 81c0 <sp_8_busy> */
+ 0xe5d16000, /* ldrb r6, [r1] */
+ 0xe0257006, /* eor r7, r5, r6 */
+ 0xe0147007, /* ands r7, r4, r7 */
+ 0x0a000001, /* beq 81f0 <sp_8_cont> */
+ 0xe3a05000, /* mov r5, #0 ; 0x0 */
+ 0x1a000004, /* bne 8204 <sp_8_done> */
+ /* */
+ /* 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>: */
+ 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 */
+ {
+ armv4_5_info.common_magic = ARMV7M_COMMON_MAGIC;
+ armv4_5_info.core_mode = ARMV7M_MODE_HANDLER;
+ armv4_5_info.core_state = ARM_STATE_ARM;
+ }
+ else
+ {
+ /* All other ARM CPUs have 32 bit instructions */
+ armv4_5_info.common_magic = ARM_COMMON_MAGIC;
+ armv4_5_info.core_mode = ARM_MODE_SVC;
+ armv4_5_info.core_state = ARM_STATE_ARM;
+ }
+
+ int target_code_size = 0;
+ const uint32_t *target_code_src = NULL;
+
+ switch (bank->bus_width)
+ {
+ case 1 :
+ if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) /* armv4_5 target */
+ {
+ target_code_src = armv4_5_word_8_code;
+ target_code_size = sizeof(armv4_5_word_8_code);
+ }
+ break;
+ case 2 :
+ /* Check for DQ5 support */
+ if( cfi_info->status_poll_mask & (1 << 5) )
+ {
+ if(armv4_5_info.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 (armv4_5_info.common_magic == ARMV7M_COMMON_MAGIC) /* cortex-m3 target */
+ {
+ target_code_src = armv7m_word_16_code;
+ target_code_size = sizeof(armv7m_word_16_code);
+ }
+ }
+ else
+ {
+ /* No DQ5 support. Use DQ7 DATA# polling only. */
+ if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
+ {
+ target_code_src = armv4_5_word_16_code_dq7only;
+ target_code_size = sizeof(armv4_5_word_16_code_dq7only);
+ }
+ }
+ break;
+ case 4 :
+ if(armv4_5_info.common_magic == ARM_COMMON_MAGIC) // armv4_5 target
+ {
+ target_code_src = armv4_5_word_32_code;
+ target_code_size = sizeof(armv4_5_word_32_code);
+ }
+ break;
+ default:
+ LOG_ERROR("Unsupported bank buswidth %d, can't do block memory writes", bank->bus_width);
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+
+ /* flash write code */
+ if (!cfi_info->write_algorithm)
+ {
+ 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);
+
+ /* 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;
+ }
+
+ /* write algorithm code to working area */
+ if ((retval = target_write_buffer(target, cfi_info->write_algorithm->address,
+ target_code_size, target_code)) != 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
+ * buffer, free the algorithm */
+ if (cfi_info->write_algorithm)
+ target_free_working_area(target, cfi_info->write_algorithm);
+
+ LOG_WARNING("not enough working area available, can't do block memory writes");
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+ };
+
+ init_reg_param(®_params[0], "r0", 32, PARAM_OUT);
+ init_reg_param(®_params[1], "r1", 32, PARAM_OUT);
+ init_reg_param(®_params[2], "r2", 32, PARAM_OUT);
+ init_reg_param(®_params[3], "r3", 32, PARAM_OUT);
+ init_reg_param(®_params[4], "r4", 32, PARAM_OUT);
+ init_reg_param(®_params[5], "r5", 32, PARAM_IN);
+ init_reg_param(®_params[6], "r8", 32, PARAM_OUT);
+ init_reg_param(®_params[7], "r9", 32, PARAM_OUT);
+ init_reg_param(®_params[8], "r10", 32, PARAM_OUT);
+ init_reg_param(®_params[9], "r11", 32, PARAM_OUT);
+
+ while (count > 0)
+ {
+ uint32_t thisrun_count = (count > buffer_size) ? buffer_size : count;
+
+ retval = target_write_buffer(target, source->address, thisrun_count, buffer);
+ if (retval != ERROR_OK)
+ {
+ break;
+ }
+
+ buf_set_u32(reg_params[0].value, 0, 32, source->address);
+ buf_set_u32(reg_params[1].value, 0, 32, address);
+ buf_set_u32(reg_params[2].value, 0, 32, thisrun_count / bank->bus_width);
+ buf_set_u32(reg_params[3].value, 0, 32, cfi_command_val(bank, 0xA0));
+ buf_set_u32(reg_params[4].value, 0, 32, cfi_command_val(bank, 0x80));
+ buf_set_u32(reg_params[6].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock1));
+ buf_set_u32(reg_params[7].value, 0, 32, 0xaaaaaaaa);
+ buf_set_u32(reg_params[8].value, 0, 32, flash_address(bank, 0, pri_ext->_unlock2));
+ 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, &armv4_5_info);
+ if (retval != ERROR_OK)
+ {
+ break;
+ }
+
+ status = buf_get_u32(reg_params[5].value, 0, 32);
+ if (status != 0x80)
+ {
+ LOG_ERROR("flash write block failed status: 0x%" PRIx32 , status);
+ retval = ERROR_FLASH_OPERATION_FAILED;
+ break;
+ }
+
+ buffer += thisrun_count;
+ address += thisrun_count;
+ count -= thisrun_count;
+ }
+
+ target_free_all_working_areas(target);
+
+ destroy_reg_param(®_params[0]);
+ destroy_reg_param(®_params[1]);
+ destroy_reg_param(®_params[2]);
+ destroy_reg_param(®_params[3]);
+ destroy_reg_param(®_params[4]);
+ destroy_reg_param(®_params[5]);
+ destroy_reg_param(®_params[6]);
+ destroy_reg_param(®_params[7]);
+ destroy_reg_param(®_params[8]);
+ destroy_reg_param(®_params[9]);
+
+ return retval;
+}
+
+static int cfi_intel_write_word(struct flash_bank *bank, uint8_t *word, uint32_t address)
+{
+ int retval;
+ struct cfi_flash_bank *cfi_info = bank->driver_priv;
+ struct target *target = bank->target;
+
+ cfi_intel_clear_status_register(bank);
+ if ((retval = cfi_send_command(bank, 0x40, address)) != ERROR_OK)
+ {
+ return retval;
+ }