]> git.sur5r.net Git - openocd/blob - src/flash/stm32x.c
David Brownell <david-b@pacbell.net>:
[openocd] / src / flash / stm32x.c
1 /***************************************************************************
2  *   Copyright (C) 2005 by Dominic Rath                                    *
3  *   Dominic.Rath@gmx.de                                                   *
4  *                                                                         *
5  *   Copyright (C) 2008 by Spencer Oliver                                  *
6  *   spen@spen-soft.co.uk                                                  *
7  *                                                                         *
8  *   This program is free software; you can redistribute it and/or modify  *
9  *   it under the terms of the GNU General Public License as published by  *
10  *   the Free Software Foundation; either version 2 of the License, or     *
11  *   (at your option) any later version.                                   *
12  *                                                                         *
13  *   This program is distributed in the hope that it will be useful,       *
14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
16  *   GNU General Public License for more details.                          *
17  *                                                                         *
18  *   You should have received a copy of the GNU General Public License     *
19  *   along with this program; if not, write to the                         *
20  *   Free Software Foundation, Inc.,                                       *
21  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
22  ***************************************************************************/
23 #ifdef HAVE_CONFIG_H
24 #include "config.h"
25 #endif
26
27 #include "stm32x.h"
28 #include "armv7m.h"
29 #include "binarybuffer.h"
30
31
32 static int stm32x_register_commands(struct command_context_s *cmd_ctx);
33 static int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
34 static int stm32x_erase(struct flash_bank_s *bank, int first, int last);
35 static int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last);
36 static int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
37 static int stm32x_probe(struct flash_bank_s *bank);
38 static int stm32x_auto_probe(struct flash_bank_s *bank);
39 //static int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
40 static int stm32x_protect_check(struct flash_bank_s *bank);
41 static int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size);
42
43 static int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
44 static int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
45 static int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
46 static int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
47 static int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
48 static int stm32x_mass_erase(struct flash_bank_s *bank);
49
50 flash_driver_t stm32x_flash =
51 {
52         .name = "stm32x",
53         .register_commands = stm32x_register_commands,
54         .flash_bank_command = stm32x_flash_bank_command,
55         .erase = stm32x_erase,
56         .protect = stm32x_protect,
57         .write = stm32x_write,
58         .probe = stm32x_probe,
59         .auto_probe = stm32x_auto_probe,
60         .erase_check = default_flash_mem_blank_check,
61         .protect_check = stm32x_protect_check,
62         .info = stm32x_info
63 };
64
65 static int stm32x_register_commands(struct command_context_s *cmd_ctx)
66 {
67         command_t *stm32x_cmd = register_command(cmd_ctx, NULL, "stm32x", NULL, COMMAND_ANY, "stm32x flash specific commands");
68
69         register_command(cmd_ctx, stm32x_cmd, "lock", stm32x_handle_lock_command, COMMAND_EXEC,
70                                          "lock device");
71         register_command(cmd_ctx, stm32x_cmd, "unlock", stm32x_handle_unlock_command, COMMAND_EXEC,
72                                          "unlock protected device");
73         register_command(cmd_ctx, stm32x_cmd, "mass_erase", stm32x_handle_mass_erase_command, COMMAND_EXEC,
74                                          "mass erase device");
75         register_command(cmd_ctx, stm32x_cmd, "options_read", stm32x_handle_options_read_command, COMMAND_EXEC,
76                                          "read device option bytes");
77         register_command(cmd_ctx, stm32x_cmd, "options_write", stm32x_handle_options_write_command, COMMAND_EXEC,
78                                          "write device option bytes");
79         return ERROR_OK;
80 }
81
82 /* flash bank stm32x <base> <size> 0 0 <target#>
83  */
84 static int stm32x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
85 {
86         stm32x_flash_bank_t *stm32x_info;
87
88         if (argc < 6)
89         {
90                 LOG_WARNING("incomplete flash_bank stm32x configuration");
91                 return ERROR_FLASH_BANK_INVALID;
92         }
93
94         stm32x_info = malloc(sizeof(stm32x_flash_bank_t));
95         bank->driver_priv = stm32x_info;
96
97         stm32x_info->write_algorithm = NULL;
98         stm32x_info->probed = 0;
99
100         return ERROR_OK;
101 }
102
103 static u32 stm32x_get_flash_status(flash_bank_t *bank)
104 {
105         target_t *target = bank->target;
106         u32 status;
107
108         target_read_u32(target, STM32_FLASH_SR, &status);
109
110         return status;
111 }
112
113 static u32 stm32x_wait_status_busy(flash_bank_t *bank, int timeout)
114 {
115         target_t *target = bank->target;
116         u32 status;
117
118         /* wait for busy to clear */
119         while (((status = stm32x_get_flash_status(bank)) & FLASH_BSY) && (timeout-- > 0))
120         {
121                 LOG_DEBUG("status: 0x%x", status);
122                 alive_sleep(1);
123         }
124         /* Clear but report errors */
125         if (status & (FLASH_WRPRTERR|FLASH_PGERR))
126         {
127                 target_write_u32(target, STM32_FLASH_SR, FLASH_WRPRTERR|FLASH_PGERR);
128         }
129         return status;
130 }
131
132 static int stm32x_read_options(struct flash_bank_s *bank)
133 {
134         u32 optiondata;
135         stm32x_flash_bank_t *stm32x_info = NULL;
136         target_t *target = bank->target;
137
138         stm32x_info = bank->driver_priv;
139
140         /* read current option bytes */
141         target_read_u32(target, STM32_FLASH_OBR, &optiondata);
142
143         stm32x_info->option_bytes.user_options = (u16)0xFFF8|((optiondata >> 2) & 0x07);
144         stm32x_info->option_bytes.RDP = (optiondata & (1 << OPT_READOUT)) ? 0xFFFF : 0x5AA5;
145
146         if (optiondata & (1 << OPT_READOUT))
147                 LOG_INFO("Device Security Bit Set");
148
149         /* each bit refers to a 4bank protection */
150         target_read_u32(target, STM32_FLASH_WRPR, &optiondata);
151
152         stm32x_info->option_bytes.protection[0] = (u16)optiondata;
153         stm32x_info->option_bytes.protection[1] = (u16)(optiondata >> 8);
154         stm32x_info->option_bytes.protection[2] = (u16)(optiondata >> 16);
155         stm32x_info->option_bytes.protection[3] = (u16)(optiondata >> 24);
156
157         return ERROR_OK;
158 }
159
160 static int stm32x_erase_options(struct flash_bank_s *bank)
161 {
162         stm32x_flash_bank_t *stm32x_info = NULL;
163         target_t *target = bank->target;
164         u32 status;
165
166         stm32x_info = bank->driver_priv;
167
168         /* read current options */
169         stm32x_read_options(bank);
170
171         /* unlock flash registers */
172         target_write_u32(target, STM32_FLASH_KEYR, KEY1);
173         target_write_u32(target, STM32_FLASH_KEYR, KEY2);
174
175         /* unlock option flash registers */
176         target_write_u32(target, STM32_FLASH_OPTKEYR, KEY1);
177         target_write_u32(target, STM32_FLASH_OPTKEYR, KEY2);
178
179         /* erase option bytes */
180         target_write_u32(target, STM32_FLASH_CR, FLASH_OPTER|FLASH_OPTWRE);
181         target_write_u32(target, STM32_FLASH_CR, FLASH_OPTER|FLASH_STRT|FLASH_OPTWRE);
182
183         status = stm32x_wait_status_busy(bank, 10);
184
185         if( status & FLASH_WRPRTERR )
186                 return ERROR_FLASH_OPERATION_FAILED;
187         if( status & FLASH_PGERR )
188                 return ERROR_FLASH_OPERATION_FAILED;
189
190         /* clear readout protection and complementary option bytes
191          * this will also force a device unlock if set */
192         stm32x_info->option_bytes.RDP = 0x5AA5;
193
194         return ERROR_OK;
195 }
196
197 static int stm32x_write_options(struct flash_bank_s *bank)
198 {
199         stm32x_flash_bank_t *stm32x_info = NULL;
200         target_t *target = bank->target;
201         u32 status;
202
203         stm32x_info = bank->driver_priv;
204
205         /* unlock flash registers */
206         target_write_u32(target, STM32_FLASH_KEYR, KEY1);
207         target_write_u32(target, STM32_FLASH_KEYR, KEY2);
208
209         /* unlock option flash registers */
210         target_write_u32(target, STM32_FLASH_OPTKEYR, KEY1);
211         target_write_u32(target, STM32_FLASH_OPTKEYR, KEY2);
212
213         /* program option bytes */
214         target_write_u32(target, STM32_FLASH_CR, FLASH_OPTPG|FLASH_OPTWRE);
215
216         /* write user option byte */
217         target_write_u16(target, STM32_OB_USER, stm32x_info->option_bytes.user_options);
218
219         status = stm32x_wait_status_busy(bank, 10);
220
221         if( status & FLASH_WRPRTERR )
222                 return ERROR_FLASH_OPERATION_FAILED;
223         if( status & FLASH_PGERR )
224                 return ERROR_FLASH_OPERATION_FAILED;
225
226         /* write protection byte 1 */
227         target_write_u16(target, STM32_OB_WRP0, stm32x_info->option_bytes.protection[0]);
228
229         status = stm32x_wait_status_busy(bank, 10);
230
231         if( status & FLASH_WRPRTERR )
232                 return ERROR_FLASH_OPERATION_FAILED;
233         if( status & FLASH_PGERR )
234                 return ERROR_FLASH_OPERATION_FAILED;
235
236         /* write protection byte 2 */
237         target_write_u16(target, STM32_OB_WRP1, stm32x_info->option_bytes.protection[1]);
238
239         status = stm32x_wait_status_busy(bank, 10);
240
241         if( status & FLASH_WRPRTERR )
242                 return ERROR_FLASH_OPERATION_FAILED;
243         if( status & FLASH_PGERR )
244                 return ERROR_FLASH_OPERATION_FAILED;
245
246         /* write protection byte 3 */
247         target_write_u16(target, STM32_OB_WRP2, stm32x_info->option_bytes.protection[2]);
248
249         status = stm32x_wait_status_busy(bank, 10);
250
251         if( status & FLASH_WRPRTERR )
252                 return ERROR_FLASH_OPERATION_FAILED;
253         if( status & FLASH_PGERR )
254                 return ERROR_FLASH_OPERATION_FAILED;
255
256         /* write protection byte 4 */
257         target_write_u16(target, STM32_OB_WRP3, stm32x_info->option_bytes.protection[3]);
258
259         status = stm32x_wait_status_busy(bank, 10);
260
261         if( status & FLASH_WRPRTERR )
262                 return ERROR_FLASH_OPERATION_FAILED;
263         if( status & FLASH_PGERR )
264                 return ERROR_FLASH_OPERATION_FAILED;
265
266         /* write readout protection bit */
267         target_write_u16(target, STM32_OB_RDP, stm32x_info->option_bytes.RDP);
268
269         status = stm32x_wait_status_busy(bank, 10);
270
271         if( status & FLASH_WRPRTERR )
272                 return ERROR_FLASH_OPERATION_FAILED;
273         if( status & FLASH_PGERR )
274                 return ERROR_FLASH_OPERATION_FAILED;
275
276         target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
277
278         return ERROR_OK;
279 }
280
281 static int stm32x_protect_check(struct flash_bank_s *bank)
282 {
283         target_t *target = bank->target;
284         stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
285
286         u32 protection;
287         int i, s;
288         int num_bits;
289         int set;
290
291         if (target->state != TARGET_HALTED)
292         {
293                 LOG_ERROR("Target not halted");
294                 return ERROR_TARGET_NOT_HALTED;
295         }
296
297         /* medium density - each bit refers to a 4bank protection
298          * high density - each bit refers to a 2bank protection */
299         target_read_u32(target, STM32_FLASH_WRPR, &protection);
300
301         /* medium density - each protection bit is for 4 * 1K pages
302          * high density - each protection bit is for 2 * 2K pages */
303         num_bits = (bank->num_sectors / stm32x_info->ppage_size);
304
305         if (stm32x_info->ppage_size == 2)
306         {
307                 /* high density flash */
308
309                 set = 1;
310
311                 if (protection & (1 << 31))
312                         set = 0;
313
314                 /* bit 31 controls sector 62 - 255 protection */
315                 for (s = 62; s < bank->num_sectors; s++)
316                 {
317                         bank->sectors[s].is_protected = set;
318                 }
319
320                 if (bank->num_sectors > 61)
321                         num_bits = 31;
322
323                 for (i = 0; i < num_bits; i++)
324                 {
325                         set = 1;
326
327                         if (protection & (1 << i))
328                                 set = 0;
329
330                         for (s = 0; s < stm32x_info->ppage_size; s++)
331                                 bank->sectors[(i * stm32x_info->ppage_size) + s].is_protected = set;
332                 }
333         }
334         else
335         {
336                 /* medium density flash */
337                 for (i = 0; i < num_bits; i++)
338                 {
339                         set = 1;
340
341                         if( protection & (1 << i))
342                                 set = 0;
343
344                         for (s = 0; s < stm32x_info->ppage_size; s++)
345                                 bank->sectors[(i * stm32x_info->ppage_size) + s].is_protected = set;
346                 }
347         }
348
349         return ERROR_OK;
350 }
351
352 static int stm32x_erase(struct flash_bank_s *bank, int first, int last)
353 {
354         target_t *target = bank->target;
355         int i;
356         u32 status;
357
358         if (bank->target->state != TARGET_HALTED)
359         {
360                 LOG_ERROR("Target not halted");
361                 return ERROR_TARGET_NOT_HALTED;
362         }
363
364         if ((first == 0) && (last == (bank->num_sectors - 1)))
365         {
366                 return stm32x_mass_erase(bank);
367         }
368
369         /* unlock flash registers */
370         target_write_u32(target, STM32_FLASH_KEYR, KEY1);
371         target_write_u32(target, STM32_FLASH_KEYR, KEY2);
372
373         for (i = first; i <= last; i++)
374         {
375                 target_write_u32(target, STM32_FLASH_CR, FLASH_PER);
376                 target_write_u32(target, STM32_FLASH_AR, bank->base + bank->sectors[i].offset);
377                 target_write_u32(target, STM32_FLASH_CR, FLASH_PER|FLASH_STRT);
378
379                 status = stm32x_wait_status_busy(bank, 10);
380
381                 if( status & FLASH_WRPRTERR )
382                         return ERROR_FLASH_OPERATION_FAILED;
383                 if( status & FLASH_PGERR )
384                         return ERROR_FLASH_OPERATION_FAILED;
385                 bank->sectors[i].is_erased = 1;
386         }
387
388         target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
389
390         return ERROR_OK;
391 }
392
393 static int stm32x_protect(struct flash_bank_s *bank, int set, int first, int last)
394 {
395         stm32x_flash_bank_t *stm32x_info = NULL;
396         target_t *target = bank->target;
397         u16 prot_reg[4] = {0xFFFF, 0xFFFF, 0xFFFF, 0xFFFF};
398         int i, reg, bit;
399         int status;
400         u32 protection;
401
402         stm32x_info = bank->driver_priv;
403
404         if (target->state != TARGET_HALTED)
405         {
406                 LOG_ERROR("Target not halted");
407                 return ERROR_TARGET_NOT_HALTED;
408         }
409
410         if ((first && (first % stm32x_info->ppage_size)) || ((last + 1) && (last + 1) % stm32x_info->ppage_size))
411         {
412                 LOG_WARNING("sector start/end incorrect - stm32 has %dK sector protection", stm32x_info->ppage_size);
413                 return ERROR_FLASH_SECTOR_INVALID;
414         }
415
416         /* medium density - each bit refers to a 4bank protection
417          * high density - each bit refers to a 2bank protection */
418         target_read_u32(target, STM32_FLASH_WRPR, &protection);
419
420         prot_reg[0] = (u16)protection;
421         prot_reg[1] = (u16)(protection >> 8);
422         prot_reg[2] = (u16)(protection >> 16);
423         prot_reg[3] = (u16)(protection >> 24);
424
425         if (stm32x_info->ppage_size == 2)
426         {
427                 /* high density flash */
428
429                 /* bit 7 controls sector 62 - 255 protection */
430                 if (last > 61)
431                 {
432                         if (set)
433                                 prot_reg[3] &= ~(1 << 7);
434                         else
435                                 prot_reg[3] |= (1 << 7);
436                 }
437
438                 if (first > 61)
439                         first = 62;
440                 if (last > 61)
441                         last = 61;
442
443                 for (i = first; i <= last; i++)
444                 {
445                         reg = (i / stm32x_info->ppage_size) / 8;
446                         bit = (i / stm32x_info->ppage_size) - (reg * 8);
447
448                         if( set )
449                                 prot_reg[reg] &= ~(1 << bit);
450                         else
451                                 prot_reg[reg] |= (1 << bit);
452                 }
453         }
454         else
455         {
456                 /* medium density flash */
457                 for (i = first; i <= last; i++)
458                 {
459                         reg = (i / stm32x_info->ppage_size) / 8;
460                         bit = (i / stm32x_info->ppage_size) - (reg * 8);
461
462                         if( set )
463                                 prot_reg[reg] &= ~(1 << bit);
464                         else
465                                 prot_reg[reg] |= (1 << bit);
466                 }
467         }
468
469         if ((status = stm32x_erase_options(bank)) != ERROR_OK)
470                 return status;
471
472         stm32x_info->option_bytes.protection[0] = prot_reg[0];
473         stm32x_info->option_bytes.protection[1] = prot_reg[1];
474         stm32x_info->option_bytes.protection[2] = prot_reg[2];
475         stm32x_info->option_bytes.protection[3] = prot_reg[3];
476
477         return stm32x_write_options(bank);
478 }
479
480 static int stm32x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
481 {
482         stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
483         target_t *target = bank->target;
484         u32 buffer_size = 16384;
485         working_area_t *source;
486         u32 address = bank->base + offset;
487         reg_param_t reg_params[4];
488         armv7m_algorithm_t armv7m_info;
489         int retval = ERROR_OK;
490
491         u8 stm32x_flash_write_code[] = {
492                                                                         /* write: */
493                 0xDF, 0xF8, 0x24, 0x40,         /* ldr  r4, STM32_FLASH_CR */
494                 0x09, 0x4D,                                     /* ldr  r5, STM32_FLASH_SR */
495                 0x4F, 0xF0, 0x01, 0x03,         /* mov  r3, #1 */
496                 0x23, 0x60,                                     /* str  r3, [r4, #0] */
497                 0x30, 0xF8, 0x02, 0x3B,         /* ldrh r3, [r0], #2 */
498                 0x21, 0xF8, 0x02, 0x3B,         /* strh r3, [r1], #2 */
499                                                                         /* busy: */
500                 0x2B, 0x68,                                     /* ldr  r3, [r5, #0] */
501                 0x13, 0xF0, 0x01, 0x0F,         /* tst  r3, #0x01 */
502                 0xFB, 0xD0,                                     /* beq  busy */
503                 0x13, 0xF0, 0x14, 0x0F,         /* tst  r3, #0x14 */
504                 0x01, 0xD1,                                     /* bne  exit */
505                 0x01, 0x3A,                                     /* subs r2, r2, #1 */
506                 0xED, 0xD1,                                     /* bne  write */
507                                                                         /* exit: */
508                 0xFE, 0xE7,                                     /* b exit */
509                 0x10, 0x20, 0x02, 0x40,         /* STM32_FLASH_CR:      .word 0x40022010 */
510                 0x0C, 0x20, 0x02, 0x40          /* STM32_FLASH_SR:      .word 0x4002200C */
511         };
512
513         /* flash write code */
514         if (target_alloc_working_area(target, sizeof(stm32x_flash_write_code), &stm32x_info->write_algorithm) != ERROR_OK)
515         {
516                 LOG_WARNING("no working area available, can't do block memory writes");
517                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
518         };
519
520         if ((retval=target_write_buffer(target, stm32x_info->write_algorithm->address, sizeof(stm32x_flash_write_code), stm32x_flash_write_code))!=ERROR_OK)
521                 return retval;
522
523         /* memory buffer */
524         while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
525         {
526                 buffer_size /= 2;
527                 if (buffer_size <= 256)
528                 {
529                         /* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
530                         if (stm32x_info->write_algorithm)
531                                 target_free_working_area(target, stm32x_info->write_algorithm);
532
533                         LOG_WARNING("no large enough working area available, can't do block memory writes");
534                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
535                 }
536         };
537
538         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
539         armv7m_info.core_mode = ARMV7M_MODE_ANY;
540
541         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
542         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
543         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
544         init_reg_param(&reg_params[3], "r3", 32, PARAM_IN);
545
546         while (count > 0)
547         {
548                 u32 thisrun_count = (count > (buffer_size / 2)) ? (buffer_size / 2) : count;
549
550                 if ((retval = target_write_buffer(target, source->address, thisrun_count * 2, buffer))!=ERROR_OK)
551                         break;
552
553                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
554                 buf_set_u32(reg_params[1].value, 0, 32, address);
555                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
556
557                 if ((retval = target_run_algorithm(target, 0, NULL, 4, reg_params, stm32x_info->write_algorithm->address, \
558                                 stm32x_info->write_algorithm->address + (sizeof(stm32x_flash_write_code) - 10), 10000, &armv7m_info)) != ERROR_OK)
559                 {
560                         LOG_ERROR("error executing stm32x flash write algorithm");
561                         retval = ERROR_FLASH_OPERATION_FAILED;
562                         break;
563                 }
564
565                 if (buf_get_u32(reg_params[3].value, 0, 32) & FLASH_PGERR)
566                 {
567                         LOG_ERROR("flash memory not erased before writing");
568                         /* Clear but report errors */
569                         target_write_u32(target, STM32_FLASH_SR, FLASH_PGERR);
570                         retval = ERROR_FLASH_OPERATION_FAILED;
571                         break;
572                 }
573
574                 if (buf_get_u32(reg_params[3].value, 0, 32) & FLASH_WRPRTERR)
575                 {
576                         LOG_ERROR("flash memory write protected");
577                         /* Clear but report errors */
578                         target_write_u32(target, STM32_FLASH_SR, FLASH_WRPRTERR);
579                         retval = ERROR_FLASH_OPERATION_FAILED;
580                         break;
581                 }
582
583                 buffer += thisrun_count * 2;
584                 address += thisrun_count * 2;
585                 count -= thisrun_count;
586         }
587
588         target_free_working_area(target, source);
589         target_free_working_area(target, stm32x_info->write_algorithm);
590
591         destroy_reg_param(&reg_params[0]);
592         destroy_reg_param(&reg_params[1]);
593         destroy_reg_param(&reg_params[2]);
594         destroy_reg_param(&reg_params[3]);
595
596         return retval;
597 }
598
599 static int stm32x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
600 {
601         target_t *target = bank->target;
602         u32 words_remaining = (count / 2);
603         u32 bytes_remaining = (count & 0x00000001);
604         u32 address = bank->base + offset;
605         u32 bytes_written = 0;
606         u8 status;
607         int retval;
608
609         if (bank->target->state != TARGET_HALTED)
610         {
611                 LOG_ERROR("Target not halted");
612                 return ERROR_TARGET_NOT_HALTED;
613         }
614
615         if (offset & 0x1)
616         {
617                 LOG_WARNING("offset 0x%x breaks required 2-byte alignment", offset);
618                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
619         }
620
621         /* unlock flash registers */
622         target_write_u32(target, STM32_FLASH_KEYR, KEY1);
623         target_write_u32(target, STM32_FLASH_KEYR, KEY2);
624
625         /* multiple half words (2-byte) to be programmed? */
626         if (words_remaining > 0)
627         {
628                 /* try using a block write */
629                 if ((retval = stm32x_write_block(bank, buffer, offset, words_remaining)) != ERROR_OK)
630                 {
631                         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
632                         {
633                                 /* if block write failed (no sufficient working area),
634                                  * we use normal (slow) single dword accesses */
635                                 LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
636                         }
637                         else if (retval == ERROR_FLASH_OPERATION_FAILED)
638                         {
639                                 LOG_ERROR("flash writing failed with error code: 0x%x", retval);
640                                 return ERROR_FLASH_OPERATION_FAILED;
641                         }
642                 }
643                 else
644                 {
645                         buffer += words_remaining * 2;
646                         address += words_remaining * 2;
647                         words_remaining = 0;
648                 }
649         }
650
651         while (words_remaining > 0)
652         {
653                 u16 value;
654                 memcpy(&value, buffer + bytes_written, sizeof(u16));
655
656                 target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
657                 target_write_u16(target, address, value);
658
659                 status = stm32x_wait_status_busy(bank, 5);
660
661                 if( status & FLASH_WRPRTERR )
662                 {
663                         LOG_ERROR("flash memory not erased before writing");
664                         return ERROR_FLASH_OPERATION_FAILED;
665                 }
666                 if( status & FLASH_PGERR )
667                 {
668                         LOG_ERROR("flash memory write protected");
669                         return ERROR_FLASH_OPERATION_FAILED;
670                 }
671
672                 bytes_written += 2;
673                 words_remaining--;
674                 address += 2;
675         }
676
677         if (bytes_remaining)
678         {
679                 u16 value = 0xffff;
680                 memcpy(&value, buffer + bytes_written, bytes_remaining);
681
682                 target_write_u32(target, STM32_FLASH_CR, FLASH_PG);
683                 target_write_u16(target, address, value);
684
685                 status = stm32x_wait_status_busy(bank, 5);
686
687                 if( status & FLASH_WRPRTERR )
688                 {
689                         LOG_ERROR("flash memory not erased before writing");
690                         return ERROR_FLASH_OPERATION_FAILED;
691                 }
692                 if( status & FLASH_PGERR )
693                 {
694                         LOG_ERROR("flash memory write protected");
695                         return ERROR_FLASH_OPERATION_FAILED;
696                 }
697         }
698
699         target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
700
701         return ERROR_OK;
702 }
703
704 static int stm32x_probe(struct flash_bank_s *bank)
705 {
706         target_t *target = bank->target;
707         stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
708         int i;
709         u16 num_pages;
710         u32 device_id;
711         int page_size;
712
713         if (bank->target->state != TARGET_HALTED)
714         {
715                 LOG_ERROR("Target not halted");
716                 return ERROR_TARGET_NOT_HALTED;
717         }
718
719         stm32x_info->probed = 0;
720
721         /* read stm32 device id register */
722         target_read_u32(target, 0xE0042000, &device_id);
723         LOG_INFO( "device id = 0x%08x", device_id );
724
725         /* get flash size from target */
726         if (target_read_u16(target, 0x1FFFF7E0, &num_pages) != ERROR_OK)
727         {
728                 /* failed reading flash size, default to max target family */
729                 num_pages = 0xffff;
730         }
731
732         if ((device_id & 0x7ff) == 0x410)
733         {
734                 /* medium density - we have 1k pages
735                  * 4 pages for a protection area */
736                 page_size = 1024;
737                 stm32x_info->ppage_size = 4;
738
739                 /* check for early silicon */
740                 if (num_pages == 0xffff)
741                 {
742                         /* number of sectors incorrect on revA */
743                         LOG_WARNING( "STM32 flash size failed, probe inaccurate - assuming 128k flash" );
744                         num_pages = 128;
745                 }
746         }
747         else if ((device_id & 0x7ff) == 0x412)
748         {
749                 /* low density - we have 1k pages
750                  * 4 pages for a protection area */
751                 page_size = 1024;
752                 stm32x_info->ppage_size = 4;
753
754                 /* check for early silicon */
755                 if (num_pages == 0xffff)
756                 {
757                         /* number of sectors incorrect on revA */
758                         LOG_WARNING( "STM32 flash size failed, probe inaccurate - assuming 32k flash" );
759                         num_pages = 32;
760                 }
761         }
762         else if ((device_id & 0x7ff) == 0x414)
763         {
764                 /* high density - we have 2k pages
765                  * 2 pages for a protection area */
766                 page_size = 2048;
767                 stm32x_info->ppage_size = 2;
768
769                 /* check for early silicon */
770                 if (num_pages == 0xffff)
771                 {
772                         /* number of sectors incorrect on revZ */
773                         LOG_WARNING( "STM32 flash size failed, probe inaccurate - assuming 512k flash" );
774                         num_pages = 512;
775                 }
776         }
777         else if ((device_id & 0x7ff) == 0x418)
778         {
779                 /* connectivity line density - we have 1k pages
780                  * 4 pages for a protection area */
781                 page_size = 1024;
782                 stm32x_info->ppage_size = 4;
783
784                 /* check for early silicon */
785                 if (num_pages == 0xffff)
786                 {
787                         /* number of sectors incorrect on revZ */
788                         LOG_WARNING( "STM32 flash size failed, probe inaccurate - assuming 256k flash" );
789                         num_pages = 256;
790                 }
791         }
792         else
793         {
794                 LOG_WARNING( "Cannot identify target as a STM32 family." );
795                 return ERROR_FLASH_OPERATION_FAILED;
796         }
797
798         LOG_INFO( "flash size = %dkbytes", num_pages );
799
800         /* calculate numbers of pages */
801         num_pages /= (page_size / 1024);
802
803         bank->base = 0x08000000;
804         bank->size = (num_pages * page_size);
805         bank->num_sectors = num_pages;
806         bank->sectors = malloc(sizeof(flash_sector_t) * num_pages);
807
808         for (i = 0; i < num_pages; i++)
809         {
810                 bank->sectors[i].offset = i * page_size;
811                 bank->sectors[i].size = page_size;
812                 bank->sectors[i].is_erased = -1;
813                 bank->sectors[i].is_protected = 1;
814         }
815
816         stm32x_info->probed = 1;
817
818         return ERROR_OK;
819 }
820
821 static int stm32x_auto_probe(struct flash_bank_s *bank)
822 {
823         stm32x_flash_bank_t *stm32x_info = bank->driver_priv;
824         if (stm32x_info->probed)
825                 return ERROR_OK;
826         return stm32x_probe(bank);
827 }
828
829 #if 0
830 static int stm32x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
831 {
832         return ERROR_OK;
833 }
834 #endif
835
836 static int stm32x_info(struct flash_bank_s *bank, char *buf, int buf_size)
837 {
838         target_t *target = bank->target;
839         u32 device_id;
840         int printed;
841
842         /* read stm32 device id register */
843         target_read_u32(target, 0xE0042000, &device_id);
844
845         if ((device_id & 0x7ff) == 0x410)
846         {
847                 printed = snprintf(buf, buf_size, "stm32x (Medium Density) - Rev: ");
848                 buf += printed;
849                 buf_size -= printed;
850
851                 switch(device_id >> 16)
852                 {
853                         case 0x0000:
854                                 snprintf(buf, buf_size, "A");
855                                 break;
856
857                         case 0x2000:
858                                 snprintf(buf, buf_size, "B");
859                                 break;
860
861                         case 0x2001:
862                                 snprintf(buf, buf_size, "Z");
863                                 break;
864
865                         case 0x2003:
866                                 snprintf(buf, buf_size, "Y");
867                                 break;
868
869                         default:
870                                 snprintf(buf, buf_size, "unknown");
871                                 break;
872                 }
873         }
874         else if ((device_id & 0x7ff) == 0x412)
875         {
876                 printed = snprintf(buf, buf_size, "stm32x (Low Density) - Rev: ");
877                 buf += printed;
878                 buf_size -= printed;
879
880                 switch(device_id >> 16)
881                 {
882                         case 0x1000:
883                                 snprintf(buf, buf_size, "A");
884                                 break;
885
886                         default:
887                                 snprintf(buf, buf_size, "unknown");
888                                 break;
889                 }
890         }
891         else if ((device_id & 0x7ff) == 0x414)
892         {
893                 printed = snprintf(buf, buf_size, "stm32x (High Density) - Rev: ");
894                 buf += printed;
895                 buf_size -= printed;
896
897                 switch(device_id >> 16)
898                 {
899                         case 0x1000:
900                                 snprintf(buf, buf_size, "A");
901                                 break;
902
903                         case 0x1001:
904                                 snprintf(buf, buf_size, "Z");
905                                 break;
906
907                         default:
908                                 snprintf(buf, buf_size, "unknown");
909                                 break;
910                 }
911         }
912         else if ((device_id & 0x7ff) == 0x418)
913         {
914                 printed = snprintf(buf, buf_size, "stm32x (Connectivity) - Rev: ");
915                 buf += printed;
916                 buf_size -= printed;
917
918                 switch(device_id >> 16)
919                 {
920                         case 0x1000:
921                                 snprintf(buf, buf_size, "A");
922                                 break;
923
924                         default:
925                                 snprintf(buf, buf_size, "unknown");
926                                 break;
927                 }
928         }
929         else
930         {
931                 snprintf(buf, buf_size, "Cannot identify target as a stm32x\n");
932                 return ERROR_FLASH_OPERATION_FAILED;
933         }
934
935         return ERROR_OK;
936 }
937
938 static int stm32x_handle_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
939 {
940         flash_bank_t *bank;
941         target_t *target = NULL;
942         stm32x_flash_bank_t *stm32x_info = NULL;
943
944         if (argc < 1)
945         {
946                 command_print(cmd_ctx, "stm32x lock <bank>");
947                 return ERROR_OK;
948         }
949
950         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
951         if (!bank)
952         {
953                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
954                 return ERROR_OK;
955         }
956
957         stm32x_info = bank->driver_priv;
958
959         target = bank->target;
960
961         if (target->state != TARGET_HALTED)
962         {
963                 LOG_ERROR("Target not halted");
964                 return ERROR_TARGET_NOT_HALTED;
965         }
966
967         if (stm32x_erase_options(bank) != ERROR_OK)
968         {
969                 command_print(cmd_ctx, "stm32x failed to erase options");
970                 return ERROR_OK;
971         }
972
973         /* set readout protection */
974         stm32x_info->option_bytes.RDP = 0;
975
976         if (stm32x_write_options(bank) != ERROR_OK)
977         {
978                 command_print(cmd_ctx, "stm32x failed to lock device");
979                 return ERROR_OK;
980         }
981
982         command_print(cmd_ctx, "stm32x locked");
983
984         return ERROR_OK;
985 }
986
987 static int stm32x_handle_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
988 {
989         flash_bank_t *bank;
990         target_t *target = NULL;
991         stm32x_flash_bank_t *stm32x_info = NULL;
992
993         if (argc < 1)
994         {
995                 command_print(cmd_ctx, "stm32x unlock <bank>");
996                 return ERROR_OK;
997         }
998
999         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1000         if (!bank)
1001         {
1002                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1003                 return ERROR_OK;
1004         }
1005
1006         stm32x_info = bank->driver_priv;
1007
1008         target = bank->target;
1009
1010         if (target->state != TARGET_HALTED)
1011         {
1012                 LOG_ERROR("Target not halted");
1013                 return ERROR_TARGET_NOT_HALTED;
1014         }
1015
1016         if (stm32x_erase_options(bank) != ERROR_OK)
1017         {
1018                 command_print(cmd_ctx, "stm32x failed to unlock device");
1019                 return ERROR_OK;
1020         }
1021
1022         if (stm32x_write_options(bank) != ERROR_OK)
1023         {
1024                 command_print(cmd_ctx, "stm32x failed to lock device");
1025                 return ERROR_OK;
1026         }
1027
1028         command_print(cmd_ctx, "stm32x unlocked");
1029
1030         return ERROR_OK;
1031 }
1032
1033 static int stm32x_handle_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1034 {
1035         flash_bank_t *bank;
1036         u32 optionbyte;
1037         target_t *target = NULL;
1038         stm32x_flash_bank_t *stm32x_info = NULL;
1039
1040         if (argc < 1)
1041         {
1042                 command_print(cmd_ctx, "stm32x options_read <bank>");
1043                 return ERROR_OK;
1044         }
1045
1046         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1047         if (!bank)
1048         {
1049                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1050                 return ERROR_OK;
1051         }
1052
1053         stm32x_info = bank->driver_priv;
1054
1055         target = bank->target;
1056
1057         if (target->state != TARGET_HALTED)
1058         {
1059                 LOG_ERROR("Target not halted");
1060                 return ERROR_TARGET_NOT_HALTED;
1061         }
1062
1063         target_read_u32(target, STM32_FLASH_OBR, &optionbyte);
1064         command_print(cmd_ctx, "Option Byte: 0x%x", optionbyte);
1065
1066         if (buf_get_u32((u8*)&optionbyte, OPT_ERROR, 1))
1067                 command_print(cmd_ctx, "Option Byte Complement Error");
1068
1069         if (buf_get_u32((u8*)&optionbyte, OPT_READOUT, 1))
1070                 command_print(cmd_ctx, "Readout Protection On");
1071         else
1072                 command_print(cmd_ctx, "Readout Protection Off");
1073
1074         if (buf_get_u32((u8*)&optionbyte, OPT_RDWDGSW, 1))
1075                 command_print(cmd_ctx, "Software Watchdog");
1076         else
1077                 command_print(cmd_ctx, "Hardware Watchdog");
1078
1079         if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTOP, 1))
1080                 command_print(cmd_ctx, "Stop: No reset generated");
1081         else
1082                 command_print(cmd_ctx, "Stop: Reset generated");
1083
1084         if (buf_get_u32((u8*)&optionbyte, OPT_RDRSTSTDBY, 1))
1085                 command_print(cmd_ctx, "Standby: No reset generated");
1086         else
1087                 command_print(cmd_ctx, "Standby: Reset generated");
1088
1089         return ERROR_OK;
1090 }
1091
1092 static int stm32x_handle_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1093 {
1094         flash_bank_t *bank;
1095         target_t *target = NULL;
1096         stm32x_flash_bank_t *stm32x_info = NULL;
1097         u16 optionbyte = 0xF8;
1098
1099         if (argc < 4)
1100         {
1101                 command_print(cmd_ctx, "stm32x options_write <bank> <SWWDG|HWWDG> <RSTSTNDBY|NORSTSTNDBY> <RSTSTOP|NORSTSTOP>");
1102                 return ERROR_OK;
1103         }
1104
1105         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1106         if (!bank)
1107         {
1108                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1109                 return ERROR_OK;
1110         }
1111
1112         stm32x_info = bank->driver_priv;
1113
1114         target = bank->target;
1115
1116         if (target->state != TARGET_HALTED)
1117         {
1118                 LOG_ERROR("Target not halted");
1119                 return ERROR_TARGET_NOT_HALTED;
1120         }
1121
1122         if (strcmp(args[1], "SWWDG") == 0)
1123         {
1124                 optionbyte |= (1<<0);
1125         }
1126         else
1127         {
1128                 optionbyte &= ~(1<<0);
1129         }
1130
1131         if (strcmp(args[2], "NORSTSTNDBY") == 0)
1132         {
1133                 optionbyte |= (1<<1);
1134         }
1135         else
1136         {
1137                 optionbyte &= ~(1<<1);
1138         }
1139
1140         if (strcmp(args[3], "NORSTSTOP") == 0)
1141         {
1142                 optionbyte |= (1<<2);
1143         }
1144         else
1145         {
1146                 optionbyte &= ~(1<<2);
1147         }
1148
1149         if (stm32x_erase_options(bank) != ERROR_OK)
1150         {
1151                 command_print(cmd_ctx, "stm32x failed to erase options");
1152                 return ERROR_OK;
1153         }
1154
1155         stm32x_info->option_bytes.user_options = optionbyte;
1156
1157         if (stm32x_write_options(bank) != ERROR_OK)
1158         {
1159                 command_print(cmd_ctx, "stm32x failed to write options");
1160                 return ERROR_OK;
1161         }
1162
1163         command_print(cmd_ctx, "stm32x write options complete");
1164
1165         return ERROR_OK;
1166 }
1167
1168 static int stm32x_mass_erase(struct flash_bank_s *bank)
1169 {
1170         target_t *target = bank->target;
1171         u32 status;
1172
1173         if (target->state != TARGET_HALTED)
1174         {
1175                 LOG_ERROR("Target not halted");
1176                 return ERROR_TARGET_NOT_HALTED;
1177         }
1178
1179         /* unlock option flash registers */
1180         target_write_u32(target, STM32_FLASH_KEYR, KEY1);
1181         target_write_u32(target, STM32_FLASH_KEYR, KEY2);
1182
1183         /* mass erase flash memory */
1184         target_write_u32(target, STM32_FLASH_CR, FLASH_MER);
1185         target_write_u32(target, STM32_FLASH_CR, FLASH_MER|FLASH_STRT);
1186
1187         status = stm32x_wait_status_busy(bank, 10);
1188
1189         target_write_u32(target, STM32_FLASH_CR, FLASH_LOCK);
1190
1191         if( status & FLASH_WRPRTERR )
1192         {
1193                 LOG_ERROR("stm32x device protected");
1194                 return ERROR_OK;
1195         }
1196
1197         if( status & FLASH_PGERR )
1198         {
1199                 LOG_ERROR("stm32x device programming failed");
1200                 return ERROR_OK;
1201         }
1202
1203         return ERROR_OK;
1204 }
1205
1206 static int stm32x_handle_mass_erase_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1207 {
1208         flash_bank_t *bank;
1209         int i;
1210
1211         if (argc < 1)
1212         {
1213                 command_print(cmd_ctx, "stm32x mass_erase <bank>");
1214                 return ERROR_OK;
1215         }
1216
1217         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1218         if (!bank)
1219         {
1220                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1221                 return ERROR_OK;
1222         }
1223
1224         if (stm32x_mass_erase(bank) == ERROR_OK)
1225         {
1226                 /* set all sectors as erased */
1227                 for (i = 0; i < bank->num_sectors; i++)
1228                 {
1229                         bank->sectors[i].is_erased = 1;
1230                 }
1231
1232                 command_print(cmd_ctx, "stm32x mass erase complete");
1233         }
1234         else
1235         {
1236                 command_print(cmd_ctx, "stm32x mass erase failed");
1237         }
1238
1239         return ERROR_OK;
1240 }