git.gag.com Git - fw/stlink/blob - src/stlink-common.c

   1 #define DEBUG_FLASH 0
   2
   3 #include <stdarg.h>
   4 #include <stdio.h>
   5 #include <stdlib.h>
   6 #include <string.h>
   7
   8 #include <unistd.h>
   9 #include <fcntl.h>
  10 #include <sys/types.h>
  11 #include <sys/stat.h>
  12 #include "mmap.h"
  13
  14 #include "stlink-common.h"
  15 #include "uglylogging.h"
  16
  17 #define LOG_TAG __FILE__
  18 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  19 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  20 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  21 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  22
  23 /* todo: stm32l15xxx flash memory, pm0062 manual */
  24
  25 /* stm32f FPEC flash controller interface, pm0063 manual */
  26 // TODO - all of this needs to be abstracted out....
  27 #define FLASH_REGS_ADDR 0x40022000
  28 #define FLASH_REGS_SIZE 0x28
  29
  30 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  31 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  32 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  33 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  34 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  35 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  36 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  37
  38 #define FLASH_RDPTR_KEY 0x00a5
  39 #define FLASH_KEY1 0x45670123
  40 #define FLASH_KEY2 0xcdef89ab
  41
  42 #define FLASH_SR_BSY 0
  43 #define FLASH_SR_EOP 5
  44
  45 #define FLASH_CR_PG 0
  46 #define FLASH_CR_PER 1
  47 #define FLASH_CR_MER 2
  48 #define FLASH_CR_STRT 6
  49 #define FLASH_CR_LOCK 7
  50
  51
  52 //32L = 32F1 same CoreID as 32F4!
  53 #define STM32L_FLASH_REGS_ADDR ((uint32_t)0x40023c00)
  54 #define STM32L_FLASH_ACR (STM32L_FLASH_REGS_ADDR + 0x00)
  55 #define STM32L_FLASH_PECR (STM32L_FLASH_REGS_ADDR + 0x04)
  56 #define STM32L_FLASH_PDKEYR (STM32L_FLASH_REGS_ADDR + 0x08)
  57 #define STM32L_FLASH_PEKEYR (STM32L_FLASH_REGS_ADDR + 0x0c)
  58 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
  59 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
  60 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
  61 #define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x1c)
  62 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
  63 #define FLASH_L1_FPRG 10
  64 #define FLASH_L1_PROG 3
  65
  66
  67 //STM32F4
  68 #define FLASH_F4_REGS_ADDR ((uint32_t)0x40023c00)
  69 #define FLASH_F4_KEYR (FLASH_F4_REGS_ADDR + 0x04)
  70 #define FLASH_F4_OPT_KEYR (FLASH_F4_REGS_ADDR + 0x08)
  71 #define FLASH_F4_SR (FLASH_F4_REGS_ADDR + 0x0c)
  72 #define FLASH_F4_CR (FLASH_F4_REGS_ADDR + 0x10)
  73 #define FLASH_F4_OPT_CR (FLASH_F4_REGS_ADDR + 0x14)
  74 #define FLASH_F4_CR_STRT 16
  75 #define FLASH_F4_CR_LOCK 31
  76 #define FLASH_F4_CR_SER 1
  77 #define FLASH_F4_CR_SNB 3
  78 #define FLASH_F4_CR_SNB_MASK 0x38
  79 #define FLASH_F4_SR_BSY 16
  80
  81
  82 void write_uint32(unsigned char* buf, uint32_t ui) {
  83     if (!is_bigendian()) { // le -> le (don't swap)
  84         buf[0] = ((unsigned char*) &ui)[0];
  85         buf[1] = ((unsigned char*) &ui)[1];
  86         buf[2] = ((unsigned char*) &ui)[2];
  87         buf[3] = ((unsigned char*) &ui)[3];
  88     } else {
  89         buf[0] = ((unsigned char*) &ui)[3];
  90         buf[1] = ((unsigned char*) &ui)[2];
  91         buf[2] = ((unsigned char*) &ui)[1];
  92         buf[3] = ((unsigned char*) &ui)[0];
  93     }
  94 }
  95
  96 void write_uint16(unsigned char* buf, uint16_t ui) {
  97     if (!is_bigendian()) { // le -> le (don't swap)
  98         buf[0] = ((unsigned char*) &ui)[0];
  99         buf[1] = ((unsigned char*) &ui)[1];
 100     } else {
 101         buf[0] = ((unsigned char*) &ui)[1];
 102         buf[1] = ((unsigned char*) &ui)[0];
 103     }
 104 }
 105
 106 uint32_t read_uint32(const unsigned char *c, const int pt) {
 107     uint32_t ui;
 108     char *p = (char *) &ui;
 109
 110     if (!is_bigendian()) { // le -> le (don't swap)
 111         p[0] = c[pt + 0];
 112         p[1] = c[pt + 1];
 113         p[2] = c[pt + 2];
 114         p[3] = c[pt + 3];
 115     } else {
 116         p[0] = c[pt + 3];
 117         p[1] = c[pt + 2];
 118         p[2] = c[pt + 1];
 119         p[3] = c[pt + 0];
 120     }
 121     return ui;
 122 }
 123
 124 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
 125     return stlink_read_debug32(sl, FLASH_WRPR) & 0xff;
 126 }
 127
 128 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 129     return stlink_read_debug32(sl, FLASH_WRPR);
 130 }
 131
 132 static inline uint32_t read_flash_obr(stlink_t *sl) {
 133     return stlink_read_debug32(sl, FLASH_OBR);
 134 }
 135
 136 static inline uint32_t read_flash_cr(stlink_t *sl) {
 137         uint32_t res;
 138         if((sl->chip_id==STM32_CHIPID_F2) ||(sl->chip_id==STM32_CHIPID_F4))
 139                 res = stlink_read_debug32(sl, FLASH_F4_CR);
 140         else
 141                 res = stlink_read_debug32(sl, FLASH_CR);
 142 #if DEBUG_FLASH
 143         fprintf(stdout, "CR:0x%x\n", res);
 144 #endif
 145         return res;
 146 }
 147
 148 static inline unsigned int is_flash_locked(stlink_t *sl) {
 149     /* return non zero for true */
 150         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 151                 return read_flash_cr(sl) & (1 << FLASH_F4_CR_LOCK);
 152         else
 153                 return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 154 }
 155
 156 static void unlock_flash(stlink_t *sl) {
 157     /* the unlock sequence consists of 2 write cycles where
 158        2 key values are written to the FLASH_KEYR register.
 159        an invalid sequence results in a definitive lock of
 160        the FPEC block until next reset.
 161      */
 162     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 163         stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY1);
 164                 stlink_write_debug32(sl, FLASH_F4_KEYR, FLASH_KEY2);
 165     }
 166         else {
 167         stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY1);
 168                 stlink_write_debug32(sl, FLASH_KEYR, FLASH_KEY2);
 169         }
 170
 171 }
 172
 173 static int unlock_flash_if(stlink_t *sl) {
 174     /* unlock flash if already locked */
 175
 176     if (is_flash_locked(sl)) {
 177         unlock_flash(sl);
 178         if (is_flash_locked(sl)) {
 179             WLOG("Failed to unlock flash!\n");
 180             return -1;
 181         }
 182     }
 183     DLOG("Successfully unlocked flash\n");
 184     return 0;
 185 }
 186
 187 static void lock_flash(stlink_t *sl) {
 188     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 189         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_F4_CR_LOCK);
 190         stlink_write_debug32(sl, FLASH_F4_CR, n);
 191     }
 192     else {
 193         /* write to 1 only. reset by hw at unlock sequence */
 194         const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 195         stlink_write_debug32(sl, FLASH_CR, n);
 196     }
 197 }
 198
 199
 200 static void set_flash_cr_pg(stlink_t *sl) {
 201     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 202                 uint32_t x = read_flash_cr(sl);
 203                 x |= (1 << FLASH_CR_PG);
 204         stlink_write_debug32(sl, FLASH_F4_CR, x);
 205     }
 206     else {
 207         const uint32_t n = 1 << FLASH_CR_PG;
 208         stlink_write_debug32(sl, FLASH_CR, n);
 209     }
 210 }
 211
 212 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 213     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 214     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 215         stlink_write_debug32(sl, FLASH_F4_CR, n);
 216     else
 217         stlink_write_debug32(sl, FLASH_CR, n);
 218 }
 219
 220 static void set_flash_cr_per(stlink_t *sl) {
 221     const uint32_t n = 1 << FLASH_CR_PER;
 222     stlink_write_debug32(sl, FLASH_CR, n);
 223 }
 224
 225 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 226     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 227     stlink_write_debug32(sl, FLASH_CR, n);
 228 }
 229
 230 static void set_flash_cr_mer(stlink_t *sl) {
 231     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 232         stlink_write_debug32(sl, FLASH_F4_CR,
 233                              stlink_read_debug32(sl, FLASH_F4_CR) | (1 << FLASH_CR_MER));
 234     else
 235         stlink_write_debug32(sl, FLASH_CR,
 236                              stlink_read_debug32(sl, FLASH_CR) | (1 << FLASH_CR_MER));
 237 }
 238
 239 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 240     if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 241         stlink_write_debug32(sl, FLASH_F4_CR,
 242                              stlink_read_debug32(sl, FLASH_F4_CR) & ~(1 << FLASH_CR_MER));
 243     else
 244         stlink_write_debug32(sl, FLASH_CR,
 245                              stlink_read_debug32(sl, FLASH_CR) & ~(1 << FLASH_CR_MER));
 246 }
 247
 248 static void set_flash_cr_strt(stlink_t *sl) {
 249         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 250         {
 251                 uint32_t x = read_flash_cr(sl);
 252                 x |= (1 << FLASH_F4_CR_STRT);
 253                 stlink_write_debug32(sl, FLASH_F4_CR, x);
 254         }
 255         else {
 256             stlink_write_debug32(
 257                 sl, FLASH_CR,
 258                 stlink_read_debug32(sl,FLASH_CR) |(1 << FLASH_CR_STRT) );
 259         }
 260 }
 261
 262 static inline uint32_t read_flash_acr(stlink_t *sl) {
 263     return stlink_read_debug32(sl, FLASH_ACR);
 264 }
 265
 266 static inline uint32_t read_flash_sr(stlink_t *sl) {
 267         uint32_t res;
 268         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 269                 res = stlink_read_debug32(sl, FLASH_F4_SR);
 270         else
 271                 res = stlink_read_debug32(sl, FLASH_SR);
 272     //fprintf(stdout, "SR:0x%x\n", *(uint32_t*) sl->q_buf);
 273     return res;
 274 }
 275
 276 static inline unsigned int is_flash_busy(stlink_t *sl) {
 277         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4))
 278                 return read_flash_sr(sl) & (1 << FLASH_F4_SR_BSY);
 279         else
 280                 return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 281 }
 282
 283 static void wait_flash_busy(stlink_t *sl) {
 284     /* todo: add some delays here */
 285     while (is_flash_busy(sl))
 286         ;
 287 }
 288
 289 static void wait_flash_busy_progress(stlink_t *sl) {
 290     int i = 0;
 291     fprintf(stdout, "Mass erasing");
 292     fflush(stdout);
 293     while (is_flash_busy(sl))
 294     {
 295         usleep(10000);
 296         i++;
 297         if (i % 100 == 0) {
 298             fprintf(stdout, ".");
 299             fflush(stdout);
 300         }
 301     }
 302     fprintf(stdout, "\n");
 303 }
 304
 305 static inline unsigned int is_flash_eop(stlink_t *sl) {
 306     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 307 }
 308
 309 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 310     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 311     stlink_write_debug32(sl, FLASH_SR, n);
 312 }
 313
 314 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 315     /* todo: add some delays here */
 316     while (is_flash_eop(sl) == 0)
 317         ;
 318 }
 319
 320 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 321     stlink_write_debug32(sl, FLASH_AR, n);
 322 }
 323
 324 static inline void write_flash_cr_psiz(stlink_t *sl, uint32_t n) {
 325     uint32_t x = read_flash_cr(sl);
 326     x &= ~(0x03 << 8);
 327     x |= (n << 8);
 328 #if DEBUG_FLASH
 329     fprintf(stdout, "PSIZ:0x%x 0x%x\n", x, n);
 330 #endif
 331     stlink_write_debug32(sl, FLASH_F4_CR, x);
 332 }
 333
 334
 335 static inline void write_flash_cr_snb(stlink_t *sl, uint32_t n) {
 336     uint32_t x = read_flash_cr(sl);
 337     x &= ~FLASH_F4_CR_SNB_MASK;
 338     x |= (n << FLASH_F4_CR_SNB);
 339     x |= (1 << FLASH_F4_CR_SER);
 340 #if DEBUG_FLASH
 341     fprintf(stdout, "SNB:0x%x 0x%x\n", x, n);
 342 #endif
 343     stlink_write_debug32(sl, FLASH_F4_CR, x);
 344 }
 345
 346 #if 0 /* todo */
 347
 348 static void disable_flash_read_protection(stlink_t *sl) {
 349     /* erase the option byte area */
 350     /* rdp = 0x00a5; */
 351     /* reset */
 352 }
 353 #endif /* todo */
 354
 355
 356 // Delegates to the backends...
 357
 358 void stlink_close(stlink_t *sl) {
 359     DLOG("*** stlink_close ***\n");
 360     sl->backend->close(sl);
 361     free(sl);
 362 }
 363
 364 void stlink_exit_debug_mode(stlink_t *sl) {
 365     DLOG("*** stlink_exit_debug_mode ***\n");
 366     stlink_write_debug32(sl, DHCSR, DBGKEY);
 367     sl->backend->exit_debug_mode(sl);
 368 }
 369
 370 void stlink_enter_swd_mode(stlink_t *sl) {
 371     DLOG("*** stlink_enter_swd_mode ***\n");
 372     sl->backend->enter_swd_mode(sl);
 373 }
 374
 375 // Force the core into the debug mode -> halted state.
 376 void stlink_force_debug(stlink_t *sl) {
 377     DLOG("*** stlink_force_debug_mode ***\n");
 378     sl->backend->force_debug(sl);
 379 }
 380
 381 void stlink_exit_dfu_mode(stlink_t *sl) {
 382     DLOG("*** stlink_exit_dfu_mode ***\n");
 383     sl->backend->exit_dfu_mode(sl);
 384 }
 385
 386 uint32_t stlink_core_id(stlink_t *sl) {
 387     DLOG("*** stlink_core_id ***\n");
 388     sl->backend->core_id(sl);
 389     if (sl->verbose > 2)
 390         stlink_print_data(sl);
 391     DLOG("core_id = 0x%08x\n", sl->core_id);
 392     return sl->core_id;
 393 }
 394
 395 uint32_t stlink_chip_id(stlink_t *sl) {
 396     uint32_t chip_id = stlink_read_debug32(sl, 0xE0042000);
 397     if (chip_id == 0) chip_id = stlink_read_debug32(sl, 0x40015800);    //Try Corex M0 DBGMCU_IDCODE register address
 398     return chip_id;
 399 }
 400
 401 /**
 402  * Cortex m3 tech ref manual, CPUID register description
 403  * @param sl stlink context
 404  * @param cpuid pointer to the result object
 405  */
 406 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 407     uint32_t raw = stlink_read_debug32(sl, CM3_REG_CPUID);
 408     cpuid->implementer_id = (raw >> 24) & 0x7f;
 409     cpuid->variant = (raw >> 20) & 0xf;
 410     cpuid->part = (raw >> 4) & 0xfff;
 411     cpuid->revision = raw & 0xf;
 412     return;
 413 }
 414
 415 /**
 416  * reads and decodes the flash parameters, as dynamically as possible
 417  * @param sl
 418  * @return 0 for success, or -1 for unsupported core type.
 419  */
 420 int stlink_load_device_params(stlink_t *sl) {
 421     ILOG("Loading device parameters....\n");
 422     const chip_params_t *params = NULL;
 423     sl->core_id = stlink_core_id(sl);
 424     uint32_t chip_id = stlink_chip_id(sl);
 425
 426     sl->chip_id = chip_id & 0xfff;
 427     /* Fix chip_id for F4 rev A errata , Read CPU ID, as CoreID is the same for F2/F4*/
 428     if (sl->chip_id == 0x411) {
 429         uint32_t cpuid = stlink_read_debug32(sl, 0xE000ED00);
 430         if((cpuid  & 0xfff0) == 0xc240)
 431             sl->chip_id = 0x413;
 432     }
 433
 434     for(size_t i = 0; i < sizeof(devices) / sizeof(devices[0]); i++) {
 435         if(devices[i].chip_id == sl->chip_id) {
 436             params = &devices[i];
 437             break;
 438         }
 439     }
 440     if (params == NULL) {
 441         WLOG("unknown chip id! %#x\n", chip_id);
 442         return -1;
 443     }
 444
 445     // These are fixed...
 446     sl->flash_base = STM32_FLASH_BASE;
 447     sl->sram_base = STM32_SRAM_BASE;
 448
 449     // read flash size from hardware, if possible...
 450     if (sl->chip_id == STM32_CHIPID_F2) {
 451         sl->flash_size = 0x100000; /* Use maximum, User must care!*/
 452     } else if (sl->chip_id == STM32_CHIPID_F4) {
 453                 sl->flash_size = 0x100000;                      //todo: RM0090 error; size register same address as unique ID
 454     } else {
 455         uint32_t flash_size = stlink_read_debug32(sl, params->flash_size_reg) & 0xffff;
 456         sl->flash_size = flash_size * 1024;
 457     }
 458     sl->flash_pgsz = params->flash_pagesize;
 459     sl->sram_size = params->sram_size;
 460     sl->sys_base = params->bootrom_base;
 461     sl->sys_size = params->bootrom_size;
 462
 463     ILOG("Device connected is: %s, id %#x\n", params->description, chip_id);
 464     // TODO make note of variable page size here.....
 465     ILOG("SRAM size: %#x bytes (%d KiB), Flash: %#x bytes (%d KiB) in pages of %zd bytes\n",
 466         sl->sram_size, sl->sram_size / 1024, sl->flash_size, sl->flash_size / 1024,
 467         sl->flash_pgsz);
 468     return 0;
 469 }
 470
 471 void stlink_reset(stlink_t *sl) {
 472     DLOG("*** stlink_reset ***\n");
 473     sl->backend->reset(sl);
 474 }
 475
 476 void stlink_jtag_reset(stlink_t *sl, int value) {
 477     DLOG("*** stlink_jtag_reset ***\n");
 478     sl->backend->jtag_reset(sl, value);
 479 }
 480
 481 void stlink_run(stlink_t *sl) {
 482     DLOG("*** stlink_run ***\n");
 483     sl->backend->run(sl);
 484 }
 485
 486 void stlink_status(stlink_t *sl) {
 487     DLOG("*** stlink_status ***\n");
 488     sl->backend->status(sl);
 489     stlink_core_stat(sl);
 490 }
 491
 492 /**
 493  * Decode the version bits, originally from -sg, verified with usb
 494  * @param sl stlink context, assumed to contain valid data in the buffer
 495  * @param slv output parsed version object
 496  */
 497 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 498     uint32_t b0 = sl->q_buf[0]; //lsb
 499     uint32_t b1 = sl->q_buf[1];
 500     uint32_t b2 = sl->q_buf[2];
 501     uint32_t b3 = sl->q_buf[3];
 502     uint32_t b4 = sl->q_buf[4];
 503     uint32_t b5 = sl->q_buf[5]; //msb
 504
 505     // b0 b1                       || b2 b3  | b4 b5
 506     // 4b        | 6b     | 6b     || 2B     | 2B
 507     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 508
 509     slv->stlink_v = (b0 & 0xf0) >> 4;
 510     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 511     slv->swim_v = b1 & 0x3f;
 512     slv->st_vid = (b3 << 8) | b2;
 513     slv->stlink_pid = (b5 << 8) | b4;
 514     return;
 515 }
 516
 517 void stlink_version(stlink_t *sl) {
 518     DLOG("*** looking up stlink version\n");
 519     sl->backend->version(sl);
 520     _parse_version(sl, &sl->version);
 521
 522     DLOG("st vid         = 0x%04x (expect 0x%04x)\n", sl->version.st_vid, USB_ST_VID);
 523     DLOG("stlink pid     = 0x%04x\n", sl->version.stlink_pid);
 524     DLOG("stlink version = 0x%x\n", sl->version.stlink_v);
 525     DLOG("jtag version   = 0x%x\n", sl->version.jtag_v);
 526     DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 527     if (sl->version.jtag_v == 0) {
 528         DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
 529     }
 530     if (sl->version.swim_v == 0) {
 531         DLOG("    notice: the firmware doesn't support a swim interface\n");
 532     }
 533 }
 534
 535 uint32_t stlink_read_debug32(stlink_t *sl, uint32_t addr) {
 536     uint32_t data = sl->backend->read_debug32(sl, addr);
 537     DLOG("*** stlink_read_debug32 %x is %#x\n", data, addr);
 538     return data;
 539 }
 540
 541 void stlink_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
 542     DLOG("*** stlink_write_debug32 %x to %#x\n", data, addr);
 543     sl->backend->write_debug32(sl, addr, data);
 544 }
 545
 546 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 547     DLOG("*** stlink_write_mem32 %u bytes to %#x\n", len, addr);
 548     if (len % 4 != 0) {
 549         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 550         return;
 551     }
 552     sl->backend->write_mem32(sl, addr, len);
 553 }
 554
 555 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 556     DLOG("*** stlink_read_mem32 ***\n");
 557     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 558         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 559                 len % 4);
 560         return;
 561     }
 562     sl->backend->read_mem32(sl, addr, len);
 563 }
 564
 565 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 566     DLOG("*** stlink_write_mem8 ***\n");
 567     if (len > 0x40 ) { // !!! never ever: Writing more then 0x40 bytes gives unexpected behaviour
 568         fprintf(stderr, "Error: Data length > 64: +%d byte.\n",
 569                 len);
 570         return;
 571     }
 572     sl->backend->write_mem8(sl, addr, len);
 573 }
 574
 575 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 576     DLOG("*** stlink_read_all_regs ***\n");
 577     sl->backend->read_all_regs(sl, regp);
 578 }
 579
 580 void stlink_read_all_unsupported_regs(stlink_t *sl, reg *regp) {
 581     DLOG("*** stlink_read_all_unsupported_regs ***\n");
 582     sl->backend->read_all_unsupported_regs(sl, regp);
 583 }
 584
 585 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 586     DLOG("*** stlink_write_reg\n");
 587     sl->backend->write_reg(sl, reg, idx);
 588 }
 589
 590 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 591     DLOG("*** stlink_read_reg\n");
 592     DLOG(" (%d) ***\n", r_idx);
 593
 594     if (r_idx > 20 || r_idx < 0) {
 595         fprintf(stderr, "Error: register index must be in [0..20]\n");
 596         return;
 597     }
 598
 599     sl->backend->read_reg(sl, r_idx, regp);
 600 }
 601
 602 void stlink_read_unsupported_reg(stlink_t *sl, int r_idx, reg *regp) {
 603     int r_convert;
 604
 605     DLOG("*** stlink_read_unsupported_reg\n");
 606     DLOG(" (%d) ***\n", r_idx);
 607
 608     /* Convert to values used by DCRSR */
 609     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 610         r_convert = 0x14;
 611     } else if (r_idx == 0x40) {     /* FPSCR */
 612         r_convert = 0x21;
 613     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 614         r_convert = 0x40 + (r_idx - 0x20);
 615     } else {
 616         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 617         return;
 618     }
 619
 620     sl->backend->read_unsupported_reg(sl, r_convert, regp);
 621 }
 622
 623 void stlink_write_unsupported_reg(stlink_t *sl, uint32_t val, int r_idx, reg *regp) {
 624     int r_convert;
 625
 626     DLOG("*** stlink_write_unsupported_reg\n");
 627     DLOG(" (%d) ***\n", r_idx);
 628
 629     /* Convert to values used by DCRSR */
 630     if (r_idx >= 0x1C && r_idx <= 0x1F) { /* primask, basepri, faultmask, or control */
 631         r_convert = r_idx;  /* The backend function handles this */
 632     } else if (r_idx == 0x40) {     /* FPSCR */
 633         r_convert = 0x21;
 634     } else if (r_idx >= 0x20 && r_idx < 0x40) {
 635         r_convert = 0x40 + (r_idx - 0x20);
 636     } else {
 637         fprintf(stderr, "Error: register address must be in [0x1C..0x40]\n");
 638         return;
 639     }
 640
 641     sl->backend->write_unsupported_reg(sl, val, r_convert, regp);
 642 }
 643
 644 unsigned int is_core_halted(stlink_t *sl) {
 645     /* return non zero if core is halted */
 646     stlink_status(sl);
 647     return sl->q_buf[0] == STLINK_CORE_HALTED;
 648 }
 649
 650 void stlink_step(stlink_t *sl) {
 651     DLOG("*** stlink_step ***\n");
 652     sl->backend->step(sl);
 653 }
 654
 655 int stlink_current_mode(stlink_t *sl) {
 656     int mode = sl->backend->current_mode(sl);
 657     switch (mode) {
 658         case STLINK_DEV_DFU_MODE:
 659             DLOG("stlink current mode: dfu\n");
 660             return mode;
 661         case STLINK_DEV_DEBUG_MODE:
 662             DLOG("stlink current mode: debug (jtag or swd)\n");
 663             return mode;
 664         case STLINK_DEV_MASS_MODE:
 665             DLOG("stlink current mode: mass\n");
 666             return mode;
 667     }
 668     DLOG("stlink mode: unknown!\n");
 669     return STLINK_DEV_UNKNOWN_MODE;
 670 }
 671
 672
 673
 674
 675 // End of delegates....  Common code below here...
 676
 677 // Endianness
 678 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 679 // const int i = 1;
 680 // #define is_bigendian() ( (*(char*)&i) == 0 )
 681
 682 inline unsigned int is_bigendian(void) {
 683     static volatile const unsigned int i = 1;
 684     return *(volatile const char*) &i == 0;
 685 }
 686
 687 uint16_t read_uint16(const unsigned char *c, const int pt) {
 688     uint32_t ui;
 689     char *p = (char *) &ui;
 690
 691     if (!is_bigendian()) { // le -> le (don't swap)
 692         p[0] = c[pt + 0];
 693         p[1] = c[pt + 1];
 694     } else {
 695         p[0] = c[pt + 1];
 696         p[1] = c[pt + 0];
 697     }
 698     return ui;
 699 }
 700
 701 // same as above with entrypoint.
 702
 703 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 704     stlink_write_reg(sl, addr, 15); /* pc register */
 705
 706     stlink_run(sl);
 707
 708     while (is_core_halted(sl) == 0)
 709         usleep(3000000);
 710 }
 711
 712 void stlink_core_stat(stlink_t *sl) {
 713     if (sl->q_len <= 0)
 714         return;
 715
 716     switch (sl->q_buf[0]) {
 717         case STLINK_CORE_RUNNING:
 718             sl->core_stat = STLINK_CORE_RUNNING;
 719             DLOG("  core status: running\n");
 720             return;
 721         case STLINK_CORE_HALTED:
 722             sl->core_stat = STLINK_CORE_HALTED;
 723             DLOG("  core status: halted\n");
 724             return;
 725         default:
 726             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 727             fprintf(stderr, "  core status: unknown\n");
 728     }
 729 }
 730
 731 void stlink_print_data(stlink_t * sl) {
 732     if (sl->q_len <= 0 || sl->verbose < UDEBUG)
 733         return;
 734     if (sl->verbose > 2)
 735         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 736
 737     for (int i = 0; i < sl->q_len; i++) {
 738         if (i % 16 == 0) {
 739             /*
 740                                     if (sl->q_data_dir == Q_DATA_OUT)
 741                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 742                                     else
 743                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 744              */
 745         }
 746         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 747     }
 748     fputs("\n\n", stdout);
 749 }
 750
 751 /* memory mapped file */
 752
 753 typedef struct mapped_file {
 754     uint8_t* base;
 755     size_t len;
 756 } mapped_file_t;
 757
 758 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 759
 760 static int map_file(mapped_file_t* mf, const char* path) {
 761     int error = -1;
 762     struct stat st;
 763
 764     const int fd = open(path, O_RDONLY);
 765     if (fd == -1) {
 766         fprintf(stderr, "open(%s) == -1\n", path);
 767         return -1;
 768     }
 769
 770     if (fstat(fd, &st) == -1) {
 771         fprintf(stderr, "fstat() == -1\n");
 772         goto on_error;
 773     }
 774
 775     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 776     if (mf->base == MAP_FAILED) {
 777         fprintf(stderr, "mmap() == MAP_FAILED\n");
 778         goto on_error;
 779     }
 780
 781     mf->len = st.st_size;
 782
 783     /* success */
 784     error = 0;
 785
 786 on_error:
 787     close(fd);
 788
 789     return error;
 790 }
 791
 792 static void unmap_file(mapped_file_t * mf) {
 793     munmap((void*) mf->base, mf->len);
 794     mf->base = (unsigned char*) MAP_FAILED;
 795     mf->len = 0;
 796 }
 797
 798 /* Limit the block size to compare to 0x1800
 799    Anything larger will stall the STLINK2
 800    Maybe STLINK V1 needs smaller value!*/
 801 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 802     size_t off;
 803     size_t n_cmp = sl->flash_pgsz;
 804     if ( n_cmp > 0x1800)
 805         n_cmp = 0x1800;
 806
 807     for (off = 0; off < mf->len; off += n_cmp) {
 808         size_t aligned_size;
 809
 810         /* adjust last page size */
 811         size_t cmp_size = n_cmp;
 812         if ((off + n_cmp) > mf->len)
 813             cmp_size = mf->len - off;
 814
 815         aligned_size = cmp_size;
 816         if (aligned_size & (4 - 1))
 817             aligned_size = (cmp_size + 4) & ~(4 - 1);
 818
 819         stlink_read_mem32(sl, addr + off, aligned_size);
 820
 821         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 822             return -1;
 823     }
 824
 825     return 0;
 826 }
 827
 828 int stlink_fwrite_sram
 829 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 830     /* write the file in sram at addr */
 831
 832     int error = -1;
 833     size_t off;
 834     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 835
 836     if (map_file(&mf, path) == -1) {
 837         fprintf(stderr, "map_file() == -1\n");
 838         return -1;
 839     }
 840
 841     /* check addr range is inside the sram */
 842     if (addr < sl->sram_base) {
 843         fprintf(stderr, "addr too low\n");
 844         goto on_error;
 845     } else if ((addr + mf.len) < addr) {
 846         fprintf(stderr, "addr overruns\n");
 847         goto on_error;
 848     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 849         fprintf(stderr, "addr too high\n");
 850         goto on_error;
 851     } else if ((addr & 3) || (mf.len & 3)) {
 852         /* todo */
 853         fprintf(stderr, "unaligned addr or size\n");
 854         goto on_error;
 855     }
 856
 857     /* do the copy by 1k blocks */
 858     for (off = 0; off < mf.len; off += 1024) {
 859         size_t size = 1024;
 860         if ((off + size) > mf.len)
 861             size = mf.len - off;
 862
 863         memcpy(sl->q_buf, mf.base + off, size);
 864
 865         /* round size if needed */
 866         if (size & 3)
 867             size += 2;
 868
 869         stlink_write_mem32(sl, addr + off, size);
 870     }
 871
 872     /* check the file ha been written */
 873     if (check_file(sl, &mf, addr) == -1) {
 874         fprintf(stderr, "check_file() == -1\n");
 875         goto on_error;
 876     }
 877
 878     /* success */
 879     error = 0;
 880     /* set stack*/
 881     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
 882     /* Set PC to the reset routine*/
 883     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
 884     stlink_run(sl);
 885
 886 on_error:
 887     unmap_file(&mf);
 888     return error;
 889 }
 890
 891 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 892     /* read size bytes from addr to file */
 893
 894     int error = -1;
 895     size_t off;
 896     int num_empty = 0;
 897     unsigned char erased_pattern =(sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
 898
 899     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 900     if (fd == -1) {
 901         fprintf(stderr, "open(%s) == -1\n", path);
 902         return -1;
 903     }
 904
 905     if (size <1)
 906         size = sl->flash_size;
 907
 908     if (size > sl->flash_size)
 909         size = sl->flash_size;
 910
 911     /* do the copy by 1k blocks */
 912     for (off = 0; off < size; off += 1024) {
 913         size_t read_size = 1024;
 914         size_t rounded_size;
 915         size_t index;
 916         if ((off + read_size) > size)
 917           read_size = size - off;
 918
 919         /* round size if needed */
 920         rounded_size = read_size;
 921         if (rounded_size & 3)
 922           rounded_size = (rounded_size + 4) & ~(3);
 923
 924         stlink_read_mem32(sl, addr + off, rounded_size);
 925
 926         for(index = 0; index < read_size; index ++) {
 927             if (sl->q_buf[index] == erased_pattern)
 928                 num_empty ++;
 929             else
 930                 num_empty = 0;
 931         }
 932         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 933             fprintf(stderr, "write() != read_size\n");
 934             goto on_error;
 935         }
 936     }
 937
 938     /* Ignore NULL Bytes at end of file */
 939     if (!ftruncate(fd, size - num_empty)) {
 940         error = -1;
 941     }
 942
 943     /* success */
 944     error = 0;
 945
 946 on_error:
 947     close(fd);
 948
 949     return error;
 950 }
 951
 952 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 953     /* write the buffer right after the loader */
 954     size_t chunk = size & ~0x3;
 955     size_t rem   = size & 0x3;
 956     if (chunk) {
 957         memcpy(sl->q_buf, buf, chunk);
 958         stlink_write_mem32(sl, fl->buf_addr, chunk);
 959     }
 960     if (rem) {
 961         memcpy(sl->q_buf, buf+chunk, rem);
 962         stlink_write_mem8(sl, (fl->buf_addr)+chunk, rem);
 963     }
 964     return 0;
 965 }
 966
 967 uint32_t calculate_F4_sectornum(uint32_t flashaddr){
 968     flashaddr &= ~STM32_FLASH_BASE;     //Page now holding the actual flash address
 969     if (flashaddr<0x4000) return (0);
 970     else if(flashaddr<0x8000) return(1);
 971     else if(flashaddr<0xc000) return(2);
 972     else if(flashaddr<0x10000) return(3);
 973     else if(flashaddr<0x20000) return(4);
 974     else return(flashaddr/0x20000)+4;
 975
 976 }
 977
 978 uint32_t stlink_calculate_pagesize(stlink_t *sl, uint32_t flashaddr){
 979         if((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 980                 uint32_t sector=calculate_F4_sectornum(flashaddr);
 981                 if (sector<4) sl->flash_pgsz=0x4000;
 982                 else if(sector<5) sl->flash_pgsz=0x10000;
 983                 else sl->flash_pgsz=0x20000;
 984         }
 985         return (sl->flash_pgsz);
 986 }
 987
 988 /**
 989  * Erase a page of flash, assumes sl is fully populated with things like chip/core ids
 990  * @param sl stlink context
 991  * @param flashaddr an address in the flash page to erase
 992  * @return 0 on success -ve on failure
 993  */
 994 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t flashaddr)
 995 {
 996   if ((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
 997     /* wait for ongoing op to finish */
 998     wait_flash_busy(sl);
 999
1000     /* unlock if locked */
1001     unlock_flash_if(sl);
1002
1003     /* select the page to erase */
1004     // calculate the actual page from the address
1005     uint32_t sector=calculate_F4_sectornum(flashaddr);
1006
1007     fprintf(stderr, "EraseFlash - Sector:0x%x Size:0x%x\n", sector, stlink_calculate_pagesize(sl, flashaddr));
1008     write_flash_cr_snb(sl, sector);
1009
1010     /* start erase operation */
1011     set_flash_cr_strt(sl);
1012
1013     /* wait for completion */
1014     wait_flash_busy(sl);
1015
1016     /* relock the flash */
1017     //todo: fails to program if this is in
1018     lock_flash(sl);
1019 #if DEBUG_FLASH
1020         fprintf(stdout, "Erase Final CR:0x%x\n", read_flash_cr(sl));
1021 #endif
1022   } else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1023
1024     uint32_t val;
1025
1026     /* disable pecr protection */
1027     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1028     stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1029
1030     /* check pecr.pelock is cleared */
1031     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1032     if (val & (1 << 0)) {
1033       WLOG("pecr.pelock not clear (%#x)\n", val);
1034       return -1;
1035     }
1036
1037     /* unlock program memory */
1038     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1039     stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1040
1041     /* check pecr.prglock is cleared */
1042     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1043     if (val & (1 << 1)) {
1044       WLOG("pecr.prglock not clear (%#x)\n", val);
1045       return -1;
1046     }
1047
1048     /* unused: unlock the option byte block */
1049 #if 0
1050     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0xfbead9c8);
1051     stlink_write_debug32(sl, STM32L_FLASH_OPTKEYR, 0x24252627);
1052
1053     /* check pecr.optlock is cleared */
1054     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1055     if (val & (1 << 2)) {
1056       fprintf(stderr, "pecr.prglock not clear\n");
1057       return -1;
1058     }
1059 #endif
1060
1061     /* set pecr.{erase,prog} */
1062     val |= (1 << 9) | (1 << 3);
1063     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1064
1065 #if 0 /* fix_to_be_confirmed */
1066
1067     /* wait for sr.busy to be cleared
1068        MP: Test shows that busy bit is not set here. Perhaps, PM0062 is
1069        wrong and we do not need to wait here for clearing the busy bit.
1070        TEXANE: ok, if experience says so and it works for you, we comment
1071        it. If someone has a problem, please drop an email.
1072      */
1073     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1074         ;
1075
1076 #endif /* fix_to_be_confirmed */
1077
1078     /* write 0 to the first word of the page to be erased */
1079     stlink_write_debug32(sl, flashaddr, 0);
1080
1081     /* MP: It is better to wait for clearing the busy bit after issuing
1082     page erase command, even though PM0062 recommends to wait before it.
1083     Test shows that a few iterations is performed in the following loop
1084     before busy bit is cleared.*/
1085     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1086         ;
1087
1088     /* reset lock bits */
1089     val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1090         | (1 << 0) | (1 << 1) | (1 << 2);
1091     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1092   } else if (sl->core_id == STM32VL_CORE_ID) {
1093     /* wait for ongoing op to finish */
1094     wait_flash_busy(sl);
1095
1096     /* unlock if locked */
1097     unlock_flash_if(sl);
1098
1099     /* set the page erase bit */
1100     set_flash_cr_per(sl);
1101
1102     /* select the page to erase */
1103     write_flash_ar(sl, flashaddr);
1104
1105     /* start erase operation, reset by hw with bsy bit */
1106     set_flash_cr_strt(sl);
1107
1108     /* wait for completion */
1109     wait_flash_busy(sl);
1110
1111     /* relock the flash */
1112     lock_flash(sl);
1113   } else {
1114     WLOG("unknown coreid: %x\n", sl->core_id);
1115     return -1;
1116   }
1117
1118   /* todo: verify the erased page */
1119
1120   return 0;
1121 }
1122
1123 int stlink_erase_flash_mass(stlink_t *sl) {
1124     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1125         /* erase each page */
1126         int i = 0, num_pages = sl->flash_size/sl->flash_pgsz;
1127         for (i = 0; i < num_pages; i++) {
1128             /* addr must be an addr inside the page */
1129             stm32_addr_t addr = sl->flash_base + i * sl->flash_pgsz;
1130             if (stlink_erase_flash_page(sl, addr) == -1) {
1131                 WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr);
1132                 return -1;
1133             }
1134             fprintf(stdout,"\rFlash page at %5d/%5d erased", i, num_pages);
1135             fflush(stdout);
1136         }
1137         fprintf(stdout, "\n");
1138     } else {
1139         /* wait for ongoing op to finish */
1140         wait_flash_busy(sl);
1141
1142         /* unlock if locked */
1143         unlock_flash_if(sl);
1144
1145         /* set the mass erase bit */
1146         set_flash_cr_mer(sl);
1147
1148         /* start erase operation, reset by hw with bsy bit */
1149         set_flash_cr_strt(sl);
1150
1151         /* wait for completion */
1152         wait_flash_busy_progress(sl);
1153
1154         /* relock the flash */
1155         lock_flash(sl);
1156
1157         /* todo: verify the erased memory */
1158     }
1159     return 0;
1160 }
1161
1162 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
1163     size_t size;
1164
1165     /* allocate the loader in sram */
1166     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
1167         WLOG("Failed to write flash loader to sram!\n");
1168         return -1;
1169     }
1170
1171     /* allocate a one page buffer in sram right after loader */
1172     fl->buf_addr = fl->loader_addr + size;
1173     ILOG("Successfully loaded flash loader in sram\n");
1174     return 0;
1175 }
1176
1177 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
1178     /* from openocd, contrib/loaders/flash/stm32.s */
1179     static const uint8_t loader_code_stm32vl[] = {
1180         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
1181         0x1c, 0x44, /* add      r4, r3 */
1182         /* write_half_word: */
1183         0x01, 0x23, /* movs     r3, #0x01 */
1184         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
1185         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
1186         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
1187         /* busy: */
1188         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
1189         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
1190         0xfb, 0xd0, /* beq      busy */
1191         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
1192         0x01, 0xd1, /* bne      exit */
1193         0x01, 0x3a, /* subs     r2, r2, #0x01 */
1194         0xf0, 0xd1, /* bne      write_half_word */
1195         /* exit: */
1196         0x00, 0xbe, /* bkpt     #0x00 */
1197         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
1198     };
1199
1200     static const uint8_t loader_code_stm32l[] = {
1201
1202         /* openocd.git/contrib/loaders/flash/stm32lx.S
1203            r0, input, dest addr
1204            r1, input, source addr
1205            r2, input, word count
1206            r3, output, word count
1207         */
1208
1209         0x00, 0x23,
1210         0x04, 0xe0,
1211
1212         0x51, 0xf8, 0x04, 0xcb,
1213         0x40, 0xf8, 0x04, 0xcb,
1214         0x01, 0x33,
1215
1216         0x93, 0x42,
1217         0xf8, 0xd3,
1218         0x00, 0xbe
1219     };
1220
1221     static const uint8_t loader_code_stm32f4[] = {
1222         // flashloaders/stm32f4.s
1223
1224         0x07, 0x4b,
1225
1226         0x62, 0xb1,
1227         0x04, 0x68,
1228         0x0c, 0x60,
1229
1230         0xdc, 0x89,
1231         0x14, 0xf0, 0x01, 0x0f,
1232         0xfb, 0xd1,
1233         0x00, 0xf1, 0x04, 0x00,
1234         0x01, 0xf1, 0x04, 0x01,
1235         0xa2, 0xf1, 0x01, 0x02,
1236         0xf1, 0xe7,
1237
1238         0x00, 0xbe,
1239
1240         0x00, 0x3c, 0x02, 0x40,
1241     };
1242
1243     const uint8_t* loader_code;
1244     size_t loader_size;
1245
1246     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) { /* stm32l */
1247       loader_code = loader_code_stm32l;
1248       loader_size = sizeof(loader_code_stm32l);
1249     } else if (sl->core_id == STM32VL_CORE_ID) {
1250       loader_code = loader_code_stm32vl;
1251       loader_size = sizeof(loader_code_stm32vl);
1252     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1253       loader_code = loader_code_stm32f4;
1254       loader_size = sizeof(loader_code_stm32f4);
1255     } else {
1256       WLOG("unknown coreid, not sure what flash loader to use, aborting!: %x\n", sl->core_id);
1257       return -1;
1258     }
1259
1260     memcpy(sl->q_buf, loader_code, loader_size);
1261     stlink_write_mem32(sl, sl->sram_base, loader_size);
1262
1263     *addr = sl->sram_base;
1264     *size = loader_size;
1265
1266     /* success */
1267     return 0;
1268 }
1269
1270 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1271     /* check the contents of path are at addr */
1272
1273     int res;
1274     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1275
1276     if (map_file(&mf, path) == -1)
1277         return -1;
1278
1279     res = check_file(sl, &mf, addr);
1280
1281     unmap_file(&mf);
1282
1283     return res;
1284 }
1285
1286 /**
1287  * Verify addr..addr+len is binary identical to base...base+len
1288  * @param sl stlink context
1289  * @param address stm device address
1290  * @param data host side buffer to check against
1291  * @param length how much
1292  * @return 0 for success, -ve for failure
1293  */
1294 int stlink_verify_write_flash(stlink_t *sl, stm32_addr_t address, uint8_t *data, unsigned length) {
1295     size_t off;
1296     size_t cmp_size = (sl->flash_pgsz > 0x1800)? 0x1800:sl->flash_pgsz;
1297     ILOG("Starting verification of write complete\n");
1298     for (off = 0; off < length; off += cmp_size) {
1299         size_t aligned_size;
1300
1301         /* adjust last page size */
1302         if ((off + cmp_size) > length)
1303             cmp_size = length - off;
1304
1305         aligned_size = cmp_size;
1306         if (aligned_size & (4 - 1))
1307             aligned_size = (cmp_size + 4) & ~(4 - 1);
1308
1309         stlink_read_mem32(sl, address + off, aligned_size);
1310
1311         if (memcmp(sl->q_buf, data + off, cmp_size)) {
1312             WLOG("Verification of flash failed at offset: %zd\n", off);
1313             return -1;
1314         }
1315     }
1316     ILOG("Flash written and verified! jolly good!\n");
1317     return 0;
1318
1319 }
1320
1321 int stm32l1_write_half_pages(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned num_half_pages)
1322 {
1323     unsigned int count;
1324     uint32_t val;
1325     flash_loader_t fl;
1326
1327     ILOG("Starting Half page flash write for STM32L core id\n");
1328     /* flash loader initialization */
1329     if (init_flash_loader(sl, &fl) == -1) {
1330         WLOG("init_flash_loader() == -1\n");
1331         return -1;
1332     }
1333     /* Unlock already done */
1334     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1335     val |= (1 << FLASH_L1_FPRG);
1336     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1337
1338     val |= (1 << FLASH_L1_PROG);
1339     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1340     while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {}
1341
1342 #define L1_WRITE_BLOCK_SIZE 0x80
1343     for (count = 0; count  < num_half_pages; count ++) {
1344         if (run_flash_loader(sl, &fl, addr + count * L1_WRITE_BLOCK_SIZE, base + count * L1_WRITE_BLOCK_SIZE, L1_WRITE_BLOCK_SIZE) == -1) {
1345             WLOG("l1_run_flash_loader(%#zx) failed! == -1\n", addr + count * L1_WRITE_BLOCK_SIZE);
1346             val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1347             val &= ~((1 << FLASH_L1_FPRG) |(1 << FLASH_L1_PROG));
1348             stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1349             return -1;
1350         }
1351         /* wait for sr.busy to be cleared */
1352         if (sl->verbose >= 1) {
1353             /* show progress. writing procedure is slow
1354                and previous errors are misleading */
1355             fprintf(stdout, "\r%3u/%u halfpages written", count + 1, num_half_pages);
1356             fflush(stdout);
1357         }
1358         while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0) {
1359         }
1360     }
1361     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1362     val &= ~(1 << FLASH_L1_PROG);
1363     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1364     val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1365     val &= ~(1 << FLASH_L1_FPRG);
1366     stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1367
1368     return 0;
1369 }
1370
1371 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
1372     size_t off;
1373     flash_loader_t fl;
1374     ILOG("Attempting to write %d (%#x) bytes to stm32 address: %u (%#x)\n",
1375         len, len, addr, addr);
1376     /* check addr range is inside the flash */
1377     stlink_calculate_pagesize(sl, addr);
1378     if (addr < sl->flash_base) {
1379         WLOG("addr too low %#x < %#x\n", addr, sl->flash_base);
1380         return -1;
1381     } else if ((addr + len) < addr) {
1382         WLOG("addr overruns\n");
1383         return -1;
1384     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
1385         WLOG("addr too high\n");
1386         return -1;
1387     } else if ((addr & 1) || (len & 1)) {
1388         WLOG("unaligned addr or size\n");
1389         return -1;
1390     } else if (addr & (sl->flash_pgsz - 1)) {
1391         WLOG("addr not a multiple of pagesize, not supported\n");
1392         return -1;
1393     }
1394
1395     // Make sure we've loaded the context with the chip details
1396     stlink_core_id(sl);
1397     /* erase each page */
1398     int page_count = 0;
1399     for (off = 0; off < len; off += stlink_calculate_pagesize(sl, addr + off)) {
1400         /* addr must be an addr inside the page */
1401         if (stlink_erase_flash_page(sl, addr + off) == -1) {
1402             WLOG("Failed to erase_flash_page(%#zx) == -1\n", addr + off);
1403             return -1;
1404         }
1405         fprintf(stdout,"\rFlash page at addr: 0x%08lx erased",
1406                 (unsigned long)addr + off);
1407         fflush(stdout);
1408         page_count++;
1409     }
1410     fprintf(stdout,"\n");
1411     ILOG("Finished erasing %d pages of %d (%#x) bytes\n",
1412         page_count, sl->flash_pgsz, sl->flash_pgsz);
1413
1414     if ((sl->chip_id == STM32_CHIPID_F2) ||(sl->chip_id == STM32_CHIPID_F4)) {
1415         /* todo: check write operation */
1416
1417         ILOG("Starting Flash write for F2/F4\n");
1418         /* flash loader initialization */
1419         if (init_flash_loader(sl, &fl) == -1) {
1420             WLOG("init_flash_loader() == -1\n");
1421             return -1;
1422         }
1423
1424         /* First unlock the cr */
1425         unlock_flash_if(sl);
1426
1427         /* TODO: Check that Voltage range is 2.7 - 3.6 V */
1428         /* set parallelisim to 32 bit*/
1429         write_flash_cr_psiz(sl, 2);
1430
1431         /* set programming mode */
1432         set_flash_cr_pg(sl);
1433
1434         for(off = 0; off < len;) {
1435             size_t size = len - off > 0x8000 ? 0x8000 : len - off;
1436
1437             printf("size: %zu\n", size);
1438
1439             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1440                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1441                 return -1;
1442             }
1443
1444             off += size;
1445         }
1446
1447 #if 0
1448 #define PROGRESS_CHUNK_SIZE 0x1000
1449         /* write a word in program memory */
1450         for (off = 0; off < len; off += sizeof(uint32_t)) {
1451             uint32_t data;
1452             if (sl->verbose >= 1) {
1453                 if ((off & (PROGRESS_CHUNK_SIZE - 1)) == 0) {
1454                     /* show progress. writing procedure is slow
1455                        and previous errors are misleading */
1456                     const uint32_t pgnum = (off / PROGRESS_CHUNK_SIZE)+1;
1457                     const uint32_t pgcount = len / PROGRESS_CHUNK_SIZE +1;
1458                     fprintf(stdout, "Writing %ukB chunk %u out of %u\n",
1459                             PROGRESS_CHUNK_SIZE/1024, pgnum, pgcount);
1460                 }
1461             }
1462
1463             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1464             stlink_write_debug32(sl, addr + off, data);
1465
1466             /* wait for sr.busy to be cleared */
1467             wait_flash_busy(sl);
1468
1469         }
1470 #endif
1471         /* Relock flash */
1472         lock_flash(sl);
1473
1474 #if 0 /* todo: debug mode */
1475         fprintf(stdout, "Final CR:0x%x\n", read_flash_cr(sl));
1476 #endif
1477
1478     }   //STM32F4END
1479
1480     else if (sl->chip_id == STM32_CHIPID_L1_MEDIUM)    {
1481         /* use fast word write. todo: half page. */
1482         uint32_t val;
1483
1484 #if 0 /* todo: check write operation */
1485
1486         uint32_t nwrites = sl->flash_pgsz;
1487
1488         redo_write:
1489
1490 #endif /* todo: check write operation */
1491
1492         /* disable pecr protection */
1493         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x89abcdef);
1494         stlink_write_debug32(sl, STM32L_FLASH_PEKEYR, 0x02030405);
1495
1496         /* check pecr.pelock is cleared */
1497         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1498         if (val & (1 << 0)) {
1499                 fprintf(stderr, "pecr.pelock not clear\n");
1500                 return -1;
1501         }
1502
1503         /* unlock program memory */
1504         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x8c9daebf);
1505         stlink_write_debug32(sl, STM32L_FLASH_PRGKEYR, 0x13141516);
1506
1507         /* check pecr.prglock is cleared */
1508         val = stlink_read_debug32(sl, STM32L_FLASH_PECR);
1509         if (val & (1 << 1)) {
1510                 fprintf(stderr, "pecr.prglock not clear\n");
1511                 return -1;
1512         }
1513         off = 0;
1514         if (len > L1_WRITE_BLOCK_SIZE) {
1515             if (stm32l1_write_half_pages(sl, addr, base, len/L1_WRITE_BLOCK_SIZE) == -1) {
1516                 /* This may happen on a blank device! */
1517                 WLOG("\nwrite_half_pages failed == -1\n");
1518             } else {
1519                 off = (len /L1_WRITE_BLOCK_SIZE)*L1_WRITE_BLOCK_SIZE;
1520             }
1521         }
1522
1523         /* write remainingword in program memory */
1524         for ( ; off < len; off += sizeof(uint32_t)) {
1525             uint32_t data;
1526             if (off > 254)
1527                 fprintf(stdout, "\r");
1528
1529             if ((off % sl->flash_pgsz) > (sl->flash_pgsz -5)) {
1530                 fprintf(stdout, "\r%3zd/%3zd pages written",
1531                         off/sl->flash_pgsz, len/sl->flash_pgsz);
1532                 fflush(stdout);
1533             }
1534
1535             write_uint32((unsigned char*) &data, *(uint32_t*) (base + off));
1536             stlink_write_debug32(sl, addr + off, data);
1537
1538             /* wait for sr.busy to be cleared */
1539             while ((stlink_read_debug32(sl, STM32L_FLASH_SR) & (1 << 0)) != 0)
1540                 ;
1541
1542 #if 0 /* todo: check redo write operation */
1543
1544             /* check written bytes. todo: should be on a per page basis. */
1545             data = stlink_read_debug32(sl, addr + off);
1546             if (data == *(uint32_t*)(base + off)) {
1547                 /* re erase the page and redo the write operation */
1548                 uint32_t page;
1549                 uint32_t val;
1550
1551                 /* fail if successive write count too low */
1552                 if (nwrites < sl->flash_pgsz) {
1553                     fprintf(stderr, "writes operation failure count too high, aborting\n");
1554                     return -1;
1555                 }
1556
1557                 nwrites = 0;
1558
1559                 /* assume addr aligned */
1560                 if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1561                 page = addr + off;
1562
1563                 fprintf(stderr, "invalid write @0x%x(0x%x): 0x%x != 0x%x. retrying.\n",
1564                         page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1565
1566                 /* reset lock bits */
1567                 val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1568                     | (1 << 0) | (1 << 1) | (1 << 2);
1569                 stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1570
1571                 stlink_erase_flash_page(sl, page);
1572
1573                 goto redo_write;
1574             }
1575
1576             /* increment successive writes counter */
1577             ++nwrites;
1578
1579 #endif /* todo: check redo write operation */
1580         }
1581         fprintf(stdout, "\n");
1582         /* reset lock bits */
1583         val = stlink_read_debug32(sl, STM32L_FLASH_PECR)
1584              | (1 << 0) | (1 << 1) | (1 << 2);
1585         stlink_write_debug32(sl, STM32L_FLASH_PECR, val);
1586     } else if (sl->core_id == STM32VL_CORE_ID) {
1587         ILOG("Starting Flash write for VL core id\n");
1588         /* flash loader initialization */
1589         if (init_flash_loader(sl, &fl) == -1) {
1590             WLOG("init_flash_loader() == -1\n");
1591             return -1;
1592         }
1593
1594         int write_block_count = 0;
1595         for (off = 0; off < len; off += sl->flash_pgsz) {
1596             /* adjust last write size */
1597             size_t size = sl->flash_pgsz;
1598             if ((off + sl->flash_pgsz) > len) size = len - off;
1599
1600             /* unlock and set programming mode */
1601             unlock_flash_if(sl);
1602             set_flash_cr_pg(sl);
1603             //DLOG("Finished setting flash cr pg, running loader!\n");
1604             if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1605                 WLOG("run_flash_loader(%#zx) failed! == -1\n", addr + off);
1606                 return -1;
1607             }
1608             lock_flash(sl);
1609             if (sl->verbose >= 1) {
1610                 /* show progress. writing procedure is slow
1611                    and previous errors are misleading */
1612               fprintf(stdout, "\r%3u/%lu pages written", write_block_count++, (unsigned long)len/sl->flash_pgsz);
1613                 fflush(stdout);
1614             }
1615         }
1616         fprintf(stdout, "\n");
1617     } else {
1618         WLOG("unknown coreid, not sure how to write: %x\n", sl->core_id);
1619         return -1;
1620     }
1621
1622     return stlink_verify_write_flash(sl, addr, base, len);
1623 }
1624
1625 /**
1626  * Write the given binary file into flash at address "addr"
1627  * @param sl
1628  * @param path readable file path, should be binary image
1629  * @param addr where to start writing
1630  * @return 0 on success, -ve on failure.
1631  */
1632 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1633     /* write the file in flash at addr */
1634     int err;
1635     unsigned int num_empty = 0, index;
1636     unsigned char erased_pattern =(sl->chip_id == STM32_CHIPID_L1_MEDIUM)?0:0xff;
1637     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1638     if (map_file(&mf, path) == -1) {
1639         WLOG("map_file() == -1\n");
1640         return -1;
1641     }
1642     for(index = 0; index < mf.len; index ++) {
1643         if (mf.base[index] == erased_pattern)
1644             num_empty ++;
1645         else
1646             num_empty = 0;
1647     }
1648     if(num_empty != 0) {
1649         ILOG("Ignoring %d bytes of Zeros at end of file\n",num_empty);
1650         mf.len -= num_empty;
1651     }
1652     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1653     /* set stack*/
1654     stlink_write_reg(sl, stlink_read_debug32(sl, addr    ),13);
1655     /* Set PC to the reset routine*/
1656     stlink_write_reg(sl, stlink_read_debug32(sl, addr + 4),15);
1657     stlink_run(sl);
1658     unmap_file(&mf);
1659     return err;
1660 }
1661
1662 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1663
1664     reg rr;
1665     int i = 0;
1666     DLOG("Running flash loader, write address:%#x, size: %zd\n", target, size);
1667     // FIXME This can never return -1
1668     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1669         // IMPOSSIBLE!
1670         WLOG("write_buffer_to_sram() == -1\n");
1671         return -1;
1672     }
1673
1674     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1675
1676         size_t count = size / sizeof(uint32_t);
1677         if (size % sizeof(uint32_t)) ++count;
1678
1679         /* setup core */
1680         stlink_write_reg(sl, target, 0); /* target */
1681         stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1682         stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1683         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1684
1685     } else if (sl->core_id == STM32VL_CORE_ID) {
1686
1687         size_t count = size / sizeof(uint16_t);
1688         if (size % sizeof(uint16_t)) ++count;
1689
1690         /* setup core */
1691         stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1692         stlink_write_reg(sl, target, 1); /* target */
1693         stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1694         stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1695         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1696
1697     } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1698
1699         size_t count = size / sizeof(uint32_t);
1700         if (size % sizeof(uint32_t)) ++count;
1701
1702         /* setup core */
1703         stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1704         stlink_write_reg(sl, target, 1); /* target */
1705         stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1706         stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1707
1708     } else {
1709         fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1710         return -1;
1711     }
1712
1713     /* run loader */
1714     stlink_run(sl);
1715
1716     /* wait until done (reaches breakpoint) */
1717     while ((is_core_halted(sl) == 0) && (i <1000)) {
1718         i++;
1719     }
1720
1721     if ( i > 999) {
1722         fprintf(stderr, "run error\n");
1723         return -1;
1724     }
1725
1726     /* check written byte count */
1727     if (sl->chip_id == STM32_CHIPID_L1_MEDIUM) {
1728
1729       size_t count = size / sizeof(uint32_t);
1730       if (size % sizeof(uint32_t)) ++count;
1731
1732       stlink_read_reg(sl, 3, &rr);
1733       if (rr.r[3] != count) {
1734         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1735         return -1;
1736       }
1737
1738     } else if (sl->core_id == STM32VL_CORE_ID) {
1739
1740       stlink_read_reg(sl, 2, &rr);
1741       if (rr.r[2] != 0) {
1742         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1743         return -1;
1744       }
1745
1746         } else if (sl->chip_id == STM32_CHIPID_F2 || sl->chip_id == STM32_CHIPID_F4) {
1747
1748                 stlink_read_reg(sl, 2, &rr);
1749                 if (rr.r[2] != 0) {
1750                         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1751                         return -1;
1752                 }
1753
1754     } else {
1755
1756       fprintf(stderr, "unknown coreid: 0x%x\n", sl->core_id);
1757       return -1;
1758
1759     }
1760
1761     return 0;
1762 }