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(int chain_pos, u32 new_instr, enum tap_state end_state)
 115 {
 116         jtag_device_t *device = jtag_get_device(chain_pos);
 117         if (device == NULL)
 118         {
 119                 return ERROR_TARGET_INVALID;
 120         }
 121
 122         if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
 123         {
 124                 scan_field_t field;
 125
 126                 field.device = chain_pos;
 127                 field.num_bits = device->ir_length;
 128                 field.out_value = calloc(CEIL(field.num_bits, 8), 1);
 129                 buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
 130                 field.out_mask = NULL;
 131                 field.in_value = NULL;
 132                 field.in_check_value = NULL;
 133                 field.in_check_mask = NULL;
 134                 field.in_handler = NULL;
 135                 field.in_handler_priv = NULL;
 136
 137                 jtag_add_ir_scan(1, &field, end_state);
 138
 139                 free(field.out_value);
 140         }
 141
 142         return ERROR_OK;
 143 }
 144
 145 u8 str9xpec_isc_status(int chain_pos)
 146 {
 147         scan_field_t field;
 148         u8 status;
 149
 150         if (str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI) != ERROR_OK)
 151                 return ISC_STATUS_ERROR;
 152
 153         field.device = chain_pos;
 154         field.num_bits = 8;
 155         field.out_value = NULL;
 156         field.out_mask = NULL;
 157         field.in_value = &status;
 158         field.in_check_value = NULL;
 159         field.in_check_mask = NULL;
 160         field.in_handler = NULL;
 161         field.in_handler_priv = NULL;
 162
 163         jtag_add_dr_scan(1, &field, TAP_RTI);
 164         jtag_execute_queue();
 165
 166         LOG_DEBUG("status: 0x%2.2x", status);
 167
 168         if (status & ISC_STATUS_SECURITY)
 169                 LOG_INFO("Device Security Bit Set");
 170
 171         return status;
 172 }
 173
 174 int str9xpec_isc_enable(struct flash_bank_s *bank)
 175 {
 176         u8 status;
 177         u32 chain_pos;
 178         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 179
 180         chain_pos = str9xpec_info->chain_pos;
 181
 182         if (str9xpec_info->isc_enable)
 183                 return ERROR_OK;
 184
 185         /* enter isc mode */
 186         if (str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI) != ERROR_OK)
 187                 return ERROR_TARGET_INVALID;
 188
 189         /* check ISC status */
 190         status = str9xpec_isc_status(chain_pos);
 191         if (status & ISC_STATUS_MODE)
 192         {
 193                 /* we have entered isc mode */
 194                 str9xpec_info->isc_enable = 1;
 195                 LOG_DEBUG("ISC_MODE Enabled");
 196         }
 197
 198         return ERROR_OK;
 199 }
 200
 201 int str9xpec_isc_disable(struct flash_bank_s *bank)
 202 {
 203         u8 status;
 204         u32 chain_pos;
 205         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 206
 207         chain_pos = str9xpec_info->chain_pos;
 208
 209         if (!str9xpec_info->isc_enable)
 210                 return ERROR_OK;
 211
 212         if (str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI) != ERROR_OK)
 213                 return ERROR_TARGET_INVALID;
 214
 215         /* delay to handle aborts */
 216         jtag_add_sleep(50);
 217
 218         /* check ISC status */
 219         status = str9xpec_isc_status(chain_pos);
 220         if (!(status & ISC_STATUS_MODE))
 221         {
 222                 /* we have left isc mode */
 223                 str9xpec_info->isc_enable = 0;
 224                 LOG_DEBUG("ISC_MODE Disabled");
 225         }
 226
 227         return ERROR_OK;
 228 }
 229
 230 int str9xpec_read_config(struct flash_bank_s *bank)
 231 {
 232         scan_field_t field;
 233         u8 status;
 234         u32 chain_pos;
 235
 236         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 237
 238         chain_pos = str9xpec_info->chain_pos;
 239
 240         LOG_DEBUG("ISC_CONFIGURATION");
 241
 242         /* execute ISC_CONFIGURATION command */
 243         str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);
 244
 245         field.device = chain_pos;
 246         field.num_bits = 64;
 247         field.out_value = NULL;
 248         field.out_mask = NULL;
 249         field.in_value = str9xpec_info->options;
 250         field.in_check_value = NULL;
 251         field.in_check_mask = NULL;
 252         field.in_handler = NULL;
 253         field.in_handler_priv = NULL;
 254
 255         jtag_add_dr_scan(1, &field, TAP_RTI);
 256         jtag_execute_queue();
 257
 258         status = str9xpec_isc_status(chain_pos);
 259
 260         return status;
 261 }
 262
 263 int str9xpec_build_block_list(struct flash_bank_s *bank)
 264 {
 265         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 266
 267         int i;
 268         int num_sectors;
 269         int b0_sectors = 0, b1_sectors = 0;
 270         u32 offset = 0;
 271         int b1_size = 0x2000;
 272
 273         switch (bank->size)
 274         {
 275                 case (256 * 1024):
 276                         b0_sectors = 4;
 277                         break;
 278                 case (512 * 1024):
 279                         b0_sectors = 8;
 280                         break;
 281                 case (1024 * 1024):
 282                         b0_sectors = 16;
 283                         break;
 284                 case (2048 * 1024):
 285                         b0_sectors = 32;
 286                         break;
 287                 case (128 * 1024):
 288                         b1_size = 0x4000;
 289                         b1_sectors = 8;
 290                         break;
 291                 case (32 * 1024):
 292                         b1_sectors = 4;
 293                         break;
 294                 default:
 295                         LOG_ERROR("BUG: unknown bank->size encountered");
 296                         exit(-1);
 297         }
 298
 299         num_sectors = b0_sectors + b1_sectors;
 300
 301         bank->num_sectors = num_sectors;
 302         bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
 303         str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);
 304
 305         num_sectors = 0;
 306
 307         for (i = 0; i < b0_sectors; i++)
 308         {
 309                 bank->sectors[num_sectors].offset = offset;
 310                 bank->sectors[num_sectors].size = 0x10000;
 311                 offset += bank->sectors[i].size;
 312                 bank->sectors[num_sectors].is_erased = -1;
 313                 bank->sectors[num_sectors].is_protected = 1;
 314                 str9xpec_info->sector_bits[num_sectors++] = i;
 315         }
 316
 317         for (i = 0; i < b1_sectors; i++)
 318         {
 319                 bank->sectors[num_sectors].offset = offset;
 320                 bank->sectors[num_sectors].size = b1_size;
 321                 offset += bank->sectors[i].size;
 322                 bank->sectors[num_sectors].is_erased = -1;
 323                 bank->sectors[num_sectors].is_protected = 1;
 324                 str9xpec_info->sector_bits[num_sectors++] = i + 32;
 325         }
 326
 327         return ERROR_OK;
 328 }
 329
 330 /* flash bank str9x <base> <size> 0 0 <target#>
 331  */
 332 int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
 333 {
 334         str9xpec_flash_controller_t *str9xpec_info;
 335         armv4_5_common_t *armv4_5 = NULL;
 336         arm7_9_common_t *arm7_9 = NULL;
 337         arm_jtag_t *jtag_info = NULL;
 338
 339         if (argc < 6)
 340         {
 341                 LOG_WARNING("incomplete flash_bank str9x configuration");
 342                 return ERROR_FLASH_BANK_INVALID;
 343         }
 344
 345         str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));
 346         bank->driver_priv = str9xpec_info;
 347
 348         /* find out jtag position of flash controller
 349          * it is always after the arm966 core */
 350
 351         armv4_5 = bank->target->arch_info;
 352         arm7_9 = armv4_5->arch_info;
 353         jtag_info = &arm7_9->jtag_info;
 354
 355         str9xpec_info->chain_pos = (jtag_info->chain_pos - 1);
 356         str9xpec_info->isc_enable = 0;
 357         str9xpec_info->devarm = NULL;
 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         u32 chain_pos;
 372         int i;
 373         u8 *buffer = NULL;
 374
 375         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 376
 377         chain_pos = str9xpec_info->chain_pos;
 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(chain_pos, ISC_BLANK_CHECK, TAP_PI);
 397
 398         field.device = chain_pos;
 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.device = chain_pos;
 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(chain_pos);
 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         u32 chain_pos;
 471         int i;
 472         u8 *buffer = NULL;
 473
 474         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 475
 476         chain_pos = str9xpec_info->chain_pos;
 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(chain_pos, ISC_ERASE, TAP_PI);
 513
 514         field.device = chain_pos;
 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(chain_pos)) & 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         u32 chain_pos;
 558         str9xpec_flash_controller_t *str9xpec_info = NULL;
 559
 560         str9xpec_info = bank->driver_priv;
 561         chain_pos = str9xpec_info->chain_pos;
 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(chain_pos, ISC_PROGRAM_SECURITY, TAP_RTI);
 576
 577         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
 578
 579         do {
 580                 field.device = chain_pos;
 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         u32 chain_pos;
 658         scan_field_t field;
 659         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 660
 661         chain_pos = str9xpec_info->chain_pos;
 662
 663         /* set flash controller address */
 664         str9xpec_set_instr(chain_pos, ISC_ADDRESS_SHIFT, TAP_PI);
 665
 666         field.device = chain_pos;
 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         u32 chain_pos;
 690         scan_field_t field;
 691         u8 *scanbuf;
 692         int i;
 693         u32 first_sector = 0;
 694         u32 last_sector = 0;
 695
 696         chain_pos = str9xpec_info->chain_pos;
 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(chain_pos, ISC_PROGRAM, TAP_PI);
 754
 755                         field.device = chain_pos;
 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(chain_pos, ISC_NOOP, TAP_PI);
 771
 772                         do {
 773                                 field.device = chain_pos;
 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(chain_pos, ISC_PROGRAM, TAP_PI);
 814
 815                 field.device = chain_pos;
 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(chain_pos, ISC_NOOP, TAP_PI);
 831
 832                 do {
 833                         field.device = chain_pos;
 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         u32 chain_pos;
 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         chain_pos = str9xpec_info->chain_pos;
 892
 893         buffer = calloc(CEIL(32, 8), 1);
 894
 895         str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);
 896
 897         field.device = chain_pos;
 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         u32 chain_pos;
 994         str9xpec_flash_controller_t *str9xpec_info = NULL;
 995
 996         str9xpec_info = bank->driver_priv;
 997         chain_pos = str9xpec_info->chain_pos;
 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(chain_pos, ISC_PROGRAM, TAP_PI);
1021
1022         field.device = chain_pos;
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(chain_pos, ISC_NOOP, TAP_PI);
1038
1039         do {
1040                 field.device = chain_pos;
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         int retval;
1269         flash_bank_t *bank;
1270         u32 chain_pos;
1271         jtag_device_t* dev0;
1272         jtag_device_t* dev2;
1273         str9xpec_flash_controller_t *str9xpec_info = NULL;
1274
1275         if (argc < 1)
1276         {
1277                 command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
1278                 return ERROR_OK;
1279         }
1280
1281         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1282         if (!bank)
1283         {
1284                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1285                 return ERROR_OK;
1286         }
1287
1288         str9xpec_info = bank->driver_priv;
1289
1290         chain_pos = str9xpec_info->chain_pos;
1291
1292         /* remove arm core from chain - enter turbo mode */
1293
1294         str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);
1295         if ((retval=jtag_execute_queue())!=ERROR_OK)
1296                 return retval;
1297
1298         /* modify scan chain - str9 core has been removed */
1299         dev0 = jtag_get_device(chain_pos);
1300         if (dev0 == NULL)
1301                 return ERROR_FAIL;
1302         str9xpec_info->devarm = jtag_get_device(chain_pos+1);
1303         dev2 = jtag_get_device(chain_pos+2);
1304         if (dev2 == NULL)
1305                 return ERROR_FAIL;
1306         dev0->next = dev2;
1307         jtag_num_devices--;
1308
1309         return ERROR_OK;
1310 }
1311
1312 int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1313 {
1314         flash_bank_t *bank;
1315         u32 chain_pos;
1316         jtag_device_t* dev0;
1317         str9xpec_flash_controller_t *str9xpec_info = NULL;
1318
1319         if (argc < 1)
1320         {
1321                 command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
1322                 return ERROR_OK;
1323         }
1324
1325         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1326         if (!bank)
1327         {
1328                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1329                 return ERROR_OK;
1330         }
1331
1332         str9xpec_info = bank->driver_priv;
1333
1334         chain_pos = str9xpec_info->chain_pos;
1335
1336         dev0 = jtag_get_device(chain_pos);
1337         if (dev0 == NULL)
1338                 return ERROR_FAIL;
1339
1340
1341         /* exit turbo mode via TLR */
1342         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);
1343         jtag_execute_queue();
1344
1345         /* restore previous scan chain */
1346         if( str9xpec_info->devarm ) {
1347                 dev0->next = str9xpec_info->devarm;
1348                 jtag_num_devices++;
1349                 str9xpec_info->devarm = NULL;
1350         }
1351
1352         return ERROR_OK;
1353 }