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

   1 /***************************************************************************
   2  *   Copyright (C) 2005 by Dominic Rath                                    *
   3  *   Dominic.Rath@gmx.de                                                   *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program; if not, write to the                         *
  17  *   Free Software Foundation, Inc.,                                       *
  18  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  19  ***************************************************************************/
  20 #ifdef HAVE_CONFIG_H
  21 #include "config.h"
  22 #endif
  23
  24 #include "replacements.h"
  25
  26 #include "str9xpec.h"
  27 #include "flash.h"
  28 #include "target.h"
  29 #include "log.h"
  30 #include "armv4_5.h"
  31 #include "arm7_9_common.h"
  32 #include "jtag.h"
  33 #include "binarybuffer.h"
  34
  35 #include <stdlib.h>
  36 #include <string.h>
  37 #include <unistd.h>
  38 #include <getopt.h>
  39
  40 str9xpec_mem_layout_t mem_layout_str9pec[] = {
  41         {0x00000000, 0x10000, 0},
  42         {0x00010000, 0x10000, 1},
  43         {0x00020000, 0x10000, 2},
  44         {0x00030000, 0x10000, 3},
  45         {0x00040000, 0x10000, 4},
  46         {0x00050000, 0x10000, 5},
  47         {0x00060000, 0x10000, 6},
  48         {0x00070000, 0x10000, 7},
  49         {0x00080000, 0x02000, 32},
  50         {0x00082000, 0x02000, 33},
  51         {0x00084000, 0x02000, 34},
  52         {0x00086000, 0x02000, 35}
  53 };
  54
  55 int str9xpec_register_commands(struct command_context_s *cmd_ctx);
  56 int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank);
  57 int str9xpec_erase(struct flash_bank_s *bank, int first, int last);
  58 int str9xpec_protect(struct flash_bank_s *bank, int set, int first, int last);
  59 int str9xpec_write(struct flash_bank_s *bank, u8 *buffer, u32 offset, u32 count);
  60 int str9xpec_probe(struct flash_bank_s *bank);
  61 int str9xpec_handle_part_id_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  62 int str9xpec_protect_check(struct flash_bank_s *bank);
  63 int str9xpec_erase_check(struct flash_bank_s *bank);
  64 int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size);
  65
  66 int str9xpec_erase_area(struct flash_bank_s *bank, int first, int last);
  67 int str9xpec_set_address(struct flash_bank_s *bank, u8 sector);
  68 int str9xpec_write_options(struct flash_bank_s *bank);
  69
  70 int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  71 int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  72 int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  73 int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  74 int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  75 int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  76 int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  77 int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  78 int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  79 int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc);
  80
  81 flash_driver_t str9xpec_flash =
  82 {
  83         .name = "str9xpec",
  84         .register_commands = str9xpec_register_commands,
  85         .flash_bank_command = str9xpec_flash_bank_command,
  86         .erase = str9xpec_erase,
  87         .protect = str9xpec_protect,
  88         .write = str9xpec_write,
  89         .probe = str9xpec_probe,
  90         .erase_check = str9xpec_erase_check,
  91         .protect_check = str9xpec_protect_check,
  92         .info = str9xpec_info
  93 };
  94
  95 int str9xpec_register_commands(struct command_context_s *cmd_ctx)
  96 {
  97         command_t *str9xpec_cmd = register_command(cmd_ctx, NULL, "str9xpec", NULL, COMMAND_ANY, "str9xpec flash specific commands");
  98
  99         register_command(cmd_ctx, str9xpec_cmd, "enable_turbo", str9xpec_handle_flash_enable_turbo_command, COMMAND_EXEC,
 100                                          "enable str9xpec turbo mode");
 101         register_command(cmd_ctx, str9xpec_cmd, "disable_turbo", str9xpec_handle_flash_disable_turbo_command, COMMAND_EXEC,
 102                                          "disable str9xpec turbo mode");
 103         register_command(cmd_ctx, str9xpec_cmd, "options_cmap", str9xpec_handle_flash_options_cmap_command, COMMAND_EXEC,
 104                                          "configure str9xpec boot sector");
 105         register_command(cmd_ctx, str9xpec_cmd, "options_lvdthd", str9xpec_handle_flash_options_lvdthd_command, COMMAND_EXEC,
 106                                          "configure str9xpec lvd threshold");
 107         register_command(cmd_ctx, str9xpec_cmd, "options_lvdsel", str9xpec_handle_flash_options_lvdsel_command, COMMAND_EXEC,
 108                                          "configure str9xpec lvd selection");
 109         register_command(cmd_ctx, str9xpec_cmd, "options_lvdwarn", str9xpec_handle_flash_options_lvdwarn_command, COMMAND_EXEC,
 110                                          "configure str9xpec lvd warning");
 111         register_command(cmd_ctx, str9xpec_cmd, "options_read", str9xpec_handle_flash_options_read_command, COMMAND_EXEC,
 112                                          "read str9xpec options");
 113         register_command(cmd_ctx, str9xpec_cmd, "options_write", str9xpec_handle_flash_options_write_command, COMMAND_EXEC,
 114                                          "write str9xpec options");
 115         register_command(cmd_ctx, str9xpec_cmd, "lock", str9xpec_handle_flash_lock_command, COMMAND_EXEC,
 116                                          "lock str9xpec device");
 117         register_command(cmd_ctx, str9xpec_cmd, "unlock", str9xpec_handle_flash_unlock_command, COMMAND_EXEC,
 118                                          "unlock str9xpec device");
 119         register_command(cmd_ctx, str9xpec_cmd, "part_id", str9xpec_handle_part_id_command, COMMAND_EXEC,
 120                                          "print part id of str9xpec flash bank <num>");
 121
 122         return ERROR_OK;
 123 }
 124
 125 int str9xpec_set_instr(int chain_pos, u32 new_instr, enum tap_state end_state)
 126 {
 127         jtag_device_t *device = jtag_get_device(chain_pos);
 128
 129         if (buf_get_u32(device->cur_instr, 0, device->ir_length) != new_instr)
 130         {
 131                 scan_field_t field;
 132
 133                 field.device = chain_pos;
 134                 field.num_bits = device->ir_length;
 135                 field.out_value = calloc(CEIL(field.num_bits, 8), 1);
 136                 buf_set_u32(field.out_value, 0, field.num_bits, new_instr);
 137                 field.out_mask = NULL;
 138                 field.in_value = NULL;
 139                 field.in_check_value = NULL;
 140                 field.in_check_mask = NULL;
 141                 field.in_handler = NULL;
 142                 field.in_handler_priv = NULL;
 143
 144                 jtag_add_ir_scan(1, &field, end_state, NULL);
 145
 146                 free(field.out_value);
 147         }
 148
 149         return ERROR_OK;
 150 }
 151
 152 u8 str9xpec_isc_status(int chain_pos)
 153 {
 154         scan_field_t field;
 155         u8 status;
 156
 157         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
 158
 159         field.device = chain_pos;
 160         field.num_bits = 8;
 161         field.out_value = NULL;
 162         field.out_mask = NULL;
 163         field.in_value = &status;
 164         field.in_check_value = NULL;
 165         field.in_check_mask = NULL;
 166         field.in_handler = NULL;
 167         field.in_handler_priv = NULL;
 168
 169         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
 170         jtag_execute_queue();
 171
 172         DEBUG("status: 0x%2.2x", status);
 173
 174         if (status & ISC_STATUS_SECURITY)
 175                 INFO("Device Security Bit Set");
 176
 177         return status;
 178 }
 179
 180 int str9xpec_isc_enable(struct flash_bank_s *bank)
 181 {
 182         u8 status;
 183         u32 chain_pos;
 184         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 185
 186         chain_pos = str9xpec_info->chain_pos;
 187
 188         if (str9xpec_info->isc_enable)
 189                 return ERROR_OK;
 190
 191         /* enter isc mode */
 192         str9xpec_set_instr(chain_pos, ISC_ENABLE, TAP_RTI);
 193
 194         /* check ISC status */
 195         status = str9xpec_isc_status(chain_pos);
 196         if (status & ISC_STATUS_MODE)
 197         {
 198                 /* we have entered isc mode */
 199                 str9xpec_info->isc_enable = 1;
 200                 DEBUG("ISC_MODE Enabled");
 201         }
 202
 203         return ERROR_OK;
 204 }
 205
 206 int str9xpec_isc_disable(struct flash_bank_s *bank)
 207 {
 208         u8 status;
 209         u32 chain_pos;
 210         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 211
 212         chain_pos = str9xpec_info->chain_pos;
 213
 214         if (!str9xpec_info->isc_enable)
 215                 return ERROR_OK;
 216
 217         str9xpec_set_instr(chain_pos, ISC_DISABLE, TAP_RTI);
 218
 219         /* delay to handle aborts */
 220         jtag_add_sleep(50);
 221
 222         /* check ISC status */
 223         status = str9xpec_isc_status(chain_pos);
 224         if (!(status & ISC_STATUS_MODE))
 225         {
 226                 /* we have left isc mode */
 227                 str9xpec_info->isc_enable = 0;
 228                 DEBUG("ISC_MODE Disabled");
 229         }
 230
 231         return ERROR_OK;
 232 }
 233
 234 int str9xpec_read_config(struct flash_bank_s *bank)
 235 {
 236         scan_field_t field;
 237         u8 status;
 238         u32 chain_pos;
 239
 240         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 241
 242         chain_pos = str9xpec_info->chain_pos;
 243
 244         DEBUG("ISC_CONFIGURATION");
 245
 246         /* execute ISC_CONFIGURATION command */
 247         str9xpec_set_instr(chain_pos, ISC_CONFIGURATION, TAP_PI);
 248
 249         field.device = chain_pos;
 250         field.num_bits = 64;
 251         field.out_value = NULL;
 252         field.out_mask = NULL;
 253         field.in_value = str9xpec_info->options;
 254         field.in_check_value = NULL;
 255         field.in_check_mask = NULL;
 256         field.in_handler = NULL;
 257         field.in_handler_priv = NULL;
 258
 259         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
 260         jtag_execute_queue();
 261
 262         status = str9xpec_isc_status(chain_pos);
 263
 264         return status;
 265 }
 266
 267 int str9xpec_build_block_list(struct flash_bank_s *bank)
 268 {
 269         str9xpec_flash_controller_t *str9xpec_info = bank->driver_priv;
 270
 271         int i;
 272         int num_sectors = 0, b0_sectors = 0;
 273
 274         switch (bank->size)
 275         {
 276                 case 256 * 1024:
 277                         b0_sectors = 4;
 278                         break;
 279                 case 512 * 1024:
 280                         b0_sectors = 8;
 281                         break;
 282                 default:
 283                         ERROR("BUG: unknown bank->size encountered");
 284                         exit(-1);
 285         }
 286
 287         num_sectors = b0_sectors + 4;
 288
 289         bank->num_sectors = num_sectors;
 290         bank->sectors = malloc(sizeof(flash_sector_t) * num_sectors);
 291         str9xpec_info->sector_bits = malloc(sizeof(u32) * num_sectors);
 292
 293         num_sectors = 0;
 294
 295         for (i = 0; i < b0_sectors; i++)
 296         {
 297                 bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
 298                 bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
 299                 bank->sectors[num_sectors].is_erased = -1;
 300                 bank->sectors[num_sectors].is_protected = 1;
 301                 str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
 302         }
 303
 304         for (i = 8; i < 12; i++)
 305         {
 306                 bank->sectors[num_sectors].offset = mem_layout_str9pec[i].sector_start;
 307                 bank->sectors[num_sectors].size = mem_layout_str9pec[i].sector_size;
 308                 bank->sectors[num_sectors].is_erased = -1;
 309                 bank->sectors[num_sectors].is_protected = 1;
 310                 str9xpec_info->sector_bits[num_sectors++] = mem_layout_str9pec[i].sector_bit;
 311         }
 312
 313         return ERROR_OK;
 314 }
 315
 316 /* flash bank str9x <base> <size> 0 0 <target#>
 317  */
 318 int str9xpec_flash_bank_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc, struct flash_bank_s *bank)
 319 {
 320         str9xpec_flash_controller_t *str9xpec_info;
 321         armv4_5_common_t *armv4_5 = NULL;
 322         arm7_9_common_t *arm7_9 = NULL;
 323         arm_jtag_t *jtag_info = NULL;
 324
 325         if (argc < 6)
 326         {
 327                 WARNING("incomplete flash_bank str9x configuration");
 328                 return ERROR_FLASH_BANK_INVALID;
 329         }
 330
 331         str9xpec_info = malloc(sizeof(str9xpec_flash_controller_t));
 332         bank->driver_priv = str9xpec_info;
 333
 334         if (bank->base != 0x00000000)
 335         {
 336                 WARNING("overriding flash base address for STR91x device with 0x00000000");
 337                 bank->base = 0x00000000;
 338         }
 339
 340         str9xpec_info->target = get_target_by_num(strtoul(args[5], NULL, 0));
 341         if (!str9xpec_info->target)
 342         {
 343                 ERROR("no target '%s' configured", args[5]);
 344                 exit(-1);
 345         }
 346
 347         /* find out jtag position of flash controller
 348          * it is always after the arm966 core */
 349
 350         armv4_5 = str9xpec_info->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         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, NULL);
 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, NULL);
 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         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         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, NULL);
 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, NULL);
 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         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, NULL);
 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                 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         DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
 739
 740         scanbuf = calloc(CEIL(64, 8), 1);
 741
 742         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, NULL);
 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, NULL);
 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, NULL);
 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, NULL);
 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                 command_print(cmd_ctx, "usage: str9xpec part_id <num>");
 880                 return ERROR_OK;
 881         }
 882
 883         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 884         if (!bank)
 885         {
 886                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 887                 return ERROR_OK;
 888         }
 889
 890         str9xpec_info = bank->driver_priv;
 891         chain_pos = str9xpec_info->chain_pos;
 892
 893         buffer = calloc(CEIL(32, 8), 1);
 894
 895         str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);
 896
 897         field.device = chain_pos;
 898         field.num_bits = 32;
 899         field.out_value = NULL;
 900         field.out_mask = NULL;
 901         field.in_value = buffer;
 902         field.in_check_value = NULL;
 903         field.in_check_mask = NULL;
 904         field.in_handler = NULL;
 905         field.in_handler_priv = NULL;
 906
 907         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
 908         jtag_execute_queue();
 909
 910         idcode = buf_get_u32(buffer, 0, 32);
 911
 912         command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
 913
 914         free(buffer);
 915
 916         return ERROR_OK;
 917 }
 918
 919 int str9xpec_erase_check(struct flash_bank_s *bank)
 920 {
 921         return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
 922 }
 923
 924 int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
 925 {
 926         snprintf(buf, buf_size, "str9xpec flash driver info" );
 927         return ERROR_OK;
 928 }
 929
 930 int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 931 {
 932         flash_bank_t *bank;
 933         u8 status;
 934         str9xpec_flash_controller_t *str9xpec_info = NULL;
 935
 936         if (argc < 1)
 937         {
 938                 command_print(cmd_ctx, "str9xpec options_read <bank>");
 939                 return ERROR_OK;
 940         }
 941
 942         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 943         if (!bank)
 944         {
 945                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 946                 return ERROR_OK;
 947         }
 948
 949         str9xpec_info = bank->driver_priv;
 950
 951         status = str9xpec_read_config(bank);
 952
 953         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 954                 return ERROR_FLASH_OPERATION_FAILED;
 955
 956         /* boot bank */
 957         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
 958                 command_print(cmd_ctx, "CS Map: bank1");
 959         else
 960                 command_print(cmd_ctx, "CS Map: bank0");
 961
 962         /* OTP lock */
 963         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
 964                 command_print(cmd_ctx, "OTP Lock: OTP Locked");
 965         else
 966                 command_print(cmd_ctx, "OTP Lock: OTP Unlocked");
 967
 968         /* LVD Threshold */
 969         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
 970                 command_print(cmd_ctx, "LVD Threshold: 2.7v");
 971         else
 972                 command_print(cmd_ctx, "LVD Threshold: 2.4v");
 973
 974         /* LVD reset warning */
 975         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
 976                 command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");
 977         else
 978                 command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");
 979
 980         /* LVD reset select */
 981         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
 982                 command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");
 983         else
 984                 command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");
 985
 986         return ERROR_OK;
 987 }
 988
 989 int str9xpec_write_options(struct flash_bank_s *bank)
 990 {
 991         scan_field_t field;
 992         u8 status;
 993         u32 chain_pos;
 994         str9xpec_flash_controller_t *str9xpec_info = NULL;
 995
 996         str9xpec_info = bank->driver_priv;
 997         chain_pos = str9xpec_info->chain_pos;
 998
 999         /* erase config options first */
1000         str9xpec_erase_area( bank, 0xFE, 0xFE );
1001
1002         if (!str9xpec_info->isc_enable) {
1003                 str9xpec_isc_enable( bank );
1004         }
1005
1006         if (!str9xpec_info->isc_enable) {
1007                 return ISC_STATUS_ERROR;
1008         }
1009
1010         /* according to data 64th bit has to be set */
1011         buf_set_u32(str9xpec_info->options, 63, 1, 1);
1012
1013         /* set option byte address */
1014         str9xpec_set_address(bank, 0x50);
1015
1016         /* execute ISC_PROGRAM command */
1017         str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);
1018
1019         field.device = chain_pos;
1020         field.num_bits = 64;
1021         field.out_value = str9xpec_info->options;
1022         field.out_mask = NULL;
1023         field.in_value = NULL;
1024         field.in_check_value = NULL;
1025         field.in_check_mask = NULL;
1026         field.in_handler = NULL;
1027         field.in_handler_priv = NULL;
1028
1029         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
1030
1031         /* small delay before polling */
1032         jtag_add_sleep(50);
1033
1034         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
1035
1036         do {
1037                 field.device = chain_pos;
1038                 field.num_bits = 8;
1039                 field.out_value = NULL;
1040                 field.out_mask = NULL;
1041                 field.in_value = &status;
1042                 field.in_check_value = NULL;
1043                 field.in_check_mask = NULL;
1044                 field.in_handler = NULL;
1045                 field.in_handler_priv = NULL;
1046
1047                 jtag_add_dr_scan(1, &field, -1, NULL);
1048                 jtag_execute_queue();
1049
1050         } while(!(status & ISC_STATUS_BUSY));
1051
1052         str9xpec_isc_disable(bank);
1053
1054         return status;
1055 }
1056
1057 int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1058 {
1059         flash_bank_t *bank;
1060         u8 status;
1061
1062         if (argc < 1)
1063         {
1064                 command_print(cmd_ctx, "str9xpec options_write <bank>");
1065                 return ERROR_OK;
1066         }
1067
1068         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1069         if (!bank)
1070         {
1071                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1072                 return ERROR_OK;
1073         }
1074
1075         status = str9xpec_write_options(bank);
1076
1077         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1078                 return ERROR_FLASH_OPERATION_FAILED;
1079
1080         return ERROR_OK;
1081 }
1082
1083 int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1084 {
1085         flash_bank_t *bank;
1086         str9xpec_flash_controller_t *str9xpec_info = NULL;
1087
1088         if (argc < 2)
1089         {
1090                 command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
1091                 return ERROR_OK;
1092         }
1093
1094         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1095         if (!bank)
1096         {
1097                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1098                 return ERROR_OK;
1099         }
1100
1101         str9xpec_info = bank->driver_priv;
1102
1103         if (strcmp(args[1], "bank1") == 0)
1104         {
1105                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
1106         }
1107         else
1108         {
1109                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
1110         }
1111
1112         return ERROR_OK;
1113 }
1114
1115 int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1116 {
1117         flash_bank_t *bank;
1118         str9xpec_flash_controller_t *str9xpec_info = NULL;
1119
1120         if (argc < 2)
1121         {
1122                 command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
1123                 return ERROR_OK;
1124         }
1125
1126         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1127         if (!bank)
1128         {
1129                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1130                 return ERROR_OK;
1131         }
1132
1133         str9xpec_info = bank->driver_priv;
1134
1135         if (strcmp(args[1], "2.7v") == 0)
1136         {
1137                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1138         }
1139         else
1140         {
1141                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1142         }
1143
1144         return ERROR_OK;
1145 }
1146
1147 int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1148 {
1149         flash_bank_t *bank;
1150         str9xpec_flash_controller_t *str9xpec_info = NULL;
1151
1152         if (argc < 2)
1153         {
1154                 command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
1155                 return ERROR_OK;
1156         }
1157
1158         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1159         if (!bank)
1160         {
1161                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1162                 return ERROR_OK;
1163         }
1164
1165         str9xpec_info = bank->driver_priv;
1166
1167         if (strcmp(args[1], "vdd_vddq") == 0)
1168         {
1169                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1170         }
1171         else
1172         {
1173                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1174         }
1175
1176         return ERROR_OK;
1177 }
1178
1179 int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1180 {
1181         flash_bank_t *bank;
1182         str9xpec_flash_controller_t *str9xpec_info = NULL;
1183
1184         if (argc < 2)
1185         {
1186                 command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
1187                 return ERROR_OK;
1188         }
1189
1190         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1191         if (!bank)
1192         {
1193                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1194                 return ERROR_OK;
1195         }
1196
1197         str9xpec_info = bank->driver_priv;
1198
1199         if (strcmp(args[1], "vdd_vddq") == 0)
1200         {
1201                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1202         }
1203         else
1204         {
1205                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1206         }
1207
1208         return ERROR_OK;
1209 }
1210
1211 int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1212 {
1213         u8 status;
1214         flash_bank_t *bank;
1215
1216         if (argc < 1)
1217         {
1218                 command_print(cmd_ctx, "str9xpec lock <bank>");
1219                 return ERROR_OK;
1220         }
1221
1222         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1223         if (!bank)
1224         {
1225                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1226                 return ERROR_OK;
1227         }
1228
1229         status = str9xpec_lock_device(bank);
1230
1231         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1232                 return ERROR_FLASH_OPERATION_FAILED;
1233
1234         return ERROR_OK;
1235 }
1236
1237 int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1238 {
1239         u8 status;
1240         flash_bank_t *bank;
1241
1242         if (argc < 1)
1243         {
1244                 command_print(cmd_ctx, "str9xpec unlock <bank>");
1245                 return ERROR_OK;
1246         }
1247
1248         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1249         if (!bank)
1250         {
1251                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1252                 return ERROR_OK;
1253         }
1254
1255         status = str9xpec_unlock_device(bank);
1256
1257         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1258                 return ERROR_FLASH_OPERATION_FAILED;
1259
1260         return ERROR_OK;
1261 }
1262
1263 int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1264 {
1265         flash_bank_t *bank;
1266         u32 chain_pos;
1267         jtag_device_t* dev0;
1268         jtag_device_t* dev2;
1269         str9xpec_flash_controller_t *str9xpec_info = NULL;
1270
1271         if (argc < 1)
1272         {
1273                 command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
1274                 return ERROR_OK;
1275         }
1276
1277         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1278         if (!bank)
1279         {
1280                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1281                 return ERROR_OK;
1282         }
1283
1284         str9xpec_info = bank->driver_priv;
1285
1286         chain_pos = str9xpec_info->chain_pos;
1287
1288         /* remove arm core from chain - enter turbo mode */
1289
1290         str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);
1291         jtag_execute_queue();
1292
1293         /* modify scan chain - str9 core has been removed */
1294         dev0 = jtag_get_device(chain_pos);
1295         str9xpec_info->devarm = jtag_get_device(chain_pos+1);
1296         dev2 = jtag_get_device(chain_pos+2);
1297         dev0->next = dev2;
1298         jtag_num_devices--;
1299
1300         return ERROR_OK;
1301 }
1302
1303 int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1304 {
1305         flash_bank_t *bank;
1306         u32 chain_pos;
1307         jtag_device_t* dev0;
1308         str9xpec_flash_controller_t *str9xpec_info = NULL;
1309
1310         if (argc < 1)
1311         {
1312                 command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
1313                 return ERROR_OK;
1314         }
1315
1316         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1317         if (!bank)
1318         {
1319                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1320                 return ERROR_OK;
1321         }
1322
1323         str9xpec_info = bank->driver_priv;
1324
1325         chain_pos = str9xpec_info->chain_pos;
1326
1327         dev0 = jtag_get_device(chain_pos);
1328
1329         /* exit turbo mode via TLR */
1330         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);
1331         jtag_execute_queue();
1332
1333         /* restore previous scan chain */
1334         if( str9xpec_info->devarm ) {
1335                 dev0->next = str9xpec_info->devarm;
1336                 jtag_num_devices++;
1337         }
1338
1339         return ERROR_OK;
1340 }