]> git.sur5r.net Git - openocd/blob - src/flash/str9xpec.c
David Brownell <david-b@pacbell.net>:
[openocd] / src / flash / str9xpec.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 "str9xpec.h"
28 #include "arm7_9_common.h"
29
30
31 static int str9xpec_register_commands(struct command_context_s *cmd_ctx);
32 static int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
33 static int str9xpec_erase(struct flash_bank_s *bank, int first, int last);
34 static int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);
35 static int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
36 static int str9xpec_probe(struct flash_bank_s *bank);
37 static int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
38 static int str9xpec_protect_check(struct flash_bank_s *bank);
39 static int str9xpec_erase_check(struct flash_bank_s *bank);
40 static int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);
41
42 static int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);
43 static int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);
44 static int str9xpec_write_options(struct flash_bank_s *bank);
45
46 static int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
47 static int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
48 static int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
49 static int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
50 static int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
51 static int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
52 static int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
53 static int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
54 static int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
55 static int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
56
57 flash_driver_t str9xpec_flash =
58 {
59         .name = "str9xpec",
60         .register_commands = str9xpec_register_commands,
61         .flash_bank_command = str9xpec_flash_bank_command,
62         .erase = str9xpec_erase,
63         .protect = str9xpec_protect,
64         .write = str9xpec_write,
65         .probe = str9xpec_probe,
66         .auto_probe = str9xpec_probe,
67         .erase_check = str9xpec_erase_check,
68         .protect_check = str9xpec_protect_check,
69         .info = str9xpec_info
70 };
71
72 static int str9xpec_register_commands(struct command_context_s *cmd_ctx)
73 {
74         command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");
75
76         register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,
77                                          "enable str9xpec turbo mode");
78         register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,
79                                          "disable str9xpec turbo mode");
80         register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,
81                                          "configure str9xpec boot sector");
82         register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,
83                                          "configure str9xpec lvd threshold");
84         register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,
85                                          "configure str9xpec lvd selection");
86         register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,
87                                          "configure str9xpec lvd warning");
88         register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,
89                                          "read str9xpec options");
90         register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,
91                                          "write str9xpec options");
92         register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,
93                                          "lock str9xpec device");
94         register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,
95                                          "unlock str9xpec device");
96         register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,
97                                          "print part id of str9xpec flash bank <num>");
98
99         return ERROR_OK;
100 }
101
102 int str9xpec_set_instr(jtag_tap_t *tap, u32 new_instr, tap_state_t end_state)
103 {
104         if( tap == NULL ){
105                 return ERROR_TARGET_INVALID;
106         }
107
108         if (buf_get_u32(tap->cur_instr, 0, tap->ir_length) != new_instr)
109         {
110                 scan_field_t field;
111
112                 field.tap = tap;
113                 field.num_bits = tap->ir_length;
114                 field.out_value = calloc(CEIL(field.num_bits, 8), 1);
115                 buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
116                 field.in_value = NULL;
117
118                 jtag_add_ir_scan(1, &field, end_state);
119
120                 free(field.out_value);
121         }
122
123         return ERROR_OK;
124 }
125
126 static u8 str9xpec_isc_status(jtag_tap_t *tap)
127 {
128         scan_field_t field;
129         u8 status;
130
131         if (str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE) != ERROR_OK)
132                 return ISC_STATUS_ERROR;
133
134         field.tap = tap;
135         field.num_bits = 8;
136         field.out_value = NULL;
137         field.in_value = &status;
138
139
140         jtag_add_dr_scan(1, &field, TAP_IDLE);
141         jtag_execute_queue();
142
143         LOG_DEBUG("status: 0x%2.2x", status);
144
145         if (status & ISC_STATUS_SECURITY)
146                 LOG_INFO("Device Security Bit Set");
147
148         return status;
149 }
150
151 static int str9xpec_isc_enable(struct flash_bank_s *bank)
152 {
153         u8 status;
154         jtag_tap_t *tap;
155         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
156
157         tap = str9xpec_info->tap;
158
159         if (str9xpec_info->isc_enable)
160                 return ERROR_OK;
161
162         /* enter isc mode */
163         if (str9xpec_set_instr(tap, ISC_ENABLE, TAP_IDLE) != ERROR_OK)
164                 return ERROR_TARGET_INVALID;
165
166         /* check ISC status */
167         status = str9xpec_isc_status(tap);
168         if (status & ISC_STATUS_MODE)
169         {
170                 /* we have entered isc mode */
171                 str9xpec_info->isc_enable = 1;
172                 LOG_DEBUG("ISC_MODE Enabled");
173         }
174
175         return ERROR_OK;
176 }
177
178 static int str9xpec_isc_disable(struct flash_bank_s *bank)
179 {
180         u8 status;
181         jtag_tap_t *tap;
182         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
183
184         tap = str9xpec_info->tap;
185
186         if (!str9xpec_info->isc_enable)
187                 return ERROR_OK;
188
189         if (str9xpec_set_instr(tap, ISC_DISABLE, TAP_IDLE) != ERROR_OK)
190                 return ERROR_TARGET_INVALID;
191
192         /* delay to handle aborts */
193         jtag_add_sleep(50);
194
195         /* check ISC status */
196         status = str9xpec_isc_status(tap);
197         if (!(status & ISC_STATUS_MODE))
198         {
199                 /* we have left isc mode */
200                 str9xpec_info->isc_enable = 0;
201                 LOG_DEBUG("ISC_MODE Disabled");
202         }
203
204         return ERROR_OK;
205 }
206
207 static int str9xpec_read_config(struct flash_bank_s *bank)
208 {
209         scan_field_t field;
210         u8 status;
211         jtag_tap_t *tap;
212
213         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
214
215         tap = str9xpec_info->tap;
216
217         LOG_DEBUG("ISC_CONFIGURATION");
218
219         /* execute ISC_CONFIGURATION command */
220         str9xpec_set_instr(tap, ISC_CONFIGURATION, TAP_IRPAUSE);
221
222         field.tap = tap;
223         field.num_bits = 64;
224         field.out_value = NULL;
225         field.in_value = str9xpec_info->options;
226
227
228         jtag_add_dr_scan(1, &field, TAP_IDLE);
229         jtag_execute_queue();
230
231         status = str9xpec_isc_status(tap);
232
233         return status;
234 }
235
236 static int str9xpec_build_block_list(struct flash_bank_s *bank)
237 {
238         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
239
240         int i;
241         int num_sectors;
242         int b0_sectors = 0, b1_sectors = 0;
243         u32 offset = 0;
244         int b1_size = 0x2000;
245
246         switch (bank->size)
247         {
248                 case (256 * 1024):
249                         b0_sectors = 4;
250                         break;
251                 case (512 * 1024):
252                         b0_sectors = 8;
253                         break;
254                 case (1024 * 1024):
255                         b0_sectors = 16;
256                         break;
257                 case (2048 * 1024):
258                         b0_sectors = 32;
259                         break;
260                 case (128 * 1024):
261                         b1_size = 0x4000;
262                         b1_sectors = 8;
263                         break;
264                 case (32 * 1024):
265                         b1_sectors = 4;
266                         break;
267                 default:
268                         LOG_ERROR("BUG: unknown bank->size encountered");
269                         exit(-1);
270         }
271
272         num_sectors = b0_sectors + b1_sectors;
273
274         bank->num_sectors = num_sectors;
275         bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
276         str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);
277
278         num_sectors = 0;
279
280         for (i = 0; i < b0_sectors; i++)
281         {
282                 bank->sectors[num_sectors].offset = offset;
283                 bank->sectors[num_sectors].size = 0x10000;
284                 offset += bank->sectors[i].size;
285                 bank->sectors[num_sectors].is_erased = -1;
286                 bank->sectors[num_sectors].is_protected = 1;
287                 str9xpec_info->sector_bits[num_sectors++] = i;
288         }
289
290         for (i = 0; i < b1_sectors; i++)
291         {
292                 bank->sectors[num_sectors].offset = offset;
293                 bank->sectors[num_sectors].size = b1_size;
294                 offset += bank->sectors[i].size;
295                 bank->sectors[num_sectors].is_erased = -1;
296                 bank->sectors[num_sectors].is_protected = 1;
297                 str9xpec_info->sector_bits[num_sectors++] = i + 32;
298         }
299
300         return ERROR_OK;
301 }
302
303 /* flash bank str9x <base> <size> 0 0 <target#>
304  */
305 static int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
306 {
307         str9xpec_flash_controller_t *str9xpec_info;
308         armv4_5_common_t *armv4_5 = NULL;
309         arm7_9_common_t *arm7_9 = NULL;
310         arm_jtag_t *jtag_info = NULL;
311
312         if (argc < 6)
313         {
314                 LOG_WARNING("incomplete flash_bank str9x configuration");
315                 return ERROR_FLASH_BANK_INVALID;
316         }
317
318         str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));
319         bank->driver_priv = str9xpec_info;
320
321         /* find out jtag position of flash controller
322          * it is always after the arm966 core */
323
324         armv4_5 = bank->target->arch_info;
325         arm7_9 = armv4_5->arch_info;
326         jtag_info = &arm7_9->jtag_info;
327
328         str9xpec_info->tap = jtag_TapByAbsPosition( jtag_info->tap->abs_chain_position - 1);
329         str9xpec_info->isc_enable = 0;
330
331         str9xpec_build_block_list(bank);
332
333         /* clear option byte register */
334         buf_set_u32(str9xpec_info->options, 0, 64, 0);
335
336         return ERROR_OK;
337 }
338
339 static int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)
340 {
341         scan_field_t field;
342         u8 status;
343         jtag_tap_t *tap;
344         int i;
345         u8 *buffer = NULL;
346
347         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
348
349         tap = str9xpec_info->tap;
350
351         if (!str9xpec_info->isc_enable) {
352                 str9xpec_isc_enable( bank );
353         }
354
355         if (!str9xpec_info->isc_enable) {
356                 return ERROR_FLASH_OPERATION_FAILED;
357         }
358
359         buffer = calloc(CEIL(64, 8), 1);
360
361         LOG_DEBUG("blank check: first_bank: %i, last_bank: %i", first, last);
362
363         for (i = first; i <= last; i++) {
364                 buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
365         }
366
367         /* execute ISC_BLANK_CHECK command */
368         str9xpec_set_instr(tap, ISC_BLANK_CHECK, TAP_IRPAUSE);
369
370         field.tap = tap;
371         field.num_bits = 64;
372         field.out_value = buffer;
373         field.in_value = NULL;
374
375         jtag_add_dr_scan(1, &field, TAP_IDLE);
376         jtag_add_sleep(40000);
377
378         /* read blank check result */
379         field.tap = tap;
380         field.num_bits = 64;
381         field.out_value = NULL;
382         field.in_value = buffer;
383
384         jtag_add_dr_scan(1, &field, TAP_IRPAUSE);
385         jtag_execute_queue();
386
387         status = str9xpec_isc_status(tap);
388
389         for (i = first; i <= last; i++)
390         {
391                 if (buf_get_u32(buffer, str9xpec_info->sector_bits[i], 1))
392                         bank->sectors[i].is_erased = 0;
393                 else
394                         bank->sectors[i].is_erased = 1;
395         }
396
397         free(buffer);
398
399         str9xpec_isc_disable(bank);
400
401         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
402                 return ERROR_FLASH_OPERATION_FAILED;
403         return ERROR_OK;
404 }
405
406 static int str9xpec_protect_check(struct flash_bank_s *bank)
407 {
408         u8 status;
409         int i;
410
411         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
412
413         status = str9xpec_read_config(bank);
414
415         for (i = 0; i < bank->num_sectors; i++)
416         {
417                 if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))
418                         bank->sectors[i].is_protected = 1;
419                 else
420                         bank->sectors[i].is_protected = 0;
421         }
422
423         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
424                 return ERROR_FLASH_OPERATION_FAILED;
425         return ERROR_OK;
426 }
427
428 static int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)
429 {
430         scan_field_t field;
431         u8 status;
432         jtag_tap_t *tap;
433         int i;
434         u8 *buffer = NULL;
435
436         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
437
438         tap = str9xpec_info->tap;
439
440         if (!str9xpec_info->isc_enable) {
441                 str9xpec_isc_enable( bank );
442         }
443
444         if (!str9xpec_info->isc_enable) {
445                 return ISC_STATUS_ERROR;
446         }
447
448         buffer = calloc(CEIL(64, 8), 1);
449
450         LOG_DEBUG("erase: first_bank: %i, last_bank: %i", first, last);
451
452         /* last bank: 0xFF signals a full erase (unlock complete device) */
453         /* last bank: 0xFE signals a option byte erase */
454         if (last == 0xFF)
455         {
456                 for (i = 0; i < 64; i++) {
457                         buf_set_u32(buffer, i, 1, 1);
458                 }
459         }
460         else if (last == 0xFE)
461         {
462                 buf_set_u32(buffer, 49, 1, 1);
463         }
464         else
465         {
466                 for (i = first; i <= last; i++) {
467                         buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
468                 }
469         }
470
471         LOG_DEBUG("ISC_ERASE");
472
473         /* execute ISC_ERASE command */
474         str9xpec_set_instr(tap, ISC_ERASE, TAP_IRPAUSE);
475
476         field.tap = tap;
477         field.num_bits = 64;
478         field.out_value = buffer;
479         field.in_value = NULL;
480
481         jtag_add_dr_scan(1, &field, TAP_IDLE);
482         jtag_execute_queue();
483
484         jtag_add_sleep(10);
485
486         /* wait for erase completion */
487         while (!((status = str9xpec_isc_status(tap)) & ISC_STATUS_BUSY)) {
488                 alive_sleep(1);
489         }
490
491         free(buffer);
492
493         str9xpec_isc_disable(bank);
494
495         return status;
496 }
497
498 static int str9xpec_erase(struct flash_bank_s *bank, int first, int last)
499 {
500         int status;
501
502         status = str9xpec_erase_area(bank, first, last);
503
504         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
505                 return ERROR_FLASH_OPERATION_FAILED;
506
507         return ERROR_OK;
508 }
509
510 static int str9xpec_lock_device(struct flash_bank_s *bank)
511 {
512         scan_field_t field;
513         u8 status;
514         jtag_tap_t *tap;
515         str9xpec_flash_controller_t *str9xpec_info = NULL;
516
517         str9xpec_info = bank->driver_priv;
518         tap = str9xpec_info->tap;
519
520         if (!str9xpec_info->isc_enable) {
521                 str9xpec_isc_enable( bank );
522         }
523
524         if (!str9xpec_info->isc_enable) {
525                 return ISC_STATUS_ERROR;
526         }
527
528         /* set security address */
529         str9xpec_set_address(bank, 0x80);
530
531         /* execute ISC_PROGRAM command */
532         str9xpec_set_instr(tap, ISC_PROGRAM_SECURITY, TAP_IDLE);
533
534         str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
535
536         do {
537                 field.tap = tap;
538                 field.num_bits = 8;
539                 field.out_value = NULL;
540                 field.in_value = &status;
541
542                 jtag_add_dr_scan(1, &field, TAP_INVALID);
543                 jtag_execute_queue();
544
545         } while(!(status & ISC_STATUS_BUSY));
546
547         str9xpec_isc_disable(bank);
548
549         return status;
550 }
551
552 static int str9xpec_unlock_device(struct flash_bank_s *bank)
553 {
554         u8 status;
555
556         status = str9xpec_erase_area(bank, 0, 255);
557
558         return status;
559 }
560
561 static int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)
562 {
563         u8 status;
564         int i;
565
566         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
567
568         status = str9xpec_read_config(bank);
569
570         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
571                 return ERROR_FLASH_OPERATION_FAILED;
572
573         LOG_DEBUG("protect: first_bank: %i, last_bank: %i", first, last);
574
575         /* last bank: 0xFF signals a full device protect */
576         if (last == 0xFF)
577         {
578                 if( set )
579                 {
580                         status = str9xpec_lock_device(bank);
581                 }
582                 else
583                 {
584                         /* perform full erase to unlock device */
585                         status = str9xpec_unlock_device(bank);
586                 }
587         }
588         else
589         {
590                 for (i = first; i <= last; i++)
591                 {
592                         if( set )
593                                 buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 1);
594                         else
595                                 buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 0);
596                 }
597
598                 status = str9xpec_write_options(bank);
599         }
600
601         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
602                 return ERROR_FLASH_OPERATION_FAILED;
603
604         return ERROR_OK;
605 }
606
607 static int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)
608 {
609         jtag_tap_t *tap;
610         scan_field_t field;
611         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
612
613         tap = str9xpec_info->tap;
614
615         /* set flash controller address */
616         str9xpec_set_instr(tap, ISC_ADDRESS_SHIFT, TAP_IRPAUSE);
617
618         field.tap = tap;
619         field.num_bits = 8;
620         field.out_value = &sector;
621         field.in_value = NULL;
622
623         jtag_add_dr_scan(1, &field, TAP_INVALID);
624
625         return ERROR_OK;
626 }
627
628 static int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
629 {
630         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
631         u32 dwords_remaining = (count / 8);
632         u32 bytes_remaining = (count & 0x00000007);
633         u32 bytes_written = 0;
634         u8 status;
635         u32 check_address = offset;
636         jtag_tap_t *tap;
637         scan_field_t field;
638         u8 *scanbuf;
639         int i;
640         int first_sector = 0;
641         int last_sector = 0;
642
643         tap = str9xpec_info->tap;
644
645         if (!str9xpec_info->isc_enable) {
646                 str9xpec_isc_enable(bank);
647         }
648
649         if (!str9xpec_info->isc_enable) {
650                 return ERROR_FLASH_OPERATION_FAILED;
651         }
652
653         if (offset & 0x7)
654         {
655                 LOG_WARNING("offset 0x%x breaks required 8-byte alignment", offset);
656                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
657         }
658
659         for (i = 0; i < bank->num_sectors; i++)
660         {
661                 u32 sec_start = bank->sectors[i].offset;
662                 u32 sec_end = sec_start + bank->sectors[i].size;
663
664                 /* check if destination falls within the current sector */
665                 if ((check_address >= sec_start) && (check_address < sec_end))
666                 {
667                         /* check if destination ends in the current sector */
668                         if (offset + count < sec_end)
669                                 check_address = offset + count;
670                         else
671                                 check_address = sec_end;
672                 }
673
674                 if ((offset >= sec_start) && (offset < sec_end)){
675                         first_sector = i;
676                 }
677
678                 if ((offset + count >= sec_start) && (offset + count < sec_end)){
679                         last_sector = i;
680                 }
681         }
682
683         if (check_address != offset + count)
684                 return ERROR_FLASH_DST_OUT_OF_BANK;
685
686         LOG_DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
687
688         scanbuf = calloc(CEIL(64, 8), 1);
689
690         LOG_DEBUG("ISC_PROGRAM");
691
692         for (i = first_sector; i <= last_sector; i++)
693         {
694                 str9xpec_set_address(bank, str9xpec_info->sector_bits[i]);
695
696                 dwords_remaining = dwords_remaining < (bank->sectors[i].size/8) ? dwords_remaining : (bank->sectors[i].size/8);
697
698                 while (dwords_remaining > 0)
699                 {
700                         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
701
702                         field.tap = tap;
703                         field.num_bits = 64;
704                         field.out_value = (buffer + bytes_written);
705                         field.in_value = NULL;
706
707                         jtag_add_dr_scan(1, &field, TAP_IDLE);
708
709                         /* small delay before polling */
710                         jtag_add_sleep(50);
711
712                         str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
713
714                         do {
715                                 field.tap = tap;
716                                 field.num_bits = 8;
717                                 field.out_value = NULL;
718                                 field.in_value = scanbuf;
719
720                                 jtag_add_dr_scan(1, &field, TAP_INVALID);
721                                 jtag_execute_queue();
722
723                                 status = buf_get_u32(scanbuf, 0, 8);
724
725                         } while(!(status & ISC_STATUS_BUSY));
726
727                         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
728                                 return ERROR_FLASH_OPERATION_FAILED;
729
730                         /* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
731                                 return ERROR_FLASH_OPERATION_FAILED; */
732
733                         dwords_remaining--;
734                         bytes_written += 8;
735                 }
736         }
737
738         if (bytes_remaining)
739         {
740                 u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
741                 int i = 0;
742
743                 while(bytes_remaining > 0)
744                 {
745                         last_dword[i++] = *(buffer + bytes_written);
746                         bytes_remaining--;
747                         bytes_written++;
748                 }
749
750                 str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
751
752                 field.tap = tap;
753                 field.num_bits = 64;
754                 field.out_value = last_dword;
755                 field.in_value = NULL;
756
757                 jtag_add_dr_scan(1, &field, TAP_IDLE);
758
759                 /* small delay before polling */
760                 jtag_add_sleep(50);
761
762                 str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
763
764                 do {
765                         field.tap = tap;
766                         field.num_bits = 8;
767                         field.out_value = NULL;
768                         field.in_value = scanbuf;
769
770                         jtag_add_dr_scan(1, &field, TAP_INVALID);
771                         jtag_execute_queue();
772
773                         status = buf_get_u32(scanbuf, 0, 8);
774
775                 } while(!(status & ISC_STATUS_BUSY));
776
777                 if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
778                         return ERROR_FLASH_OPERATION_FAILED;
779
780                 /* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
781                         return ERROR_FLASH_OPERATION_FAILED; */
782         }
783
784         free(scanbuf);
785
786         str9xpec_isc_disable(bank);
787
788         return ERROR_OK;
789 }
790
791 static int str9xpec_probe(struct flash_bank_s *bank)
792 {
793         return ERROR_OK;
794 }
795
796 static int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
797 {
798         flash_bank_t *bank;
799         scan_field_t field;
800         u8 *buffer = NULL;
801         jtag_tap_t *tap;
802         u32 idcode;
803         str9xpec_flash_controller_t *str9xpec_info = NULL;
804
805         if (argc < 1)
806         {
807                 return ERROR_COMMAND_SYNTAX_ERROR;
808         }
809
810         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
811         if (!bank)
812         {
813                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
814                 return ERROR_OK;
815         }
816
817         str9xpec_info = bank->driver_priv;
818         tap = str9xpec_info->tap;
819
820         buffer = calloc(CEIL(32, 8), 1);
821
822         str9xpec_set_instr(tap, ISC_IDCODE, TAP_IRPAUSE);
823
824         field.tap = tap;
825         field.num_bits = 32;
826         field.out_value = NULL;
827         field.in_value = buffer;
828
829         jtag_add_dr_scan(1, &field, TAP_IDLE);
830         jtag_execute_queue();
831
832         idcode = buf_get_u32(buffer, 0, 32);
833
834         command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
835
836         free(buffer);
837
838         return ERROR_OK;
839 }
840
841 static int str9xpec_erase_check(struct flash_bank_s *bank)
842 {
843         return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
844 }
845
846 static int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
847 {
848         snprintf(buf, buf_size, "str9xpec flash driver info" );
849         return ERROR_OK;
850 }
851
852 static int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
853 {
854         flash_bank_t *bank;
855         u8 status;
856         str9xpec_flash_controller_t *str9xpec_info = NULL;
857
858         if (argc < 1)
859         {
860                 command_print(cmd_ctx, "str9xpec options_read <bank>");
861                 return ERROR_OK;
862         }
863
864         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
865         if (!bank)
866         {
867                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
868                 return ERROR_OK;
869         }
870
871         str9xpec_info = bank->driver_priv;
872
873         status = str9xpec_read_config(bank);
874
875         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
876                 return ERROR_FLASH_OPERATION_FAILED;
877
878         /* boot bank */
879         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
880                 command_print(cmd_ctx, "CS Map: bank1");
881         else
882                 command_print(cmd_ctx, "CS Map: bank0");
883
884         /* OTP lock */
885         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
886                 command_print(cmd_ctx, "OTP Lock: OTP Locked");
887         else
888                 command_print(cmd_ctx, "OTP Lock: OTP Unlocked");
889
890         /* LVD Threshold */
891         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
892                 command_print(cmd_ctx, "LVD Threshold: 2.7v");
893         else
894                 command_print(cmd_ctx, "LVD Threshold: 2.4v");
895
896         /* LVD reset warning */
897         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
898                 command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");
899         else
900                 command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");
901
902         /* LVD reset select */
903         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
904                 command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");
905         else
906                 command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");
907
908         return ERROR_OK;
909 }
910
911 static int str9xpec_write_options(struct flash_bank_s *bank)
912 {
913         scan_field_t field;
914         u8 status;
915         jtag_tap_t *tap;
916         str9xpec_flash_controller_t *str9xpec_info = NULL;
917
918         str9xpec_info = bank->driver_priv;
919         tap = str9xpec_info->tap;
920
921         /* erase config options first */
922         status = str9xpec_erase_area( bank, 0xFE, 0xFE );
923
924         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
925                 return status;
926
927         if (!str9xpec_info->isc_enable) {
928                 str9xpec_isc_enable( bank );
929         }
930
931         if (!str9xpec_info->isc_enable) {
932                 return ISC_STATUS_ERROR;
933         }
934
935         /* according to data 64th bit has to be set */
936         buf_set_u32(str9xpec_info->options, 63, 1, 1);
937
938         /* set option byte address */
939         str9xpec_set_address(bank, 0x50);
940
941         /* execute ISC_PROGRAM command */
942         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
943
944         field.tap = tap;
945         field.num_bits = 64;
946         field.out_value = str9xpec_info->options;
947         field.in_value = NULL;
948
949         jtag_add_dr_scan(1, &field, TAP_IDLE);
950
951         /* small delay before polling */
952         jtag_add_sleep(50);
953
954         str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
955
956         do {
957                 field.tap = tap;
958                 field.num_bits = 8;
959                 field.out_value = NULL;
960                 field.in_value = &status;
961
962                 jtag_add_dr_scan(1, &field, TAP_INVALID);
963                 jtag_execute_queue();
964
965         } while(!(status & ISC_STATUS_BUSY));
966
967         str9xpec_isc_disable(bank);
968
969         return status;
970 }
971
972 static int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
973 {
974         flash_bank_t *bank;
975         u8 status;
976
977         if (argc < 1)
978         {
979                 command_print(cmd_ctx, "str9xpec options_write <bank>");
980                 return ERROR_OK;
981         }
982
983         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
984         if (!bank)
985         {
986                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
987                 return ERROR_OK;
988         }
989
990         status = str9xpec_write_options(bank);
991
992         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
993                 return ERROR_FLASH_OPERATION_FAILED;
994
995         return ERROR_OK;
996 }
997
998 static int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
999 {
1000         flash_bank_t *bank;
1001         str9xpec_flash_controller_t *str9xpec_info = NULL;
1002
1003         if (argc < 2)
1004         {
1005                 command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
1006                 return ERROR_OK;
1007         }
1008
1009         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1010         if (!bank)
1011         {
1012                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1013                 return ERROR_OK;
1014         }
1015
1016         str9xpec_info = bank->driver_priv;
1017
1018         if (strcmp(args[1], "bank1") == 0)
1019         {
1020                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
1021         }
1022         else
1023         {
1024                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
1025         }
1026
1027         return ERROR_OK;
1028 }
1029
1030 static int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1031 {
1032         flash_bank_t *bank;
1033         str9xpec_flash_controller_t *str9xpec_info = NULL;
1034
1035         if (argc < 2)
1036         {
1037                 command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
1038                 return ERROR_OK;
1039         }
1040
1041         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1042         if (!bank)
1043         {
1044                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1045                 return ERROR_OK;
1046         }
1047
1048         str9xpec_info = bank->driver_priv;
1049
1050         if (strcmp(args[1], "2.7v") == 0)
1051         {
1052                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1053         }
1054         else
1055         {
1056                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1057         }
1058
1059         return ERROR_OK;
1060 }
1061
1062 int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1063 {
1064         flash_bank_t *bank;
1065         str9xpec_flash_controller_t *str9xpec_info = NULL;
1066
1067         if (argc < 2)
1068         {
1069                 command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
1070                 return ERROR_OK;
1071         }
1072
1073         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1074         if (!bank)
1075         {
1076                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1077                 return ERROR_OK;
1078         }
1079
1080         str9xpec_info = bank->driver_priv;
1081
1082         if (strcmp(args[1], "vdd_vddq") == 0)
1083         {
1084                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1085         }
1086         else
1087         {
1088                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1089         }
1090
1091         return ERROR_OK;
1092 }
1093
1094 static int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1095 {
1096         flash_bank_t *bank;
1097         str9xpec_flash_controller_t *str9xpec_info = NULL;
1098
1099         if (argc < 2)
1100         {
1101                 command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
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         str9xpec_info = bank->driver_priv;
1113
1114         if (strcmp(args[1], "vdd_vddq") == 0)
1115         {
1116                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1117         }
1118         else
1119         {
1120                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1121         }
1122
1123         return ERROR_OK;
1124 }
1125
1126 static int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1127 {
1128         u8 status;
1129         flash_bank_t *bank;
1130
1131         if (argc < 1)
1132         {
1133                 command_print(cmd_ctx, "str9xpec lock <bank>");
1134                 return ERROR_OK;
1135         }
1136
1137         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1138         if (!bank)
1139         {
1140                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1141                 return ERROR_OK;
1142         }
1143
1144         status = str9xpec_lock_device(bank);
1145
1146         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1147                 return ERROR_FLASH_OPERATION_FAILED;
1148
1149         return ERROR_OK;
1150 }
1151
1152 static int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1153 {
1154         u8 status;
1155         flash_bank_t *bank;
1156
1157         if (argc < 1)
1158         {
1159                 command_print(cmd_ctx, "str9xpec unlock <bank>");
1160                 return ERROR_OK;
1161         }
1162
1163         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1164         if (!bank)
1165         {
1166                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1167                 return ERROR_OK;
1168         }
1169
1170         status = str9xpec_unlock_device(bank);
1171
1172         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1173                 return ERROR_FLASH_OPERATION_FAILED;
1174
1175         return ERROR_OK;
1176 }
1177
1178 static int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1179 {
1180         int retval;
1181         flash_bank_t *bank;
1182         jtag_tap_t *tap0;
1183         jtag_tap_t *tap1;
1184         jtag_tap_t *tap2;
1185         str9xpec_flash_controller_t *str9xpec_info = NULL;
1186
1187         if (argc < 1)
1188         {
1189                 command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
1190                 return ERROR_OK;
1191         }
1192
1193         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1194         if (!bank)
1195         {
1196                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1197                 return ERROR_OK;
1198         }
1199
1200         str9xpec_info = bank->driver_priv;
1201
1202         tap0 = str9xpec_info->tap;
1203
1204         /* remove arm core from chain - enter turbo mode */
1205         tap1 = tap0->next_tap;
1206         if (tap1 == NULL)
1207         {
1208                 /* things are *WRONG* */
1209                 command_print(cmd_ctx,"**STR9FLASH** (tap1) invalid chain?");
1210                 return ERROR_OK;
1211         }
1212         tap2 = tap1->next_tap;
1213         if (tap2 == NULL)
1214         {
1215                 /* things are *WRONG* */
1216                 command_print(cmd_ctx,"**STR9FLASH** (tap2) invalid chain?");
1217                 return ERROR_OK;
1218         }
1219
1220         /* enable turbo mode - TURBO-PROG-ENABLE */
1221         str9xpec_set_instr(tap2, 0xD, TAP_IDLE);
1222         if ((retval = jtag_execute_queue()) != ERROR_OK)
1223                 return retval;
1224
1225         /* modify scan chain - str9 core has been removed */
1226         tap1->enabled = 0;
1227
1228         return ERROR_OK;
1229 }
1230
1231 static int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1232 {
1233         flash_bank_t *bank;
1234         jtag_tap_t *tap;
1235         str9xpec_flash_controller_t *str9xpec_info = NULL;
1236
1237         if (argc < 1)
1238         {
1239                 command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
1240                 return ERROR_OK;
1241         }
1242
1243         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1244         if (!bank)
1245         {
1246                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1247                 return ERROR_OK;
1248         }
1249
1250         str9xpec_info = bank->driver_priv;
1251         tap = str9xpec_info->tap;
1252
1253         if (tap == NULL)
1254                 return ERROR_FAIL;
1255
1256         /* exit turbo mode via RESET */
1257         str9xpec_set_instr(tap, ISC_NOOP, TAP_RESET);
1258         jtag_execute_queue();
1259
1260         /* restore previous scan chain */
1261         if (tap->next_tap) {
1262                 tap->next_tap->enabled = 1;
1263         }
1264
1265         return ERROR_OK;
1266 }