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

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