src/flash/nor/str9xpec.c

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