]> git.sur5r.net Git - openocd/blob - src/flash/str7x.c
David Brownell <david-b@pacbell.net>:
[openocd] / src / flash / str7x.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 "str7x.h"
28 #include "armv4_5.h"
29 #include "binarybuffer.h"
30
31
32 str7x_mem_layout_t mem_layout_str7bank0[] = {
33         {0x00000000, 0x02000, 0x01},
34         {0x00002000, 0x02000, 0x02},
35         {0x00004000, 0x02000, 0x04},
36         {0x00006000, 0x02000, 0x08},
37         {0x00008000, 0x08000, 0x10},
38         {0x00010000, 0x10000, 0x20},
39         {0x00020000, 0x10000, 0x40},
40         {0x00030000, 0x10000, 0x80}
41 };
42
43 str7x_mem_layout_t mem_layout_str7bank1[] = {
44         {0x00000000, 0x02000, 0x10000},
45         {0x00002000, 0x02000, 0x20000}
46 };
47
48 static int str7x_register_commands(struct command_context_s *cmd_ctx);
49 static int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
50 static int str7x_erase(struct flash_bank_s *bank, int first, int last);
51 static int str7x_protect(struct flash_bank_s *bank, int set, int first, int last);
52 static int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
53 static int str7x_probe(struct flash_bank_s *bank);
54 //static int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
55 static int str7x_protect_check(struct flash_bank_s *bank);
56 static int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size);
57
58 static int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
59
60 flash_driver_t str7x_flash =
61 {
62         .name = "str7x",
63         .register_commands = str7x_register_commands,
64         .flash_bank_command = str7x_flash_bank_command,
65         .erase = str7x_erase,
66         .protect = str7x_protect,
67         .write = str7x_write,
68         .probe = str7x_probe,
69         .auto_probe = str7x_probe,
70         .erase_check = default_flash_blank_check,
71         .protect_check = str7x_protect_check,
72         .info = str7x_info
73 };
74
75 static int str7x_register_commands(struct command_context_s *cmd_ctx)
76 {
77         command_t *str7x_cmd = register_command(cmd_ctx, NULL, "str7x", NULL, COMMAND_ANY, NULL);
78         
79         register_command(cmd_ctx, str7x_cmd, "disable_jtag", str7x_handle_disable_jtag_command, COMMAND_EXEC,
80                                          "disable jtag access");
81                                          
82         return ERROR_OK;
83 }
84
85 static int str7x_get_flash_adr(struct flash_bank_s *bank, u32 reg)
86 {
87         str7x_flash_bank_t *str7x_info = bank->driver_priv;
88         return (str7x_info->register_base | reg);
89 }
90
91 static int str7x_build_block_list(struct flash_bank_s *bank)
92 {
93         str7x_flash_bank_t *str7x_info = bank->driver_priv;
94
95         int i;
96         int num_sectors;
97         int b0_sectors = 0, b1_sectors = 0;
98                 
99         switch (bank->size)
100         {
101                 case 16 * 1024:
102                         b1_sectors = 2;
103                         break;
104                 case 64 * 1024:
105                         b0_sectors = 5;
106                         break;
107                 case 128 * 1024:
108                         b0_sectors = 6;
109                         break;
110                 case 256 * 1024:
111                         b0_sectors = 8;
112                         break;
113                 default:
114                         LOG_ERROR("BUG: unknown bank->size encountered");
115                         exit(-1);
116         }
117                 
118         num_sectors = b0_sectors + b1_sectors;
119         
120         bank->num_sectors = num_sectors;
121         bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
122         str7x_info->sector_bits = malloc(sizeof(u32) * num_sectors);
123         
124         num_sectors = 0;
125         
126         for (i = 0; i < b0_sectors; i++)
127         {
128                 bank->sectors[num_sectors].offset = mem_layout_str7bank0[i].sector_start;
129                 bank->sectors[num_sectors].size = mem_layout_str7bank0[i].sector_size;
130                 bank->sectors[num_sectors].is_erased = -1;
131                 bank->sectors[num_sectors].is_protected = 1;
132                 str7x_info->sector_bits[num_sectors++] = mem_layout_str7bank0[i].sector_bit;
133         }
134         
135         for (i = 0; i < b1_sectors; i++)
136         {
137                 bank->sectors[num_sectors].offset = mem_layout_str7bank1[i].sector_start;
138                 bank->sectors[num_sectors].size = mem_layout_str7bank1[i].sector_size;
139                 bank->sectors[num_sectors].is_erased = -1;
140                 bank->sectors[num_sectors].is_protected = 1;
141                 str7x_info->sector_bits[num_sectors++] = mem_layout_str7bank1[i].sector_bit;
142         }
143         
144         return ERROR_OK;
145 }
146
147 /* flash bank str7x <base> <size> 0 0 <target#> <str71_variant>
148  */
149 static int str7x_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
150 {
151         str7x_flash_bank_t *str7x_info;
152         
153         if (argc < 7)
154         {
155                 LOG_WARNING("incomplete flash_bank str7x configuration");
156                 return ERROR_FLASH_BANK_INVALID;
157         }
158         
159         str7x_info = malloc(sizeof(str7x_flash_bank_t));
160         bank->driver_priv = str7x_info;
161         
162         /* set default bits for str71x flash */
163         str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA1|FLASH_BSYA0);
164         str7x_info->disable_bit = (1<<1);
165         
166         if (strcmp(args[6], "STR71x") == 0)
167         {
168                 str7x_info->register_base = 0x40100000;
169         }
170         else if (strcmp(args[6], "STR73x") == 0)
171         {
172                 str7x_info->register_base = 0x80100000;
173                 str7x_info->busy_bits = (FLASH_LOCK|FLASH_BSYA0);
174         }
175         else if (strcmp(args[6], "STR75x") == 0)
176         {
177                 str7x_info->register_base = 0x20100000;
178                 str7x_info->disable_bit = (1<<0);
179         }
180         else
181         {
182                 LOG_ERROR("unknown STR7x variant: '%s'", args[6]);
183                 free(str7x_info);
184                 return ERROR_FLASH_BANK_INVALID;
185         }
186
187         str7x_build_block_list(bank);
188         
189         str7x_info->write_algorithm = NULL;
190         
191         return ERROR_OK;
192 }
193
194 static u32 str7x_status(struct flash_bank_s *bank)
195 {
196         target_t *target = bank->target;
197         u32 retval;
198
199         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), &retval);
200
201         return retval;
202 }
203
204 static u32 str7x_result(struct flash_bank_s *bank)
205 {
206         target_t *target = bank->target;
207         u32 retval;
208
209         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_ER), &retval);
210         
211         return retval;
212 }
213
214 static int str7x_protect_check(struct flash_bank_s *bank)
215 {
216         str7x_flash_bank_t *str7x_info = bank->driver_priv;
217         target_t *target = bank->target;
218         
219         int i;
220         u32 retval;
221
222         if (bank->target->state != TARGET_HALTED)
223         {
224                 LOG_ERROR("Target not halted");
225                 return ERROR_TARGET_NOT_HALTED;
226         }
227
228         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVWPAR), &retval);
229
230         for (i = 0; i < bank->num_sectors; i++)
231         {
232                 if (retval & str7x_info->sector_bits[i])
233                         bank->sectors[i].is_protected = 0;
234                 else
235                         bank->sectors[i].is_protected = 1;
236         }
237
238         return ERROR_OK;
239 }
240
241 static int str7x_erase(struct flash_bank_s *bank, int first, int last)
242 {
243         str7x_flash_bank_t *str7x_info = bank->driver_priv;
244         target_t *target = bank->target;
245         
246         int i;
247         u32 cmd;
248         u32 retval;
249         u32 sectors = 0;
250         
251         if (bank->target->state != TARGET_HALTED)
252         {
253                 LOG_ERROR("Target not halted");
254                 return ERROR_TARGET_NOT_HALTED;
255         }
256
257         for (i = first; i <= last; i++)
258         {
259                 sectors |= str7x_info->sector_bits[i];
260         }
261         
262         LOG_DEBUG("sectors: 0x%x", sectors);
263         
264         /* clear FLASH_ER register */   
265         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
266         
267         cmd = FLASH_SER;
268         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
269         
270         cmd = sectors;
271         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR1), cmd);
272         
273         cmd = FLASH_SER|FLASH_WMS;
274         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
275         
276         while (((retval = str7x_status(bank)) & str7x_info->busy_bits)){
277                 alive_sleep(1);
278         }
279         
280         retval = str7x_result(bank);
281         
282         if (retval)
283         {
284                 LOG_ERROR("error erasing flash bank, FLASH_ER: 0x%x", retval);
285                 return ERROR_FLASH_OPERATION_FAILED;
286         }
287         
288         for (i = first; i <= last; i++)
289                 bank->sectors[i].is_erased = 1;
290
291         return ERROR_OK;
292 }
293
294 static int str7x_protect(struct flash_bank_s *bank, int set, int first, int last)
295 {
296         str7x_flash_bank_t *str7x_info = bank->driver_priv;
297         target_t *target = bank->target;
298         int i;
299         u32 cmd;
300         u32 retval;
301         u32 protect_blocks;
302         
303         if (bank->target->state != TARGET_HALTED)
304         {
305                 LOG_ERROR("Target not halted");
306                 return ERROR_TARGET_NOT_HALTED;
307         }
308         
309         protect_blocks = 0xFFFFFFFF;
310
311         if (set)
312         {
313                 for (i = first; i <= last; i++)
314                         protect_blocks &= ~(str7x_info->sector_bits[i]);
315         }
316         
317         /* clear FLASH_ER register */   
318         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
319
320         cmd = FLASH_SPR;
321         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
322         
323         cmd = str7x_get_flash_adr(bank, FLASH_NVWPAR);
324         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), cmd);
325         
326         cmd = protect_blocks;
327         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), cmd);
328         
329         cmd = FLASH_SPR|FLASH_WMS;
330         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
331         
332         while (((retval = str7x_status(bank)) & str7x_info->busy_bits)){
333                 alive_sleep(1);
334         }
335         
336         retval = str7x_result(bank);
337         
338         LOG_DEBUG("retval: 0x%8.8x", retval);
339         
340         if (retval & FLASH_ERER)
341                 return ERROR_FLASH_SECTOR_NOT_ERASED;
342         else if (retval & FLASH_WPF)
343                 return ERROR_FLASH_OPERATION_FAILED;
344
345         return ERROR_OK;
346 }
347
348 static int str7x_write_block(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
349 {
350         str7x_flash_bank_t *str7x_info = bank->driver_priv;
351         target_t *target = bank->target;
352         u32 buffer_size = 8192;
353         working_area_t *source;
354         u32 address = bank->base + offset;
355         reg_param_t reg_params[6];
356         armv4_5_algorithm_t armv4_5_info;
357         int retval = ERROR_OK;
358         
359         u32 str7x_flash_write_code[] = {
360                                         /* write:                               */
361                 0xe3a04201, /*  mov r4, #0x10000000     */
362                 0xe5824000, /*  str r4, [r2, #0x0]      */
363                 0xe5821010, /*  str r1, [r2, #0x10]     */
364                 0xe4904004, /*  ldr r4, [r0], #4        */
365                 0xe5824008, /*  str r4, [r2, #0x8]      */
366                 0xe4904004, /*  ldr r4, [r0], #4        */
367                 0xe582400c, /*  str r4, [r2, #0xc]      */
368                 0xe3a04209, /*  mov r4, #0x90000000     */
369                 0xe5824000, /*  str r4, [r2, #0x0]      */
370                             /* busy:                            */
371                 0xe5924000, /*  ldr r4, [r2, #0x0]      */
372                 0xe1140005,     /*      tst r4, r5                      */
373                 0x1afffffc, /*  bne busy                        */
374                 0xe5924014, /*  ldr r4, [r2, #0x14]     */
375                 0xe31400ff, /*  tst r4, #0xff           */
376                 0x03140c01, /*  tsteq r4, #0x100        */
377                 0x1a000002, /*  bne exit                        */
378                 0xe2811008, /*  add r1, r1, #0x8        */
379                 0xe2533001, /*  subs r3, r3, #1         */
380                 0x1affffec, /*  bne write                       */
381                                         /* exit:                                */
382                 0xeafffffe, /*  b exit                          */
383         };
384         
385         /* flash write code */
386         if (target_alloc_working_area(target, 4 * 20, &str7x_info->write_algorithm) != ERROR_OK)
387         {
388                 LOG_WARNING("no working area available, can't do block memory writes");
389                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
390         };
391         
392         target_write_buffer(target, str7x_info->write_algorithm->address, 20 * 4, (u8*)str7x_flash_write_code);
393
394         /* memory buffer */
395         while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK)
396         {
397                 buffer_size /= 2;
398                 if (buffer_size <= 256)
399                 {
400                         /* if we already allocated the writing code, but failed to get a buffer, free the algorithm */
401                         if (str7x_info->write_algorithm)
402                                 target_free_working_area(target, str7x_info->write_algorithm);
403                         
404                         LOG_WARNING("no large enough working area available, can't do block memory writes");
405                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
406                 }
407         }
408         
409         armv4_5_info.common_magic = ARMV4_5_COMMON_MAGIC;
410         armv4_5_info.core_mode = ARMV4_5_MODE_SVC;
411         armv4_5_info.core_state = ARMV4_5_STATE_ARM;
412         
413         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT);
414         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);
415         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);
416         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);
417         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN);
418         init_reg_param(&reg_params[5], "r5", 32, PARAM_OUT);
419         
420         while (count > 0)
421         {
422                 u32 thisrun_count = (count > (buffer_size / 8)) ? (buffer_size / 8) : count;
423                 
424                 target_write_buffer(target, source->address, thisrun_count * 8, buffer);
425                 
426                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
427                 buf_set_u32(reg_params[1].value, 0, 32, address);
428                 buf_set_u32(reg_params[2].value, 0, 32, str7x_get_flash_adr(bank, FLASH_CR0));
429                 buf_set_u32(reg_params[3].value, 0, 32, thisrun_count);
430                 buf_set_u32(reg_params[5].value, 0, 32, str7x_info->busy_bits);
431         
432                 if ((retval = target_run_algorithm(target, 0, NULL, 6, reg_params, str7x_info->write_algorithm->address, str7x_info->write_algorithm->address + (19 * 4), 10000, &armv4_5_info)) != ERROR_OK)
433                 {
434                         LOG_ERROR("error executing str7x flash write algorithm");
435                         retval = ERROR_FLASH_OPERATION_FAILED;
436                         break;
437                 }
438         
439                 if (buf_get_u32(reg_params[4].value, 0, 32) != 0x00)
440                 {
441                         retval = ERROR_FLASH_OPERATION_FAILED;
442                         break;
443                 }
444                 
445                 buffer += thisrun_count * 8;
446                 address += thisrun_count * 8;
447                 count -= thisrun_count;
448         }
449         
450         target_free_working_area(target, source);
451         target_free_working_area(target, str7x_info->write_algorithm);
452         
453         destroy_reg_param(&reg_params[0]);
454         destroy_reg_param(&reg_params[1]);
455         destroy_reg_param(&reg_params[2]);
456         destroy_reg_param(&reg_params[3]);
457         destroy_reg_param(&reg_params[4]);
458         destroy_reg_param(&reg_params[5]);
459         
460         return retval;
461 }
462
463 static int str7x_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
464 {
465         target_t *target = bank->target;
466         str7x_flash_bank_t *str7x_info = bank->driver_priv;
467         u32 dwords_remaining = (count / 8);
468         u32 bytes_remaining = (count & 0x00000007);
469         u32 address = bank->base + offset;
470         u32 bytes_written = 0;
471         u32 cmd;
472         int retval;
473         u32 check_address = offset;
474         int i;
475         
476         if (bank->target->state != TARGET_HALTED)
477         {
478                 LOG_ERROR("Target not halted");
479                 return ERROR_TARGET_NOT_HALTED;
480         }
481
482         if (offset & 0x7)
483         {
484                 LOG_WARNING("offset 0x%x breaks required 8-byte alignment", offset);
485                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
486         }
487         
488         for (i = 0; i < bank->num_sectors; i++)
489         {
490                 u32 sec_start = bank->sectors[i].offset;
491                 u32 sec_end = sec_start + bank->sectors[i].size;
492                 
493                 /* check if destination falls within the current sector */
494                 if ((check_address >= sec_start) && (check_address < sec_end))
495                 {
496                         /* check if destination ends in the current sector */
497                         if (offset + count < sec_end)
498                                 check_address = offset + count;
499                         else
500                                 check_address = sec_end;
501                 }
502         }
503         
504         if (check_address != offset + count)
505                 return ERROR_FLASH_DST_OUT_OF_BANK;
506                 
507         /* clear FLASH_ER register */   
508         target_write_u32(target, str7x_get_flash_adr(bank, FLASH_ER), 0x0);
509
510         /* multiple dwords (8-byte) to be programmed? */
511         if (dwords_remaining > 0) 
512         {
513                 /* try using a block write */
514                 if ((retval = str7x_write_block(bank, buffer, offset, dwords_remaining)) != ERROR_OK)
515                 {
516                         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE)
517                         {
518                                 /* if block write failed (no sufficient working area),
519                                  * we use normal (slow) single dword accesses */ 
520                                 LOG_WARNING("couldn't use block writes, falling back to single memory accesses");
521                         }
522                         else if (retval == ERROR_FLASH_OPERATION_FAILED)
523                         {
524                                 /* if an error occured, we examine the reason, and quit */
525                                 retval = str7x_result(bank);
526                                 
527                                 LOG_ERROR("flash writing failed with error code: 0x%x", retval);
528                                 return ERROR_FLASH_OPERATION_FAILED;
529                         }
530                 }
531                 else
532                 {
533                         buffer += dwords_remaining * 8;
534                         address += dwords_remaining * 8;
535                         dwords_remaining = 0;
536                 }
537         }
538
539         while (dwords_remaining > 0)
540         {
541                 /* command */
542                 cmd = FLASH_DWPG;
543                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
544                 
545                 /* address */
546                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);
547                 
548                 /* data word 1 */
549                 target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, buffer + bytes_written);
550                 bytes_written += 4;
551                 
552                 /* data word 2 */
553                 target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, buffer + bytes_written);
554                 bytes_written += 4;
555                 
556                 /* start programming cycle */
557                 cmd = FLASH_DWPG | FLASH_WMS;
558                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
559                 
560                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits))
561                 {
562                         alive_sleep(1);
563                 }
564                 
565                 retval = str7x_result(bank);
566                 
567                 if (retval & FLASH_PGER)
568                         return ERROR_FLASH_OPERATION_FAILED;
569                 else if (retval & FLASH_WPF)
570                         return ERROR_FLASH_OPERATION_FAILED;
571
572                 dwords_remaining--;
573                 address += 8;
574         }
575         
576         if (bytes_remaining)
577         {
578                 u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
579                 int i = 0;
580                                 
581                 while(bytes_remaining > 0)
582                 {
583                         last_dword[i++] = *(buffer + bytes_written); 
584                         bytes_remaining--;
585                         bytes_written++;
586                 }
587                 
588                 /* command */
589                 cmd = FLASH_DWPG;
590                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
591                 
592                 /* address */
593                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), address);
594                 
595                 /* data word 1 */
596                 target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR0), 4, 1, last_dword);
597                 bytes_written += 4;
598                 
599                 /* data word 2 */
600                 target_write_memory(target, str7x_get_flash_adr(bank, FLASH_DR1), 4, 1, last_dword + 4);
601                 bytes_written += 4;
602                 
603                 /* start programming cycle */
604                 cmd = FLASH_DWPG | FLASH_WMS;
605                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), cmd);
606                 
607                 while (((retval = str7x_status(bank)) & str7x_info->busy_bits))
608                 {
609                         alive_sleep(1);
610                 }
611                 
612                 retval = str7x_result(bank);
613                 
614                 if (retval & FLASH_PGER)
615                         return ERROR_FLASH_OPERATION_FAILED;
616                 else if (retval & FLASH_WPF)
617                         return ERROR_FLASH_OPERATION_FAILED;
618         }
619                 
620         return ERROR_OK;
621 }
622
623 static int str7x_probe(struct flash_bank_s *bank)
624 {
625         return ERROR_OK;
626 }
627
628 #if 0
629 static int str7x_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
630 {
631         return ERROR_OK;
632 }
633 #endif
634
635 static int str7x_info(struct flash_bank_s *bank, char *buf, int buf_size)
636 {
637         snprintf(buf, buf_size, "str7x flash driver info" );
638         return ERROR_OK;
639 }
640
641 static int str7x_handle_disable_jtag_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
642 {
643         flash_bank_t *bank;
644         target_t *target = NULL;
645         str7x_flash_bank_t *str7x_info = NULL;
646         
647         u32 flash_cmd;
648         u32 retval;
649         u16 ProtectionLevel = 0;
650         u16 ProtectionRegs;
651         
652         if (argc < 1)
653         {
654                 command_print(cmd_ctx, "str7x disable_jtag <bank>");
655                 return ERROR_OK;        
656         }
657         
658         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
659         if (!bank)
660         {
661                 command_print(cmd_ctx, "str7x disable_jtag <bank> ok");
662                 return ERROR_OK;
663         }
664         
665         str7x_info = bank->driver_priv;
666         
667         target = bank->target;
668         
669         if (target->state != TARGET_HALTED)
670         {
671                 LOG_ERROR("Target not halted");
672                 return ERROR_TARGET_NOT_HALTED;
673         }
674         
675         /* first we get protection status */
676         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR0), &retval);
677
678         if (!(retval & str7x_info->disable_bit))
679         {
680                 ProtectionLevel = 1;
681         }
682         
683         target_read_u32(target, str7x_get_flash_adr(bank, FLASH_NVAPR1), &retval);
684         ProtectionRegs = ~(retval >> 16);
685
686         while (((ProtectionRegs) != 0) && (ProtectionLevel < 16))
687         {
688                 ProtectionRegs >>= 1;
689                 ProtectionLevel++;
690         }
691         
692         if (ProtectionLevel == 0)
693         {
694                 flash_cmd = FLASH_SPR;
695                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
696                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFB8);
697                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), 0xFFFFFFFD);
698                 flash_cmd = FLASH_SPR | FLASH_WMS;
699                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
700         }
701         else
702         {
703                 flash_cmd = FLASH_SPR;
704                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
705                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_AR), 0x4010DFBC);
706                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_DR0), ~(1<<(15+ProtectionLevel)));
707                 flash_cmd = FLASH_SPR | FLASH_WMS;
708                 target_write_u32(target, str7x_get_flash_adr(bank, FLASH_CR0), flash_cmd);
709         }
710         
711         return ERROR_OK;
712 }