+ 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.");