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