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