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