+ else
+ {
+ /* No DQ5 support. Use DQ7 DATA# polling only. */
+ target_code_src = word_16_code_dq7only;
+ target_code_size = sizeof(word_16_code_dq7only);
+ }
+ break;
+ case 4 :
+ target_code_src = word_32_code;
+ target_code_size = sizeof(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);
+ 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(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;
+
+ retvaltemp = target_write_buffer(target, source->address, thisrun_count, buffer);
+
+ 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);
+
+ status = buf_get_u32(reg_params[5].value, 0, 32);
+
+ if ((retval != ERROR_OK) || (retvaltemp != ERROR_OK) || status != 0x80)
+ {
+ LOG_DEBUG("status: 0x%" PRIx32 , status);
+ exit_code = 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 exit_code;
+}
+
+static int cfi_intel_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
+{
+ int retval;
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+ target_t *target = bank->target;
+ uint8_t command[8];
+
+ cfi_intel_clear_status_register(bank);
+ cfi_command(bank, 0x40, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != 0x80)
+ {
+ cfi_command(bank, 0xff, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ return ERROR_OK;
+}
+
+static int cfi_intel_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+ int retval;
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+ target_t *target = bank->target;
+ uint8_t command[8];
+
+ /* Calculate buffer size and boundary mask */
+ uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
+ uint32_t buffermask = buffersize-1;
+ uint32_t bufferwsize;
+
+ /* Check for valid range */
+ if (address & buffermask)
+ {
+ LOG_ERROR("Write address at base 0x%" PRIx32 ", address %" PRIx32 " not aligned to 2^%d boundary",
+ bank->base, address, cfi_info->max_buf_write_size);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+ switch (bank->chip_width)
+ {
+ case 4 : bufferwsize = buffersize / 4; break;
+ case 2 : bufferwsize = buffersize / 2; break;
+ case 1 : bufferwsize = buffersize; break;
+ default:
+ LOG_ERROR("Unsupported chip width %d", bank->chip_width);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ bufferwsize/=(bank->bus_width / bank->chip_width);
+
+
+ /* Check for valid size */
+ if (wordcount > bufferwsize)
+ {
+ LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32 , wordcount, buffersize);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ /* Write to flash buffer */
+ cfi_intel_clear_status_register(bank);
+
+ /* Initiate buffer operation _*/
+ cfi_command(bank, 0xE8, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+ if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->buf_write_timeout_max)) != 0x80)
+ {
+ cfi_command(bank, 0xff, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ LOG_ERROR("couldn't start buffer write operation at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ /* Write buffer wordcount-1 and data words */
+ cfi_command(bank, bufferwsize-1, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if ((retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ /* Commit write operation */
+ cfi_command(bank, 0xd0, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+ if (cfi_intel_wait_status_busy(bank, 1000 * (1 << cfi_info->buf_write_timeout_max)) != 0x80)
+ {
+ cfi_command(bank, 0xff, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ LOG_ERROR("Buffer write at base 0x%" PRIx32 ", address %" PRIx32 " failed.", bank->base, address);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ return ERROR_OK;
+}
+
+static int cfi_spansion_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
+{
+ int retval;
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+ cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
+ target_t *target = bank->target;
+ uint8_t command[8];
+
+ cfi_command(bank, 0xaa, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ cfi_command(bank, 0x55, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ cfi_command(bank, 0xa0, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, word)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if (cfi_spansion_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != ERROR_OK)
+ {
+ cfi_command(bank, 0xf0, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ LOG_ERROR("couldn't write word at base 0x%" PRIx32 ", address %" PRIx32 , bank->base, address);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ return ERROR_OK;
+}
+
+static int cfi_spansion_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+ int retval;
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+ target_t *target = bank->target;
+ uint8_t command[8];
+ cfi_spansion_pri_ext_t *pri_ext = cfi_info->pri_ext;
+
+ /* Calculate buffer size and boundary mask */
+ uint32_t buffersize = (1UL << cfi_info->max_buf_write_size) * (bank->bus_width / bank->chip_width);
+ uint32_t buffermask = buffersize-1;
+ uint32_t bufferwsize;
+
+ /* Check for valid range */
+ if (address & buffermask)
+ {
+ LOG_ERROR("Write address at base 0x%" PRIx32 ", address %" PRIx32 " not aligned to 2^%d boundary", bank->base, address, cfi_info->max_buf_write_size);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+ switch (bank->chip_width)
+ {
+ case 4 : bufferwsize = buffersize / 4; break;
+ case 2 : bufferwsize = buffersize / 2; break;
+ case 1 : bufferwsize = buffersize; break;
+ default:
+ LOG_ERROR("Unsupported chip width %d", bank->chip_width);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ bufferwsize/=(bank->bus_width / bank->chip_width);
+
+ /* Check for valid size */
+ if (wordcount > bufferwsize)
+ {
+ LOG_ERROR("Number of data words %" PRId32 " exceeds available buffersize %" PRId32, wordcount, buffersize);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ // Unlock
+ cfi_command(bank, 0xaa, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock1), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ cfi_command(bank, 0x55, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, pri_ext->_unlock2), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ // Buffer load command
+ cfi_command(bank, 0x25, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ /* Write buffer wordcount-1 and data words */
+ cfi_command(bank, bufferwsize-1, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if ((retval = target_write_memory(target, address, bank->bus_width, bufferwsize, word)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ /* Commit write operation */
+ cfi_command(bank, 0x29, command);
+ if ((retval = target_write_memory(target, address, bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ if (cfi_spansion_wait_status_busy(bank, 1000 * (1 << cfi_info->word_write_timeout_max)) != ERROR_OK)
+ {
+ cfi_command(bank, 0xf0, command);
+ if ((retval = target_write_memory(target, flash_address(bank, 0, 0x0), bank->bus_width, 1, command)) != ERROR_OK)
+ {
+ return retval;
+ }
+
+ LOG_ERROR("couldn't write block at base 0x%" PRIx32 ", address %" PRIx32 ", size %" PRIx32 , bank->base, address, bufferwsize);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ return ERROR_OK;
+}
+
+static int cfi_write_word(struct flash_bank_s *bank, uint8_t *word, uint32_t address)
+{
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+
+ switch (cfi_info->pri_id)
+ {
+ case 1:
+ case 3:
+ return cfi_intel_write_word(bank, word, address);
+ break;
+ case 2:
+ return cfi_spansion_write_word(bank, word, address);
+ break;
+ default:
+ LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+ break;
+ }
+
+ return ERROR_FLASH_OPERATION_FAILED;
+}
+
+static int cfi_write_words(struct flash_bank_s *bank, uint8_t *word, uint32_t wordcount, uint32_t address)
+{
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+
+ switch (cfi_info->pri_id)
+ {
+ case 1:
+ case 3:
+ return cfi_intel_write_words(bank, word, wordcount, address);
+ break;
+ case 2:
+ return cfi_spansion_write_words(bank, word, wordcount, address);
+ break;
+ default:
+ LOG_ERROR("cfi primary command set %i unsupported", cfi_info->pri_id);
+ break;
+ }
+
+ return ERROR_FLASH_OPERATION_FAILED;
+}
+
+int cfi_write(struct flash_bank_s *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+ cfi_flash_bank_t *cfi_info = bank->driver_priv;
+ target_t *target = bank->target;
+ uint32_t address = bank->base + offset; /* address of first byte to be programmed */
+ uint32_t write_p, copy_p;
+ int align; /* number of unaligned bytes */
+ 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 */
+ int i;
+ int retval;
+
+ if (bank->target->state != TARGET_HALTED)
+ {
+ LOG_ERROR("Target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ if (offset + count > bank->size)
+ return ERROR_FLASH_DST_OUT_OF_BANK;
+
+ if (cfi_info->qry[0] != 'Q')
+ return ERROR_FLASH_BANK_NOT_PROBED;
+
+ /* start at the first byte of the first word (bus_width size) */
+ write_p = address & ~(bank->bus_width - 1);
+ if ((align = address - write_p) != 0)
+ {
+ LOG_INFO("Fixup %d unaligned head bytes", align);
+
+ for (i = 0; i < bank->bus_width; i++)
+ current_word[i] = 0;
+ copy_p = write_p;
+
+ /* copy bytes before the first write address */
+ for (i = 0; i < align; ++i, ++copy_p)
+ {
+ uint8_t byte;
+ if ((retval = target_read_memory(target, copy_p, 1, 1, &byte)) != ERROR_OK)
+ {
+ return retval;
+ }
+ cfi_add_byte(bank, current_word, byte);
+ }
+
+ /* add bytes from the buffer */
+ for (; (i < bank->bus_width) && (count > 0); i++)