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

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