git.sur5r.net Git - 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         str9xpec_info->tap = jtag_TapByAbsPosition( jtag_info->tap->abs_chain_position - 1);
 354         str9xpec_info->isc_enable = 0;
 355
 356         str9xpec_build_block_list(bank);
 357
 358         /* clear option byte register */
 359         buf_set_u32(str9xpec_info->options, 0, 64, 0);
 360
 361         return ERROR_OK;
 362 }
 363
 364 int str9xpec_blank_check(struct flash_bank_s *bank, int first, int last)
 365 {
 366         scan_field_t field;
 367         u8 status;
 368         jtag_tap_t *tap;
 369         int i;
 370         u8 *buffer = NULL;
 371
 372         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 373
 374         tap = str9xpec_info->tap;
 375
 376         if (!str9xpec_info->isc_enable) {
 377                 str9xpec_isc_enable( bank );
 378         }
 379
 380         if (!str9xpec_info->isc_enable) {
 381                 return ERROR_FLASH_OPERATION_FAILED;
 382         }
 383
 384         buffer = calloc(CEIL(64, 8), 1);
 385
 386         LOG_DEBUG("blank check: first_bank: %i, last_bank: %i", first, last);
 387
 388         for (i = first; i <= last; i++) {
 389                 buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
 390         }
 391
 392         /* execute ISC_BLANK_CHECK command */
 393         str9xpec_set_instr(tap, ISC_BLANK_CHECK, TAP_PI);
 394
 395         field.tap = tap;
 396         field.num_bits = 64;
 397         field.out_value = buffer;
 398         field.out_mask = NULL;
 399         field.in_value = NULL;
 400         field.in_check_value = NULL;
 401         field.in_check_mask = NULL;
 402         field.in_handler = NULL;
 403         field.in_handler_priv = NULL;
 404
 405         jtag_add_dr_scan(1, &field, TAP_RTI);
 406         jtag_add_sleep(40000);
 407
 408         /* read blank check result */
 409         field.tap = tap;
 410         field.num_bits = 64;
 411         field.out_value = NULL;
 412         field.out_mask = NULL;
 413         field.in_value = buffer;
 414         field.in_check_value = NULL;
 415         field.in_check_mask = NULL;
 416         field.in_handler = NULL;
 417         field.in_handler_priv = NULL;
 418
 419         jtag_add_dr_scan(1, &field, TAP_PI);
 420         jtag_execute_queue();
 421
 422         status = str9xpec_isc_status(tap);
 423
 424         for (i = first; i <= last; i++)
 425         {
 426                 if (buf_get_u32(buffer, str9xpec_info->sector_bits[i], 1))
 427                         bank->sectors[i].is_erased = 0;
 428                 else
 429                         bank->sectors[i].is_erased = 1;
 430         }
 431
 432         free(buffer);
 433
 434         str9xpec_isc_disable(bank);
 435
 436         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 437                 return ERROR_FLASH_OPERATION_FAILED;
 438         return ERROR_OK;
 439 }
 440
 441 int str9xpec_protect_check(struct flash_bank_s *bank)
 442 {
 443         u8 status;
 444         int i;
 445
 446         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 447
 448         status = str9xpec_read_config(bank);
 449
 450         for (i = 0; i < bank->num_sectors; i++)
 451         {
 452                 if (buf_get_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1))
 453                         bank->sectors[i].is_protected = 1;
 454                 else
 455                         bank->sectors[i].is_protected = 0;
 456         }
 457
 458         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 459                 return ERROR_FLASH_OPERATION_FAILED;
 460         return ERROR_OK;
 461 }
 462
 463 int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last)
 464 {
 465         scan_field_t field;
 466         u8 status;
 467         jtag_tap_t *tap;
 468         int i;
 469         u8 *buffer = NULL;
 470
 471         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 472
 473         tap = str9xpec_info->tap;
 474
 475         if (!str9xpec_info->isc_enable) {
 476                 str9xpec_isc_enable( bank );
 477         }
 478
 479         if (!str9xpec_info->isc_enable) {
 480                 return ISC_STATUS_ERROR;
 481         }
 482
 483         buffer = calloc(CEIL(64, 8), 1);
 484
 485         LOG_DEBUG("erase: first_bank: %i, last_bank: %i", first, last);
 486
 487         /* last bank: 0xFF signals a full erase (unlock complete device) */
 488         /* last bank: 0xFE signals a option byte erase */
 489         if (last == 0xFF)
 490         {
 491                 for (i = 0; i < 64; i++) {
 492                         buf_set_u32(buffer, i, 1, 1);
 493                 }
 494         }
 495         else if (last == 0xFE)
 496         {
 497                 buf_set_u32(buffer, 49, 1, 1);
 498         }
 499         else
 500         {
 501                 for (i = first; i <= last; i++) {
 502                         buf_set_u32(buffer, str9xpec_info->sector_bits[i], 1, 1);
 503                 }
 504         }
 505
 506         LOG_DEBUG("ISC_ERASE");
 507
 508         /* execute ISC_ERASE command */
 509         str9xpec_set_instr(tap, ISC_ERASE, TAP_PI);
 510
 511         field.tap = tap;
 512         field.num_bits = 64;
 513         field.out_value = buffer;
 514         field.out_mask = NULL;
 515         field.in_value = NULL;
 516         field.in_check_value = NULL;
 517         field.in_check_mask = NULL;
 518         field.in_handler = NULL;
 519         field.in_handler_priv = NULL;
 520
 521         jtag_add_dr_scan(1, &field, TAP_RTI);
 522         jtag_execute_queue();
 523
 524         jtag_add_sleep(10);
 525
 526         /* wait for erase completion */
 527         while (!((status = str9xpec_isc_status(tap)) & ISC_STATUS_BUSY)) {
 528                 alive_sleep(1);
 529         }
 530
 531         free(buffer);
 532
 533         str9xpec_isc_disable(bank);
 534
 535         return status;
 536 }
 537
 538 int str9xpec_erase(struct flash_bank_s *bank, int first, int last)
 539 {
 540         int status;
 541
 542         status = str9xpec_erase_area(bank, first, last);
 543
 544         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 545                 return ERROR_FLASH_OPERATION_FAILED;
 546
 547         return ERROR_OK;
 548 }
 549
 550 int str9xpec_lock_device(struct flash_bank_s *bank)
 551 {
 552         scan_field_t field;
 553         u8 status;
 554         jtag_tap_t *tap;
 555         str9xpec_flash_controller_t *str9xpec_info = NULL;
 556
 557         str9xpec_info = bank->driver_priv;
 558         tap = str9xpec_info->tap;
 559
 560         if (!str9xpec_info->isc_enable) {
 561                 str9xpec_isc_enable( bank );
 562         }
 563
 564         if (!str9xpec_info->isc_enable) {
 565                 return ISC_STATUS_ERROR;
 566         }
 567
 568         /* set security address */
 569         str9xpec_set_address(bank, 0x80);
 570
 571         /* execute ISC_PROGRAM command */
 572         str9xpec_set_instr(tap, ISC_PROGRAM_SECURITY, TAP_RTI);
 573
 574         str9xpec_set_instr(tap, ISC_NOOP, TAP_PI);
 575
 576         do {
 577                 field.tap = tap;
 578                 field.num_bits = 8;
 579                 field.out_value = NULL;
 580                 field.out_mask = NULL;
 581                 field.in_value = &status;
 582                 field.in_check_value = NULL;
 583                 field.in_check_mask = NULL;
 584                 field.in_handler = NULL;
 585                 field.in_handler_priv = NULL;
 586
 587                 jtag_add_dr_scan(1, &field, -1);
 588                 jtag_execute_queue();
 589
 590         } while(!(status & ISC_STATUS_BUSY));
 591
 592         str9xpec_isc_disable(bank);
 593
 594         return status;
 595 }
 596
 597 int str9xpec_unlock_device(struct flash_bank_s *bank)
 598 {
 599         u8 status;
 600
 601         status = str9xpec_erase_area(bank, 0, 255);
 602
 603         return status;
 604 }
 605
 606 int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last)
 607 {
 608         u8 status;
 609         int i;
 610
 611         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 612
 613         status = str9xpec_read_config(bank);
 614
 615         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 616                 return ERROR_FLASH_OPERATION_FAILED;
 617
 618         LOG_DEBUG("protect: first_bank: %i, last_bank: %i", first, last);
 619
 620         /* last bank: 0xFF signals a full device protect */
 621         if (last == 0xFF)
 622         {
 623                 if( set )
 624                 {
 625                         status = str9xpec_lock_device(bank);
 626                 }
 627                 else
 628                 {
 629                         /* perform full erase to unlock device */
 630                         status = str9xpec_unlock_device(bank);
 631                 }
 632         }
 633         else
 634         {
 635                 for (i = first; i <= last; i++)
 636                 {
 637                         if( set )
 638                                 buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 1);
 639                         else
 640                                 buf_set_u32(str9xpec_info->options, str9xpec_info->sector_bits[i], 1, 0);
 641                 }
 642
 643                 status = str9xpec_write_options(bank);
 644         }
 645
 646         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 647                 return ERROR_FLASH_OPERATION_FAILED;
 648
 649         return ERROR_OK;
 650 }
 651
 652 int str9xpec_set_address(struct flash_bank_s *bank, u8 sector)
 653 {
 654         jtag_tap_t *tap;
 655         scan_field_t field;
 656         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 657
 658         tap = str9xpec_info->tap;
 659
 660         /* set flash controller address */
 661         str9xpec_set_instr(tap, ISC_ADDRESS_SHIFT, TAP_PI);
 662
 663         field.tap = tap;
 664         field.num_bits = 8;
 665         field.out_value = &sector;
 666         field.out_mask = NULL;
 667         field.in_value = NULL;
 668         field.in_check_value = NULL;
 669         field.in_check_mask = NULL;
 670         field.in_handler = NULL;
 671         field.in_handler_priv = NULL;
 672
 673         jtag_add_dr_scan(1, &field, -1);
 674
 675         return ERROR_OK;
 676 }
 677
 678 int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count)
 679 {
 680         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 681         u32 dwords_remaining = (count / 8);
 682         u32 bytes_remaining = (count & 0x00000007);
 683         u32 bytes_written = 0;
 684         u8 status;
 685         u32 check_address = offset;
 686         jtag_tap_t *tap;
 687         scan_field_t field;
 688         u8 *scanbuf;
 689         int i;
 690         u32 first_sector = 0;
 691         u32 last_sector = 0;
 692
 693         tap = str9xpec_info->tap;
 694
 695         if (!str9xpec_info->isc_enable) {
 696                 str9xpec_isc_enable(bank);
 697         }
 698
 699         if (!str9xpec_info->isc_enable) {
 700                 return ERROR_FLASH_OPERATION_FAILED;
 701         }
 702
 703         if (offset & 0x7)
 704         {
 705                 LOG_WARNING("offset 0x%x breaks required 8-byte alignment", offset);
 706                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 707         }
 708
 709         for (i = 0; i < bank->num_sectors; i++)
 710         {
 711                 u32 sec_start = bank->sectors[i].offset;
 712                 u32 sec_end = sec_start + bank->sectors[i].size;
 713
 714                 /* check if destination falls within the current sector */
 715                 if ((check_address >= sec_start) && (check_address < sec_end))
 716                 {
 717                         /* check if destination ends in the current sector */
 718                         if (offset + count < sec_end)
 719                                 check_address = offset + count;
 720                         else
 721                                 check_address = sec_end;
 722                 }
 723
 724                 if ((offset >= sec_start) && (offset < sec_end)){
 725                         first_sector = i;
 726                 }
 727
 728                 if ((offset + count >= sec_start) && (offset + count < sec_end)){
 729                         last_sector = i;
 730                 }
 731         }
 732
 733         if (check_address != offset + count)
 734                 return ERROR_FLASH_DST_OUT_OF_BANK;
 735
 736         LOG_DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
 737
 738         scanbuf = calloc(CEIL(64, 8), 1);
 739
 740         LOG_DEBUG("ISC_PROGRAM");
 741
 742         for (i = first_sector; i <= last_sector; i++)
 743         {
 744                 str9xpec_set_address(bank, str9xpec_info->sector_bits[i]);
 745
 746                 dwords_remaining = dwords_remaining < (bank->sectors[i].size/8) ? dwords_remaining : (bank->sectors[i].size/8);
 747
 748                 while (dwords_remaining > 0)
 749                 {
 750                         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_PI);
 751
 752                         field.tap = tap;
 753                         field.num_bits = 64;
 754                         field.out_value = (buffer + bytes_written);
 755                         field.out_mask = NULL;
 756                         field.in_value = NULL;
 757                         field.in_check_value = NULL;
 758                         field.in_check_mask = NULL;
 759                         field.in_handler = NULL;
 760                         field.in_handler_priv = NULL;
 761
 762                         jtag_add_dr_scan(1, &field, TAP_RTI);
 763
 764                         /* small delay before polling */
 765                         jtag_add_sleep(50);
 766
 767                         str9xpec_set_instr(tap, ISC_NOOP, TAP_PI);
 768
 769                         do {
 770                                 field.tap = tap;
 771                                 field.num_bits = 8;
 772                                 field.out_value = NULL;
 773                                 field.out_mask = NULL;
 774                                 field.in_value = scanbuf;
 775                                 field.in_check_value = NULL;
 776                                 field.in_check_mask = NULL;
 777                                 field.in_handler = NULL;
 778                                 field.in_handler_priv = NULL;
 779
 780                                 jtag_add_dr_scan(1, &field, -1);
 781                                 jtag_execute_queue();
 782
 783                                 status = buf_get_u32(scanbuf, 0, 8);
 784
 785                         } while(!(status & ISC_STATUS_BUSY));
 786
 787                         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 788                                 return ERROR_FLASH_OPERATION_FAILED;
 789
 790                         /* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
 791                                 return ERROR_FLASH_OPERATION_FAILED; */
 792
 793                         dwords_remaining--;
 794                         bytes_written += 8;
 795                 }
 796         }
 797
 798         if (bytes_remaining)
 799         {
 800                 u8 last_dword[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
 801                 int i = 0;
 802
 803                 while(bytes_remaining > 0)
 804                 {
 805                         last_dword[i++] = *(buffer + bytes_written);
 806                         bytes_remaining--;
 807                         bytes_written++;
 808                 }
 809
 810                 str9xpec_set_instr(tap, ISC_PROGRAM, TAP_PI);
 811
 812                 field.tap = tap;
 813                 field.num_bits = 64;
 814                 field.out_value = last_dword;
 815                 field.out_mask = NULL;
 816                 field.in_value = NULL;
 817                 field.in_check_value = NULL;
 818                 field.in_check_mask = NULL;
 819                 field.in_handler = NULL;
 820                 field.in_handler_priv = NULL;
 821
 822                 jtag_add_dr_scan(1, &field, TAP_RTI);
 823
 824                 /* small delay before polling */
 825                 jtag_add_sleep(50);
 826
 827                 str9xpec_set_instr(tap, ISC_NOOP, TAP_PI);
 828
 829                 do {
 830                         field.tap = tap;
 831                         field.num_bits = 8;
 832                         field.out_value = NULL;
 833                         field.out_mask = NULL;
 834                         field.in_value = scanbuf;
 835                         field.in_check_value = NULL;
 836                         field.in_check_mask = NULL;
 837                         field.in_handler = NULL;
 838                         field.in_handler_priv = NULL;
 839
 840                         jtag_add_dr_scan(1, &field, -1);
 841                         jtag_execute_queue();
 842
 843                         status = buf_get_u32(scanbuf, 0, 8);
 844
 845                 } while(!(status & ISC_STATUS_BUSY));
 846
 847                 if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 848                         return ERROR_FLASH_OPERATION_FAILED;
 849
 850                 /* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
 851                         return ERROR_FLASH_OPERATION_FAILED; */
 852         }
 853
 854         free(scanbuf);
 855
 856         str9xpec_isc_disable(bank);
 857
 858         return ERROR_OK;
 859 }
 860
 861 int str9xpec_probe(struct flash_bank_s *bank)
 862 {
 863         return ERROR_OK;
 864 }
 865
 866 int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 867 {
 868         flash_bank_t *bank;
 869         scan_field_t field;
 870         u8 *buffer = NULL;
 871         jtag_tap_t *tap;
 872         u32 idcode;
 873         str9xpec_flash_controller_t *str9xpec_info = NULL;
 874
 875         if (argc < 1)
 876         {
 877                 return ERROR_COMMAND_SYNTAX_ERROR;
 878         }
 879
 880         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 881         if (!bank)
 882         {
 883                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 884                 return ERROR_OK;
 885         }
 886
 887         str9xpec_info = bank->driver_priv;
 888         tap = str9xpec_info->tap;
 889
 890         buffer = calloc(CEIL(32, 8), 1);
 891
 892         str9xpec_set_instr(tap, ISC_IDCODE, TAP_PI);
 893
 894         field.tap = tap;
 895         field.num_bits = 32;
 896         field.out_value = NULL;
 897         field.out_mask = NULL;
 898         field.in_value = buffer;
 899         field.in_check_value = NULL;
 900         field.in_check_mask = NULL;
 901         field.in_handler = NULL;
 902         field.in_handler_priv = NULL;
 903
 904         jtag_add_dr_scan(1, &field, TAP_RTI);
 905         jtag_execute_queue();
 906
 907         idcode = buf_get_u32(buffer, 0, 32);
 908
 909         command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
 910
 911         free(buffer);
 912
 913         return ERROR_OK;
 914 }
 915
 916 int str9xpec_erase_check(struct flash_bank_s *bank)
 917 {
 918         return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
 919 }
 920
 921 int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
 922 {
 923         snprintf(buf, buf_size, "str9xpec flash driver info" );
 924         return ERROR_OK;
 925 }
 926
 927 int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 928 {
 929         flash_bank_t *bank;
 930         u8 status;
 931         str9xpec_flash_controller_t *str9xpec_info = NULL;
 932
 933         if (argc < 1)
 934         {
 935                 command_print(cmd_ctx, "str9xpec options_read <bank>");
 936                 return ERROR_OK;
 937         }
 938
 939         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 940         if (!bank)
 941         {
 942                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 943                 return ERROR_OK;
 944         }
 945
 946         str9xpec_info = bank->driver_priv;
 947
 948         status = str9xpec_read_config(bank);
 949
 950         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 951                 return ERROR_FLASH_OPERATION_FAILED;
 952
 953         /* boot bank */
 954         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
 955                 command_print(cmd_ctx, "CS Map: bank1");
 956         else
 957                 command_print(cmd_ctx, "CS Map: bank0");
 958
 959         /* OTP lock */
 960         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
 961                 command_print(cmd_ctx, "OTP Lock: OTP Locked");
 962         else
 963                 command_print(cmd_ctx, "OTP Lock: OTP Unlocked");
 964
 965         /* LVD Threshold */
 966         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
 967                 command_print(cmd_ctx, "LVD Threshold: 2.7v");
 968         else
 969                 command_print(cmd_ctx, "LVD Threshold: 2.4v");
 970
 971         /* LVD reset warning */
 972         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
 973                 command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");
 974         else
 975                 command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");
 976
 977         /* LVD reset select */
 978         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
 979                 command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");
 980         else
 981                 command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");
 982
 983         return ERROR_OK;
 984 }
 985
 986 int str9xpec_write_options(struct flash_bank_s *bank)
 987 {
 988         scan_field_t field;
 989         u8 status;
 990         jtag_tap_t *tap;
 991         str9xpec_flash_controller_t *str9xpec_info = NULL;
 992
 993         str9xpec_info = bank->driver_priv;
 994         tap = str9xpec_info->tap;
 995
 996         /* erase config options first */
 997         status = str9xpec_erase_area( bank, 0xFE, 0xFE );
 998
 999         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1000                 return status;
1001
1002         if (!str9xpec_info->isc_enable) {
1003                 str9xpec_isc_enable( bank );
1004         }
1005
1006         if (!str9xpec_info->isc_enable) {
1007                 return ISC_STATUS_ERROR;
1008         }
1009
1010         /* according to data 64th bit has to be set */
1011         buf_set_u32(str9xpec_info->options, 63, 1, 1);
1012
1013         /* set option byte address */
1014         str9xpec_set_address(bank, 0x50);
1015
1016         /* execute ISC_PROGRAM command */
1017         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_PI);
1018
1019         field.tap = tap;
1020         field.num_bits = 64;
1021         field.out_value = str9xpec_info->options;
1022         field.out_mask = NULL;
1023         field.in_value = NULL;
1024         field.in_check_value = NULL;
1025         field.in_check_mask = NULL;
1026         field.in_handler = NULL;
1027         field.in_handler_priv = NULL;
1028
1029         jtag_add_dr_scan(1, &field, TAP_RTI);
1030
1031         /* small delay before polling */
1032         jtag_add_sleep(50);
1033
1034         str9xpec_set_instr(tap, ISC_NOOP, TAP_PI);
1035
1036         do {
1037                 field.tap = tap;
1038                 field.num_bits = 8;
1039                 field.out_value = NULL;
1040                 field.out_mask = NULL;
1041                 field.in_value = &status;
1042                 field.in_check_value = NULL;
1043                 field.in_check_mask = NULL;
1044                 field.in_handler = NULL;
1045                 field.in_handler_priv = NULL;
1046
1047                 jtag_add_dr_scan(1, &field, -1);
1048                 jtag_execute_queue();
1049
1050         } while(!(status & ISC_STATUS_BUSY));
1051
1052         str9xpec_isc_disable(bank);
1053
1054         return status;
1055 }
1056
1057 int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1058 {
1059         flash_bank_t *bank;
1060         u8 status;
1061
1062         if (argc < 1)
1063         {
1064                 command_print(cmd_ctx, "str9xpec options_write <bank>");
1065                 return ERROR_OK;
1066         }
1067
1068         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1069         if (!bank)
1070         {
1071                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1072                 return ERROR_OK;
1073         }
1074
1075         status = str9xpec_write_options(bank);
1076
1077         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1078                 return ERROR_FLASH_OPERATION_FAILED;
1079
1080         return ERROR_OK;
1081 }
1082
1083 int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1084 {
1085         flash_bank_t *bank;
1086         str9xpec_flash_controller_t *str9xpec_info = NULL;
1087
1088         if (argc < 2)
1089         {
1090                 command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
1091                 return ERROR_OK;
1092         }
1093
1094         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1095         if (!bank)
1096         {
1097                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1098                 return ERROR_OK;
1099         }
1100
1101         str9xpec_info = bank->driver_priv;
1102
1103         if (strcmp(args[1], "bank1") == 0)
1104         {
1105                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
1106         }
1107         else
1108         {
1109                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
1110         }
1111
1112         return ERROR_OK;
1113 }
1114
1115 int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1116 {
1117         flash_bank_t *bank;
1118         str9xpec_flash_controller_t *str9xpec_info = NULL;
1119
1120         if (argc < 2)
1121         {
1122                 command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
1123                 return ERROR_OK;
1124         }
1125
1126         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1127         if (!bank)
1128         {
1129                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1130                 return ERROR_OK;
1131         }
1132
1133         str9xpec_info = bank->driver_priv;
1134
1135         if (strcmp(args[1], "2.7v") == 0)
1136         {
1137                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1138         }
1139         else
1140         {
1141                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1142         }
1143
1144         return ERROR_OK;
1145 }
1146
1147 int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1148 {
1149         flash_bank_t *bank;
1150         str9xpec_flash_controller_t *str9xpec_info = NULL;
1151
1152         if (argc < 2)
1153         {
1154                 command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
1155                 return ERROR_OK;
1156         }
1157
1158         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1159         if (!bank)
1160         {
1161                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1162                 return ERROR_OK;
1163         }
1164
1165         str9xpec_info = bank->driver_priv;
1166
1167         if (strcmp(args[1], "vdd_vddq") == 0)
1168         {
1169                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1170         }
1171         else
1172         {
1173                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1174         }
1175
1176         return ERROR_OK;
1177 }
1178
1179 int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1180 {
1181         flash_bank_t *bank;
1182         str9xpec_flash_controller_t *str9xpec_info = NULL;
1183
1184         if (argc < 2)
1185         {
1186                 command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
1187                 return ERROR_OK;
1188         }
1189
1190         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1191         if (!bank)
1192         {
1193                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1194                 return ERROR_OK;
1195         }
1196
1197         str9xpec_info = bank->driver_priv;
1198
1199         if (strcmp(args[1], "vdd_vddq") == 0)
1200         {
1201                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1202         }
1203         else
1204         {
1205                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1206         }
1207
1208         return ERROR_OK;
1209 }
1210
1211 int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1212 {
1213         u8 status;
1214         flash_bank_t *bank;
1215
1216         if (argc < 1)
1217         {
1218                 command_print(cmd_ctx, "str9xpec lock <bank>");
1219                 return ERROR_OK;
1220         }
1221
1222         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1223         if (!bank)
1224         {
1225                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1226                 return ERROR_OK;
1227         }
1228
1229         status = str9xpec_lock_device(bank);
1230
1231         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1232                 return ERROR_FLASH_OPERATION_FAILED;
1233
1234         return ERROR_OK;
1235 }
1236
1237 int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1238 {
1239         u8 status;
1240         flash_bank_t *bank;
1241
1242         if (argc < 1)
1243         {
1244                 command_print(cmd_ctx, "str9xpec unlock <bank>");
1245                 return ERROR_OK;
1246         }
1247
1248         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1249         if (!bank)
1250         {
1251                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1252                 return ERROR_OK;
1253         }
1254
1255         status = str9xpec_unlock_device(bank);
1256
1257         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1258                 return ERROR_FLASH_OPERATION_FAILED;
1259
1260         return ERROR_OK;
1261 }
1262
1263 int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1264 {
1265         int retval;
1266         flash_bank_t *bank;
1267         jtag_tap_t *tap0;
1268         jtag_tap_t *tap1;
1269         jtag_tap_t *tap2;
1270         str9xpec_flash_controller_t *str9xpec_info = NULL;
1271
1272         if (argc < 1)
1273         {
1274                 command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
1275                 return ERROR_OK;
1276         }
1277
1278         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1279         if (!bank)
1280         {
1281                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1282                 return ERROR_OK;
1283         }
1284
1285         str9xpec_info = bank->driver_priv;
1286
1287         tap0 = str9xpec_info->tap;
1288
1289         /* remove arm core from chain - enter turbo mode */
1290         tap1 = tap0->next_tap;
1291         if (tap1 == NULL)
1292         {
1293                 /* things are *WRONG* */
1294                 command_print(cmd_ctx,"**STR9FLASH** (tap1) invalid chain?");
1295                 return ERROR_OK;
1296         }
1297         tap2 = tap1->next_tap;
1298         if (tap2 == NULL)
1299         {
1300                 /* things are *WRONG* */
1301                 command_print(cmd_ctx,"**STR9FLASH** (tap2) invalid chain?");
1302                 return ERROR_OK;
1303         }
1304
1305         /* enable turbo mode - TURBO-PROG-ENABLE */
1306         str9xpec_set_instr(tap2, 0xD, TAP_RTI);
1307         if ((retval = jtag_execute_queue()) != ERROR_OK)
1308                 return retval;
1309
1310         /* modify scan chain - str9 core has been removed */
1311         tap1->enabled = 0;
1312
1313         return ERROR_OK;
1314 }
1315
1316 int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1317 {
1318         flash_bank_t *bank;
1319         jtag_tap_t *tap;
1320         str9xpec_flash_controller_t *str9xpec_info = NULL;
1321
1322         if (argc < 1)
1323         {
1324                 command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
1325                 return ERROR_OK;
1326         }
1327
1328         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1329         if (!bank)
1330         {
1331                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1332                 return ERROR_OK;
1333         }
1334
1335         str9xpec_info = bank->driver_priv;
1336         tap = str9xpec_info->tap;
1337
1338         if (tap == NULL)
1339                 return ERROR_FAIL;
1340
1341         /* exit turbo mode via TLR */
1342         str9xpec_set_instr(tap, ISC_NOOP, TAP_TLR);
1343         jtag_execute_queue();
1344
1345         /* restore previous scan chain */
1346         if (tap->next_tap) {
1347                 tap->next_tap->enabled = 1;
1348         }
1349
1350         return ERROR_OK;
1351 }