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                 i = 0;
 718
 719                 while (bytes_remaining > 0)
 720                 {
 721                         last_dword[i++] = *(buffer + bytes_written);
 722                         bytes_remaining--;
 723                         bytes_written++;
 724                 }
 725
 726                 str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
 727
 728                 field.num_bits = 64;
 729                 field.out_value = last_dword;
 730                 field.in_value = NULL;
 731
 732                 jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 733
 734                 /* small delay before polling */
 735                 jtag_add_sleep(50);
 736
 737                 str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
 738
 739                 do {
 740                         field.num_bits = 8;
 741                         field.out_value = NULL;
 742                         field.in_value = scanbuf;
 743
 744                         jtag_add_dr_scan(tap, 1, &field, TAP_IRPAUSE);
 745                         jtag_execute_queue();
 746
 747                         status = buf_get_u32(scanbuf, 0, 8);
 748
 749                 } while (!(status & ISC_STATUS_BUSY));
 750
 751                 if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 752                         return ERROR_FLASH_OPERATION_FAILED;
 753
 754                 /* if ((status & ISC_STATUS_INT_ERROR) != STR9XPEC_ISC_INTFAIL)
 755                         return ERROR_FLASH_OPERATION_FAILED; */
 756         }
 757
 758         free(scanbuf);
 759
 760         str9xpec_isc_disable(bank);
 761
 762         return ERROR_OK;
 763 }
 764
 765 static int str9xpec_probe(struct flash_bank *bank)
 766 {
 767         return ERROR_OK;
 768 }
 769
 770 COMMAND_HANDLER(str9xpec_handle_part_id_command)
 771 {
 772         struct scan_field field;
 773         uint8_t *buffer = NULL;
 774         struct jtag_tap *tap;
 775         uint32_t idcode;
 776         struct str9xpec_flash_controller *str9xpec_info = NULL;
 777
 778         if (CMD_ARGC < 1)
 779                 return ERROR_COMMAND_SYNTAX_ERROR;
 780
 781         struct flash_bank *bank;
 782         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 783         if (ERROR_OK != retval)
 784                 return retval;
 785
 786         str9xpec_info = bank->driver_priv;
 787         tap = str9xpec_info->tap;
 788
 789         buffer = calloc(DIV_ROUND_UP(32, 8), 1);
 790
 791         str9xpec_set_instr(tap, ISC_IDCODE, TAP_IRPAUSE);
 792
 793         field.num_bits = 32;
 794         field.out_value = NULL;
 795         field.in_value = buffer;
 796
 797         jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 798         jtag_execute_queue();
 799
 800         idcode = buf_get_u32(buffer, 0, 32);
 801
 802         command_print(CMD_CTX, "str9xpec part id: 0x%8.8" PRIx32 "", idcode);
 803
 804         free(buffer);
 805
 806         return ERROR_OK;
 807 }
 808
 809 static int str9xpec_erase_check(struct flash_bank *bank)
 810 {
 811         return str9xpec_blank_check(bank, 0, bank->num_sectors - 1);
 812 }
 813
 814 static int get_str9xpec_info(struct flash_bank *bank, char *buf, int buf_size)
 815 {
 816         snprintf(buf, buf_size, "str9xpec flash driver info");
 817         return ERROR_OK;
 818 }
 819
 820 COMMAND_HANDLER(str9xpec_handle_flash_options_read_command)
 821 {
 822         uint8_t status;
 823         struct str9xpec_flash_controller *str9xpec_info = NULL;
 824
 825         if (CMD_ARGC < 1)
 826         {
 827                 return ERROR_COMMAND_SYNTAX_ERROR;
 828         }
 829
 830         struct flash_bank *bank;
 831         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 832         if (ERROR_OK != retval)
 833                 return retval;
 834
 835         str9xpec_info = bank->driver_priv;
 836
 837         status = str9xpec_read_config(bank);
 838
 839         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 840                 return ERROR_FLASH_OPERATION_FAILED;
 841
 842         /* boot bank */
 843         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
 844                 command_print(CMD_CTX, "CS Map: bank1");
 845         else
 846                 command_print(CMD_CTX, "CS Map: bank0");
 847
 848         /* OTP lock */
 849         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
 850                 command_print(CMD_CTX, "OTP Lock: OTP Locked");
 851         else
 852                 command_print(CMD_CTX, "OTP Lock: OTP Unlocked");
 853
 854         /* LVD Threshold */
 855         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
 856                 command_print(CMD_CTX, "LVD Threshold: 2.7v");
 857         else
 858                 command_print(CMD_CTX, "LVD Threshold: 2.4v");
 859
 860         /* LVD reset warning */
 861         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
 862                 command_print(CMD_CTX, "LVD Reset Warning: VDD or VDDQ Inputs");
 863         else
 864                 command_print(CMD_CTX, "LVD Reset Warning: VDD Input Only");
 865
 866         /* LVD reset select */
 867         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
 868                 command_print(CMD_CTX, "LVD Reset Selection: VDD or VDDQ Inputs");
 869         else
 870                 command_print(CMD_CTX, "LVD Reset Selection: VDD Input Only");
 871
 872         return ERROR_OK;
 873 }
 874
 875 static int str9xpec_write_options(struct flash_bank *bank)
 876 {
 877         struct scan_field field;
 878         uint8_t status;
 879         struct jtag_tap *tap;
 880         struct str9xpec_flash_controller *str9xpec_info = NULL;
 881
 882         str9xpec_info = bank->driver_priv;
 883         tap = str9xpec_info->tap;
 884
 885         /* erase config options first */
 886         status = str9xpec_erase_area(bank, 0xFE, 0xFE);
 887
 888         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 889                 return status;
 890
 891         if (!str9xpec_info->isc_enable) {
 892                 str9xpec_isc_enable(bank);
 893         }
 894
 895         if (!str9xpec_info->isc_enable) {
 896                 return ISC_STATUS_ERROR;
 897         }
 898
 899         /* according to data 64th bit has to be set */
 900         buf_set_u32(str9xpec_info->options, 63, 1, 1);
 901
 902         /* set option byte address */
 903         str9xpec_set_address(bank, 0x50);
 904
 905         /* execute ISC_PROGRAM command */
 906         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
 907
 908         field.num_bits = 64;
 909         field.out_value = str9xpec_info->options;
 910         field.in_value = NULL;
 911
 912         jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 913
 914         /* small delay before polling */
 915         jtag_add_sleep(50);
 916
 917         str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
 918
 919         do {
 920                 field.num_bits = 8;
 921                 field.out_value = NULL;
 922                 field.in_value = &status;
 923
 924                 jtag_add_dr_scan(tap, 1, &field, TAP_IRPAUSE);
 925                 jtag_execute_queue();
 926
 927         } while (!(status & ISC_STATUS_BUSY));
 928
 929         str9xpec_isc_disable(bank);
 930
 931         return status;
 932 }
 933
 934 COMMAND_HANDLER(str9xpec_handle_flash_options_write_command)
 935 {
 936         uint8_t status;
 937
 938         if (CMD_ARGC < 1)
 939         {
 940                 return ERROR_COMMAND_SYNTAX_ERROR;
 941         }
 942
 943         struct flash_bank *bank;
 944         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 945         if (ERROR_OK != retval)
 946                 return retval;
 947
 948         status = str9xpec_write_options(bank);
 949
 950         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 951                 return ERROR_FLASH_OPERATION_FAILED;
 952
 953         command_print(CMD_CTX, "str9xpec write options complete.\n"
 954                         "INFO: a reset or power cycle is required "
 955                         "for the new settings to take effect.");
 956
 957         return ERROR_OK;
 958 }
 959
 960 COMMAND_HANDLER(str9xpec_handle_flash_options_cmap_command)
 961 {
 962         struct str9xpec_flash_controller *str9xpec_info = NULL;
 963
 964         if (CMD_ARGC < 2)
 965         {
 966                 return ERROR_COMMAND_SYNTAX_ERROR;
 967         }
 968
 969         struct flash_bank *bank;
 970         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 971         if (ERROR_OK != retval)
 972                 return retval;
 973
 974         str9xpec_info = bank->driver_priv;
 975
 976         if (strcmp(CMD_ARGV[1], "bank1") == 0)
 977         {
 978                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
 979         }
 980         else
 981         {
 982                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
 983         }
 984
 985         return ERROR_OK;
 986 }
 987
 988 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdthd_command)
 989 {
 990         struct str9xpec_flash_controller *str9xpec_info = NULL;
 991
 992         if (CMD_ARGC < 2)
 993         {
 994                 return ERROR_COMMAND_SYNTAX_ERROR;
 995         }
 996
 997         struct flash_bank *bank;
 998         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 999         if (ERROR_OK != retval)
1000                 return retval;
1001
1002         str9xpec_info = bank->driver_priv;
1003
1004         if (strcmp(CMD_ARGV[1], "2.7v") == 0)
1005         {
1006                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1007         }
1008         else
1009         {
1010                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1011         }
1012
1013         return ERROR_OK;
1014 }
1015
1016 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdsel_command)
1017 {
1018         struct str9xpec_flash_controller *str9xpec_info = NULL;
1019
1020         if (CMD_ARGC < 2)
1021         {
1022                 return ERROR_COMMAND_SYNTAX_ERROR;
1023         }
1024
1025         struct flash_bank *bank;
1026         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1027         if (ERROR_OK != retval)
1028                 return retval;
1029
1030         str9xpec_info = bank->driver_priv;
1031
1032         if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
1033         {
1034                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1035         }
1036         else
1037         {
1038                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1039         }
1040
1041         return ERROR_OK;
1042 }
1043
1044 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdwarn_command)
1045 {
1046         struct str9xpec_flash_controller *str9xpec_info = NULL;
1047
1048         if (CMD_ARGC < 2)
1049         {
1050                 return ERROR_COMMAND_SYNTAX_ERROR;
1051         }
1052
1053         struct flash_bank *bank;
1054         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1055         if (ERROR_OK != retval)
1056                 return retval;
1057
1058         str9xpec_info = bank->driver_priv;
1059
1060         if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
1061         {
1062                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1063         }
1064         else
1065         {
1066                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1067         }
1068
1069         return ERROR_OK;
1070 }
1071
1072 COMMAND_HANDLER(str9xpec_handle_flash_lock_command)
1073 {
1074         uint8_t status;
1075
1076         if (CMD_ARGC < 1)
1077         {
1078                 return ERROR_COMMAND_SYNTAX_ERROR;
1079         }
1080
1081         struct flash_bank *bank;
1082         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1083         if (ERROR_OK != retval)
1084                 return retval;
1085
1086         status = str9xpec_lock_device(bank);
1087
1088         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1089                 return ERROR_FLASH_OPERATION_FAILED;
1090
1091         return ERROR_OK;
1092 }
1093
1094 COMMAND_HANDLER(str9xpec_handle_flash_unlock_command)
1095 {
1096         uint8_t status;
1097
1098         if (CMD_ARGC < 1)
1099         {
1100                 return ERROR_COMMAND_SYNTAX_ERROR;
1101         }
1102
1103         struct flash_bank *bank;
1104         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1105         if (ERROR_OK != retval)
1106                 return retval;
1107
1108         status = str9xpec_unlock_device(bank);
1109
1110         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1111                 return ERROR_FLASH_OPERATION_FAILED;
1112
1113         command_print(CMD_CTX, "str9xpec unlocked.\n"
1114                         "INFO: a reset or power cycle is required "
1115                         "for the new settings to take effect.");
1116
1117         return ERROR_OK;
1118 }
1119
1120 COMMAND_HANDLER(str9xpec_handle_flash_enable_turbo_command)
1121 {
1122         struct jtag_tap *tap0;
1123         struct jtag_tap *tap1;
1124         struct jtag_tap *tap2;
1125         struct str9xpec_flash_controller *str9xpec_info = NULL;
1126
1127         if (CMD_ARGC < 1)
1128         {
1129                 return ERROR_COMMAND_SYNTAX_ERROR;
1130         }
1131
1132         struct flash_bank *bank;
1133         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1134         if (ERROR_OK != retval)
1135                 return retval;
1136
1137         str9xpec_info = bank->driver_priv;
1138
1139         tap0 = str9xpec_info->tap;
1140
1141         /* remove arm core from chain - enter turbo mode */
1142         tap1 = tap0->next_tap;
1143         if (tap1 == NULL)
1144         {
1145                 /* things are *WRONG* */
1146                 command_print(CMD_CTX,"**STR9FLASH** (tap1) invalid chain?");
1147                 return ERROR_OK;
1148         }
1149         tap2 = tap1->next_tap;
1150         if (tap2 == NULL)
1151         {
1152                 /* things are *WRONG* */
1153                 command_print(CMD_CTX,"**STR9FLASH** (tap2) invalid chain?");
1154                 return ERROR_OK;
1155         }
1156
1157         /* enable turbo mode - TURBO-PROG-ENABLE */
1158         str9xpec_set_instr(tap2, 0xD, TAP_IDLE);
1159         if ((retval = jtag_execute_queue()) != ERROR_OK)
1160                 return retval;
1161
1162         /* modify scan chain - str9 core has been removed */
1163         tap1->enabled = 0;
1164
1165         return ERROR_OK;
1166 }
1167
1168 COMMAND_HANDLER(str9xpec_handle_flash_disable_turbo_command)
1169 {
1170         struct jtag_tap *tap;
1171         struct str9xpec_flash_controller *str9xpec_info = NULL;
1172
1173         if (CMD_ARGC < 1)
1174         {
1175                 return ERROR_COMMAND_SYNTAX_ERROR;
1176         }
1177
1178         struct flash_bank *bank;
1179         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1180         if (ERROR_OK != retval)
1181                 return retval;
1182
1183         str9xpec_info = bank->driver_priv;
1184         tap = str9xpec_info->tap;
1185
1186         if (tap == NULL)
1187                 return ERROR_FAIL;
1188
1189         /* exit turbo mode via RESET */
1190         str9xpec_set_instr(tap, ISC_NOOP, TAP_IDLE);
1191         jtag_add_tlr();
1192         jtag_execute_queue();
1193
1194         /* restore previous scan chain */
1195         if (tap->next_tap) {
1196                 tap->next_tap->enabled = 1;
1197         }
1198
1199         return ERROR_OK;
1200 }
1201
1202 static const struct command_registration str9xpec_config_command_handlers[] = {
1203         {
1204                 .name = "enable_turbo",
1205                 .usage = "<bank>",
1206                 .handler = str9xpec_handle_flash_enable_turbo_command,
1207                 .mode = COMMAND_EXEC,
1208                 .help = "enable str9xpec turbo mode",
1209         },
1210         {
1211                 .name = "disable_turbo",
1212                 .usage = "<bank>",
1213                 .handler = str9xpec_handle_flash_disable_turbo_command,
1214                 .mode = COMMAND_EXEC,
1215                 .help = "disable str9xpec turbo mode",
1216         },
1217         {
1218                 .name = "options_cmap",
1219                 .usage = "<bank> <bank0 | bank1>",
1220                 .handler = str9xpec_handle_flash_options_cmap_command,
1221                 .mode = COMMAND_EXEC,
1222                 .help = "configure str9xpec boot sector",
1223         },
1224         {
1225                 .name = "options_lvdthd",
1226                 .usage = "<bank> <2.4v | 2.7v>",
1227                 .handler = str9xpec_handle_flash_options_lvdthd_command,
1228                 .mode = COMMAND_EXEC,
1229                 .help = "configure str9xpec lvd threshold",
1230         },
1231         {
1232                 .name = "options_lvdsel",
1233                 .usage = "<bank> <vdd | vdd_vddq>",
1234                 .handler = str9xpec_handle_flash_options_lvdsel_command,
1235                 .mode = COMMAND_EXEC,
1236                 .help = "configure str9xpec lvd selection",
1237         },
1238         {
1239                 .name = "options_lvdwarn",
1240                 .usage = "<bank> <vdd | vdd_vddq>",
1241                 .handler = str9xpec_handle_flash_options_lvdwarn_command,
1242                 .mode = COMMAND_EXEC,
1243                 .help = "configure str9xpec lvd warning",
1244         },
1245         {
1246                 .name = "options_read",
1247                 .usage = "<bank>",
1248                 .handler = str9xpec_handle_flash_options_read_command,
1249                 .mode = COMMAND_EXEC,
1250                 .help = "read str9xpec options",
1251         },
1252         {
1253                 .name = "options_write",
1254                 .usage = "<bank>",
1255                 .handler = str9xpec_handle_flash_options_write_command,
1256                 .mode = COMMAND_EXEC,
1257                 .help = "write str9xpec options",
1258         },
1259         {
1260                 .name = "lock",
1261                 .usage = "<bank>",
1262                 .handler = str9xpec_handle_flash_lock_command,
1263                 .mode = COMMAND_EXEC,
1264                 .help = "lock str9xpec device",
1265         },
1266         {
1267                 .name = "unlock",
1268                 .usage = "<bank>",
1269                 .handler = str9xpec_handle_flash_unlock_command,
1270                 .mode = COMMAND_EXEC,
1271                 .help = "unlock str9xpec device",
1272         },
1273         {
1274                 .name = "part_id",
1275                 .handler = str9xpec_handle_part_id_command,
1276                 .mode = COMMAND_EXEC,
1277                 .help = "print part id of str9xpec flash bank <num>",
1278         },
1279         COMMAND_REGISTRATION_DONE
1280 };
1281
1282 static const struct command_registration str9xpec_command_handlers[] = {
1283         {
1284                 .name = "str9xpec",
1285                 .mode = COMMAND_ANY,
1286                 .help = "str9xpec flash command group",
1287                 .chain = str9xpec_config_command_handlers,
1288         },
1289         COMMAND_REGISTRATION_DONE
1290 };
1291
1292 struct flash_driver str9xpec_flash = {
1293         .name = "str9xpec",
1294         .commands = str9xpec_command_handlers,
1295         .flash_bank_command = str9xpec_flash_bank_command,
1296         .erase = str9xpec_erase,
1297         .protect = str9xpec_protect,
1298         .write = str9xpec_write,
1299         .read = default_flash_read,
1300         .probe = str9xpec_probe,
1301         .auto_probe = str9xpec_probe,
1302         .erase_check = str9xpec_erase_check,
1303         .protect_check = str9xpec_protect_check,
1304         .info = get_str9xpec_info,
1305 };