+ bank->driver_priv = bank_info;
+
+ return ERROR_OK;
+}
+
+/* Disable the watchdog on Kinetis devices */
+int kinetis_disable_wdog(struct target *target, uint32_t sim_sdid)
+{
+ struct working_area *wdog_algorithm;
+ struct armv7m_algorithm armv7m_info;
+ uint16_t wdog;
+ int retval;
+
+ static const uint8_t kinetis_unlock_wdog_code[] = {
+#include "../../../contrib/loaders/watchdog/armv7m_kinetis_wdog.inc"
+ };
+
+ /* Decide whether the connected device needs watchdog disabling.
+ * Disable for all Kx and KVx devices, return if it is a KLx */
+
+ if ((sim_sdid & KINETIS_SDID_SERIESID_MASK) == KINETIS_SDID_SERIESID_KL)
+ return ERROR_OK;
+
+ /* The connected device requires watchdog disabling. */
+ retval = target_read_u16(target, WDOG_STCTRH, &wdog);
+ if (retval != ERROR_OK)
+ return retval;
+
+ if ((wdog & 0x1) == 0) {
+ /* watchdog already disabled */
+ return ERROR_OK;
+ }
+ LOG_INFO("Disabling Kinetis watchdog (initial WDOG_STCTRLH = 0x%x)", wdog);
+
+ if (target->state != TARGET_HALTED) {
+ LOG_ERROR("Target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ retval = target_alloc_working_area(target, sizeof(kinetis_unlock_wdog_code), &wdog_algorithm);
+ if (retval != ERROR_OK)
+ return retval;
+
+ retval = target_write_buffer(target, wdog_algorithm->address,
+ sizeof(kinetis_unlock_wdog_code), (uint8_t *)kinetis_unlock_wdog_code);
+ if (retval != ERROR_OK) {
+ target_free_working_area(target, wdog_algorithm);
+ return retval;
+ }
+
+ armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+ armv7m_info.core_mode = ARM_MODE_THREAD;
+
+ retval = target_run_algorithm(target, 0, NULL, 0, NULL, wdog_algorithm->address,
+ wdog_algorithm->address + (sizeof(kinetis_unlock_wdog_code) - 2),
+ 10000, &armv7m_info);
+
+ if (retval != ERROR_OK)
+ LOG_ERROR("error executing kinetis wdog unlock algorithm");
+
+ retval = target_read_u16(target, WDOG_STCTRH, &wdog);
+ if (retval != ERROR_OK)
+ return retval;
+ LOG_INFO("WDOG_STCTRLH = 0x%x", wdog);
+
+ target_free_working_area(target, wdog_algorithm);
+
+ return retval;
+}
+
+COMMAND_HANDLER(kinetis_disable_wdog_handler)
+{
+ int result;
+ uint32_t sim_sdid;
+ struct target *target = get_current_target(CMD_CTX);
+
+ if (CMD_ARGC > 0)
+ return ERROR_COMMAND_SYNTAX_ERROR;
+
+ result = target_read_u32(target, SIM_SDID, &sim_sdid);
+ if (result != ERROR_OK) {
+ LOG_ERROR("Failed to read SIMSDID");
+ return result;
+ }
+
+ result = kinetis_disable_wdog(target, sim_sdid);
+ return result;
+}
+
+
+static int kinetis_ftfx_decode_error(uint8_t fstat)
+{
+ if (fstat & 0x20) {
+ LOG_ERROR("Flash operation failed, illegal command");
+ return ERROR_FLASH_OPER_UNSUPPORTED;
+
+ } else if (fstat & 0x10)
+ LOG_ERROR("Flash operation failed, protection violated");
+
+ else if (fstat & 0x40)
+ LOG_ERROR("Flash operation failed, read collision");
+
+ else if (fstat & 0x80)
+ return ERROR_OK;
+
+ else
+ LOG_ERROR("Flash operation timed out");
+
+ return ERROR_FLASH_OPERATION_FAILED;
+}
+
+static int kinetis_ftfx_clear_error(struct target *target)
+{
+ /* reset error flags */
+ return target_write_u8(target, FTFx_FSTAT, 0x70);
+}
+
+
+static int kinetis_ftfx_prepare(struct target *target)
+{
+ int result, i;
+ uint8_t fstat;
+
+ /* wait until busy */
+ for (i = 0; i < 50; i++) {
+ result = target_read_u8(target, FTFx_FSTAT, &fstat);
+ if (result != ERROR_OK)
+ return result;
+
+ if (fstat & 0x80)
+ break;
+ }
+
+ if ((fstat & 0x80) == 0) {
+ LOG_ERROR("Flash controller is busy");
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+ if (fstat != 0x80) {
+ /* reset error flags */
+ result = kinetis_ftfx_clear_error(target);
+ }
+ return result;
+}
+
+/* Kinetis Program-LongWord Microcodes */
+static const uint8_t kinetis_flash_write_code[] = {
+#include "../../../contrib/loaders/flash/kinetis/kinetis_flash.inc"
+};
+
+/* Program LongWord Block Write */
+static int kinetis_write_block(struct flash_bank *bank, const uint8_t *buffer,
+ uint32_t offset, uint32_t wcount)
+{
+ struct target *target = bank->target;
+ uint32_t buffer_size = 2048; /* Default minimum value */
+ struct working_area *write_algorithm;
+ struct working_area *source;
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+ uint32_t address = kinfo->prog_base + offset;
+ uint32_t end_address;
+ struct reg_param reg_params[5];
+ struct armv7m_algorithm armv7m_info;
+ int retval;
+ uint8_t fstat;
+
+ /* Increase buffer_size if needed */
+ if (buffer_size < (target->working_area_size/2))
+ buffer_size = (target->working_area_size/2);
+
+ /* allocate working area with flash programming code */
+ if (target_alloc_working_area(target, sizeof(kinetis_flash_write_code),
+ &write_algorithm) != ERROR_OK) {
+ LOG_WARNING("no working area available, can't do block memory writes");
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+
+ retval = target_write_buffer(target, write_algorithm->address,
+ sizeof(kinetis_flash_write_code), kinetis_flash_write_code);
+ if (retval != ERROR_OK)
+ return retval;
+
+ /* memory buffer */
+ while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK) {
+ buffer_size /= 4;
+ if (buffer_size <= 256) {
+ /* free working area, write algorithm already allocated */
+ target_free_working_area(target, write_algorithm);
+
+ LOG_WARNING("No large enough working area available, can't do block memory writes");
+ return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
+ }
+ }
+
+ armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
+ armv7m_info.core_mode = ARM_MODE_THREAD;
+
+ init_reg_param(®_params[0], "r0", 32, PARAM_IN_OUT); /* address */
+ init_reg_param(®_params[1], "r1", 32, PARAM_OUT); /* word count */
+ 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);
+
+ buf_set_u32(reg_params[0].value, 0, 32, address);
+ buf_set_u32(reg_params[1].value, 0, 32, wcount);
+ buf_set_u32(reg_params[2].value, 0, 32, source->address);
+ buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
+ buf_set_u32(reg_params[4].value, 0, 32, FTFx_FSTAT);
+
+ retval = target_run_flash_async_algorithm(target, buffer, wcount, 4,
+ 0, NULL,
+ 5, reg_params,
+ source->address, source->size,
+ write_algorithm->address, 0,
+ &armv7m_info);
+
+ if (retval == ERROR_FLASH_OPERATION_FAILED) {
+ end_address = buf_get_u32(reg_params[0].value, 0, 32);
+
+ LOG_ERROR("Error writing flash at %08" PRIx32, end_address);
+
+ retval = target_read_u8(target, FTFx_FSTAT, &fstat);
+ if (retval == ERROR_OK) {
+ retval = kinetis_ftfx_decode_error(fstat);
+
+ /* reset error flags */
+ target_write_u8(target, FTFx_FSTAT, 0x70);
+ }
+ } else if (retval != ERROR_OK)
+ LOG_ERROR("Error executing kinetis Flash programming algorithm");
+
+ target_free_working_area(target, source);
+ target_free_working_area(target, write_algorithm);
+
+ 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]);
+
+ return retval;
+}
+
+static int kinetis_protect(struct flash_bank *bank, int set, int first, int last)
+{
+ int i;
+
+ if (allow_fcf_writes) {
+ LOG_ERROR("Protection setting is possible with 'kinetis fcf_source protection' only!");
+ return ERROR_FAIL;
+ }
+
+ if (!bank->prot_blocks || bank->num_prot_blocks == 0) {
+ LOG_ERROR("No protection possible for current bank!");
+ return ERROR_FLASH_BANK_INVALID;
+ }
+
+ for (i = first; i < bank->num_prot_blocks && i <= last; i++)
+ bank->prot_blocks[i].is_protected = set;
+
+ LOG_INFO("Protection bits will be written at the next FCF sector erase or write.");
+ LOG_INFO("Do not issue 'flash info' command until protection is written,");
+ LOG_INFO("doing so would re-read protection status from MCU.");
+
+ return ERROR_OK;
+}
+
+static int kinetis_protect_check(struct flash_bank *bank)
+{
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+ int result;
+ int i, b;
+ uint32_t fprot;
+
+ if (kinfo->flash_class == FC_PFLASH) {
+
+ /* read protection register */
+ result = target_read_u32(bank->target, FTFx_FPROT3, &fprot);
+ if (result != ERROR_OK)
+ return result;
+
+ /* Every bit protects 1/32 of the full flash (not necessarily just this bank) */
+
+ } else if (kinfo->flash_class == FC_FLEX_NVM) {
+ uint8_t fdprot;
+
+ /* read protection register */
+ result = target_read_u8(bank->target, FTFx_FDPROT, &fdprot);
+ if (result != ERROR_OK)
+ return result;
+
+ fprot = fdprot;
+
+ } else {
+ LOG_ERROR("Protection checks for FlexRAM not supported");
+ return ERROR_FLASH_BANK_INVALID;
+ }
+
+ b = kinfo->protection_block;
+ for (i = 0; i < bank->num_prot_blocks; i++) {
+ if ((fprot >> b) & 1)
+ bank->prot_blocks[i].is_protected = 0;
+ else
+ bank->prot_blocks[i].is_protected = 1;
+
+ b++;
+ }
+
+ return ERROR_OK;
+}
+
+
+static int kinetis_fill_fcf(struct flash_bank *bank, uint8_t *fcf)
+{
+ uint32_t fprot = 0xffffffff;
+ uint8_t fsec = 0xfe; /* set MCU unsecure */
+ uint8_t fdprot = 0xff;
+ int i;
+ uint32_t pflash_bit;
+ uint8_t dflash_bit;
+ struct flash_bank *bank_iter;
+ struct kinetis_flash_bank *kinfo;
+
+ memset(fcf, 0xff, FCF_SIZE);
+
+ pflash_bit = 1;
+ dflash_bit = 1;
+
+ /* iterate over all kinetis banks */
+ /* current bank is bank 0, it contains FCF */
+ for (bank_iter = bank; bank_iter; bank_iter = bank_iter->next) {
+ if (bank_iter->driver != &kinetis_flash
+ || bank_iter->target != bank->target)
+ continue;
+
+ kinetis_auto_probe(bank_iter);
+
+ kinfo = bank->driver_priv;
+ if (!kinfo)
+ continue;
+
+ if (kinfo->flash_class == FC_PFLASH) {
+ for (i = 0; i < bank_iter->num_prot_blocks; i++) {
+ if (bank_iter->prot_blocks[i].is_protected == 1)
+ fprot &= ~pflash_bit;
+
+ pflash_bit <<= 1;
+ }
+
+ } else if (kinfo->flash_class == FC_FLEX_NVM) {
+ for (i = 0; i < bank_iter->num_prot_blocks; i++) {
+ if (bank_iter->prot_blocks[i].is_protected == 1)
+ fdprot &= ~dflash_bit;
+
+ dflash_bit <<= 1;
+ }
+
+ }
+ }
+
+ target_buffer_set_u32(bank->target, fcf + FCF_FPROT, fprot);
+ fcf[FCF_FSEC] = fsec;
+ fcf[FCF_FOPT] = fcf_fopt;
+ fcf[FCF_FDPROT] = fdprot;
+ return ERROR_OK;
+}
+
+static int kinetis_ftfx_command(struct target *target, uint8_t fcmd, uint32_t faddr,
+ uint8_t fccob4, uint8_t fccob5, uint8_t fccob6, uint8_t fccob7,
+ uint8_t fccob8, uint8_t fccob9, uint8_t fccoba, uint8_t fccobb,
+ uint8_t *ftfx_fstat)
+{
+ uint8_t command[12] = {faddr & 0xff, (faddr >> 8) & 0xff, (faddr >> 16) & 0xff, fcmd,
+ fccob7, fccob6, fccob5, fccob4,
+ fccobb, fccoba, fccob9, fccob8};
+ int result;
+ uint8_t fstat;
+ int64_t ms_timeout = timeval_ms() + 250;
+
+ result = target_write_memory(target, FTFx_FCCOB3, 4, 3, command);
+ if (result != ERROR_OK)
+ return result;
+
+ /* start command */
+ result = target_write_u8(target, FTFx_FSTAT, 0x80);
+ if (result != ERROR_OK)
+ return result;
+
+ /* wait for done */
+ do {
+ result = target_read_u8(target, FTFx_FSTAT, &fstat);
+
+ if (result != ERROR_OK)
+ return result;
+
+ if (fstat & 0x80)
+ break;
+
+ } while (timeval_ms() < ms_timeout);
+
+ if (ftfx_fstat)
+ *ftfx_fstat = fstat;
+
+ if ((fstat & 0xf0) != 0x80) {
+ LOG_DEBUG("ftfx command failed FSTAT: %02X FCCOB: %02X%02X%02X%02X %02X%02X%02X%02X %02X%02X%02X%02X",
+ fstat, command[3], command[2], command[1], command[0],
+ command[7], command[6], command[5], command[4],
+ command[11], command[10], command[9], command[8]);
+
+ return kinetis_ftfx_decode_error(fstat);
+ }
+
+ return ERROR_OK;
+}
+
+
+static int kinetis_check_run_mode(struct target *target)
+{
+ int result, i;
+ uint8_t pmctrl, pmstat;
+
+ if (target->state != TARGET_HALTED) {
+ LOG_ERROR("Target not halted");
+ return ERROR_TARGET_NOT_HALTED;
+ }
+
+ result = target_read_u8(target, SMC_PMSTAT, &pmstat);
+ if (result != ERROR_OK)
+ return result;
+
+ if (pmstat == PM_STAT_RUN)
+ return ERROR_OK;
+
+ if (pmstat == PM_STAT_VLPR) {
+ /* It is safe to switch from VLPR to RUN mode without changing clock */
+ LOG_INFO("Switching from VLPR to RUN mode.");
+ pmctrl = PM_CTRL_RUNM_RUN;
+ result = target_write_u8(target, SMC_PMCTRL, pmctrl);
+ if (result != ERROR_OK)
+ return result;
+
+ for (i = 100; i; i--) {
+ result = target_read_u8(target, SMC_PMSTAT, &pmstat);
+ if (result != ERROR_OK)
+ return result;
+
+ if (pmstat == PM_STAT_RUN)
+ return ERROR_OK;
+ }
+ }
+
+ LOG_ERROR("Flash operation not possible in current run mode: SMC_PMSTAT: 0x%x", pmstat);
+ LOG_ERROR("Issue a 'reset init' command.");
+ return ERROR_TARGET_NOT_HALTED;
+}
+
+
+static void kinetis_invalidate_flash_cache(struct flash_bank *bank)
+{
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+
+ if (kinfo->flash_support & FS_INVALIDATE_CACHE_K)
+ target_write_u8(bank->target, FMC_PFB01CR + 2, 0xf0);
+ /* Set CINV_WAY bits - request invalidate of all cache ways */
+ /* FMC_PFB0CR has same address and CINV_WAY bits as FMC_PFB01CR */
+
+ else if (kinfo->flash_support & FS_INVALIDATE_CACHE_L)
+ target_write_u8(bank->target, MCM_PLACR + 1, 0x04);
+ /* set bit CFCC - Clear Flash Controller Cache */
+
+ else if (kinfo->flash_support & FS_INVALIDATE_CACHE_MSCM)
+ target_write_u32(bank->target, MSCM_OCMDR0, 0x30);
+ /* disable data prefetch and flash speculate */
+
+ return;
+}
+
+
+static int kinetis_erase(struct flash_bank *bank, int first, int last)
+{
+ int result, i;
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+
+ result = kinetis_check_run_mode(bank->target);
+ if (result != ERROR_OK)
+ return result;
+
+ /* reset error flags */
+ result = kinetis_ftfx_prepare(bank->target);
+ if (result != ERROR_OK)
+ return result;
+
+ if ((first > bank->num_sectors) || (last > bank->num_sectors))
+ return ERROR_FLASH_OPERATION_FAILED;
+
+ /*
+ * FIXME: TODO: use the 'Erase Flash Block' command if the
+ * requested erase is PFlash or NVM and encompasses the entire
+ * block. Should be quicker.
+ */
+ for (i = first; i <= last; i++) {
+ /* set command and sector address */
+ result = kinetis_ftfx_command(bank->target, FTFx_CMD_SECTERASE, kinfo->prog_base + bank->sectors[i].offset,
+ 0, 0, 0, 0, 0, 0, 0, 0, NULL);
+
+ if (result != ERROR_OK) {
+ LOG_WARNING("erase sector %d failed", i);
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ bank->sectors[i].is_erased = 1;
+
+ if (bank->base == 0
+ && bank->sectors[i].offset <= FCF_ADDRESS
+ && bank->sectors[i].offset + bank->sectors[i].size > FCF_ADDRESS + FCF_SIZE) {
+ if (allow_fcf_writes) {
+ LOG_WARNING("Flash Configuration Field erased, DO NOT reset or power off the device");
+ LOG_WARNING("until correct FCF is programmed or MCU gets security lock.");
+ } else {
+ uint8_t fcf_buffer[FCF_SIZE];
+
+ kinetis_fill_fcf(bank, fcf_buffer);
+ result = kinetis_write_inner(bank, fcf_buffer, FCF_ADDRESS, FCF_SIZE);
+ if (result != ERROR_OK)
+ LOG_WARNING("Flash Configuration Field write failed");
+ bank->sectors[i].is_erased = 0;
+ }
+ }
+ }
+
+ kinetis_invalidate_flash_cache(bank);
+
+ return ERROR_OK;
+}
+
+static int kinetis_make_ram_ready(struct target *target)
+{
+ int result;
+ uint8_t ftfx_fcnfg;
+
+ /* check if ram ready */
+ result = target_read_u8(target, FTFx_FCNFG, &ftfx_fcnfg);
+ if (result != ERROR_OK)
+ return result;
+
+ if (ftfx_fcnfg & (1 << 1))
+ return ERROR_OK; /* ram ready */
+
+ /* make flex ram available */
+ result = kinetis_ftfx_command(target, FTFx_CMD_SETFLEXRAM, 0x00ff0000,
+ 0, 0, 0, 0, 0, 0, 0, 0, NULL);
+ if (result != ERROR_OK)
+ return ERROR_FLASH_OPERATION_FAILED;
+
+ /* check again */
+ result = target_read_u8(target, FTFx_FCNFG, &ftfx_fcnfg);
+ if (result != ERROR_OK)
+ return result;
+
+ if (ftfx_fcnfg & (1 << 1))
+ return ERROR_OK; /* ram ready */
+
+ return ERROR_FLASH_OPERATION_FAILED;
+}
+
+
+static int kinetis_write_sections(struct flash_bank *bank, const uint8_t *buffer,
+ uint32_t offset, uint32_t count)
+{
+ int result = ERROR_OK;
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+ uint8_t *buffer_aligned = NULL;
+ /*
+ * Kinetis uses different terms for the granularity of
+ * sector writes, e.g. "phrase" or "128 bits". We use
+ * the generic term "chunk". The largest possible
+ * Kinetis "chunk" is 16 bytes (128 bits).
+ */
+ uint32_t prog_section_chunk_bytes = kinfo->sector_size >> 8;
+ uint32_t prog_size_bytes = kinfo->max_flash_prog_size;
+
+ while (count > 0) {
+ uint32_t size = prog_size_bytes - offset % prog_size_bytes;
+ uint32_t align_begin = offset % prog_section_chunk_bytes;
+ uint32_t align_end;
+ uint32_t size_aligned;
+ uint16_t chunk_count;
+ uint8_t ftfx_fstat;
+
+ if (size > count)
+ size = count;
+
+ align_end = (align_begin + size) % prog_section_chunk_bytes;
+ if (align_end)
+ align_end = prog_section_chunk_bytes - align_end;
+
+ size_aligned = align_begin + size + align_end;
+ chunk_count = size_aligned / prog_section_chunk_bytes;
+
+ if (size != size_aligned) {
+ /* aligned section: the first, the last or the only */
+ if (!buffer_aligned)
+ buffer_aligned = malloc(prog_size_bytes);
+
+ memset(buffer_aligned, 0xff, size_aligned);
+ memcpy(buffer_aligned + align_begin, buffer, size);
+
+ result = target_write_memory(bank->target, FLEXRAM,
+ 4, size_aligned / 4, buffer_aligned);
+
+ LOG_DEBUG("section @ %08" PRIx32 " aligned begin %" PRIu32 ", end %" PRIu32,
+ bank->base + offset, align_begin, align_end);
+ } else
+ result = target_write_memory(bank->target, FLEXRAM,
+ 4, size_aligned / 4, buffer);
+
+ LOG_DEBUG("write section @ %08" PRIx32 " with length %" PRIu32 " bytes",
+ bank->base + offset, size);
+
+ if (result != ERROR_OK) {
+ LOG_ERROR("target_write_memory failed");
+ break;
+ }
+
+ /* execute section-write command */
+ result = kinetis_ftfx_command(bank->target, FTFx_CMD_SECTWRITE,
+ kinfo->prog_base + offset - align_begin,
+ chunk_count>>8, chunk_count, 0, 0,
+ 0, 0, 0, 0, &ftfx_fstat);
+
+ if (result != ERROR_OK) {
+ LOG_ERROR("Error writing section at %08" PRIx32, bank->base + offset);
+ break;
+ }
+
+ if (ftfx_fstat & 0x01)
+ LOG_ERROR("Flash write error at %08" PRIx32, bank->base + offset);
+
+ buffer += size;
+ offset += size;
+ count -= size;
+ }
+
+ free(buffer_aligned);
+ return result;
+}
+
+
+static int kinetis_write_inner(struct flash_bank *bank, const uint8_t *buffer,
+ uint32_t offset, uint32_t count)
+{
+ int result, fallback = 0;
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+
+ if (!(kinfo->flash_support & FS_PROGRAM_SECTOR)) {
+ /* fallback to longword write */
+ fallback = 1;
+ LOG_INFO("This device supports Program Longword execution only.");
+ } else {
+ result = kinetis_make_ram_ready(bank->target);
+ if (result != ERROR_OK) {
+ fallback = 1;
+ LOG_WARNING("FlexRAM not ready, fallback to slow longword write.");
+ }
+ }
+
+ LOG_DEBUG("flash write @08%" PRIx32, bank->base + offset);
+
+ if (fallback == 0) {
+ /* program section command */
+ kinetis_write_sections(bank, buffer, offset, count);
+ }
+ else if (kinfo->flash_support & FS_PROGRAM_LONGWORD) {
+ /* program longword command, not supported in FTFE */
+ uint8_t *new_buffer = NULL;
+
+ /* check word alignment */
+ if (offset & 0x3) {
+ LOG_ERROR("offset 0x%" PRIx32 " breaks the required alignment", offset);
+ return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+ }
+
+ if (count & 0x3) {
+ uint32_t old_count = count;
+ count = (old_count | 3) + 1;
+ new_buffer = malloc(count);
+ if (new_buffer == NULL) {
+ LOG_ERROR("odd number of bytes to write and no memory "
+ "for padding buffer");
+ return ERROR_FAIL;
+ }
+ LOG_INFO("odd number of bytes to write (%" PRIu32 "), extending to %" PRIu32 " "
+ "and padding with 0xff", old_count, count);
+ memset(new_buffer + old_count, 0xff, count - old_count);
+ buffer = memcpy(new_buffer, buffer, old_count);
+ }
+
+ uint32_t words_remaining = count / 4;
+
+ kinetis_disable_wdog(bank->target, kinfo->sim_sdid);
+
+ /* try using a block write */
+ result = kinetis_write_block(bank, buffer, offset, words_remaining);
+
+ if (result == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
+ /* if block write failed (no sufficient working area),
+ * we use normal (slow) single word accesses */
+ LOG_WARNING("couldn't use block writes, falling back to single "
+ "memory accesses");
+
+ while (words_remaining) {
+ uint8_t ftfx_fstat;
+
+ LOG_DEBUG("write longword @ %08" PRIx32, (uint32_t)(bank->base + offset));
+
+ result = kinetis_ftfx_command(bank->target, FTFx_CMD_LWORDPROG, kinfo->prog_base + offset,
+ buffer[3], buffer[2], buffer[1], buffer[0],
+ 0, 0, 0, 0, &ftfx_fstat);
+
+ if (result != ERROR_OK) {
+ LOG_ERROR("Error writing longword at %08" PRIx32, bank->base + offset);
+ break;
+ }
+
+ if (ftfx_fstat & 0x01)
+ LOG_ERROR("Flash write error at %08" PRIx32, bank->base + offset);
+
+ buffer += 4;
+ offset += 4;
+ words_remaining--;
+ }
+ }
+ free(new_buffer);
+ } else {
+ LOG_ERROR("Flash write strategy not implemented");
+ return ERROR_FLASH_OPERATION_FAILED;
+ }
+
+ kinetis_invalidate_flash_cache(bank);
+ return result;
+}
+
+
+static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
+ uint32_t offset, uint32_t count)
+{
+ int result;
+ bool set_fcf = false;
+ int sect = 0;
+
+ result = kinetis_check_run_mode(bank->target);
+ if (result != ERROR_OK)
+ return result;
+
+ /* reset error flags */
+ result = kinetis_ftfx_prepare(bank->target);
+ if (result != ERROR_OK)
+ return result;
+
+ if (bank->base == 0 && !allow_fcf_writes) {
+ if (bank->sectors[1].offset <= FCF_ADDRESS)
+ sect = 1; /* 1kb sector, FCF in 2nd sector */
+
+ if (offset < bank->sectors[sect].offset + bank->sectors[sect].size
+ && offset + count > bank->sectors[sect].offset)
+ set_fcf = true; /* write to any part of sector with FCF */
+ }
+
+ if (set_fcf) {
+ uint8_t fcf_buffer[FCF_SIZE];
+ uint8_t fcf_current[FCF_SIZE];
+
+ kinetis_fill_fcf(bank, fcf_buffer);
+
+ if (offset < FCF_ADDRESS) {
+ /* write part preceding FCF */
+ result = kinetis_write_inner(bank, buffer, offset, FCF_ADDRESS - offset);
+ if (result != ERROR_OK)
+ return result;
+ }
+
+ result = target_read_memory(bank->target, FCF_ADDRESS, 4, FCF_SIZE / 4, fcf_current);
+ if (result == ERROR_OK && memcmp(fcf_current, fcf_buffer, FCF_SIZE) == 0)
+ set_fcf = false;
+
+ if (set_fcf) {
+ /* write FCF if differs from flash - eliminate multiple writes */
+ result = kinetis_write_inner(bank, fcf_buffer, FCF_ADDRESS, FCF_SIZE);
+ if (result != ERROR_OK)
+ return result;
+ }
+
+ LOG_WARNING("Flash Configuration Field written.");
+ LOG_WARNING("Reset or power off the device to make settings effective.");
+
+ if (offset + count > FCF_ADDRESS + FCF_SIZE) {
+ uint32_t delta = FCF_ADDRESS + FCF_SIZE - offset;
+ /* write part after FCF */
+ result = kinetis_write_inner(bank, buffer + delta, FCF_ADDRESS + FCF_SIZE, count - delta);
+ }
+ return result;
+
+ } else
+ /* no FCF fiddling, normal write */
+ return kinetis_write_inner(bank, buffer, offset, count);
+}
+
+
+static int kinetis_probe(struct flash_bank *bank)
+{
+ int result, i;
+ uint8_t fcfg1_nvmsize, fcfg1_pfsize, fcfg1_eesize, fcfg1_depart;
+ uint8_t fcfg2_maxaddr0, fcfg2_pflsh, fcfg2_maxaddr1;
+ uint32_t nvm_size = 0, pf_size = 0, df_size = 0, ee_size = 0;
+ unsigned num_blocks = 0, num_pflash_blocks = 0, num_nvm_blocks = 0, first_nvm_bank = 0,
+ pflash_sector_size_bytes = 0, nvm_sector_size_bytes = 0;
+ struct target *target = bank->target;
+ struct kinetis_flash_bank *kinfo = bank->driver_priv;
+
+ kinfo->probed = false;
+
+ result = target_read_u32(target, SIM_SDID, &kinfo->sim_sdid);
+ if (result != ERROR_OK)
+ return result;
+
+ if ((kinfo->sim_sdid & (~KINETIS_SDID_K_SERIES_MASK)) == 0) {
+ /* older K-series MCU */
+ uint32_t mcu_type = kinfo->sim_sdid & KINETIS_K_SDID_TYPE_MASK;
+
+ switch (mcu_type) {
+ case KINETIS_K_SDID_K10_M50:
+ case KINETIS_K_SDID_K20_M50:
+ /* 1kB sectors */
+ pflash_sector_size_bytes = 1<<10;
+ nvm_sector_size_bytes = 1<<10;
+ num_blocks = 2;
+ kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
+ break;
+ case KINETIS_K_SDID_K10_M72:
+ case KINETIS_K_SDID_K20_M72:
+ case KINETIS_K_SDID_K30_M72:
+ case KINETIS_K_SDID_K30_M100:
+ case KINETIS_K_SDID_K40_M72:
+ case KINETIS_K_SDID_K40_M100:
+ case KINETIS_K_SDID_K50_M72:
+ /* 2kB sectors, 1kB FlexNVM sectors */
+ pflash_sector_size_bytes = 2<<10;
+ nvm_sector_size_bytes = 1<<10;
+ num_blocks = 2;
+ kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
+ kinfo->max_flash_prog_size = 1<<10;
+ break;
+ case KINETIS_K_SDID_K10_M100:
+ case KINETIS_K_SDID_K20_M100:
+ case KINETIS_K_SDID_K11:
+ case KINETIS_K_SDID_K12:
+ case KINETIS_K_SDID_K21_M50:
+ case KINETIS_K_SDID_K22_M50:
+ case KINETIS_K_SDID_K51_M72:
+ case KINETIS_K_SDID_K53:
+ case KINETIS_K_SDID_K60_M100:
+ /* 2kB sectors */
+ pflash_sector_size_bytes = 2<<10;
+ nvm_sector_size_bytes = 2<<10;
+ num_blocks = 2;
+ kinfo->flash_support = FS_PROGRAM_LONGWORD | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
+ break;
+ case KINETIS_K_SDID_K21_M120:
+ case KINETIS_K_SDID_K22_M120:
+ /* 4kB sectors (MK21FN1M0, MK21FX512, MK22FN1M0, MK22FX512) */
+ pflash_sector_size_bytes = 4<<10;
+ kinfo->max_flash_prog_size = 1<<10;
+ nvm_sector_size_bytes = 4<<10;
+ num_blocks = 2;
+ kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
+ break;
+ case KINETIS_K_SDID_K10_M120:
+ case KINETIS_K_SDID_K20_M120:
+ case KINETIS_K_SDID_K60_M150:
+ case KINETIS_K_SDID_K70_M150:
+ /* 4kB sectors */
+ pflash_sector_size_bytes = 4<<10;
+ nvm_sector_size_bytes = 4<<10;
+ num_blocks = 4;
+ kinfo->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_INVALIDATE_CACHE_K;
+ break;
+ default:
+ LOG_ERROR("Unsupported K-family FAMID");