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

   1
   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 <sys/mman.h>
  13
  14
  15 #include "stlink-common.h"
  16 #include "uglylogging.h"
  17
  18 #define LOG_TAG __FILE__
  19 #define DLOG(format, args...)         ugly_log(UDEBUG, LOG_TAG, format, ## args)
  20 #define ILOG(format, args...)         ugly_log(UINFO, LOG_TAG, format, ## args)
  21 #define WLOG(format, args...)         ugly_log(UWARN, LOG_TAG, format, ## args)
  22 #define fatal(format, args...)        ugly_log(UFATAL, LOG_TAG, format, ## args)
  23
  24 /* todo: stm32l15xxx flash memory, pm0062 manual */
  25
  26 /* stm32f FPEC flash controller interface, pm0063 manual */
  27
  28 #define FLASH_REGS_ADDR 0x40022000
  29 #define FLASH_REGS_SIZE 0x28
  30
  31 #define FLASH_ACR (FLASH_REGS_ADDR + 0x00)
  32 #define FLASH_KEYR (FLASH_REGS_ADDR + 0x04)
  33 #define FLASH_SR (FLASH_REGS_ADDR + 0x0c)
  34 #define FLASH_CR (FLASH_REGS_ADDR + 0x10)
  35 #define FLASH_AR (FLASH_REGS_ADDR + 0x14)
  36 #define FLASH_OBR (FLASH_REGS_ADDR + 0x1c)
  37 #define FLASH_WRPR (FLASH_REGS_ADDR + 0x20)
  38
  39 #define FLASH_RDPTR_KEY 0x00a5
  40 #define FLASH_KEY1 0x45670123
  41 #define FLASH_KEY2 0xcdef89ab
  42
  43 #define FLASH_SR_BSY 0
  44 #define FLASH_SR_EOP 5
  45
  46 #define FLASH_CR_PG 0
  47 #define FLASH_CR_PER 1
  48 #define FLASH_CR_MER 2
  49 #define FLASH_CR_STRT 6
  50 #define FLASH_CR_LOCK 7
  51
  52 void write_uint32(unsigned char* buf, uint32_t ui) {
  53     if (!is_bigendian()) { // le -> le (don't swap)
  54         buf[0] = ((unsigned char*) &ui)[0];
  55         buf[1] = ((unsigned char*) &ui)[1];
  56         buf[2] = ((unsigned char*) &ui)[2];
  57         buf[3] = ((unsigned char*) &ui)[3];
  58     } else {
  59         buf[0] = ((unsigned char*) &ui)[3];
  60         buf[1] = ((unsigned char*) &ui)[2];
  61         buf[2] = ((unsigned char*) &ui)[1];
  62         buf[3] = ((unsigned char*) &ui)[0];
  63     }
  64 }
  65
  66 void write_uint16(unsigned char* buf, uint16_t ui) {
  67     if (!is_bigendian()) { // le -> le (don't swap)
  68         buf[0] = ((unsigned char*) &ui)[0];
  69         buf[1] = ((unsigned char*) &ui)[1];
  70     } else {
  71         buf[0] = ((unsigned char*) &ui)[1];
  72         buf[1] = ((unsigned char*) &ui)[0];
  73     }
  74 }
  75
  76 uint32_t read_uint32(const unsigned char *c, const int pt) {
  77     uint32_t ui;
  78     char *p = (char *) &ui;
  79
  80     if (!is_bigendian()) { // le -> le (don't swap)
  81         p[0] = c[pt + 0];
  82         p[1] = c[pt + 1];
  83         p[2] = c[pt + 2];
  84         p[3] = c[pt + 3];
  85     } else {
  86         p[0] = c[pt + 3];
  87         p[1] = c[pt + 2];
  88         p[2] = c[pt + 1];
  89         p[3] = c[pt + 0];
  90     }
  91     return ui;
  92 }
  93
  94 static uint32_t __attribute__((unused)) read_flash_rdp(stlink_t *sl) {
  95     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
  96     return (*(uint32_t*) sl->q_buf) & 0xff;
  97 }
  98
  99 static inline uint32_t read_flash_wrpr(stlink_t *sl) {
 100     stlink_read_mem32(sl, FLASH_WRPR, sizeof (uint32_t));
 101     return *(uint32_t*) sl->q_buf;
 102 }
 103
 104 static inline uint32_t read_flash_obr(stlink_t *sl) {
 105     stlink_read_mem32(sl, FLASH_OBR, sizeof (uint32_t));
 106     return *(uint32_t*) sl->q_buf;
 107 }
 108
 109 static inline uint32_t read_flash_cr(stlink_t *sl) {
 110     stlink_read_mem32(sl, FLASH_CR, sizeof (uint32_t));
 111     return *(uint32_t*) sl->q_buf;
 112 }
 113
 114 static inline unsigned int is_flash_locked(stlink_t *sl) {
 115     /* return non zero for true */
 116     return read_flash_cr(sl) & (1 << FLASH_CR_LOCK);
 117 }
 118
 119 static void unlock_flash(stlink_t *sl) {
 120     /* the unlock sequence consists of 2 write cycles where
 121        2 key values are written to the FLASH_KEYR register.
 122        an invalid sequence results in a definitive lock of
 123        the FPEC block until next reset.
 124      */
 125
 126     write_uint32(sl->q_buf, FLASH_KEY1);
 127     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 128
 129     write_uint32(sl->q_buf, FLASH_KEY2);
 130     stlink_write_mem32(sl, FLASH_KEYR, sizeof (uint32_t));
 131 }
 132
 133 static int unlock_flash_if(stlink_t *sl) {
 134     /* unlock flash if already locked */
 135
 136     if (is_flash_locked(sl)) {
 137         unlock_flash(sl);
 138         if (is_flash_locked(sl))
 139             return -1;
 140     }
 141
 142     return 0;
 143 }
 144
 145 static void lock_flash(stlink_t *sl) {
 146     /* write to 1 only. reset by hw at unlock sequence */
 147
 148     const uint32_t n = read_flash_cr(sl) | (1 << FLASH_CR_LOCK);
 149
 150     write_uint32(sl->q_buf, n);
 151     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 152 }
 153
 154 static void set_flash_cr_pg(stlink_t *sl) {
 155     const uint32_t n = 1 << FLASH_CR_PG;
 156     write_uint32(sl->q_buf, n);
 157     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 158 }
 159
 160 static void __attribute__((unused)) clear_flash_cr_pg(stlink_t *sl) {
 161     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PG);
 162     write_uint32(sl->q_buf, n);
 163     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 164 }
 165
 166 static void set_flash_cr_per(stlink_t *sl) {
 167     const uint32_t n = 1 << FLASH_CR_PER;
 168     write_uint32(sl->q_buf, n);
 169     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 170 }
 171
 172 static void __attribute__((unused)) clear_flash_cr_per(stlink_t *sl) {
 173     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_PER);
 174     write_uint32(sl->q_buf, n);
 175     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 176 }
 177
 178 static void set_flash_cr_mer(stlink_t *sl) {
 179     const uint32_t n = 1 << FLASH_CR_MER;
 180     write_uint32(sl->q_buf, n);
 181     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 182 }
 183
 184 static void __attribute__((unused)) clear_flash_cr_mer(stlink_t *sl) {
 185     const uint32_t n = read_flash_cr(sl) & ~(1 << FLASH_CR_MER);
 186     write_uint32(sl->q_buf, n);
 187     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 188 }
 189
 190 static void set_flash_cr_strt(stlink_t *sl) {
 191     /* assume come on the flash_cr_per path */
 192     const uint32_t n = (1 << FLASH_CR_PER) | (1 << FLASH_CR_STRT);
 193     write_uint32(sl->q_buf, n);
 194     stlink_write_mem32(sl, FLASH_CR, sizeof (uint32_t));
 195 }
 196
 197 static inline uint32_t read_flash_acr(stlink_t *sl) {
 198     stlink_read_mem32(sl, FLASH_ACR, sizeof (uint32_t));
 199     return *(uint32_t*) sl->q_buf;
 200 }
 201
 202 static inline uint32_t read_flash_sr(stlink_t *sl) {
 203     stlink_read_mem32(sl, FLASH_SR, sizeof (uint32_t));
 204     return *(uint32_t*) sl->q_buf;
 205 }
 206
 207 static inline unsigned int is_flash_busy(stlink_t *sl) {
 208     return read_flash_sr(sl) & (1 << FLASH_SR_BSY);
 209 }
 210
 211 static void wait_flash_busy(stlink_t *sl) {
 212     /* todo: add some delays here */
 213     while (is_flash_busy(sl))
 214         ;
 215 }
 216
 217 static inline unsigned int is_flash_eop(stlink_t *sl) {
 218     return read_flash_sr(sl) & (1 << FLASH_SR_EOP);
 219 }
 220
 221 static void __attribute__((unused)) clear_flash_sr_eop(stlink_t *sl) {
 222     const uint32_t n = read_flash_sr(sl) & ~(1 << FLASH_SR_EOP);
 223     write_uint32(sl->q_buf, n);
 224     stlink_write_mem32(sl, FLASH_SR, sizeof (uint32_t));
 225 }
 226
 227 static void __attribute__((unused)) wait_flash_eop(stlink_t *sl) {
 228     /* todo: add some delays here */
 229     while (is_flash_eop(sl) == 0)
 230         ;
 231 }
 232
 233 static inline void write_flash_ar(stlink_t *sl, uint32_t n) {
 234     write_uint32(sl->q_buf, n);
 235     stlink_write_mem32(sl, FLASH_AR, sizeof (uint32_t));
 236 }
 237
 238 #if 0 /* todo */
 239
 240 static void disable_flash_read_protection(stlink_t *sl) {
 241     /* erase the option byte area */
 242     /* rdp = 0x00a5; */
 243     /* reset */
 244 }
 245 #endif /* todo */
 246
 247
 248 // Delegates to the backends...
 249
 250 void stlink_close(stlink_t *sl) {
 251     DLOG("*** stlink_close ***\n");
 252     sl->backend->close(sl);
 253     free(sl);
 254 }
 255
 256 void stlink_exit_debug_mode(stlink_t *sl) {
 257     DLOG("*** stlink_exit_debug_mode ***\n");
 258     sl->backend->exit_debug_mode(sl);
 259 }
 260
 261 void stlink_enter_swd_mode(stlink_t *sl) {
 262     DLOG("*** stlink_enter_swd_mode ***\n");
 263     sl->backend->enter_swd_mode(sl);
 264 }
 265
 266 // Force the core into the debug mode -> halted state.
 267 void stlink_force_debug(stlink_t *sl) {
 268     DLOG("*** stlink_force_debug_mode ***\n");
 269     sl->backend->force_debug(sl);
 270 }
 271
 272 void stlink_exit_dfu_mode(stlink_t *sl) {
 273     DLOG("*** stlink_exit_dfu_mode ***\n");
 274     sl->backend->exit_dfu_mode(sl);
 275 }
 276
 277 uint32_t stlink_core_id(stlink_t *sl) {
 278     DLOG("*** stlink_core_id ***\n");
 279     sl->backend->core_id(sl);
 280     if (sl->verbose > 2)
 281         stlink_print_data(sl);
 282     DLOG("core_id = 0x%08x\n", sl->core_id);
 283     return sl->core_id;
 284 }
 285
 286 uint16_t stlink_chip_id(stlink_t *sl) {
 287     stlink_read_mem32(sl, 0xE0042000, 4);
 288     uint32_t chip_id = sl->q_buf[0] | (sl->q_buf[1] << 8) | (sl->q_buf[2] << 16) |
 289             (sl->q_buf[3] << 24);
 290     return chip_id;
 291 }
 292
 293 /**
 294  * Cortex m3 tech ref manual, CPUID register description
 295  * @param sl stlink context
 296  * @param cpuid pointer to the result object
 297  */
 298 void stlink_cpu_id(stlink_t *sl, cortex_m3_cpuid_t *cpuid) {
 299     stlink_read_mem32(sl, CM3_REG_CPUID, 4);
 300     uint32_t raw = read_uint32(sl->q_buf, 0);
 301     cpuid->implementer_id = (raw >> 24) & 0x7f;
 302     cpuid->variant = (raw >> 20) & 0xf;
 303     cpuid->part = (raw >> 4) & 0xfff;
 304     cpuid->revision = raw & 0xf;
 305     return;
 306 }
 307
 308 void stlink_reset(stlink_t *sl) {
 309     DLOG("*** stlink_reset ***\n");
 310     sl->backend->reset(sl);
 311 }
 312
 313 void stlink_run(stlink_t *sl) {
 314     DLOG("*** stlink_run ***\n");
 315     sl->backend->run(sl);
 316 }
 317
 318 void stlink_status(stlink_t *sl) {
 319     DLOG("*** stlink_status ***\n");
 320     sl->backend->status(sl);
 321     stlink_core_stat(sl);
 322 }
 323
 324 /**
 325  * Decode the version bits, originally from -sg, verified with usb
 326  * @param sl stlink context, assumed to contain valid data in the buffer
 327  * @param slv output parsed version object
 328  */
 329 void _parse_version(stlink_t *sl, stlink_version_t *slv) {
 330     uint32_t b0 = sl->q_buf[0]; //lsb
 331     uint32_t b1 = sl->q_buf[1];
 332     uint32_t b2 = sl->q_buf[2];
 333     uint32_t b3 = sl->q_buf[3];
 334     uint32_t b4 = sl->q_buf[4];
 335     uint32_t b5 = sl->q_buf[5]; //msb
 336
 337     // b0 b1                       || b2 b3  | b4 b5
 338     // 4b        | 6b     | 6b     || 2B     | 2B
 339     // stlink_v  | jtag_v | swim_v || st_vid | stlink_pid
 340
 341     slv->stlink_v = (b0 & 0xf0) >> 4;
 342     slv->jtag_v = ((b0 & 0x0f) << 2) | ((b1 & 0xc0) >> 6);
 343     slv->swim_v = b1 & 0x3f;
 344     slv->st_vid = (b3 << 8) | b2;
 345     slv->stlink_pid = (b5 << 8) | b4;
 346     return;
 347 }
 348
 349 void stlink_version(stlink_t *sl) {
 350     DLOG("*** looking up stlink version\n");
 351     sl->backend->version(sl);
 352     _parse_version(sl, &sl->version);
 353
 354     DLOG("st vid         = 0x%04x (expect 0x%04x)\n", sl->version.st_vid, USB_ST_VID);
 355     DLOG("stlink pid     = 0x%04x\n", sl->version.stlink_pid);
 356     DLOG("stlink version = 0x%x\n", sl->version.stlink_v);
 357     DLOG("jtag version   = 0x%x\n", sl->version.jtag_v);
 358     DLOG("swim version   = 0x%x\n", sl->version.swim_v);
 359     if (sl->version.jtag_v == 0) {
 360         DLOG("    notice: the firmware doesn't support a jtag/swd interface\n");
 361     }
 362     if (sl->version.swim_v == 0) {
 363         DLOG("    notice: the firmware doesn't support a swim interface\n");
 364     }
 365 }
 366
 367 void stlink_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 368     DLOG("*** stlink_write_mem32 ***\n");
 369     if (len % 4 != 0) {
 370         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n", len % 4);
 371         return;
 372     }
 373     sl->backend->write_mem32(sl, addr, len);
 374 }
 375
 376 void stlink_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
 377     DLOG("*** stlink_read_mem32 ***\n");
 378     if (len % 4 != 0) { // !!! never ever: fw gives just wrong values
 379         fprintf(stderr, "Error: Data length doesn't have a 32 bit alignment: +%d byte.\n",
 380                 len % 4);
 381         return;
 382     }
 383     sl->backend->read_mem32(sl, addr, len);
 384 }
 385
 386 void stlink_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
 387     DLOG("*** stlink_write_mem8 ***\n");
 388     sl->backend->write_mem8(sl, addr, len);
 389 }
 390
 391 void stlink_read_all_regs(stlink_t *sl, reg *regp) {
 392     DLOG("*** stlink_read_all_regs ***\n");
 393     sl->backend->read_all_regs(sl, regp);
 394 }
 395
 396 void stlink_write_reg(stlink_t *sl, uint32_t reg, int idx) {
 397     DLOG("*** stlink_write_reg\n");
 398     sl->backend->write_reg(sl, reg, idx);
 399 }
 400
 401 void stlink_read_reg(stlink_t *sl, int r_idx, reg *regp) {
 402     DLOG("*** stlink_read_reg\n");
 403     DLOG(" (%d) ***\n", r_idx);
 404
 405     if (r_idx > 20 || r_idx < 0) {
 406         fprintf(stderr, "Error: register index must be in [0..20]\n");
 407         return;
 408     }
 409
 410     sl->backend->read_reg(sl, r_idx, regp);
 411 }
 412
 413 unsigned int is_core_halted(stlink_t *sl) {
 414     /* return non zero if core is halted */
 415     stlink_status(sl);
 416     return sl->q_buf[0] == STLINK_CORE_HALTED;
 417 }
 418
 419 void stlink_step(stlink_t *sl) {
 420     DLOG("*** stlink_step ***\n");
 421     sl->backend->step(sl);
 422 }
 423
 424 int stlink_current_mode(stlink_t *sl) {
 425     int mode = sl->backend->current_mode(sl);
 426     switch (mode) {
 427         case STLINK_DEV_DFU_MODE:
 428             DLOG("stlink current mode: dfu\n");
 429             return mode;
 430         case STLINK_DEV_DEBUG_MODE:
 431             DLOG("stlink current mode: debug (jtag or swd)\n");
 432             return mode;
 433         case STLINK_DEV_MASS_MODE:
 434             DLOG("stlink current mode: mass\n");
 435             return mode;
 436     }
 437     DLOG("stlink mode: unknown!\n");
 438     return STLINK_DEV_UNKNOWN_MODE;
 439 }
 440
 441
 442
 443
 444 // End of delegates....  Common code below here...
 445
 446 // Endianness
 447 // http://www.ibm.com/developerworks/aix/library/au-endianc/index.html
 448 // const int i = 1;
 449 // #define is_bigendian() ( (*(char*)&i) == 0 )
 450
 451 inline unsigned int is_bigendian(void) {
 452     static volatile const unsigned int i = 1;
 453     return *(volatile const char*) &i == 0;
 454 }
 455
 456 uint16_t read_uint16(const unsigned char *c, const int pt) {
 457     uint32_t ui;
 458     char *p = (char *) &ui;
 459
 460     if (!is_bigendian()) { // le -> le (don't swap)
 461         p[0] = c[pt + 0];
 462         p[1] = c[pt + 1];
 463     } else {
 464         p[0] = c[pt + 1];
 465         p[1] = c[pt + 0];
 466     }
 467     return ui;
 468 }
 469
 470 // same as above with entrypoint.
 471
 472 void stlink_run_at(stlink_t *sl, stm32_addr_t addr) {
 473     stlink_write_reg(sl, addr, 15); /* pc register */
 474
 475     stlink_run(sl);
 476
 477     while (is_core_halted(sl) == 0)
 478         usleep(3000000);
 479 }
 480
 481 void stlink_core_stat(stlink_t *sl) {
 482     if (sl->q_len <= 0)
 483         return;
 484
 485     switch (sl->q_buf[0]) {
 486         case STLINK_CORE_RUNNING:
 487             sl->core_stat = STLINK_CORE_RUNNING;
 488             DLOG("  core status: running\n");
 489             return;
 490         case STLINK_CORE_HALTED:
 491             sl->core_stat = STLINK_CORE_HALTED;
 492             DLOG("  core status: halted\n");
 493             return;
 494         default:
 495             sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
 496             fprintf(stderr, "  core status: unknown\n");
 497     }
 498 }
 499
 500 void stlink_print_data(stlink_t * sl) {
 501     if (sl->q_len <= 0 || sl->verbose < 2)
 502         return;
 503     if (sl->verbose > 2)
 504         fprintf(stdout, "data_len = %d 0x%x\n", sl->q_len, sl->q_len);
 505
 506     for (int i = 0; i < sl->q_len; i++) {
 507         if (i % 16 == 0) {
 508             /*
 509                                     if (sl->q_data_dir == Q_DATA_OUT)
 510                                             fprintf(stdout, "\n<- 0x%08x ", sl->q_addr + i);
 511                                     else
 512                                             fprintf(stdout, "\n-> 0x%08x ", sl->q_addr + i);
 513              */
 514         }
 515         fprintf(stdout, " %02x", (unsigned int) sl->q_buf[i]);
 516     }
 517     fputs("\n\n", stdout);
 518 }
 519
 520 /* memory mapped file */
 521
 522 typedef struct mapped_file {
 523     uint8_t* base;
 524     size_t len;
 525 } mapped_file_t;
 526
 527 #define MAPPED_FILE_INITIALIZER { NULL, 0 }
 528
 529 static int map_file(mapped_file_t* mf, const char* path) {
 530     int error = -1;
 531     struct stat st;
 532
 533     const int fd = open(path, O_RDONLY);
 534     if (fd == -1) {
 535         fprintf(stderr, "open(%s) == -1\n", path);
 536         return -1;
 537     }
 538
 539     if (fstat(fd, &st) == -1) {
 540         fprintf(stderr, "fstat() == -1\n");
 541         goto on_error;
 542     }
 543
 544     mf->base = (uint8_t*) mmap(NULL, st.st_size, PROT_READ, MAP_SHARED, fd, 0);
 545     if (mf->base == MAP_FAILED) {
 546         fprintf(stderr, "mmap() == MAP_FAILED\n");
 547         goto on_error;
 548     }
 549
 550     mf->len = st.st_size;
 551
 552     /* success */
 553     error = 0;
 554
 555 on_error:
 556     close(fd);
 557
 558     return error;
 559 }
 560
 561 static void unmap_file(mapped_file_t * mf) {
 562     munmap((void*) mf->base, mf->len);
 563     mf->base = (unsigned char*) MAP_FAILED;
 564     mf->len = 0;
 565 }
 566
 567 static int check_file(stlink_t* sl, mapped_file_t* mf, stm32_addr_t addr) {
 568     size_t off;
 569
 570     for (off = 0; off < mf->len; off += sl->flash_pgsz) {
 571         size_t aligned_size;
 572
 573         /* adjust last page size */
 574         size_t cmp_size = sl->flash_pgsz;
 575         if ((off + sl->flash_pgsz) > mf->len)
 576             cmp_size = mf->len - off;
 577
 578         aligned_size = cmp_size;
 579         if (aligned_size & (4 - 1))
 580             aligned_size = (cmp_size + 4) & ~(4 - 1);
 581
 582         stlink_read_mem32(sl, addr + off, aligned_size);
 583
 584         if (memcmp(sl->q_buf, mf->base + off, cmp_size))
 585             return -1;
 586     }
 587
 588     return 0;
 589 }
 590
 591 int stlink_fwrite_sram
 592 (stlink_t * sl, const char* path, stm32_addr_t addr) {
 593     /* write the file in sram at addr */
 594
 595     int error = -1;
 596     size_t off;
 597     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 598
 599     if (map_file(&mf, path) == -1) {
 600         fprintf(stderr, "map_file() == -1\n");
 601         return -1;
 602     }
 603
 604     /* check addr range is inside the sram */
 605     if (addr < sl->sram_base) {
 606         fprintf(stderr, "addr too low\n");
 607         goto on_error;
 608     } else if ((addr + mf.len) < addr) {
 609         fprintf(stderr, "addr overruns\n");
 610         goto on_error;
 611     } else if ((addr + mf.len) > (sl->sram_base + sl->sram_size)) {
 612         fprintf(stderr, "addr too high\n");
 613         goto on_error;
 614     } else if ((addr & 3) || (mf.len & 3)) {
 615         /* todo */
 616         fprintf(stderr, "unaligned addr or size\n");
 617         goto on_error;
 618     }
 619
 620     /* do the copy by 1k blocks */
 621     for (off = 0; off < mf.len; off += 1024) {
 622         size_t size = 1024;
 623         if ((off + size) > mf.len)
 624             size = mf.len - off;
 625
 626         memcpy(sl->q_buf, mf.base + off, size);
 627
 628         /* round size if needed */
 629         if (size & 3)
 630             size += 2;
 631
 632         stlink_write_mem32(sl, addr + off, size);
 633     }
 634
 635     /* check the file ha been written */
 636     if (check_file(sl, &mf, addr) == -1) {
 637         fprintf(stderr, "check_file() == -1\n");
 638         goto on_error;
 639     }
 640
 641     /* success */
 642     error = 0;
 643
 644 on_error:
 645     unmap_file(&mf);
 646     return error;
 647 }
 648
 649 int stlink_fread(stlink_t* sl, const char* path, stm32_addr_t addr, size_t size) {
 650     /* read size bytes from addr to file */
 651
 652     int error = -1;
 653     size_t off;
 654
 655     const int fd = open(path, O_RDWR | O_TRUNC | O_CREAT, 00700);
 656     if (fd == -1) {
 657         fprintf(stderr, "open(%s) == -1\n", path);
 658         return -1;
 659     }
 660
 661     /* do the copy by 1k blocks */
 662     for (off = 0; off < size; off += 1024) {
 663         size_t read_size = 1024;
 664         size_t rounded_size;
 665         if ((off + read_size) > size)
 666           read_size = size - off;
 667
 668         /* round size if needed */
 669         rounded_size = read_size;
 670         if (rounded_size & 3)
 671           rounded_size = (rounded_size + 4) & ~(3);
 672
 673         stlink_read_mem32(sl, addr + off, rounded_size);
 674
 675         if (write(fd, sl->q_buf, read_size) != (ssize_t) read_size) {
 676             fprintf(stderr, "write() != read_size\n");
 677             goto on_error;
 678         }
 679     }
 680
 681     /* success */
 682     error = 0;
 683
 684 on_error:
 685     close(fd);
 686
 687     return error;
 688 }
 689
 690 int write_buffer_to_sram(stlink_t *sl, flash_loader_t* fl, const uint8_t* buf, size_t size) {
 691     /* write the buffer right after the loader */
 692     memcpy(sl->q_buf, buf, size);
 693     stlink_write_mem8(sl, fl->buf_addr, size);
 694     return 0;
 695 }
 696
 697 int stlink_erase_flash_page(stlink_t *sl, stm32_addr_t page)
 698 {
 699   /* page an addr in the page to erase */
 700
 701   stlink_core_id(sl);
 702   if (sl->core_id == STM32L_CORE_ID)
 703   {
 704 #define STM32L_FLASH_REGS_ADDR ((uint32_t)0x40023c00)
 705 #define STM32L_FLASH_ACR (STM32L_FLASH_REGS_ADDR + 0x00)
 706 #define STM32L_FLASH_PECR (STM32L_FLASH_REGS_ADDR + 0x04)
 707 #define STM32L_FLASH_PDKEYR (STM32L_FLASH_REGS_ADDR + 0x08)
 708 #define STM32L_FLASH_PEKEYR (STM32L_FLASH_REGS_ADDR + 0x0c)
 709 #define STM32L_FLASH_PRGKEYR (STM32L_FLASH_REGS_ADDR + 0x10)
 710 #define STM32L_FLASH_OPTKEYR (STM32L_FLASH_REGS_ADDR + 0x14)
 711 #define STM32L_FLASH_SR (STM32L_FLASH_REGS_ADDR + 0x18)
 712 #define STM32L_FLASH_OBR (STM32L_FLASH_REGS_ADDR + 0x0c)
 713 #define STM32L_FLASH_WRPR (STM32L_FLASH_REGS_ADDR + 0x20)
 714
 715     uint32_t val;
 716
 717     /* disable pecr protection */
 718     write_uint32(sl->q_buf, 0x89abcdef);
 719     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 720     write_uint32(sl->q_buf, 0x02030405);
 721     stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 722
 723     /* check pecr.pelock is cleared */
 724     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 725     val = read_uint32(sl->q_buf, 0);
 726     if (val & (1 << 0))
 727     {
 728       fprintf(stderr, "pecr.pelock not clear (0x%x)\n", val);
 729       return -1;
 730     }
 731
 732     /* unlock program memory */
 733     write_uint32(sl->q_buf, 0x8c9daebf);
 734     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 735     write_uint32(sl->q_buf, 0x13141516);
 736     stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
 737
 738     /* check pecr.prglock is cleared */
 739     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 740     val = read_uint32(sl->q_buf, 0);
 741     if (val & (1 << 1))
 742     {
 743       fprintf(stderr, "pecr.prglock not clear (0x%x)\n", val);
 744       return -1;
 745     }
 746
 747     /* unused: unlock the option byte block */
 748 #if 0
 749     write_uint32(sl->q_buf, 0xfbead9c8);
 750     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 751     write_uint32(sl->q_buf, 0x24252627);
 752     stlink_write_mem32(sl, STM32L_FLASH_OPTKEYR, sizeof(uint32_t));
 753
 754     /* check pecr.optlock is cleared */
 755     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 756     val = read_uint32(sl->q_buf, 0);
 757     if (val & (1 << 2))
 758     {
 759       fprintf(stderr, "pecr.prglock not clear\n");
 760       return -1;
 761     }
 762 #endif
 763
 764     /* set pecr.{erase,prog} */
 765     val |= (1 << 9) | (1 << 3);
 766     write_uint32(sl->q_buf, val);
 767     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 768
 769     /* wait for sr.busy to be cleared */
 770     while (1)
 771     {
 772       stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
 773       if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
 774     }
 775
 776     /* write 0 to the first word of the page to be erased */
 777     memset(sl->q_buf, 0, sizeof(uint32_t));
 778     stlink_write_mem32(sl, page, sizeof(uint32_t));
 779
 780     /* reset lock bits */
 781     stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 782     val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
 783     write_uint32(sl->q_buf, val);
 784     stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
 785   }
 786   else if (sl->core_id == STM32VL_CORE_ID)
 787   {
 788     /* wait for ongoing op to finish */
 789     wait_flash_busy(sl);
 790
 791     /* unlock if locked */
 792     unlock_flash_if(sl);
 793
 794     /* set the page erase bit */
 795     set_flash_cr_per(sl);
 796
 797     /* select the page to erase */
 798     write_flash_ar(sl, page);
 799
 800     /* start erase operation, reset by hw with bsy bit */
 801     set_flash_cr_strt(sl);
 802
 803     /* wait for completion */
 804     wait_flash_busy(sl);
 805
 806     /* relock the flash */
 807     lock_flash(sl);
 808   }
 809   else {
 810     fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
 811     return -1;
 812   }
 813
 814   /* todo: verify the erased page */
 815
 816   return 0;
 817 }
 818
 819 int stlink_erase_flash_mass(stlink_t *sl) {
 820     /* wait for ongoing op to finish */
 821     wait_flash_busy(sl);
 822
 823     /* unlock if locked */
 824     unlock_flash_if(sl);
 825
 826     /* set the mass erase bit */
 827     set_flash_cr_mer(sl);
 828
 829     /* start erase operation, reset by hw with bsy bit */
 830     set_flash_cr_strt(sl);
 831
 832     /* wait for completion */
 833     wait_flash_busy(sl);
 834
 835     /* relock the flash */
 836     lock_flash(sl);
 837
 838     /* todo: verify the erased memory */
 839
 840     return 0;
 841 }
 842
 843 int init_flash_loader(stlink_t *sl, flash_loader_t* fl) {
 844     size_t size;
 845
 846     /* allocate the loader in sram */
 847     if (write_loader_to_sram(sl, &fl->loader_addr, &size) == -1) {
 848         fprintf(stderr, "write_loader_to_sram() == -1\n");
 849         return -1;
 850     }
 851
 852     /* allocate a one page buffer in sram right after loader */
 853     fl->buf_addr = fl->loader_addr + size;
 854
 855     return 0;
 856 }
 857
 858 int write_loader_to_sram(stlink_t *sl, stm32_addr_t* addr, size_t* size) {
 859     /* from openocd, contrib/loaders/flash/stm32.s */
 860     static const uint8_t loader_code_stm32vl[] = {
 861         0x08, 0x4c, /* ldr      r4, STM32_FLASH_BASE */
 862         0x1c, 0x44, /* add      r4, r3 */
 863         /* write_half_word: */
 864         0x01, 0x23, /* movs     r3, #0x01 */
 865         0x23, 0x61, /* str      r3, [r4, #STM32_FLASH_CR_OFFSET] */
 866         0x30, 0xf8, 0x02, 0x3b, /* ldrh r3, [r0], #0x02 */
 867         0x21, 0xf8, 0x02, 0x3b, /* strh r3, [r1], #0x02 */
 868         /* busy: */
 869         0xe3, 0x68, /* ldr      r3, [r4, #STM32_FLASH_SR_OFFSET] */
 870         0x13, 0xf0, 0x01, 0x0f, /* tst  r3, #0x01 */
 871         0xfb, 0xd0, /* beq      busy */
 872         0x13, 0xf0, 0x14, 0x0f, /* tst  r3, #0x14 */
 873         0x01, 0xd1, /* bne      exit */
 874         0x01, 0x3a, /* subs     r2, r2, #0x01 */
 875         0xf0, 0xd1, /* bne      write_half_word */
 876         /* exit: */
 877         0x00, 0xbe, /* bkpt     #0x00 */
 878         0x00, 0x20, 0x02, 0x40, /* STM32_FLASH_BASE: .word 0x40022000 */
 879     };
 880
 881     static const uint8_t loader_code_stm32l[] = {
 882
 883       /* openocd.git/contrib/loaders/flash/stm32lx.S
 884          r0, input, dest addr
 885          r1, input, source addr
 886          r2, input, word count
 887          r3, output, word count
 888        */
 889
 890       0x00, 0x23,
 891       0x04, 0xe0,
 892
 893       0x51, 0xf8, 0x04, 0xcb,
 894       0x40, 0xf8, 0x04, 0xcb,
 895       0x01, 0x33,
 896
 897       0x93, 0x42,
 898       0xf8, 0xd3,
 899       0x00, 0xbe
 900     };
 901
 902     const uint8_t* loader_code;
 903     size_t loader_size;
 904
 905     if (sl->core_id == STM32L_CORE_ID) /* stm32l */
 906     {
 907       loader_code = loader_code_stm32l;
 908       loader_size = sizeof(loader_code_stm32l);
 909     }
 910     else if (sl->core_id == STM32VL_CORE_ID)
 911     {
 912       loader_code = loader_code_stm32vl;
 913       loader_size = sizeof(loader_code_stm32vl);
 914     }
 915     else
 916     {
 917       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
 918       return -1;
 919     }
 920
 921     memcpy(sl->q_buf, loader_code, loader_size);
 922     stlink_write_mem32(sl, sl->sram_base, loader_size);
 923
 924     *addr = sl->sram_base;
 925     *size = loader_size;
 926
 927     /* success */
 928     return 0;
 929 }
 930
 931 int stlink_fcheck_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
 932     /* check the contents of path are at addr */
 933
 934     int res;
 935     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
 936
 937     if (map_file(&mf, path) == -1)
 938         return -1;
 939
 940     res = check_file(sl, &mf, addr);
 941
 942     unmap_file(&mf);
 943
 944     return res;
 945 }
 946
 947
 948 int stlink_write_flash(stlink_t *sl, stm32_addr_t addr, uint8_t* base, unsigned len) {
 949     size_t off;
 950     flash_loader_t fl;
 951
 952     /* check addr range is inside the flash */
 953     if (addr < sl->flash_base) {
 954         fprintf(stderr, "addr too low\n");
 955         return -1;
 956     } else if ((addr + len) < addr) {
 957         fprintf(stderr, "addr overruns\n");
 958         return -1;
 959     } else if ((addr + len) > (sl->flash_base + sl->flash_size)) {
 960         fprintf(stderr, "addr too high\n");
 961         return -1;
 962     } else if ((addr & 1) || (len & 1)) {
 963         fprintf(stderr, "unaligned addr or size\n");
 964         return -1;
 965     } else if (addr & (sl->flash_pgsz - 1)) {
 966         fprintf(stderr, "addr not a multiple of pagesize, not supported\n");
 967         return -1;
 968     }
 969
 970     /* erase each page */
 971     for (off = 0; off < len; off += sl->flash_pgsz) {
 972         /* addr must be an addr inside the page */
 973         if (stlink_erase_flash_page(sl, addr + off) == -1) {
 974             fprintf(stderr, "erase_flash_page(0x%zx) == -1\n", addr + off);
 975             return -1;
 976         }
 977     }
 978
 979     stlink_core_id(sl);
 980     if (sl->core_id == STM32L_CORE_ID)
 981     {
 982       /* use fast word write. todo: half page. */
 983
 984       uint32_t val;
 985
 986 #if 0 /* todo: check write operation */
 987
 988       uint32_t nwrites = sl->flash_pgsz;
 989
 990     redo_write:
 991
 992 #endif /* todo: check write operation */
 993
 994       /* disable pecr protection */
 995       write_uint32(sl->q_buf, 0x89abcdef);
 996       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 997       write_uint32(sl->q_buf, 0x02030405);
 998       stlink_write_mem32(sl, STM32L_FLASH_PEKEYR, sizeof(uint32_t));
 999
1000       /* check pecr.pelock is cleared */
1001       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1002       val = read_uint32(sl->q_buf, 0);
1003       if (val & (1 << 0))
1004       {
1005         fprintf(stderr, "pecr.pelock not clear\n");
1006         return -1;
1007       }
1008
1009       /* unlock program memory */
1010       write_uint32(sl->q_buf, 0x8c9daebf);
1011       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1012       write_uint32(sl->q_buf, 0x13141516);
1013       stlink_write_mem32(sl, STM32L_FLASH_PRGKEYR, sizeof(uint32_t));
1014
1015       /* check pecr.prglock is cleared */
1016       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1017       val = read_uint32(sl->q_buf, 0);
1018       if (val & (1 << 1))
1019       {
1020         fprintf(stderr, "pecr.prglock not clear\n");
1021         return -1;
1022       }
1023
1024       /* write a word in program memory */
1025       for (off = 0; off < len; off += sizeof(uint32_t))
1026       {
1027         if (sl->verbose >= 1)
1028         {
1029           if ((off & (sl->flash_pgsz - 1)) == 0)
1030           {
1031             /* show progress. writing procedure is slow
1032                and previous errors are misleading */
1033             const uint32_t pgnum = off / sl->flash_pgsz;
1034             const uint32_t pgcount = len / sl->flash_pgsz;
1035             fprintf(stdout, "%u pages written out of %u\n", pgnum, pgcount);
1036           }
1037         }
1038
1039         memcpy(sl->q_buf, (const void*)(base + off), sizeof(uint32_t));
1040         stlink_write_mem32(sl, addr + off, sizeof(uint32_t));
1041
1042         /* wait for sr.busy to be cleared */
1043         while (1)
1044         {
1045           stlink_read_mem32(sl, STM32L_FLASH_SR, sizeof(uint32_t));
1046           if ((read_uint32(sl->q_buf, 0) & (1 << 0)) == 0) break ;
1047         }
1048
1049 #if 0 /* todo: check redo write operation */
1050
1051         /* check written bytes. todo: should be on a per page basis. */
1052         stlink_read_mem32(sl, addr + off, sizeof(uint32_t));
1053         if (memcmp(sl->q_buf, base + off, sizeof(uint32_t)))
1054         {
1055           /* re erase the page and redo the write operation */
1056           uint32_t page;
1057           uint32_t val;
1058
1059           /* fail if successive write count too low */
1060           if (nwrites < sl->flash_pgsz) {
1061             fprintf(stderr, "writes operation failure count too high, aborting\n");
1062             return -1;
1063           }
1064
1065           nwrites = 0;
1066
1067           /* assume addr aligned */
1068           if (off % sl->flash_pgsz) off &= ~(sl->flash_pgsz - 1);
1069           page = addr + off;
1070
1071           fprintf(stderr, "invalid write @%x(%x): %x != %x. retrying.\n",
1072                   page, addr + off, read_uint32(base + off, 0), read_uint32(sl->q_buf, 0));
1073
1074           /* reset lock bits */
1075           stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1076           val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1077           write_uint32(sl->q_buf, val);
1078           stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1079
1080           stlink_erase_flash_page(sl, page);
1081
1082           goto redo_write;
1083         }
1084
1085         /* increment successive writes counter */
1086         ++nwrites;
1087
1088 #endif /* todo: check redo write operation */
1089
1090       }
1091
1092       /* reset lock bits */
1093       stlink_read_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1094       val = read_uint32(sl->q_buf, 0) | (1 << 0) | (1 << 1) | (1 << 2);
1095       write_uint32(sl->q_buf, val);
1096       stlink_write_mem32(sl, STM32L_FLASH_PECR, sizeof(uint32_t));
1097     }
1098     else if (sl->core_id == STM32VL_CORE_ID)
1099     {
1100       /* flash loader initialization */
1101       if (init_flash_loader(sl, &fl) == -1) {
1102         fprintf(stderr, "init_flash_loader() == -1\n");
1103         return -1;
1104       }
1105
1106       /* write each page. above WRITE_BLOCK_SIZE fails? */
1107 #define WRITE_BLOCK_SIZE 0x40
1108       for (off = 0; off < len; off += WRITE_BLOCK_SIZE)
1109       {
1110         /* adjust last write size */
1111         size_t size = WRITE_BLOCK_SIZE;
1112         if ((off + WRITE_BLOCK_SIZE) > len) size = len - off;
1113
1114         /* unlock and set programming mode */
1115         unlock_flash_if(sl);
1116         set_flash_cr_pg(sl);
1117
1118         if (run_flash_loader(sl, &fl, addr + off, base + off, size) == -1) {
1119           fprintf(stderr, "run_flash_loader(0x%zx) == -1\n", addr + off);
1120           return -1;
1121         }
1122
1123         lock_flash(sl);
1124       }
1125     } else {
1126       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1127       return -1;
1128     }
1129
1130     for (off = 0; off < len; off += sl->flash_pgsz) {
1131         size_t aligned_size;
1132
1133         /* adjust last page size */
1134         size_t cmp_size = sl->flash_pgsz;
1135         if ((off + sl->flash_pgsz) > len)
1136             cmp_size = len - off;
1137
1138         aligned_size = cmp_size;
1139         if (aligned_size & (4 - 1))
1140             aligned_size = (cmp_size + 4) & ~(4 - 1);
1141
1142         stlink_read_mem32(sl, addr + off, aligned_size);
1143
1144         if (memcmp(sl->q_buf, base + off, cmp_size))
1145             return -1;
1146     }
1147
1148     return 0;
1149 }
1150
1151 int stlink_fwrite_flash(stlink_t *sl, const char* path, stm32_addr_t addr) {
1152     /* write the file in flash at addr */
1153
1154     int err;
1155     mapped_file_t mf = MAPPED_FILE_INITIALIZER;
1156
1157     if (map_file(&mf, path) == -1) {
1158         fprintf(stderr, "map_file() == -1\n");
1159         return -1;
1160     }
1161
1162     err = stlink_write_flash(sl, addr, mf.base, mf.len);
1163
1164     unmap_file(&mf);
1165
1166     return err;
1167 }
1168
1169 int run_flash_loader(stlink_t *sl, flash_loader_t* fl, stm32_addr_t target, const uint8_t* buf, size_t size) {
1170
1171     reg rr;
1172
1173     if (write_buffer_to_sram(sl, fl, buf, size) == -1) {
1174         fprintf(stderr, "write_buffer_to_sram() == -1\n");
1175         return -1;
1176     }
1177
1178     if (sl->core_id == STM32L_CORE_ID) {
1179
1180       size_t count = size / sizeof(uint32_t);
1181       if (size % sizeof(uint32_t)) ++count;
1182
1183       /* setup core */
1184       stlink_write_reg(sl, target, 0); /* target */
1185       stlink_write_reg(sl, fl->buf_addr, 1); /* source */
1186       stlink_write_reg(sl, count, 2); /* count (32 bits words) */
1187       stlink_write_reg(sl, 0, 3); /* output count */
1188       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1189
1190     } else if (sl->core_id == STM32VL_CORE_ID) {
1191
1192       size_t count = size / sizeof(uint16_t);
1193       if (size % sizeof(uint16_t)) ++count;
1194
1195       /* setup core */
1196       stlink_write_reg(sl, fl->buf_addr, 0); /* source */
1197       stlink_write_reg(sl, target, 1); /* target */
1198       stlink_write_reg(sl, count, 2); /* count (16 bits half words) */
1199       stlink_write_reg(sl, 0, 3); /* flash bank 0 (input) */
1200       stlink_write_reg(sl, fl->loader_addr, 15); /* pc register */
1201
1202     } else {
1203       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1204       return -1;
1205     }
1206
1207     /* run loader */
1208     stlink_step(sl);
1209
1210     /* wait until done (reaches breakpoint) */
1211     while (is_core_halted(sl) == 0) ;
1212
1213     /* check written byte count */
1214     if (sl->core_id == STM32L_CORE_ID) {
1215
1216       size_t count = size / sizeof(uint32_t);
1217       if (size % sizeof(uint32_t)) ++count;
1218
1219       stlink_read_reg(sl, 3, &rr);
1220       if (rr.r[3] != count) {
1221         fprintf(stderr, "write error, count == %u\n", rr.r[3]);
1222         return -1;
1223       }
1224
1225     } else if (sl->core_id == STM32VL_CORE_ID) {
1226
1227       stlink_read_reg(sl, 2, &rr);
1228       if (rr.r[2] != 0) {
1229         fprintf(stderr, "write error, count == %u\n", rr.r[2]);
1230         return -1;
1231       }
1232
1233     } else {
1234
1235       fprintf(stderr, "unknown coreid: %x\n", sl->core_id);
1236       return -1;
1237
1238     }
1239
1240     return 0;
1241 }