git.sur5r.net Git - openocd/blob - 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                 LOG_WARNING("incomplete flash_bank str9x configuration");
 296                 return ERROR_FLASH_BANK_INVALID;
 297         }
 298
 299         str9xpec_info = malloc(sizeof(struct str9xpec_flash_controller));
 300         bank->driver_priv = str9xpec_info;
 301
 302         /* REVISIT verify that the jtag position of flash controller is
 303          * right after *THIS* core, which must be a STR9xx core ...
 304          */
 305         armv4_5 = bank->target->arch_info;
 306         arm7_9 = armv4_5->arch_info;
 307         jtag_info = &arm7_9->jtag_info;
 308
 309         str9xpec_info->tap = bank->target->tap;
 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                 command_print(CMD_CTX, "str9xpec options_read <bank>");
 828                 return ERROR_OK;
 829         }
 830
 831         struct flash_bank *bank;
 832         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 833         if (ERROR_OK != retval)
 834                 return retval;
 835
 836         str9xpec_info = bank->driver_priv;
 837
 838         status = str9xpec_read_config(bank);
 839
 840         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 841                 return ERROR_FLASH_OPERATION_FAILED;
 842
 843         /* boot bank */
 844         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1))
 845                 command_print(CMD_CTX, "CS Map: bank1");
 846         else
 847                 command_print(CMD_CTX, "CS Map: bank0");
 848
 849         /* OTP lock */
 850         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_OTPBIT, 1))
 851                 command_print(CMD_CTX, "OTP Lock: OTP Locked");
 852         else
 853                 command_print(CMD_CTX, "OTP Lock: OTP Unlocked");
 854
 855         /* LVD Threshold */
 856         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1))
 857                 command_print(CMD_CTX, "LVD Threshold: 2.7v");
 858         else
 859                 command_print(CMD_CTX, "LVD Threshold: 2.4v");
 860
 861         /* LVD reset warning */
 862         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1))
 863                 command_print(CMD_CTX, "LVD Reset Warning: VDD or VDDQ Inputs");
 864         else
 865                 command_print(CMD_CTX, "LVD Reset Warning: VDD Input Only");
 866
 867         /* LVD reset select */
 868         if (buf_get_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1))
 869                 command_print(CMD_CTX, "LVD Reset Selection: VDD or VDDQ Inputs");
 870         else
 871                 command_print(CMD_CTX, "LVD Reset Selection: VDD Input Only");
 872
 873         return ERROR_OK;
 874 }
 875
 876 static int str9xpec_write_options(struct flash_bank *bank)
 877 {
 878         struct scan_field field;
 879         uint8_t status;
 880         struct jtag_tap *tap;
 881         struct str9xpec_flash_controller *str9xpec_info = NULL;
 882
 883         str9xpec_info = bank->driver_priv;
 884         tap = str9xpec_info->tap;
 885
 886         /* erase config options first */
 887         status = str9xpec_erase_area(bank, 0xFE, 0xFE);
 888
 889         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 890                 return status;
 891
 892         if (!str9xpec_info->isc_enable) {
 893                 str9xpec_isc_enable(bank);
 894         }
 895
 896         if (!str9xpec_info->isc_enable) {
 897                 return ISC_STATUS_ERROR;
 898         }
 899
 900         /* according to data 64th bit has to be set */
 901         buf_set_u32(str9xpec_info->options, 63, 1, 1);
 902
 903         /* set option byte address */
 904         str9xpec_set_address(bank, 0x50);
 905
 906         /* execute ISC_PROGRAM command */
 907         str9xpec_set_instr(tap, ISC_PROGRAM, TAP_IRPAUSE);
 908
 909         field.num_bits = 64;
 910         field.out_value = str9xpec_info->options;
 911         field.in_value = NULL;
 912
 913         jtag_add_dr_scan(tap, 1, &field, TAP_IDLE);
 914
 915         /* small delay before polling */
 916         jtag_add_sleep(50);
 917
 918         str9xpec_set_instr(tap, ISC_NOOP, TAP_IRPAUSE);
 919
 920         do {
 921                 field.num_bits = 8;
 922                 field.out_value = NULL;
 923                 field.in_value = &status;
 924
 925                 jtag_add_dr_scan(tap, 1, &field, TAP_IRPAUSE);
 926                 jtag_execute_queue();
 927
 928         } while (!(status & ISC_STATUS_BUSY));
 929
 930         str9xpec_isc_disable(bank);
 931
 932         return status;
 933 }
 934
 935 COMMAND_HANDLER(str9xpec_handle_flash_options_write_command)
 936 {
 937         uint8_t status;
 938
 939         if (CMD_ARGC < 1)
 940         {
 941                 command_print(CMD_CTX, "str9xpec options_write <bank>");
 942                 return ERROR_OK;
 943         }
 944
 945         struct flash_bank *bank;
 946         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 947         if (ERROR_OK != retval)
 948                 return retval;
 949
 950         status = str9xpec_write_options(bank);
 951
 952         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
 953                 return ERROR_FLASH_OPERATION_FAILED;
 954
 955         command_print(CMD_CTX, "str9xpec write options complete.\n"
 956                         "INFO: a reset or power cycle is required "
 957                         "for the new settings to take effect.");
 958
 959         return ERROR_OK;
 960 }
 961
 962 COMMAND_HANDLER(str9xpec_handle_flash_options_cmap_command)
 963 {
 964         struct str9xpec_flash_controller *str9xpec_info = NULL;
 965
 966         if (CMD_ARGC < 2)
 967         {
 968                 command_print(CMD_CTX, "str9xpec options_cmap <bank> <bank0 | bank1>");
 969                 return ERROR_OK;
 970         }
 971
 972         struct flash_bank *bank;
 973         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 974         if (ERROR_OK != retval)
 975                 return retval;
 976
 977         str9xpec_info = bank->driver_priv;
 978
 979         if (strcmp(CMD_ARGV[1], "bank1") == 0)
 980         {
 981                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 1);
 982         }
 983         else
 984         {
 985                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_CSMAPBIT, 1, 0);
 986         }
 987
 988         return ERROR_OK;
 989 }
 990
 991 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdthd_command)
 992 {
 993         struct str9xpec_flash_controller *str9xpec_info = NULL;
 994
 995         if (CMD_ARGC < 2)
 996         {
 997                 command_print(CMD_CTX, "str9xpec options_lvdthd <bank> <2.4v | 2.7v>");
 998                 return ERROR_OK;
 999         }
1000
1001         struct flash_bank *bank;
1002         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1003         if (ERROR_OK != retval)
1004                 return retval;
1005
1006         str9xpec_info = bank->driver_priv;
1007
1008         if (strcmp(CMD_ARGV[1], "2.7v") == 0)
1009         {
1010                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 1);
1011         }
1012         else
1013         {
1014                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDTHRESBIT, 1, 0);
1015         }
1016
1017         return ERROR_OK;
1018 }
1019
1020 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdsel_command)
1021 {
1022         struct str9xpec_flash_controller *str9xpec_info = NULL;
1023
1024         if (CMD_ARGC < 2)
1025         {
1026                 command_print(CMD_CTX, "str9xpec options_lvdsel <bank> <vdd | vdd_vddq>");
1027                 return ERROR_OK;
1028         }
1029
1030         struct flash_bank *bank;
1031         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1032         if (ERROR_OK != retval)
1033                 return retval;
1034
1035         str9xpec_info = bank->driver_priv;
1036
1037         if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
1038         {
1039                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 1);
1040         }
1041         else
1042         {
1043                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDSELBIT, 1, 0);
1044         }
1045
1046         return ERROR_OK;
1047 }
1048
1049 COMMAND_HANDLER(str9xpec_handle_flash_options_lvdwarn_command)
1050 {
1051         struct str9xpec_flash_controller *str9xpec_info = NULL;
1052
1053         if (CMD_ARGC < 2)
1054         {
1055                 command_print(CMD_CTX, "str9xpec options_lvdwarn <bank> <vdd | vdd_vddq>");
1056                 return ERROR_OK;
1057         }
1058
1059         struct flash_bank *bank;
1060         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1061         if (ERROR_OK != retval)
1062                 return retval;
1063
1064         str9xpec_info = bank->driver_priv;
1065
1066         if (strcmp(CMD_ARGV[1], "vdd_vddq") == 0)
1067         {
1068                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 1);
1069         }
1070         else
1071         {
1072                 buf_set_u32(str9xpec_info->options, STR9XPEC_OPT_LVDWARNBIT, 1, 0);
1073         }
1074
1075         return ERROR_OK;
1076 }
1077
1078 COMMAND_HANDLER(str9xpec_handle_flash_lock_command)
1079 {
1080         uint8_t status;
1081
1082         if (CMD_ARGC < 1)
1083         {
1084                 command_print(CMD_CTX, "str9xpec lock <bank>");
1085                 return ERROR_OK;
1086         }
1087
1088         struct flash_bank *bank;
1089         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1090         if (ERROR_OK != retval)
1091                 return retval;
1092
1093         status = str9xpec_lock_device(bank);
1094
1095         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1096                 return ERROR_FLASH_OPERATION_FAILED;
1097
1098         return ERROR_OK;
1099 }
1100
1101 COMMAND_HANDLER(str9xpec_handle_flash_unlock_command)
1102 {
1103         uint8_t status;
1104
1105         if (CMD_ARGC < 1)
1106         {
1107                 command_print(CMD_CTX, "str9xpec unlock <bank>");
1108                 return ERROR_OK;
1109         }
1110
1111         struct flash_bank *bank;
1112         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1113         if (ERROR_OK != retval)
1114                 return retval;
1115
1116         status = str9xpec_unlock_device(bank);
1117
1118         if ((status & ISC_STATUS_ERROR) != STR9XPEC_ISC_SUCCESS)
1119                 return ERROR_FLASH_OPERATION_FAILED;
1120
1121         command_print(CMD_CTX, "str9xpec unlocked.\n"
1122                         "INFO: a reset or power cycle is required "
1123                         "for the new settings to take effect.");
1124
1125         return ERROR_OK;
1126 }
1127
1128 COMMAND_HANDLER(str9xpec_handle_flash_enable_turbo_command)
1129 {
1130         struct jtag_tap *tap0;
1131         struct jtag_tap *tap1;
1132         struct jtag_tap *tap2;
1133         struct str9xpec_flash_controller *str9xpec_info = NULL;
1134
1135         if (CMD_ARGC < 1)
1136         {
1137                 command_print(CMD_CTX, "str9xpec enable_turbo <bank>");
1138                 return ERROR_OK;
1139         }
1140
1141         struct flash_bank *bank;
1142         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1143         if (ERROR_OK != retval)
1144                 return retval;
1145
1146         str9xpec_info = bank->driver_priv;
1147
1148         tap0 = str9xpec_info->tap;
1149
1150         /* remove arm core from chain - enter turbo mode */
1151         tap1 = tap0->next_tap;
1152         if (tap1 == NULL)
1153         {
1154                 /* things are *WRONG* */
1155                 command_print(CMD_CTX,"**STR9FLASH** (tap1) invalid chain?");
1156                 return ERROR_OK;
1157         }
1158         tap2 = tap1->next_tap;
1159         if (tap2 == NULL)
1160         {
1161                 /* things are *WRONG* */
1162                 command_print(CMD_CTX,"**STR9FLASH** (tap2) invalid chain?");
1163                 return ERROR_OK;
1164         }
1165
1166         /* enable turbo mode - TURBO-PROG-ENABLE */
1167         str9xpec_set_instr(tap2, 0xD, TAP_IDLE);
1168         if ((retval = jtag_execute_queue()) != ERROR_OK)
1169                 return retval;
1170
1171         /* modify scan chain - str9 core has been removed */
1172         tap1->enabled = 0;
1173
1174         return ERROR_OK;
1175 }
1176
1177 COMMAND_HANDLER(str9xpec_handle_flash_disable_turbo_command)
1178 {
1179         struct jtag_tap *tap;
1180         struct str9xpec_flash_controller *str9xpec_info = NULL;
1181
1182         if (CMD_ARGC < 1)
1183         {
1184                 command_print(CMD_CTX, "str9xpec disable_turbo <bank>");
1185                 return ERROR_OK;
1186         }
1187
1188         struct flash_bank *bank;
1189         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
1190         if (ERROR_OK != retval)
1191                 return retval;
1192
1193         str9xpec_info = bank->driver_priv;
1194         tap = str9xpec_info->tap;
1195
1196         if (tap == NULL)
1197                 return ERROR_FAIL;
1198
1199         /* exit turbo mode via RESET */
1200         str9xpec_set_instr(tap, ISC_NOOP, TAP_IDLE);
1201         jtag_add_tlr();
1202         jtag_execute_queue();
1203
1204         /* restore previous scan chain */
1205         if (tap->next_tap) {
1206                 tap->next_tap->enabled = 1;
1207         }
1208
1209         return ERROR_OK;
1210 }
1211
1212 static const struct command_registration str9xpec_config_command_handlers[] = {
1213         {
1214                 .name = "enable_turbo",
1215                 .handler = str9xpec_handle_flash_enable_turbo_command,
1216                 .mode = COMMAND_EXEC,
1217                 .help = "enable str9xpec turbo mode",
1218         },
1219         {
1220                 .name = "disable_turbo",
1221                 .handler = str9xpec_handle_flash_disable_turbo_command,
1222                 .mode = COMMAND_EXEC,
1223                 .help = "disable str9xpec turbo mode",
1224         },
1225         {
1226                 .name = "options_cmap",
1227                 .handler = str9xpec_handle_flash_options_cmap_command,
1228                 .mode = COMMAND_EXEC,
1229                 .help = "configure str9xpec boot sector",
1230         },
1231         {
1232                 .name = "options_lvdthd",
1233                 .handler = str9xpec_handle_flash_options_lvdthd_command,
1234                 .mode = COMMAND_EXEC,
1235                 .help = "configure str9xpec lvd threshold",
1236         },
1237         {
1238                 .name = "options_lvdsel",
1239                 .handler = str9xpec_handle_flash_options_lvdsel_command,
1240                 .mode = COMMAND_EXEC,
1241                 .help = "configure str9xpec lvd selection",
1242         },
1243         {
1244                 .name = "options_lvdwarn",
1245                 .handler = str9xpec_handle_flash_options_lvdwarn_command,
1246                 .mode = COMMAND_EXEC,
1247                 .help = "configure str9xpec lvd warning",
1248         },
1249         {
1250                 .name = "options_read",
1251                 .handler = str9xpec_handle_flash_options_read_command,
1252                 .mode = COMMAND_EXEC,
1253                 .help = "read str9xpec options",
1254         },
1255         {
1256                 .name = "options_write",
1257                 .handler = str9xpec_handle_flash_options_write_command,
1258                 .mode = COMMAND_EXEC,
1259                 .help = "write str9xpec options",
1260         },
1261         {
1262                 .name = "lock",
1263                 .handler = str9xpec_handle_flash_lock_command,
1264                 .mode = COMMAND_EXEC,
1265                 .help = "lock str9xpec device",
1266         },
1267         {
1268                 .name = "unlock",
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 };