git.gag.com Git - fw/openocd/blob - src/flash/str9xpec.c

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