diff --git a/src/Makefile b/src/Makefile index 87f0b0a..db5535f 100644 --- a/src/Makefile +++ b/src/Makefile @@ -21,6 +21,7 @@ SRC = gdb_if.c \ adiv5_swdp.c \ cortexm3.c \ stm32_tgt.c \ + nxp_tgt.c \ main.c \ platform.c \ command.c \ diff --git a/src/cortexm3.c b/src/cortexm3.c index 0dbb722..0c6e151 100644 --- a/src/cortexm3.c +++ b/src/cortexm3.c @@ -23,6 +23,8 @@ * implemented according to the "ARMv7-M Architectue Reference Manual", * ARM doc DDI0403C. * + * Also supports Cortex-M0 / ARMv6-M + * * Issues: * There are way too many magic numbers used here. */ @@ -37,6 +39,7 @@ #include "cortexm3.h" #include "lmi.h" #include "stm32_tgt.h" +#include "nxp_tgt.h" static char cm3_driver_str[] = "ARM Cortex-M3"; @@ -107,7 +110,7 @@ static char cm3_driver_str[] = "ARM Cortex-M3"; #define CM3_DHCSR_S_HALT (1 << 17) #define CM3_DHCSR_S_REGRDY (1 << 16) /* Bits 15:6 - Reserved */ -#define CM3_DHCSR_C_SNAPSTALL (1 << 5) +#define CM3_DHCSR_C_SNAPSTALL (1 << 5) /* v7m only */ /* Bit 4 - Reserved */ #define CM3_DHCSR_C_MASKINTS (1 << 3) #define CM3_DHCSR_C_STEP (1 << 2) @@ -124,25 +127,25 @@ static char cm3_driver_str[] = "ARM Cortex-M3"; /* Bits 31:25 - Reserved */ #define CM3_DEMCR_TRCENA (1 << 24) /* Bits 23:20 - Reserved */ -#define CM3_DEMCR_MON_REQ (1 << 19) -#define CM3_DEMCR_MON_STEP (1 << 18) -#define CM3_DEMCR_VC_MON_PEND (1 << 17) -#define CM3_DEMCR_VC_MON_EN (1 << 16) +#define CM3_DEMCR_MON_REQ (1 << 19) /* v7m only */ +#define CM3_DEMCR_MON_STEP (1 << 18) /* v7m only */ +#define CM3_DEMCR_VC_MON_PEND (1 << 17) /* v7m only */ +#define CM3_DEMCR_VC_MON_EN (1 << 16) /* v7m only */ /* Bits 15:11 - Reserved */ #define CM3_DEMCR_VC_HARDERR (1 << 10) -#define CM3_DEMCR_VC_INTERR (1 << 9) -#define CM3_DEMCR_VC_BUSERR (1 << 8) -#define CM3_DEMCR_VC_STATERR (1 << 7) -#define CM3_DEMCR_VC_CHKERR (1 << 6) -#define CM3_DEMCR_VC_NOCPERR (1 << 5) -#define CM3_DEMCR_VC_MMERR (1 << 4) +#define CM3_DEMCR_VC_INTERR (1 << 9) /* v7m only */ +#define CM3_DEMCR_VC_BUSERR (1 << 8) /* v7m only */ +#define CM3_DEMCR_VC_STATERR (1 << 7) /* v7m only */ +#define CM3_DEMCR_VC_CHKERR (1 << 6) /* v7m only */ +#define CM3_DEMCR_VC_NOCPERR (1 << 5) /* v7m only */ +#define CM3_DEMCR_VC_MMERR (1 << 4) /* v7m only */ /* Bits 3:1 - Reserved */ #define CM3_DEMCR_VC_CORERESET (1 << 0) /* Flash Patch and Breakpoint Control Register (FP_CTRL) */ /* Bits 32:15 - Reserved */ -/* Bits 14:12 - NUM_CODE2 */ -/* Bits 11:8 - NUM_LIT */ +/* Bits 14:12 - NUM_CODE2 */ /* v7m only */ +/* Bits 11:8 - NUM_LIT */ /* v7m only */ /* Bits 7:4 - NUM_CODE1 */ /* Bits 3:2 - Unspecified */ #define CM3_FPB_CTRL_KEY (1 << 1) @@ -155,7 +158,7 @@ static char cm3_driver_str[] = "ARM Cortex-M3"; /* Data Watchpoint and Trace Function Register (DWT_FUNCTIONx) */ #define CM3_DWT_FUNC_MATCHED (1 << 24) -#define CM3_DWT_FUNC_DATAVSIZE_WORD (2 << 10) +#define CM3_DWT_FUNC_DATAVSIZE_WORD (2 << 10) /* v7m only */ #define CM3_DWT_FUNC_FUNC_READ (5 << 0) #define CM3_DWT_FUNC_FUNC_WRITE (6 << 0) #define CM3_DWT_FUNC_FUNC_ACCESS (7 << 0) @@ -182,17 +185,21 @@ static int cm3_clear_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, static int cm3_check_hw_wp(struct target_s *target, uint32_t *addr); /* Watchpoint unit status */ +#define CM3_MAX_WATCHPOINTS 4 /* architecture says up to 15, no implementation has > 4 */ static struct wp_unit_s { uint32_t addr; uint8_t type; uint8_t size; -} hw_watchpoint[4]; +} hw_watchpoint[CM3_MAX_WATCHPOINTS]; +static unsigned hw_watchpoint_max; /* Breakpoint unit status */ -static uint32_t hw_breakpoint[6]; +#define CM3_MAX_BREAKPOINTS 6 /* architecture says up to 127, no implementation has > 6 */ +static uint32_t hw_breakpoint[CM3_MAX_BREAKPOINTS]; +static unsigned hw_breakpoint_max; /* Register number tables */ -static uint32_t regnum_v7m[] = { +static uint32_t regnum_cortex_m[] = { 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, /* standard r0-r15 */ 0x10, /* xpsr */ 0x11, /* msp */ @@ -201,7 +208,7 @@ static uint32_t regnum_v7m[] = { }; #if 0 /* XXX: need some way for a specific CPU to indicate it has FP registers */ -static uint32_t regnum_v7mf[] = { +static uint32_t regnum_cortex_mf[] = { 0x21, /* fpscr */ 0x40, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46, 0x47, /* s0-s7 */ 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e, 0x4f, /* s8-s15 */ @@ -210,7 +217,7 @@ static uint32_t regnum_v7mf[] = { }; #endif -static const char tdesc_armv7m[] = +static const char tdesc_cortex_m[] = "" "" "" @@ -248,7 +255,7 @@ cm3_probe(struct target_s *target) target->detach = cm3_detach; /* Should probe here to make sure it's Cortex-M3 */ - target->tdesc = tdesc_armv7m; + target->tdesc = tdesc_cortex_m; target->regs_read = cm3_regs_read; target->regs_write = cm3_regs_write; target->pc_write = cm3_pc_write; @@ -258,10 +265,11 @@ cm3_probe(struct target_s *target) target->halt_wait = cm3_halt_wait; target->halt_resume = cm3_halt_resume; target->fault_unwind = cm3_fault_unwind; - target->regs_size = sizeof(regnum_v7m); /* XXX: detect FP extension */ + target->regs_size = sizeof(regnum_cortex_m); /* XXX: detect FP extension */ if(stm32_probe(target) == 0) return 0; if(stm32f4_probe(target) == 0) return 0; + if(lpc11xx_probe(target) == 0) return 0; /* if not STM32 try LMI which I don't know how to detect reliably */ lmi_probe(target); @@ -272,7 +280,8 @@ static void cm3_attach(struct target_s *target) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; + uint32_t r; target_halt_request(target); while(!target_halt_wait(target)); @@ -285,14 +294,24 @@ cm3_attach(struct target_s *target) /* Reset DFSR flags */ adiv5_ap_mem_write(t->ap, CM3_DFSR, CM3_DFSR_RESETALL); + /* size the break/watchpoint units */ + hw_breakpoint_max = CM3_MAX_BREAKPOINTS; + r = adiv5_ap_mem_read(t->ap, CM3_FPB_CTRL); + if (((r >> 4) & 0xf) < hw_breakpoint_max) /* only look at NUM_COMP1 */ + hw_breakpoint_max = (r >> 4) & 0xf; + hw_watchpoint_max = CM3_MAX_WATCHPOINTS; + r = adiv5_ap_mem_read(t->ap, CM3_DWT_CTRL); + if ((r >> 28) > hw_watchpoint_max) + hw_watchpoint_max = r >> 28; + /* Clear any stale breakpoints */ - for(i = 0; i < 6; i++) { + for(i = 0; i < hw_breakpoint_max; i++) { adiv5_ap_mem_write(t->ap, CM3_FPB_COMP(i), 0); hw_breakpoint[i] = 0; } /* Clear any stale watchpoints */ - for(i = 0; i < 4; i++) { + for(i = 0; i < hw_watchpoint_max; i++) { adiv5_ap_mem_write(t->ap, CM3_DWT_FUNC(i), 0); hw_watchpoint[i].type = 0; } @@ -313,14 +332,14 @@ static void cm3_detach(struct target_s *target) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; /* Clear any stale breakpoints */ - for(i = 0; i < 6; i++) + for(i = 0; i < hw_breakpoint_max; i++) adiv5_ap_mem_write(t->ap, CM3_FPB_COMP(i), 0); /* Clear any stale watchpoints */ - for(i = 0; i < 4; i++) + for(i = 0; i < hw_watchpoint_max; i++) adiv5_ap_mem_write(t->ap, CM3_DWT_FUNC(i), 0); /* Disable debug */ @@ -341,12 +360,12 @@ cm3_regs_read(struct target_s *target, void *data) * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */ adiv5_dp_low_access(t->ap->dp, 1, 0, 0x04, CM3_DHCSR); - /* Walk the regnum_v7m array, reading the registers it + /* Walk the regnum_cortex_m array, reading the registers it * calls out. */ - adiv5_ap_write(t->ap, 0x14, regnum_v7m[0]); /* Required to switch banks */ + adiv5_ap_write(t->ap, 0x14, regnum_cortex_m[0]); /* Required to switch banks */ *regs++ = adiv5_dp_read_ap(t->ap->dp, 0x18); - for(i = 1; i < sizeof(regnum_v7m) / 4; i++) { - adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, regnum_v7m[i]); + for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) { + adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, regnum_cortex_m[i]); *regs++ = adiv5_dp_read_ap(t->ap->dp, 0x18); } @@ -358,7 +377,7 @@ cm3_regs_write(struct target_s *target, const void *data) { struct target_ap_s *t = (void *)target; const uint32_t *regs = data; - int i; + unsigned i; /* FIXME: Describe what's really going on here */ adiv5_ap_write(t->ap, 0x00, 0xA2000052); @@ -367,13 +386,13 @@ cm3_regs_write(struct target_s *target, const void *data) * debug registers DHCSR, DCRSR, DCRDR and DEMCR respectively */ adiv5_dp_low_access(t->ap->dp, 1, 0, 0x04, CM3_DHCSR); - /* Walk the regnum_v7m array, writing the registers it + /* Walk the regnum_cortex_m array, writing the registers it * calls out. */ adiv5_ap_write(t->ap, 0x18, *regs++); /* Required to switch banks */ - adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, 0x10000 | regnum_v7m[0]); - for(i = 1; i < sizeof(regnum_v7m) / 4; i++) { + adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, 0x10000 | regnum_cortex_m[0]); + for(i = 1; i < sizeof(regnum_cortex_m) / 4; i++) { adiv5_dp_low_access(t->ap->dp, 1, 0, 0x18, *regs++); - adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, 0x10000 | regnum_v7m[i]); + adiv5_dp_low_access(t->ap->dp, 1, 0, 0x14, 0x10000 | regnum_cortex_m[i]); } return 0; @@ -508,15 +527,15 @@ cm3_set_hw_bp(struct target_s *target, uint32_t addr) { struct target_ap_s *t = (void *)target; uint32_t val = addr & 0x1FFFFFFC; - int i; + unsigned i; val |= (addr & 2)?0x80000000:0x40000000; val |= 1; - for(i = 0; i < 6; i++) + for(i = 0; i < hw_breakpoint_max; i++) if((hw_breakpoint[i] & 1) == 0) break; - if(i == 6) return -1; + if(i == hw_breakpoint_max) return -1; hw_breakpoint[i] = addr | 1; @@ -529,12 +548,12 @@ static int cm3_clear_hw_bp(struct target_s *target, uint32_t addr) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; - for(i = 0; i < 6; i++) + for(i = 0; i < hw_breakpoint_max; i++) if((hw_breakpoint[i] & ~1) == addr) break; - if(i == 6) return -1; + if(i == hw_breakpoint_max) return -1; hw_breakpoint[i] = 0; @@ -551,7 +570,7 @@ static int cm3_set_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, uint8_t len) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; switch(len) { /* Convert bytes size to mask size */ case 1: len = CM3_DWT_MASK_BYTE; break; @@ -569,10 +588,10 @@ cm3_set_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, uint8_t len) return -1; } - for(i = 0; i < 4; i++) + for(i = 0; i < hw_watchpoint_max; i++) if((hw_watchpoint[i].type) == 0) break; - if(i == 4) return -2; + if(i == hw_watchpoint_max) return -2; hw_watchpoint[i].type = type; hw_watchpoint[i].addr = addr; @@ -580,8 +599,8 @@ cm3_set_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, uint8_t len) adiv5_ap_mem_write(t->ap, CM3_DWT_COMP(i), addr); adiv5_ap_mem_write(t->ap, CM3_DWT_MASK(i), len); - adiv5_ap_mem_write(t->ap, CM3_DWT_FUNC(i), - CM3_DWT_FUNC_DATAVSIZE_WORD | type); + adiv5_ap_mem_write(t->ap, CM3_DWT_FUNC(i), type | + ((target->target_options & TOPT_FLAVOUR_V6M) ? 0: CM3_DWT_FUNC_DATAVSIZE_WORD)); return 0; } @@ -590,7 +609,7 @@ static int cm3_clear_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, uint8_t len) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; switch(len) { case 1: len = CM3_DWT_MASK_BYTE; break; @@ -608,12 +627,12 @@ cm3_clear_hw_wp(struct target_s *target, uint8_t type, uint32_t addr, uint8_t le return -1; } - for(i = 0; i < 4; i++) + for(i = 0; i < hw_watchpoint_max; i++) if((hw_watchpoint[i].addr == addr) && (hw_watchpoint[i].type == type) && (hw_watchpoint[i].size == len)) break; - if(i == 4) return -2; + if(i == hw_watchpoint_max) return -2; hw_watchpoint[i].type = 0; @@ -626,16 +645,16 @@ static int cm3_check_hw_wp(struct target_s *target, uint32_t *addr) { struct target_ap_s *t = (void *)target; - int i; + unsigned i; - for(i = 0; i < 4; i++) + for(i = 0; i < hw_watchpoint_max; i++) /* if SET and MATCHED then break */ if(hw_watchpoint[i].type && (adiv5_ap_mem_read(t->ap, CM3_DWT_FUNC(i)) & CM3_DWT_FUNC_MATCHED)) break; - if(i == 4) return 0; + if(i == hw_watchpoint_max) return 0; *addr = hw_watchpoint[i].addr; return 1; diff --git a/src/include/cortexm3.h b/src/include/cortexm3.h index 7a3d449..9f6f24b 100644 --- a/src/include/cortexm3.h +++ b/src/include/cortexm3.h @@ -23,6 +23,9 @@ #include "target.h" +/* target options recognised by the Cortex-M target */ +#define TOPT_FLAVOUR_V6M (1<<0) /* if not set, target is assumed to be v7m */ + int cm3_probe(struct target_s *target); #endif diff --git a/src/include/nxp_tgt.h b/src/include/nxp_tgt.h new file mode 100644 index 0000000..20e4d5a --- /dev/null +++ b/src/include/nxp_tgt.h @@ -0,0 +1,28 @@ +/* + * This file is part of the Black Magic Debug project. + * + * Copyright (C) 2011 Black Sphere Technologies Ltd. + * Written by Gareth McMullin + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * 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, see . + */ + +#ifndef __NXP_TGT_H +#define __NXP_TGT_H + +#include "target.h" + +int lpc11xx_probe(struct target_s *target); + +#endif diff --git a/src/include/target.h b/src/include/target.h index 1ff61b5..5630b93 100644 --- a/src/include/target.h +++ b/src/include/target.h @@ -159,6 +159,9 @@ typedef struct target_s { int (*check_hw_wp)(struct target_s *target, uint32_t *addr); + /* target-defined options */ + unsigned target_options; + /* Flash memory access functions */ const char *xml_mem_map; int (*flash_erase)(struct target_s *target, uint32_t addr, int len); diff --git a/src/jtag_scan.c b/src/jtag_scan.c index ce4e584..a2b8029 100644 --- a/src/jtag_scan.c +++ b/src/jtag_scan.c @@ -68,6 +68,8 @@ static struct jtag_dev_descr_s { .descr = "ST Microelectronics: STM32F2xx."}, {.idcode = 0x06413041 , .idmask = 0xFFFFFFFF, .descr = "ST Microelectronics: STM32F4xx."}, + {.idcode = 0x0BB11477 , .idmask = 0xFFFFFFFF, + .descr = "NPX: LPC11C24."}, /* Just for fun, unsupported */ {.idcode = 0x8940303F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: ATMega16."}, {.idcode = 0x0792603F, .idmask = 0xFFFFFFFF, .descr = "ATMEL: AT91SAM9261."}, diff --git a/src/nxp_tgt.c b/src/nxp_tgt.c new file mode 100644 index 0000000..9e97622 --- /dev/null +++ b/src/nxp_tgt.c @@ -0,0 +1,255 @@ + +#include +#include +#include + +#include "general.h" +#include "adiv5.h" +#include "target.h" +#include "nxp_tgt.h" + +struct flash_param { + uint16_t opcodes[2]; /* two opcodes to return to after calling the ROM */ + uint32_t command[5]; /* command operands */ + uint32_t result[4]; /* result data */ +}; + +#define IAP_PGM_CHUNKSIZE 256 /* should fit in RAM on any device */ + +struct flash_program { + struct flash_param p; + uint8_t data[IAP_PGM_CHUNKSIZE]; +}; + +#define IAP_ENTRYPOINT 0x1fff1ff1 +#define IAP_RAM_BASE 0x10000000 + + +#define IAP_CMD_PREPARE 50 +#define IAP_CMD_PROGRAM 51 +#define IAP_CMD_ERASE 52 +#define IAP_CMD_BLANKCHECK 53 + +#define IAP_STATUS_CMD_SUCCESS 0 +#define IAP_STATUS_INVALID_COMMAND 1 +#define IAP_STATUS_SRC_ADDR_ERROR 2 +#define IAP_STATUS_DST_ADDR_ERROR 3 +#define IAP_STATUS_SRC_ADDR_NOT_MAPPED 4 +#define IAP_STATUS_DST_ADDR_NOT_MAPPED 5 +#define IAP_STATUS_COUNT_ERROR 6 +#define IAP_STATUS_INVALID_SECTOR 7 +#define IAP_STATUS_SECTOR_NOT_BLANK 8 +#define IAP_STATUS_SECTOR_NOT_PREPARED 9 +#define IAP_STATUS_COMPARE_ERROR 10 +#define IAP_STATUS_BUSY 11 + +static void lpc11x_iap_call(struct target_s *target, struct flash_param *param, unsigned param_len); +static int lpc11xx_flash_prepare(struct target_s *target, uint32_t addr, int len); +static int lpc11xx_flash_erase(struct target_s *target, uint32_t addr, int len); +static int lpc11xx_flash_write_words(struct target_s *target, uint32_t dest, const uint32_t *src, + int len); + +/* + * Note that this memory map is actually for the largest of the lpc11xx devices; + * There seems to be no good way to decode the part number to determine the RAM + * and flash sizes. + */ +static const char lpc11xx_xml_memory_map[] = "" +/* ""*/ + "" + " " + " 0x1000" + " " + " " + ""; + + +int +lpc11xx_probe(struct target_s *target) +{ + struct target_ap_s *t = (void *)target; + uint32_t idcode; + + /* read the device ID register */ + idcode = adiv5_ap_mem_read(t->ap, 0x400483F4); + + switch (idcode) { + + case 0x041E502B: + case 0x2516D02B: + case 0x0416502B: + case 0x2516902B: /* lpc1111 */ + case 0x2524D02B: + case 0x0425502B: + case 0x2524902B: + case 0x1421102B: /* lpc1112 */ + case 0x0434502B: + case 0x2532902B: + case 0x0434102B: + case 0x2532102B: /* lpc1113 */ + case 0x0444502B: + case 0x2540902B: + case 0x0444102B: + case 0x2540102B: + case 0x1440102B: /* lpc1114 */ + case 0x1431102B: /* lpc11c22 */ + case 0x1430102B: /* lpc11c24 */ + target->driver = "lpc11xx"; + target->xml_mem_map = lpc11xx_xml_memory_map; + target->flash_erase = lpc11xx_flash_erase; + target->flash_write_words = lpc11xx_flash_write_words; + + return 0; + + default: + break; + } + + return -1; +} + +static void +lpc11x_iap_call(struct target_s *target, struct flash_param *param, unsigned param_len) +{ + uint32_t regs[target->regs_size]; + + /* fill out the remainder of the parameters and copy the structure to RAM */ + param->opcodes[0] = 0xbe00; + param->opcodes[1] = 0x0000; + target_mem_write_words(target, IAP_RAM_BASE, (void *)param, param_len); + + /* set up for the call to the IAP ROM */ + target_regs_read(target, regs); + regs[0] = IAP_RAM_BASE + offsetof(struct flash_param, command); + regs[1] = IAP_RAM_BASE + offsetof(struct flash_param, result); + + regs[14] = IAP_RAM_BASE | 1; + regs[15] = IAP_ENTRYPOINT; + target_regs_write(target, regs); + + /* start the target and wait for it to halt again */ + target_halt_resume(target, 0); + while (!target_halt_wait(target)); + + /* copy back just the parameters structure */ + target_mem_read_words(target, (void *)param, IAP_RAM_BASE, sizeof(struct flash_param)); +} + +static int +lpc11xx_flash_prepare(struct target_s *target, uint32_t addr, int len) +{ + struct flash_param p; + + /* prepare the sector(s) to be erased */ + memset(&p, 0, sizeof(p)); + p.command[0] = IAP_CMD_PREPARE; + p.command[1] = addr / 4096; + p.command[2] = p.command[1] + ((len + 4095) / 4096) - 1; + + lpc11x_iap_call(target, &p, sizeof(p)); + if (p.result[0] != IAP_STATUS_CMD_SUCCESS) { + return -1; + } + + return 0; +} + +static int +lpc11xx_flash_erase(struct target_s *target, uint32_t addr, int len) +{ + struct flash_param p; + + if (addr % 4096) + return -1; + + /* prepare... */ + if (lpc11xx_flash_prepare(target, addr, len)) + return -1; + + /* and now erase them */ + p.command[0] = IAP_CMD_ERASE; + p.command[1] = addr / 4096; + p.command[2] = p.command[1] + ((len + 4095) / 4096) - 1; + p.command[3] = 12000; /* XXX safe to assume this? */ + lpc11x_iap_call(target, &p, sizeof(p)); + if (p.result[0] != IAP_STATUS_CMD_SUCCESS) { + return -1; + } + p.command[0] = IAP_CMD_BLANKCHECK; + lpc11x_iap_call(target, &p, sizeof(p)); + if (p.result[0] != IAP_STATUS_CMD_SUCCESS) { + return -1; + } + + return 0; +} + +static int +lpc11xx_flash_write_words(struct target_s *target, uint32_t dest, const uint32_t *src, int len) +{ + struct flash_program pgm; + const uint8_t *s = (const uint8_t *)src; + unsigned first_chunk = dest / IAP_PGM_CHUNKSIZE; + unsigned last_chunk = (dest + len - 1) / IAP_PGM_CHUNKSIZE; + unsigned chunk_offset = dest % IAP_PGM_CHUNKSIZE; + unsigned chunk; + + for (chunk = first_chunk; chunk <= last_chunk; chunk++) { + + printf("chunk %u len %d\n", chunk, len); + /* first and last chunk may require special handling */ + if ((chunk == first_chunk) || (chunk == last_chunk)) { + + /* fill with all ff to avoid sector rewrite corrupting other writes */ + memset(pgm.data, 0xff, sizeof(pgm.data)); + + /* copy as much as fits */ + int copylen = IAP_PGM_CHUNKSIZE - chunk_offset; + if (copylen > len) + copylen = len; + memcpy(&pgm.data[chunk_offset], s, copylen); + + /* update to suit */ + len -= copylen; + s += copylen; + chunk_offset = 0; + + /* if we are programming the vectors, calculate the magic number */ + if (chunk == 0) { + uint32_t *w = (uint32_t *)(&pgm.data[0]); + uint32_t sum = 0; + + for (unsigned i = 0; i < 7; i++) + sum += w[i]; + w[7] = 0 - sum; + } + + } else { + + /* interior chunk, must be aligned and full-sized */ + memcpy(pgm.data, s, IAP_PGM_CHUNKSIZE); + len -= IAP_PGM_CHUNKSIZE; + s += IAP_PGM_CHUNKSIZE; + } + + /* prepare... */ + if (lpc11xx_flash_prepare(target, chunk * IAP_PGM_CHUNKSIZE, IAP_PGM_CHUNKSIZE)) + return -1; + + /* set the destination address and program */ + pgm.p.command[0] = IAP_CMD_PROGRAM; + pgm.p.command[1] = chunk * IAP_PGM_CHUNKSIZE; + pgm.p.command[2] = IAP_RAM_BASE + offsetof(struct flash_program, data); + pgm.p.command[3] = IAP_PGM_CHUNKSIZE; + pgm.p.command[4] = 12000; /* XXX safe to presume this? */ + lpc11x_iap_call(target, &pgm.p, sizeof(pgm)); + if (pgm.p.result[0] != IAP_STATUS_CMD_SUCCESS) { + return -1; + } + + } + + return 0; +}