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                 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, NULL);
 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, NULL);
 178         jtag_execute_queue();
 179
 180         DEBUG("status: 0x%2.2x", status);
 181
 182         if (status & ISC_STATUS_SECURITY)
 183                 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                 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                 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         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, NULL);
 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                         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                 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                 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         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, NULL);
 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, NULL);
 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         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         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, NULL);
 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, NULL);
 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         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, NULL);
 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                 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         DEBUG("first_sector: %i, last_sector: %i", first_sector, last_sector);
 744
 745         scanbuf = calloc(CEIL(64, 8), 1);
 746
 747         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, NULL);
 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, NULL);
 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, NULL);
 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, NULL);
 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                 command_print(cmd_ctx, "usage: str9xpec part_id <num>");
 885                 return ERROR_OK;
 886         }
 887
 888         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 889         if (!bank)
 890         {
 891                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 892                 return ERROR_OK;
 893         }
 894
 895         str9xpec_info = bank->driver_priv;
 896         chain_pos = str9xpec_info->chain_pos;
 897
 898         buffer = calloc(CEIL(32, 8), 1);
 899
 900         str9xpec_set_instr(chain_pos, ISC_IDCODE, TAP_PI);
 901
 902         field.device = chain_pos;
 903         field.num_bits = 32;
 904         field.out_value = NULL;
 905         field.out_mask = NULL;
 906         field.in_value = buffer;
 907         field.in_check_value = NULL;
 908         field.in_check_mask = NULL;
 909         field.in_handler = NULL;
 910         field.in_handler_priv = NULL;
 911
 912         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
 913         jtag_execute_queue();
 914
 915         idcode = buf_get_u32(buffer, 0, 32);
 916
 917         command_print(cmd_ctx, "str9xpec part id: 0x%8.8x", idcode);
 918
 919         free(buffer);
 920
 921         return ERROR_OK;
 922 }
 923
 924 int str9xpec_erase_check(struct flash_bank_s *bank)
 925 {
 926         return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
 927 }
 928
 929 int str9xpec_info(struct flash_bank_s *bank, char *buf, int buf_size)
 930 {
 931         snprintf(buf, buf_size, "str9xpec flash driver info" );
 932         return ERROR_OK;
 933 }
 934
 935 int str9xpec_handle_flash_options_read_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
 936 {
 937         flash_bank_t *bank;
 938         u8 status;
 939         str9xpec_flash_controller_t *str9xpec_info = NULL;
 940
 941         if (argc < 1)
 942         {
 943                 command_print(cmd_ctx, "str9xpec options_read <bank>");
 944                 return ERROR_OK;
 945         }
 946
 947         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
 948         if (!bank)
 949         {
 950                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
 951                 return ERROR_OK;
 952         }
 953
 954         str9xpec_info = bank->driver_priv;
 955
 956         status = str9xpec_read_config(bank);
 957
 958         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 959                 return ERROR_FLASH_OPERATION_FAILED;
 960
 961         /* boot bank */
 962         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
 963                 command_print(cmd_ctx, "CS Map: bank1");
 964         else
 965                 command_print(cmd_ctx, "CS Map: bank0");
 966
 967         /* OTP lock */
 968         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
 969                 command_print(cmd_ctx, "OTP Lock: OTP Locked");
 970         else
 971                 command_print(cmd_ctx, "OTP Lock: OTP Unlocked");
 972
 973         /* LVD Threshold */
 974         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
 975                 command_print(cmd_ctx, "LVD Threshold: 2.7v");
 976         else
 977                 command_print(cmd_ctx, "LVD Threshold: 2.4v");
 978
 979         /* LVD reset warning */
 980         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
 981                 command_print(cmd_ctx, "LVD Reset Warning: VDD or VDDQ Inputs");
 982         else
 983                 command_print(cmd_ctx, "LVD Reset Warning: VDD Input Only");
 984
 985         /* LVD reset select */
 986         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
 987                 command_print(cmd_ctx, "LVD Reset Selection: VDD or VDDQ Inputs");
 988         else
 989                 command_print(cmd_ctx, "LVD Reset Selection: VDD Input Only");
 990
 991         return ERROR_OK;
 992 }
 993
 994 int str9xpec_write_options(struct flash_bank_s *bank)
 995 {
 996         scan_field_t field;
 997         u8 status;
 998         u32 chain_pos;
 999         str9xpec_flash_controller_t *str9xpec_info = NULL;
1000
1001         str9xpec_info = bank->driver_priv;
1002         chain_pos = str9xpec_info->chain_pos;
1003
1004         /* erase config options first */
1005         status = str9xpec_erase_area( bank, 0xFE, 0xFE );
1006
1007         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1008                 return status;
1009
1010         if (!str9xpec_info->isc_enable) {
1011                 str9xpec_isc_enable( bank );
1012         }
1013
1014         if (!str9xpec_info->isc_enable) {
1015                 return ISC_STATUS_ERROR;
1016         }
1017
1018         /* according to data 64th bit has to be set */
1019         buf_set_u32(str9xpec_info->options, 63, 1, 1);
1020
1021         /* set option byte address */
1022         str9xpec_set_address(bank, 0x50);
1023
1024         /* execute ISC_PROGRAM command */
1025         str9xpec_set_instr(chain_pos, ISC_PROGRAM, TAP_PI);
1026
1027         field.device = chain_pos;
1028         field.num_bits = 64;
1029         field.out_value = str9xpec_info->options;
1030         field.out_mask = NULL;
1031         field.in_value = NULL;
1032         field.in_check_value = NULL;
1033         field.in_check_mask = NULL;
1034         field.in_handler = NULL;
1035         field.in_handler_priv = NULL;
1036
1037         jtag_add_dr_scan(1, &field, TAP_RTI, NULL);
1038
1039         /* small delay before polling */
1040         jtag_add_sleep(50);
1041
1042         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_PI);
1043
1044         do {
1045                 field.device = chain_pos;
1046                 field.num_bits = 8;
1047                 field.out_value = NULL;
1048                 field.out_mask = NULL;
1049                 field.in_value = &status;
1050                 field.in_check_value = NULL;
1051                 field.in_check_mask = NULL;
1052                 field.in_handler = NULL;
1053                 field.in_handler_priv = NULL;
1054
1055                 jtag_add_dr_scan(1, &field, -1, NULL);
1056                 jtag_execute_queue();
1057
1058         } while(!(status & ISC_STATUS_BUSY));
1059
1060         str9xpec_isc_disable(bank);
1061
1062         return status;
1063 }
1064
1065 int str9xpec_handle_flash_options_write_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1066 {
1067         flash_bank_t *bank;
1068         u8 status;
1069
1070         if (argc < 1)
1071         {
1072                 command_print(cmd_ctx, "str9xpec options_write <bank>");
1073                 return ERROR_OK;
1074         }
1075
1076         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1077         if (!bank)
1078         {
1079                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1080                 return ERROR_OK;
1081         }
1082
1083         status = str9xpec_write_options(bank);
1084
1085         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1086                 return ERROR_FLASH_OPERATION_FAILED;
1087
1088         return ERROR_OK;
1089 }
1090
1091 int str9xpec_handle_flash_options_cmap_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1092 {
1093         flash_bank_t *bank;
1094         str9xpec_flash_controller_t *str9xpec_info = NULL;
1095
1096         if (argc < 2)
1097         {
1098                 command_print(cmd_ctx, "str9xpec options_cmap <bank> <bank0|bank1>");
1099                 return ERROR_OK;
1100         }
1101
1102         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1103         if (!bank)
1104         {
1105                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1106                 return ERROR_OK;
1107         }
1108
1109         str9xpec_info = bank->driver_priv;
1110
1111         if (strcmp(args[1], "bank1") == 0)
1112         {
1113                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
1114         }
1115         else
1116         {
1117                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
1118         }
1119
1120         return ERROR_OK;
1121 }
1122
1123 int str9xpec_handle_flash_options_lvdthd_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1124 {
1125         flash_bank_t *bank;
1126         str9xpec_flash_controller_t *str9xpec_info = NULL;
1127
1128         if (argc < 2)
1129         {
1130                 command_print(cmd_ctx, "str9xpec options_lvdthd <bank> <2.4v|2.7v>");
1131                 return ERROR_OK;
1132         }
1133
1134         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1135         if (!bank)
1136         {
1137                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1138                 return ERROR_OK;
1139         }
1140
1141         str9xpec_info = bank->driver_priv;
1142
1143         if (strcmp(args[1], "2.7v") == 0)
1144         {
1145                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1146         }
1147         else
1148         {
1149                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1150         }
1151
1152         return ERROR_OK;
1153 }
1154
1155 int str9xpec_handle_flash_options_lvdsel_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1156 {
1157         flash_bank_t *bank;
1158         str9xpec_flash_controller_t *str9xpec_info = NULL;
1159
1160         if (argc < 2)
1161         {
1162                 command_print(cmd_ctx, "str9xpec options_lvdsel <bank> <vdd|vdd_vddq>");
1163                 return ERROR_OK;
1164         }
1165
1166         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1167         if (!bank)
1168         {
1169                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1170                 return ERROR_OK;
1171         }
1172
1173         str9xpec_info = bank->driver_priv;
1174
1175         if (strcmp(args[1], "vdd_vddq") == 0)
1176         {
1177                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1178         }
1179         else
1180         {
1181                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1182         }
1183
1184         return ERROR_OK;
1185 }
1186
1187 int str9xpec_handle_flash_options_lvdwarn_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1188 {
1189         flash_bank_t *bank;
1190         str9xpec_flash_controller_t *str9xpec_info = NULL;
1191
1192         if (argc < 2)
1193         {
1194                 command_print(cmd_ctx, "str9xpec options_lvdwarn <bank> <vdd|vdd_vddq>");
1195                 return ERROR_OK;
1196         }
1197
1198         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1199         if (!bank)
1200         {
1201                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1202                 return ERROR_OK;
1203         }
1204
1205         str9xpec_info = bank->driver_priv;
1206
1207         if (strcmp(args[1], "vdd_vddq") == 0)
1208         {
1209                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1210         }
1211         else
1212         {
1213                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1214         }
1215
1216         return ERROR_OK;
1217 }
1218
1219 int str9xpec_handle_flash_lock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1220 {
1221         u8 status;
1222         flash_bank_t *bank;
1223
1224         if (argc < 1)
1225         {
1226                 command_print(cmd_ctx, "str9xpec lock <bank>");
1227                 return ERROR_OK;
1228         }
1229
1230         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1231         if (!bank)
1232         {
1233                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1234                 return ERROR_OK;
1235         }
1236
1237         status = str9xpec_lock_device(bank);
1238
1239         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1240                 return ERROR_FLASH_OPERATION_FAILED;
1241
1242         return ERROR_OK;
1243 }
1244
1245 int str9xpec_handle_flash_unlock_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1246 {
1247         u8 status;
1248         flash_bank_t *bank;
1249
1250         if (argc < 1)
1251         {
1252                 command_print(cmd_ctx, "str9xpec unlock <bank>");
1253                 return ERROR_OK;
1254         }
1255
1256         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1257         if (!bank)
1258         {
1259                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1260                 return ERROR_OK;
1261         }
1262
1263         status = str9xpec_unlock_device(bank);
1264
1265         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1266                 return ERROR_FLASH_OPERATION_FAILED;
1267
1268         return ERROR_OK;
1269 }
1270
1271 int str9xpec_handle_flash_enable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1272 {
1273         flash_bank_t *bank;
1274         u32 chain_pos;
1275         jtag_device_t* dev0;
1276         jtag_device_t* dev2;
1277         str9xpec_flash_controller_t *str9xpec_info = NULL;
1278
1279         if (argc < 1)
1280         {
1281                 command_print(cmd_ctx, "str9xpec enable_turbo <bank>");
1282                 return ERROR_OK;
1283         }
1284
1285         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1286         if (!bank)
1287         {
1288                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1289                 return ERROR_OK;
1290         }
1291
1292         str9xpec_info = bank->driver_priv;
1293
1294         chain_pos = str9xpec_info->chain_pos;
1295
1296         /* remove arm core from chain - enter turbo mode */
1297
1298         str9xpec_set_instr(chain_pos+2, 0xD, TAP_RTI);
1299         jtag_execute_queue();
1300
1301         /* modify scan chain - str9 core has been removed */
1302         dev0 = jtag_get_device(chain_pos);
1303         str9xpec_info->devarm = jtag_get_device(chain_pos+1);
1304         dev2 = jtag_get_device(chain_pos+2);
1305         dev0->next = dev2;
1306         jtag_num_devices--;
1307
1308         return ERROR_OK;
1309 }
1310
1311 int str9xpec_handle_flash_disable_turbo_command(struct command_context_s *cmd_ctx, char *cmd, char **args, int argc)
1312 {
1313         flash_bank_t *bank;
1314         u32 chain_pos;
1315         jtag_device_t* dev0;
1316         str9xpec_flash_controller_t *str9xpec_info = NULL;
1317
1318         if (argc < 1)
1319         {
1320                 command_print(cmd_ctx, "str9xpec disable_turbo <bank>");
1321                 return ERROR_OK;
1322         }
1323
1324         bank = get_flash_bank_by_num(strtoul(args[0], NULL, 0));
1325         if (!bank)
1326         {
1327                 command_print(cmd_ctx, "flash bank '#%s' is out of bounds", args[0]);
1328                 return ERROR_OK;
1329         }
1330
1331         str9xpec_info = bank->driver_priv;
1332
1333         chain_pos = str9xpec_info->chain_pos;
1334
1335         dev0 = jtag_get_device(chain_pos);
1336
1337         /* exit turbo mode via TLR */
1338         str9xpec_set_instr(chain_pos, ISC_NOOP, TAP_TLR);
1339         jtag_execute_queue();
1340
1341         /* restore previous scan chain */
1342         if( str9xpec_info->devarm ) {
1343                 dev0->next = str9xpec_info->devarm;
1344                 jtag_num_devices++;
1345                 str9xpec_info->devarm = NULL;
1346         }
1347
1348         return ERROR_OK;
1349 }