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

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