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

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