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