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