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

   1 /***************************************************************************
   2  *   Copyright (C) 2015 by Bogdan Kolbov                                   *
   3  *   kolbov@niiet.ru                                                       *
   4  *                                                                         *
   5  *   This program is free software; you can redistribute it and/or modify  *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation; either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   This program is distributed in the hope that it will be useful,       *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  17  ***************************************************************************/
  18
  19 #ifdef HAVE_CONFIG_H
  20 #include "config.h"
  21 #endif
  22
  23 #include "imp.h"
  24 #include <helper/binarybuffer.h>
  25 #include <target/algorithm.h>
  26 #include <target/armv7m.h>
  27
  28 #define FLASH_DRIVER_VER                        0x00010000
  29 #define CHIPID_ADDR                                     0xF0000000
  30 #define K1921VK01T_ID                           0x00000000
  31
  32 /*==============================================================================
  33  *                                                      FLASH CONTROL REGS
  34  *==============================================================================
  35  */
  36
  37 #define MAIN_MEM_TYPE                           0
  38 #define INFO_MEM_TYPE                           1
  39 #define SERVICE_MODE_ERASE_ADDR         0x80030164
  40 #define MAGIC_KEY                                       0xA442
  41
  42 /*-- BOOTFLASH ---------------------------------------------------------------*/
  43 #define BOOTFLASH_BASE                          0xA001C000
  44 #define FMA                                                     (BOOTFLASH_BASE + 0x00)
  45 #define FMD1                                            (BOOTFLASH_BASE + 0x04)
  46 #define FMC                                                     (BOOTFLASH_BASE + 0x08)
  47 #define FCIS                                            (BOOTFLASH_BASE + 0x0C)
  48 #define FCIM                                            (BOOTFLASH_BASE + 0x10)
  49 #define FCIC                                            (BOOTFLASH_BASE + 0x14)
  50 #define FMD2                                            (BOOTFLASH_BASE + 0x50)
  51 #define FMD3                                            (BOOTFLASH_BASE + 0x54)
  52 #define FMD4                                            (BOOTFLASH_BASE + 0x58)
  53
  54
  55 /*---- FMC: Command register */
  56 #define FMC_WRITE                                       (1<<0)                          /* Writing in main region */
  57 #define FMC_PAGE_ERASE                          (1<<1)                          /* Page erase the main region */
  58 #define FMC_FULL_ERASE                          (1<<2)                          /* Erase full flash */
  59 #define FMC_WRITE_IFB                           (1<<4)                          /* Writing in info region */
  60 #define FMC_PAGEERASE_IFB                       (1<<5)                          /* Erase page of info region */
  61 #define FMC_MAGIC_KEY                           (MAGIC_KEY<<16)         /* Operation run command */
  62
  63 /*---- FCIS: Status register */
  64 #define FCIS_OP_CMLT                            (1<<0)                          /* Completion flag operation */
  65 #define FCIS_OP_ERROR                           (1<<1)                          /* Flag operation error */
  66
  67 /*---- FCIC: CLear status register */
  68 #define FCIC_CLR_OPCMLT                         (1<<0)                          /* Clear completion flag in register FCIS */
  69 #define FCIC_CLR_OPERROR                        (1<<1)                          /* Clear error flag in register FCIS */
  70
  71 /*-- USERFLASH ---------------------------------------------------------------*/
  72 #define USERFLASH_PAGE_SIZE                     256
  73 #define USERFLASH_PAGE_TOTALNUM         256
  74
  75 #define USERFLASH_BASE                          0xA0022000
  76 #define UFMA                                            (USERFLASH_BASE + 0x00)
  77 #define UFMD                                            (USERFLASH_BASE + 0x04)
  78 #define UFMC                                            (USERFLASH_BASE + 0x08)
  79 #define UFCIS                                           (USERFLASH_BASE + 0x0C)
  80 #define UFCIM                                           (USERFLASH_BASE + 0x10)
  81 #define UFCIC                                           (USERFLASH_BASE + 0x14)
  82
  83 /*---- UFMC: Command register */
  84 #define UFMC_WRITE                                      (1<<0)                          /* Writing in main region */
  85 #define UFMC_PAGE_ERASE                         (1<<1)                          /* Paged erase the main region */
  86 #define UFMC_FULL_ERASE                         (1<<2)                          /* Erase full flash */
  87 #define UFMC_READ                                       (1<<3)                          /* Reading from main region */
  88 #define UFMC_WRITE_IFB                          (1<<4)                          /* Writing in info region */
  89 #define UFMC_PAGEERASE_IFB                      (1<<5)                          /* Erase page of info region */
  90 #define UFMC_READ_IFB                           (1<<6)                          /* Reading from info region */
  91 #define UFMC_MAGIC_KEY                          (MAGIC_KEY<<16)         /* Operation run command */
  92
  93 /*---- UFCIS: Status register */
  94 #define UFCIS_OP_CMLT                           (1<<0)                          /* Completion flag operation */
  95 #define UFCIS_OP_ERROR                          (1<<1)                          /* Flag operation error */
  96
  97 /*---- UFCIC: CLear status register */
  98 #define UFCIC_CLR_OPCMLT                        (1<<0)                          /* Clear completion flag in register FCIS */
  99 #define UFCIC_CLR_OPERROR                       (1<<1)                          /* Clear error flag in register FCIS */
 100
 101 /*---- In info userflash address space */
 102 #define INFOWORD0_ADDR                          0x00
 103 #define INFOWORD0_BOOTFROM_IFB          (1<<0)                          /* Boot from bootflash or bootflash_ifb */
 104 #define INFOWORD0_EN_GPIO                       (1<<1)                          /* Remap to 0x00000000 extmem or bootflash */
 105 #define INFOWORD0_BOOTFROM_IFB_POS      0
 106 #define INFOWORD0_EN_GPIO_POS           1
 107 #define INFOWORD0_EXTMEM_SEL_POS        3                                       /* Choose altfunc of gpio to work with extmem */
 108
 109 #define INFOWORD1_ADDR                          0x01
 110 #define INFOWORD1_PINNUM_POS            0                                       /* Choose gpio pin number to control extmem boot */
 111 #define INFOWORD1_PORTNUM_POS           4                                       /* Choose gpio port to control extmem boot */
 112
 113 #define INFOWORD2_ADDR                          0x02
 114 #define INFOWORD2_LOCK_IFB_BF           (1<<0)                          /* Protect info part of bootflash */
 115
 116 #define INFOWORD3_ADDR                          0x03
 117 #define INFOWORD3_LOCK_IFB_UF           (1<<0)                          /* Protect info part of userflash */
 118
 119 #define BF_LOCK_ADDR                            0x40
 120 #define UF_LOCK_ADDR                            0x80
 121
 122 /**
 123  * Private data for flash driver.
 124  */
 125 struct niietcm4_flash_bank {
 126         /* target params */
 127         bool probed;
 128         uint32_t chipid;
 129         char *chip_name;
 130         char chip_brief[4096];
 131         /* not mapped userflash params */
 132         uint32_t uflash_width;
 133         uint32_t uflash_size;
 134         uint32_t uflash_pagetotal;
 135         uint32_t uflash_info_size;
 136         uint32_t uflash_info_pagetotal;
 137         /* boot params */
 138         bool bflash_info_remap;
 139         char *extmem_boot_port;
 140         uint32_t extmem_boot_pin;
 141         uint32_t extmem_boot_altfunc;
 142         bool extmem_boot;
 143 };
 144
 145 /*==============================================================================
 146  *                                                      HELPER FUNCTIONS
 147  *==============================================================================
 148  */
 149
 150 /**
 151  * Wait while operation with bootflash being performed and check result status
 152  */
 153 static int niietcm4_opstatus_check(struct flash_bank *bank)
 154 {
 155         struct target *target = bank->target;
 156         int retval;
 157         int timeout = 5000;
 158
 159         uint32_t flash_status;
 160         retval = target_read_u32(target, FCIS, &flash_status);
 161         if (retval != ERROR_OK)
 162                 return retval;
 163
 164         while (flash_status == 0x00) {
 165                 retval = target_read_u32(target, FCIS, &flash_status);
 166                 if (retval != ERROR_OK)
 167                         return retval;
 168                 if (timeout-- <= 0) {
 169                         LOG_ERROR("Bootflash operation timeout");
 170                         return ERROR_FLASH_OPERATION_FAILED;
 171                         }
 172                 busy_sleep(1);  /* can use busy sleep for short times. */
 173         }
 174         if (flash_status == FCIS_OP_ERROR) {
 175                 LOG_ERROR("Bootflash operation error");
 176                 return ERROR_FLASH_OPERATION_FAILED;
 177         }
 178         /* clear status */
 179         uint32_t flash_cmd = FCIC_CLR_OPCMLT | FCIC_CLR_OPERROR;
 180         retval = target_write_u32(target, FCIC, flash_cmd);
 181         if (retval != ERROR_OK)
 182                 return retval;
 183
 184         return retval;
 185 }
 186
 187 /**
 188  * Wait while operation with userflash being performed and check result status
 189  */
 190 static int niietcm4_uopstatus_check(struct flash_bank *bank)
 191 {
 192         struct target *target = bank->target;
 193         int retval;
 194         int timeout = 5000;
 195
 196         uint32_t uflash_status;
 197         retval = target_read_u32(target, UFCIS, &uflash_status);
 198         if (retval != ERROR_OK)
 199                 return retval;
 200
 201         while (uflash_status == 0x00) {
 202                 retval = target_read_u32(target, UFCIS, &uflash_status);
 203                 if (retval != ERROR_OK)
 204                         return retval;
 205                 if (timeout-- <= 0) {
 206                         LOG_ERROR("Userflash operation timeout");
 207                         return ERROR_FLASH_OPERATION_FAILED;
 208                         }
 209                 busy_sleep(1);  /* can use busy sleep for short times. */
 210         }
 211         if (uflash_status == UFCIS_OP_ERROR) {
 212                 LOG_ERROR("Userflash operation error");
 213                 return ERROR_FLASH_OPERATION_FAILED;
 214         }
 215         /* clear status */
 216         uint32_t uflash_cmd = UFCIC_CLR_OPCMLT | UFCIC_CLR_OPERROR;
 217         retval = target_write_u32(target, UFCIC, uflash_cmd);
 218         if (retval != ERROR_OK)
 219                 return retval;
 220
 221         return retval;
 222 }
 223
 224 /**
 225  * Dump page of userflash region.
 226  * If we want to change some settings, we have to dump it full, because userflash is flash(not EEPROM).
 227  * And correct write to flash can be performed only after erase.
 228  * So without dump, changing one registers will clear others.
 229  */
 230 static int niietcm4_dump_uflash_page(struct flash_bank *bank, uint32_t *dump, int page_num, int mem_type)
 231 {
 232         struct target *target = bank->target;
 233         int i;
 234         int retval = ERROR_OK;
 235
 236         uint32_t uflash_cmd;
 237         if (mem_type == INFO_MEM_TYPE)
 238                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
 239         else
 240                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ;
 241
 242         int first = page_num*USERFLASH_PAGE_SIZE;
 243         int last = first + USERFLASH_PAGE_SIZE;
 244
 245         for (i = first; i < last; i++) {
 246                 retval = target_write_u32(target, UFMA, i);
 247                 if (retval != ERROR_OK)
 248                         return retval;
 249                 retval = target_write_u32(target, UFMC, uflash_cmd);
 250                 if (retval != ERROR_OK)
 251                         return retval;
 252                 retval = niietcm4_uopstatus_check(bank);
 253                 if (retval != ERROR_OK)
 254                         return retval;
 255                 retval = target_read_u32(target, UFMD, &dump[i]);
 256                 if (retval != ERROR_OK)
 257                         return retval;
 258         }
 259
 260         return retval;
 261 }
 262
 263 /**
 264  * Load modified page dump to userflash region page.
 265  */
 266 static int niietcm4_load_uflash_page(struct flash_bank *bank, uint32_t *dump, int page_num, int mem_type)
 267 {
 268         struct target *target = bank->target;
 269         int i;
 270         int retval = ERROR_OK;
 271
 272         uint32_t uflash_cmd;
 273         if (mem_type == INFO_MEM_TYPE)
 274                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_WRITE_IFB;
 275         else
 276                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_WRITE;
 277
 278         int first = page_num*USERFLASH_PAGE_SIZE;
 279         int last = first + USERFLASH_PAGE_SIZE;
 280
 281         for (i = first; i < last; i++) {
 282                 retval = target_write_u32(target, UFMA, i);
 283                 if (retval != ERROR_OK)
 284                         return retval;
 285                 retval = target_write_u32(target, UFMD, dump[i]);
 286                 if (retval != ERROR_OK)
 287                         return retval;
 288                 retval = target_write_u32(target, UFMC, uflash_cmd);
 289                 if (retval != ERROR_OK)
 290                         return retval;
 291                 retval = niietcm4_uopstatus_check(bank);
 292                 if (retval != ERROR_OK)
 293                         return retval;
 294         }
 295
 296         return retval;
 297 }
 298
 299 /**
 300  * Erase one page of userflash info or main region
 301  */
 302 static int niietcm4_uflash_page_erase(struct flash_bank *bank, int page_num, int mem_type)
 303 {
 304         struct target *target = bank->target;
 305         int retval;
 306
 307         uint32_t uflash_cmd;
 308         if (mem_type == INFO_MEM_TYPE)
 309                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_PAGEERASE_IFB;
 310         else
 311                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_PAGE_ERASE;
 312
 313         retval = target_write_u32(target, UFMA, page_num*USERFLASH_PAGE_SIZE);
 314         if (retval != ERROR_OK)
 315                 return retval;
 316         retval = target_write_u32(target, UFMD, 0xFF);
 317         if (retval != ERROR_OK)
 318                 return retval;
 319         retval = target_write_u32(target, UFMC, uflash_cmd);
 320         if (retval != ERROR_OK)
 321                 return retval;
 322         /* status check */
 323         retval = niietcm4_uopstatus_check(bank);
 324         if (retval != ERROR_OK)
 325                         return retval;
 326
 327         return retval;
 328 }
 329
 330 /**
 331  * Enable or disable protection of userflash pages
 332  */
 333 static int niietcm4_uflash_protect(struct flash_bank *bank, int mem_type,
 334                 int set, unsigned int first, unsigned int last)
 335 {
 336         int retval;
 337         if (mem_type == INFO_MEM_TYPE) {
 338                 /* read dump */
 339                 uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 340                 retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
 341                 if (retval != ERROR_OK)
 342                         return retval;
 343                 /* modify dump */
 344                 if (set)
 345                         uflash_dump[INFOWORD2_ADDR] &= ~INFOWORD3_LOCK_IFB_UF;
 346                 else
 347                         uflash_dump[INFOWORD2_ADDR] |= INFOWORD3_LOCK_IFB_UF;
 348                 /* erase page 0 userflash */
 349                 retval = niietcm4_uflash_page_erase(bank, 0, 1);
 350                 if (retval != ERROR_OK)
 351                         return retval;
 352                 /* write dump to userflash */
 353                 retval = niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
 354                 if (retval != ERROR_OK)
 355                         return retval;
 356         } else {
 357                 /* read dump */
 358                 uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 359                 retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
 360                 if (retval != ERROR_OK)
 361                         return retval;
 362                 /* modify dump */
 363                 for (unsigned int i = first; i <= last; i++) {
 364                         uint32_t reg_num = i/8;
 365                         uint32_t bit_num = i%8;
 366                         if (set)
 367                                 uflash_dump[UF_LOCK_ADDR+reg_num] &= ~(1<<bit_num);
 368                         else
 369                                 uflash_dump[UF_LOCK_ADDR+reg_num] |= (1<<bit_num);
 370                 }
 371                 /* erase page 0 info userflash */
 372                 retval = niietcm4_uflash_page_erase(bank, 0, 1);
 373                 if (retval != ERROR_OK)
 374                         return retval;
 375                 /* write dump to userflash */
 376                 retval = niietcm4_load_uflash_page(bank, uflash_dump,  0, 1);
 377                 if (retval != ERROR_OK)
 378                         return retval;
 379         }
 380
 381         return retval;
 382 }
 383
 384 /*==============================================================================
 385  *                                                      FLASH COMMANDS
 386  *==============================================================================
 387  */
 388 COMMAND_HANDLER(niietcm4_handle_uflash_read_byte_command)
 389 {
 390         if (CMD_ARGC < 3)
 391                 return ERROR_COMMAND_SYNTAX_ERROR;
 392
 393         struct flash_bank *bank;
 394         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 395         if (retval != ERROR_OK)
 396                 return retval;
 397         struct target *target = bank->target;
 398
 399         /* skip over flash bank */
 400         CMD_ARGC--;
 401         CMD_ARGV++;
 402
 403         uint32_t uflash_addr;
 404         uint32_t uflash_cmd;
 405         uint32_t uflash_data;
 406
 407         if (strcmp("info", CMD_ARGV[0]) == 0)
 408                         uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
 409         else if (strcmp("main", CMD_ARGV[0]) == 0)
 410                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ;
 411         else
 412                 return ERROR_COMMAND_SYNTAX_ERROR;
 413
 414         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], uflash_addr);
 415
 416         retval = target_write_u32(target, UFMA, uflash_addr);
 417         if (retval != ERROR_OK)
 418                 return retval;
 419         retval = target_write_u32(target, UFMC, uflash_cmd);
 420         if (retval != ERROR_OK)
 421                 return retval;
 422         /* status check */
 423         retval = niietcm4_uopstatus_check(bank);
 424         if (retval != ERROR_OK)
 425                 return retval;
 426         retval = target_read_u32(target, UFMD, &uflash_data);
 427         if (retval != ERROR_OK)
 428                 return retval;
 429         command_print(CMD,  "Read userflash %s region:\n"
 430                                                 "address = 0x%04" PRIx32 ",\n"
 431                                                 "value   = 0x%02" PRIx32 ".", CMD_ARGV[0], uflash_addr, uflash_data);
 432         return retval;
 433 }
 434
 435 COMMAND_HANDLER(niietcm4_handle_uflash_write_byte_command)
 436 {
 437         if (CMD_ARGC < 4)
 438                 return ERROR_COMMAND_SYNTAX_ERROR;
 439
 440         struct flash_bank *bank;
 441         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 442         if (retval != ERROR_OK)
 443                 return retval;
 444         struct target *target = bank->target;
 445
 446         if (target->state != TARGET_HALTED) {
 447                 LOG_ERROR("Target not halted");
 448                 return ERROR_TARGET_NOT_HALTED;
 449         }
 450
 451         /* skip over flash bank */
 452         CMD_ARGC--;
 453         CMD_ARGV++;
 454
 455         uint32_t uflash_addr;
 456         uint32_t uflash_data;
 457         int mem_type;
 458
 459         if (strcmp("info", CMD_ARGV[0]) == 0)
 460                 mem_type = 1;
 461         else if (strcmp("main", CMD_ARGV[0]) == 0)
 462                 mem_type = 0;
 463         else
 464                 return ERROR_COMMAND_SYNTAX_ERROR;
 465
 466         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], uflash_addr);
 467         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[2], uflash_data);
 468
 469         int page_num = uflash_addr/USERFLASH_PAGE_SIZE;
 470
 471         command_print(CMD, "Write userflash %s region:\n"
 472                                            "address = 0x%04" PRIx32 ",\n"
 473                                            "value   = 0x%02" PRIx32 ".\n"
 474                                            "Please wait ... ", CMD_ARGV[0], uflash_addr, uflash_data);
 475         /* dump */
 476         uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 477         niietcm4_dump_uflash_page(bank, uflash_dump, page_num, mem_type);
 478
 479         /* modify dump */
 480         uflash_dump[uflash_addr%USERFLASH_PAGE_SIZE] = uflash_data;
 481
 482         /* erase page userflash */
 483         niietcm4_uflash_page_erase(bank, page_num, mem_type);
 484
 485         /* write dump to userflash */
 486         niietcm4_load_uflash_page(bank, uflash_dump, page_num, mem_type);
 487         command_print(CMD, "done!");
 488         return retval;
 489 }
 490
 491 COMMAND_HANDLER(niietcm4_handle_uflash_full_erase_command)
 492 {
 493         if (CMD_ARGC < 1)
 494                 return ERROR_COMMAND_SYNTAX_ERROR;
 495
 496         struct flash_bank *bank;
 497         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 498         if (retval != ERROR_OK)
 499                 return retval;
 500         struct target *target = bank->target;
 501
 502         if (target->state != TARGET_HALTED) {
 503                 LOG_ERROR("Target not halted");
 504                 return ERROR_TARGET_NOT_HALTED;
 505         }
 506
 507         uint32_t uflash_addr = 0;
 508         uint32_t uflash_data = 0xFF;
 509         uint32_t uflash_cmd = UFMC_MAGIC_KEY | UFMC_FULL_ERASE;
 510
 511         retval = target_write_u32(target, UFMA, uflash_addr);
 512         if (retval != ERROR_OK)
 513                 return retval;
 514         retval = target_write_u32(target, UFMD, uflash_data);
 515         if (retval != ERROR_OK)
 516                 return retval;
 517         retval = target_write_u32(target, UFMC, uflash_cmd);
 518         if (retval != ERROR_OK)
 519                 return retval;
 520         /* status check */
 521         retval = niietcm4_uopstatus_check(bank);
 522         if (retval != ERROR_OK)
 523                 return retval;
 524         command_print(CMD, "Userflash full erase done!");
 525
 526         return retval;
 527 }
 528
 529 COMMAND_HANDLER(niietcm4_handle_uflash_erase_command)
 530 {
 531         if (CMD_ARGC < 4)
 532                 return ERROR_COMMAND_SYNTAX_ERROR;
 533
 534         struct flash_bank *bank;
 535         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 536         if (retval != ERROR_OK)
 537                 return retval;
 538         struct target *target = bank->target;
 539
 540         if (target->state != TARGET_HALTED) {
 541                 LOG_ERROR("Target not halted");
 542                 return ERROR_TARGET_NOT_HALTED;
 543         }
 544
 545         /* skip over flash bank */
 546         CMD_ARGC--;
 547         CMD_ARGV++;
 548
 549         unsigned int first, last;
 550         int mem_type;
 551
 552         if (strcmp("info", CMD_ARGV[0]) == 0)
 553                         mem_type = 1;
 554         else if (strcmp("main", CMD_ARGV[0]) == 0)
 555                 mem_type = 0;
 556         else
 557                 return ERROR_COMMAND_SYNTAX_ERROR;
 558
 559         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], first);
 560         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[2], last);
 561         for (unsigned int i = first; i <= last; i++) {
 562                 retval = niietcm4_uflash_page_erase(bank, i, mem_type);
 563                 if (retval != ERROR_OK)
 564                         return retval;
 565         }
 566
 567         command_print(CMD, "Erase %s userflash pages %u through %u done!", CMD_ARGV[0], first, last);
 568
 569         return retval;
 570 }
 571
 572 COMMAND_HANDLER(niietcm4_handle_uflash_protect_check_command)
 573 {
 574         if (CMD_ARGC < 2)
 575                 return ERROR_COMMAND_SYNTAX_ERROR;
 576
 577         struct flash_bank *bank;
 578         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 579         if (retval != ERROR_OK)
 580                 return retval;
 581
 582         struct target *target = bank->target;
 583         if (target->state != TARGET_HALTED) {
 584                 LOG_ERROR("Target not halted");
 585                 return ERROR_TARGET_NOT_HALTED;
 586         }
 587
 588         /* skip over flash bank */
 589         CMD_ARGC--;
 590         CMD_ARGV++;
 591
 592         int mem_type;
 593         if (strcmp("info", CMD_ARGV[0]) == 0)
 594                 mem_type = 1;
 595         else if (strcmp("main", CMD_ARGV[0]) == 0)
 596                 mem_type = 0;
 597         else
 598                 return ERROR_COMMAND_SYNTAX_ERROR;
 599
 600         int i, j;
 601         uint32_t uflash_addr;
 602         uint32_t uflash_cmd;
 603         uint32_t uflash_data;
 604
 605         /* chose between main userflash and info userflash */
 606         if (mem_type == INFO_MEM_TYPE) {
 607                 uflash_addr = INFOWORD3_ADDR;
 608                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
 609                 retval = target_write_u32(target, UFMA, uflash_addr);
 610                 if (retval != ERROR_OK)
 611                         return retval;
 612                 retval = target_write_u32(target, UFMC, uflash_cmd);
 613                 if (retval != ERROR_OK)
 614                         return retval;
 615
 616                 /* status check */
 617                 retval = niietcm4_uopstatus_check(bank);
 618                 if (retval != ERROR_OK)
 619                         return retval;
 620                 retval = target_read_u32(target, UFMD, &uflash_data);
 621                 if (retval != ERROR_OK)
 622                         return retval;
 623
 624                 if (uflash_data & INFOWORD3_LOCK_IFB_UF)
 625                         command_print(CMD, "All sectors of info userflash are not protected!");
 626                 else
 627                         command_print(CMD, "All sectors of info userflash are protected!");
 628         } else {
 629                 uflash_addr = UF_LOCK_ADDR;
 630                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
 631                 for (i = 0; i < USERFLASH_PAGE_TOTALNUM/8; i++) {
 632                         retval = target_write_u32(target, UFMA, uflash_addr);
 633                         if (retval != ERROR_OK)
 634                                 return retval;
 635                         retval = target_write_u32(target, UFMC, uflash_cmd);
 636                         if (retval != ERROR_OK)
 637                                 return retval;
 638
 639                         /* status check */
 640                         retval = niietcm4_uopstatus_check(bank);
 641                         if (retval != ERROR_OK)
 642                                 return retval;
 643                         retval = target_read_u32(target, UFMD, &uflash_data);
 644                         if (retval != ERROR_OK)
 645                                 return retval;
 646
 647                         for (j = 0; j < 8; j++) {
 648                                 if (uflash_data & 0x1)
 649                                         command_print(CMD, "Userflash sector #%03d: 0x%04x (0x100) is not protected!",
 650                                                                                         i*8+j, (i*8+j)*USERFLASH_PAGE_SIZE);
 651                                 else
 652                                         command_print(CMD, "Userflash sector #%03d: 0x%04x (0x100) is protected!",
 653                                                                                         i*8+j, (i*8+j)*USERFLASH_PAGE_SIZE);
 654                                 uflash_data = uflash_data >> 1;
 655                         }
 656                         uflash_addr++;
 657                 }
 658         }
 659
 660         return retval;
 661 }
 662
 663 COMMAND_HANDLER(niietcm4_handle_uflash_protect_command)
 664 {
 665         if (CMD_ARGC < 5)
 666                 return ERROR_COMMAND_SYNTAX_ERROR;
 667
 668         struct flash_bank *bank;
 669         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 670         if (retval != ERROR_OK)
 671                 return retval;
 672         struct target *target = bank->target;
 673
 674         if (target->state != TARGET_HALTED) {
 675                 LOG_ERROR("Target not halted");
 676                 return ERROR_TARGET_NOT_HALTED;
 677         }
 678
 679         /* skip over flash bank */
 680         CMD_ARGC--;
 681         CMD_ARGV++;
 682
 683         int mem_type;
 684         if (strcmp("info", CMD_ARGV[0]) == 0)
 685                 mem_type = 1;
 686         else if (strcmp("main", CMD_ARGV[0]) == 0)
 687                 mem_type = 0;
 688         else
 689                 return ERROR_COMMAND_SYNTAX_ERROR;
 690
 691         unsigned int first, last;
 692         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], first);
 693         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[2], last);
 694
 695         int set;
 696         if (strcmp("on", CMD_ARGV[3]) == 0) {
 697                 command_print(CMD, "Try to enable %s userflash sectors %u through %u protection. Please wait ... ",
 698                                                                 CMD_ARGV[0], first, last);
 699                 set = 1;
 700         } else if (strcmp("off", CMD_ARGV[3]) == 0) {
 701                 command_print(CMD, "Try to disable %s userflash sectors %u through %u protection. Please wait ... ",
 702                                                                 CMD_ARGV[0], first, last);
 703                 set = 0;
 704         } else
 705                 return ERROR_COMMAND_SYNTAX_ERROR;
 706
 707         retval = niietcm4_uflash_protect(bank, mem_type, set, first, last);
 708                 if (retval != ERROR_OK)
 709                         return retval;
 710
 711         command_print(CMD, "done!");
 712         return retval;
 713 }
 714
 715 COMMAND_HANDLER(niietcm4_handle_bflash_info_remap_command)
 716 {
 717         if (CMD_ARGC < 2)
 718                 return ERROR_COMMAND_SYNTAX_ERROR;
 719
 720         struct flash_bank *bank;
 721         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 722         if (retval != ERROR_OK)
 723                 return retval;
 724         struct target *target = bank->target;
 725
 726         if (target->state != TARGET_HALTED) {
 727                 LOG_ERROR("Target not halted");
 728                 return ERROR_TARGET_NOT_HALTED;
 729         }
 730
 731         /* skip over flash bank */
 732         CMD_ARGC--;
 733         CMD_ARGV++;
 734
 735         int set;
 736         if (strcmp("on", CMD_ARGV[0]) == 0) {
 737                 command_print(CMD, "Try to enable bootflash info region remap. Please wait ...");
 738                 set = 1;
 739         } else if (strcmp("off", CMD_ARGV[0]) == 0) {
 740                 command_print(CMD, "Try to disable bootflash info region remap. Please wait ...");
 741                 set = 0;
 742         } else
 743                 return ERROR_COMMAND_SYNTAX_ERROR;
 744
 745         /* dump */
 746         uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 747         niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
 748
 749         /* modify dump */
 750         if (set)
 751                 uflash_dump[INFOWORD0_ADDR] &= ~INFOWORD0_BOOTFROM_IFB;
 752         else
 753                 uflash_dump[INFOWORD0_ADDR] |= INFOWORD0_BOOTFROM_IFB;
 754
 755         /* erase page userflash */
 756         niietcm4_uflash_page_erase(bank, 0, 1);
 757
 758         /* write dump to userflash */
 759         niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
 760         command_print(CMD, "done!");
 761
 762         return retval;
 763 }
 764
 765 COMMAND_HANDLER(niietcm4_handle_extmem_cfg_command)
 766 {
 767         if (CMD_ARGC < 4)
 768                 return ERROR_COMMAND_SYNTAX_ERROR;
 769
 770         struct flash_bank *bank;
 771         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 772         if (retval != ERROR_OK)
 773                 return retval;
 774         struct target *target = bank->target;
 775
 776         if (target->state != TARGET_HALTED) {
 777                 LOG_ERROR("Target not halted");
 778                 return ERROR_TARGET_NOT_HALTED;
 779         }
 780
 781         /* skip over flash bank */
 782         CMD_ARGC--;
 783         CMD_ARGV++;
 784
 785         uint32_t port;
 786         if (strcmp("gpioa", CMD_ARGV[0]) == 0)
 787                 port = 8;
 788         else if (strcmp("gpiob", CMD_ARGV[0]) == 0)
 789                 port = 9;
 790         else if (strcmp("gpioc", CMD_ARGV[0]) == 0)
 791                 port = 10;
 792         else if (strcmp("gpiod", CMD_ARGV[0]) == 0)
 793                 port = 11;
 794         else if (strcmp("gpioe", CMD_ARGV[0]) == 0)
 795                 port = 12;
 796         else if (strcmp("gpiof", CMD_ARGV[0]) == 0)
 797                 port = 13;
 798         else if (strcmp("gpiog", CMD_ARGV[0]) == 0)
 799                 port = 14;
 800         else if (strcmp("gpioh", CMD_ARGV[0]) == 0)
 801                 port = 15;
 802         else
 803                 return ERROR_COMMAND_SYNTAX_ERROR;
 804
 805         uint32_t pin;
 806         COMMAND_PARSE_NUMBER(u32, CMD_ARGV[1], pin);
 807         if (pin > 15)
 808                 return ERROR_COMMAND_SYNTAX_ERROR;
 809
 810         uint32_t func;
 811         if (strcmp("func1", CMD_ARGV[2]) == 0)
 812                 func = 0;
 813         else if (strcmp("func3", CMD_ARGV[2]) == 0)
 814                 func = 3;
 815         else
 816                 return ERROR_COMMAND_SYNTAX_ERROR;
 817
 818         command_print(CMD,  "Try to configure external memory boot interface:\n"
 819                                                 "port = %s\n"
 820                                                 "pin  = %s\n"
 821                                                 "func = %s\n"
 822                                                 "Please wait ...", CMD_ARGV[0], CMD_ARGV[1], CMD_ARGV[2]);
 823         /* dump */
 824         uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 825         niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
 826
 827         /* modify dump */
 828         uflash_dump[INFOWORD0_ADDR] &= ~(3<<INFOWORD0_EXTMEM_SEL_POS);
 829         uflash_dump[INFOWORD0_ADDR] |= func<<INFOWORD0_EXTMEM_SEL_POS;
 830         uflash_dump[INFOWORD1_ADDR] = (port<<INFOWORD1_PORTNUM_POS) | (pin<<INFOWORD1_PINNUM_POS);
 831
 832         /* erase page userflash */
 833         niietcm4_uflash_page_erase(bank, 0, 1);
 834
 835         /* write dump to userflash */
 836         niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
 837         command_print(CMD, "done!");
 838
 839         return retval;
 840 }
 841
 842 COMMAND_HANDLER(niietcm4_handle_extmem_boot_command)
 843 {
 844         if (CMD_ARGC < 2)
 845                 return ERROR_COMMAND_SYNTAX_ERROR;
 846
 847         struct flash_bank *bank;
 848         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 849         if (retval != ERROR_OK)
 850                 return retval;
 851         struct target *target = bank->target;
 852
 853         if (target->state != TARGET_HALTED) {
 854                 LOG_ERROR("Target not halted");
 855                 return ERROR_TARGET_NOT_HALTED;
 856         }
 857
 858         /* skip over flash bank */
 859         CMD_ARGC--;
 860         CMD_ARGV++;
 861
 862         int set;
 863
 864         if (strcmp("on", CMD_ARGV[0]) == 0) {
 865                 command_print(CMD, "Try to enable boot from external memory. Please wait ...");
 866                 set = 1;
 867         } else if (strcmp("off", CMD_ARGV[0]) == 0) {
 868                 command_print(CMD, "Try to disable boot from external memory. Please wait ...");
 869                 set = 0;
 870         } else
 871                 return ERROR_COMMAND_SYNTAX_ERROR;
 872
 873         /* dump */
 874         uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
 875         niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
 876
 877         /* modify dump */
 878         if (set)
 879                 uflash_dump[INFOWORD0_ADDR] &= ~INFOWORD0_EN_GPIO;
 880         else
 881                 uflash_dump[INFOWORD0_ADDR] |= INFOWORD0_EN_GPIO;
 882
 883         /* erase page userflash */
 884         niietcm4_uflash_page_erase(bank, 0, 1);
 885
 886         /* write dump to userflash */
 887         niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
 888         command_print(CMD, "done!");
 889
 890         return retval;
 891 }
 892
 893 COMMAND_HANDLER(niietcm4_handle_service_mode_erase_command)
 894 {
 895         if (CMD_ARGC < 1)
 896                 return ERROR_COMMAND_SYNTAX_ERROR;
 897
 898         struct flash_bank *bank;
 899         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 900         if (retval != ERROR_OK)
 901                 return retval;
 902         struct target *target = bank->target;
 903
 904         command_print(CMD, "Try to perform service mode erase. Please wait ...");
 905
 906         retval = target_write_u32(target, SERVICE_MODE_ERASE_ADDR, 1);
 907         if (retval != ERROR_OK)
 908                 return retval;
 909
 910         int timeout = 500;
 911         uint32_t status;
 912
 913         retval = target_read_u32(target, SERVICE_MODE_ERASE_ADDR, &status);
 914         if (retval != ERROR_OK)
 915                 return retval;
 916
 917         while (status != 0x03) {
 918                 retval = target_read_u32(target, SERVICE_MODE_ERASE_ADDR, &status);
 919                 if (retval != ERROR_OK)
 920                         return retval;
 921                 if (timeout-- <= 0) {
 922                         LOG_ERROR("Service mode erase timeout");
 923                         return ERROR_FLASH_OPERATION_FAILED;
 924                         }
 925                 busy_sleep(1);  /* can use busy sleep for short times. */
 926         }
 927         command_print(CMD, "done! All data erased.");
 928
 929         return retval;
 930 }
 931
 932 COMMAND_HANDLER(niietcm4_handle_driver_info_command)
 933 {
 934         if (CMD_ARGC < 1)
 935                 return ERROR_COMMAND_SYNTAX_ERROR;
 936
 937         struct flash_bank *bank;
 938         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 939         if (retval != ERROR_OK)
 940                 return retval;
 941
 942         command_print(CMD, "niietcm4 flash driver\n"
 943                                                    "version: %d.%d\n"
 944                                                    "author: Bogdan Kolbov\n"
 945                                                    "mail: kolbov@niiet.ru",
 946                                                    FLASH_DRIVER_VER>>16,
 947                                                    FLASH_DRIVER_VER&0xFFFF);
 948
 949         return retval;
 950 }
 951
 952 static const struct command_registration niietcm4_exec_command_handlers[] = {
 953         {
 954                 .name = "uflash_read_byte",
 955                 .handler = niietcm4_handle_uflash_read_byte_command,
 956                 .mode = COMMAND_EXEC,
 957                 .usage = "bank_id ('main'|'info') address",
 958                 .help = "Read byte from main or info userflash region",
 959         },
 960         {
 961                 .name = "uflash_write_byte",
 962                 .handler = niietcm4_handle_uflash_write_byte_command,
 963                 .mode = COMMAND_EXEC,
 964                 .usage = "bank_id ('main'|'info') address value",
 965                 .help = "Write byte to main or info userflash region",
 966         },
 967         {
 968                 .name = "uflash_full_erase",
 969                 .handler = niietcm4_handle_uflash_full_erase_command,
 970                 .mode = COMMAND_EXEC,
 971                 .usage = "bank_id",
 972                 .help = "Erase all userflash including info region",
 973         },
 974         {
 975                 .name = "uflash_erase",
 976                 .handler = niietcm4_handle_uflash_erase_command,
 977                 .mode = COMMAND_EXEC,
 978                 .usage = "bank_id ('main'|'info') first_sector_num last_sector_num",
 979                 .help = "Erase sectors of main or info userflash region, starting at sector first up to and including last.",
 980         },
 981         {
 982                 .name = "uflash_protect_check",
 983                 .handler = niietcm4_handle_uflash_protect_check_command,
 984                 .mode = COMMAND_EXEC,
 985                 .usage = "bank_id ('main'|'info')",
 986                 .help = "Check sectors protect.",
 987         },
 988         {
 989                 .name = "uflash_protect",
 990                 .handler = niietcm4_handle_uflash_protect_command,
 991                 .mode = COMMAND_EXEC,
 992                 .usage = "bank_id ('main'|'info') first_sector_num last_sector_num ('on'|'off')",
 993                 .help = "Protect sectors of main or info userflash region, starting at sector first up to and including last.",
 994         },
 995         {
 996                 .name = "bflash_info_remap",
 997                 .handler = niietcm4_handle_bflash_info_remap_command,
 998                 .mode = COMMAND_EXEC,
 999                 .usage = "bank_id ('on'|'off')",
1000                 .help = "Enable remapping bootflash info region to 0x00000000 (or 0x40000000 if external memory boot used).",
1001         },
1002         {
1003                 .name = "extmem_cfg",
1004                 .handler = niietcm4_handle_extmem_cfg_command,
1005                 .mode = COMMAND_EXEC,
1006                 .usage = "bank_id ('gpioa'|'gpiob'|'gpioc'|'gpiod'|'gpioe'|'gpiof'|'gpiog'|'gpioh') pin_num ('func1'|'func3')",
1007                 .help = "Configure external memory interface for boot.",
1008         },
1009         {
1010                 .name = "extmem_boot",
1011                 .handler = niietcm4_handle_extmem_boot_command,
1012                 .mode = COMMAND_EXEC,
1013                 .usage = "bank_id ('on'|'off')",
1014                 .help = "Enable boot from external memory.",
1015         },
1016         {
1017                 .name = "service_mode_erase",
1018                 .handler = niietcm4_handle_service_mode_erase_command,
1019                 .mode = COMMAND_EXEC,
1020                 .usage = "bank_id",
1021                 .help = "Perform emergency erase of all flash (bootflash and userflash).",
1022         },
1023         {
1024                 .name = "driver_info",
1025                 .handler = niietcm4_handle_driver_info_command,
1026                 .mode = COMMAND_EXEC,
1027                 .usage = "bank_id",
1028                 .help = "Show information about flash driver.",
1029         },
1030         COMMAND_REGISTRATION_DONE
1031 };
1032
1033 static const struct command_registration niietcm4_command_handlers[] = {
1034         {
1035                 .name = "niietcm4",
1036                 .mode = COMMAND_ANY,
1037                 .help = "niietcm4 flash command group",
1038                 .usage = "",
1039                 .chain = niietcm4_exec_command_handlers,
1040         },
1041         COMMAND_REGISTRATION_DONE
1042 };
1043
1044 /*==============================================================================
1045  *                                                      FLASH INTERFACE
1046  *==============================================================================
1047  */
1048
1049 FLASH_BANK_COMMAND_HANDLER(niietcm4_flash_bank_command)
1050 {
1051         struct niietcm4_flash_bank *niietcm4_info;
1052
1053         if (CMD_ARGC < 6)
1054                 return ERROR_COMMAND_SYNTAX_ERROR;
1055
1056         niietcm4_info = malloc(sizeof(struct niietcm4_flash_bank));
1057
1058         bank->driver_priv = niietcm4_info;
1059
1060         /* information will be updated by probing */
1061         niietcm4_info->probed = false;
1062         niietcm4_info->chipid = 0;
1063         niietcm4_info->chip_name = NULL;
1064         niietcm4_info->uflash_width = 0;
1065         niietcm4_info->uflash_size = 0;
1066         niietcm4_info->uflash_pagetotal = 0;
1067         niietcm4_info->uflash_info_size = 0;
1068         niietcm4_info->uflash_info_pagetotal = 0;
1069         niietcm4_info->bflash_info_remap = false;
1070         niietcm4_info->extmem_boot_port = NULL;
1071         niietcm4_info->extmem_boot_pin = 0;
1072         niietcm4_info->extmem_boot_altfunc = 0;
1073         niietcm4_info->extmem_boot = false;
1074
1075         return ERROR_OK;
1076 }
1077
1078 static int niietcm4_protect_check(struct flash_bank *bank)
1079 {
1080         struct target *target = bank->target;
1081         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1082
1083         int retval = ERROR_FLASH_OPERATION_FAILED;
1084         int set;
1085         uint32_t uflash_addr;
1086         uint32_t uflash_cmd;
1087         uint32_t uflash_data;
1088         /* chose between main bootflash and info bootflash  */
1089         if (niietcm4_info->bflash_info_remap) {
1090                 uflash_addr = INFOWORD2_ADDR;
1091                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
1092                 retval = target_write_u32(target, UFMA, uflash_addr);
1093                 if (retval != ERROR_OK)
1094                         return retval;
1095                 retval = target_write_u32(target, UFMC, uflash_cmd);
1096                 if (retval != ERROR_OK)
1097                         return retval;
1098
1099                 /* status check */
1100                 retval = niietcm4_uopstatus_check(bank);
1101                 if (retval != ERROR_OK)
1102                         return retval;
1103                 retval = target_read_u32(target, UFMD, &uflash_data);
1104                 if (retval != ERROR_OK)
1105                         return retval;
1106
1107                 if (uflash_data & INFOWORD2_LOCK_IFB_BF)
1108                         set = 0;
1109                 else
1110                         set = 1;
1111                 bank->sectors[0].is_protected = set;
1112         } else {
1113                 uflash_addr = BF_LOCK_ADDR;
1114                 uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
1115                 for (unsigned int i = 0; i < bank->num_sectors/8; i++) {
1116                         retval = target_write_u32(target, UFMA, uflash_addr);
1117                         if (retval != ERROR_OK)
1118                                 return retval;
1119                         retval = target_write_u32(target, UFMC, uflash_cmd);
1120                         if (retval != ERROR_OK)
1121                                 return retval;
1122
1123                         /* status check */
1124                         retval = niietcm4_uopstatus_check(bank);
1125                         if (retval != ERROR_OK)
1126                                 return retval;
1127                         retval = target_read_u32(target, UFMD, &uflash_data);
1128                         if (retval != ERROR_OK)
1129                                 return retval;
1130
1131                         for (int j = 0; j < 8; j++) {
1132                                 if (uflash_data & 0x1)
1133                                         set = 0;
1134                                 else
1135                                         set = 1;
1136                                 bank->sectors[i*8+j].is_protected = set;
1137                                 uflash_data = uflash_data >> 1;
1138                         }
1139                         uflash_addr++;
1140                 }
1141         }
1142
1143         return retval;
1144 }
1145
1146 static int niietcm4_mass_erase(struct flash_bank *bank)
1147 {
1148         struct target *target = bank->target;
1149
1150         int retval;
1151         uint32_t flash_cmd;
1152
1153         /* start mass erase */
1154         flash_cmd = FMC_MAGIC_KEY | FMC_FULL_ERASE;
1155         retval = target_write_u32(target, FMC, flash_cmd);
1156         if (retval != ERROR_OK)
1157                 return retval;
1158
1159         /* status check */
1160         retval = niietcm4_opstatus_check(bank);
1161         if (retval != ERROR_OK)
1162                 return retval;
1163
1164         return retval;
1165 }
1166
1167 static int niietcm4_erase(struct flash_bank *bank, unsigned int first,
1168                 unsigned int last)
1169 {
1170         struct target *target = bank->target;
1171         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1172         int retval = ERROR_FLASH_OPERATION_FAILED;
1173
1174         if (bank->target->state != TARGET_HALTED) {
1175                 LOG_ERROR("Target not halted");
1176                 return ERROR_TARGET_NOT_HALTED;
1177         }
1178
1179         if ((first == 0) && (last == (bank->num_sectors - 1))) {
1180                 retval = niietcm4_mass_erase(bank);
1181                 return retval;
1182         }
1183
1184         /* chose between main bootflash and info bootflash */
1185         uint32_t flash_cmd, flash_addr;
1186         if (niietcm4_info->bflash_info_remap)
1187                 flash_cmd = FMC_MAGIC_KEY | FMC_PAGEERASE_IFB;
1188         else
1189                 flash_cmd = FMC_MAGIC_KEY | FMC_PAGE_ERASE;
1190
1191         /* erasing pages */
1192         unsigned int page_size = bank->size / bank->num_sectors;
1193         for (unsigned int i = first; i <= last; i++) {
1194                 /* current page addr */
1195                 flash_addr = i*page_size;
1196                 retval = target_write_u32(target, FMA, flash_addr);
1197                 if (retval != ERROR_OK)
1198                         return retval;
1199
1200                 /* start erase */
1201                 retval = target_write_u32(target, FMC, flash_cmd);
1202                 if (retval != ERROR_OK)
1203                         return retval;
1204
1205                 /* status check */
1206                 retval = niietcm4_opstatus_check(bank);
1207                 if (retval != ERROR_OK)
1208                         return retval;
1209
1210                 bank->sectors[i].is_erased = 1;
1211         }
1212
1213         return retval;
1214 }
1215
1216 static int niietcm4_protect(struct flash_bank *bank, int set,
1217                 unsigned int first, unsigned int last)
1218 {
1219         struct target *target = bank->target;
1220         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1221
1222         int retval;
1223
1224         if (target->state != TARGET_HALTED) {
1225                 LOG_ERROR("Target not halted");
1226                 return ERROR_TARGET_NOT_HALTED;
1227         }
1228
1229         LOG_INFO("Please wait ..."); /* it`s quite a long process */
1230         /* chose between main bootflash and info bootflash */
1231         if (niietcm4_info->bflash_info_remap) {
1232                 /* dump */
1233                 uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
1234                 retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
1235                 if (retval != ERROR_OK)
1236                         return retval;
1237                 /* modify dump */
1238                 if (set)
1239                         uflash_dump[INFOWORD2_ADDR] &= ~INFOWORD2_LOCK_IFB_BF;
1240                 else
1241                         uflash_dump[INFOWORD2_ADDR] |= INFOWORD2_LOCK_IFB_BF;
1242                 /* erase page 0 userflash */
1243                 retval = niietcm4_uflash_page_erase(bank, 0, 1);
1244                 if (retval != ERROR_OK)
1245                         return retval;
1246                 /* write dump to userflash */
1247                 retval = niietcm4_load_uflash_page(bank, uflash_dump, 0, 1);
1248                 if (retval != ERROR_OK)
1249                         return retval;
1250         } else {
1251                 /* read dump*/
1252                 uint32_t uflash_dump[USERFLASH_PAGE_SIZE];
1253                 retval = niietcm4_dump_uflash_page(bank, uflash_dump, 0, 1);
1254                 if (retval != ERROR_OK)
1255                         return retval;
1256                 /* modify dump */
1257                 for (unsigned int i = first; i <= last; i++)    {
1258                         uint32_t reg_num = i/8;
1259                         uint32_t bit_num = i%8;
1260                         if (set)
1261                                 uflash_dump[BF_LOCK_ADDR+reg_num] &= ~(1<<bit_num);
1262                         else
1263                                 uflash_dump[BF_LOCK_ADDR+reg_num] |= (1<<bit_num);
1264                 }
1265                 /* erase page 0 info userflash */
1266                 retval = niietcm4_uflash_page_erase(bank, 0, 1);
1267                 if (retval != ERROR_OK)
1268                         return retval;
1269                 /* write dump to userflash */
1270                 retval = niietcm4_load_uflash_page(bank, uflash_dump,  0, 1);
1271                 if (retval != ERROR_OK)
1272                         return retval;
1273         }
1274
1275         return retval;
1276 }
1277
1278 static int niietcm4_write_block(struct flash_bank *bank, const uint8_t *buffer,
1279                 uint32_t offset, uint32_t count)
1280 {
1281         struct target *target = bank->target;
1282         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1283         uint32_t buffer_size = 32768 + 8; /* 8 bytes for rp and wp */
1284         struct working_area *write_algorithm;
1285         struct working_area *source;
1286         uint32_t address = bank->base + offset;
1287         struct reg_param reg_params[5];
1288         struct armv7m_algorithm armv7m_info;
1289         int retval = ERROR_OK;
1290
1291         /* see contrib/loaders/flash/k1921vk01t.S for src */
1292         static const uint8_t niietcm4_flash_write_code[] = {
1293                 0x14, 0x4f, 0x16, 0x68, 0x00, 0x2e, 0x23, 0xd0, 0x55, 0x68, 0xb5, 0x42, 0xf9, 0xd0, 0x2e, 0x68,
1294                 0x7e, 0x60, 0x04, 0x35, 0x2e, 0x68, 0x3e, 0x65, 0x04, 0x35, 0x2e, 0x68, 0x7e, 0x65, 0x04, 0x35,
1295                 0x2e, 0x68, 0xbe, 0x65, 0x04, 0x35, 0x3c, 0x60, 0x10, 0x34, 0xb8, 0x60, 0xfe, 0x68, 0x00, 0x2e,
1296                 0xfc, 0xd0, 0x02, 0x2e, 0x0a, 0xd0, 0x01, 0x26, 0x7e, 0x61, 0x9d, 0x42, 0x01, 0xd3, 0x15, 0x46,
1297                 0x08, 0x35, 0x55, 0x60, 0x01, 0x39, 0x00, 0x29, 0x02, 0xd0, 0xda, 0xe7, 0x00, 0x20, 0x50, 0x60,
1298                 0x30, 0x46, 0x00, 0xbe, 0x00, 0xc0, 0x01, 0xa0
1299         };
1300
1301         /* flash write code */
1302         if (target_alloc_working_area(target, sizeof(niietcm4_flash_write_code),
1303                         &write_algorithm) != ERROR_OK) {
1304                 LOG_WARNING("no working area available, can't do block memory writes");
1305                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1306         }
1307
1308         retval = target_write_buffer(target, write_algorithm->address,
1309                         sizeof(niietcm4_flash_write_code), niietcm4_flash_write_code);
1310         if (retval != ERROR_OK)
1311                 return retval;
1312
1313         /* memory buffer */
1314         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
1315                 buffer_size /= 2;
1316                 buffer_size &= ~15UL; /* Make sure it's 16 byte aligned */
1317                 buffer_size += 8; /* And 8 bytes for WP and RP */
1318                 if (buffer_size <= 256) {
1319                         /* we already allocated the writing code, but failed to get a
1320                          * buffer, free the algorithm */
1321                         target_free_working_area(target, write_algorithm);
1322
1323                         LOG_WARNING("no large enough working area available, can't do block memory writes");
1324                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
1325                 }
1326         }
1327
1328         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* write_cmd base (in), status (out) */
1329         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* count (128bit) */
1330         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);    /* buffer start */
1331         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);    /* buffer end */
1332         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT); /* target address */
1333
1334         uint32_t flash_cmd;
1335         if (niietcm4_info->bflash_info_remap)
1336                 flash_cmd = FMC_MAGIC_KEY | FMC_WRITE_IFB;
1337         else
1338                 flash_cmd = FMC_MAGIC_KEY | FMC_WRITE;
1339
1340         buf_set_u32(reg_params[0].value, 0, 32, flash_cmd);
1341         buf_set_u32(reg_params[1].value, 0, 32, count);
1342         buf_set_u32(reg_params[2].value, 0, 32, source->address);
1343         buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
1344         buf_set_u32(reg_params[4].value, 0, 32, address);
1345
1346         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
1347         armv7m_info.core_mode = ARM_MODE_THREAD;
1348
1349         retval = target_run_flash_async_algorithm(target, buffer, count, 16,
1350                         0, NULL,
1351                         5, reg_params,
1352                         source->address, source->size,
1353                         write_algorithm->address, 0,
1354                         &armv7m_info);
1355
1356         if (retval == ERROR_FLASH_OPERATION_FAILED)
1357                 LOG_ERROR("flash write failed at address 0x%"PRIx32,
1358                                 buf_get_u32(reg_params[4].value, 0, 32));
1359
1360         target_free_working_area(target, source);
1361         target_free_working_area(target, write_algorithm);
1362
1363         destroy_reg_param(&reg_params[0]);
1364         destroy_reg_param(&reg_params[1]);
1365         destroy_reg_param(&reg_params[2]);
1366         destroy_reg_param(&reg_params[3]);
1367         destroy_reg_param(&reg_params[4]);
1368
1369         return retval;
1370 }
1371
1372 static int niietcm4_write(struct flash_bank *bank, const uint8_t *buffer,
1373                 uint32_t offset, uint32_t count)
1374 {
1375         struct target *target = bank->target;
1376         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1377         uint8_t *new_buffer = NULL;
1378
1379         if (bank->target->state != TARGET_HALTED) {
1380                 LOG_ERROR("Target not halted");
1381                 return ERROR_TARGET_NOT_HALTED;
1382         }
1383
1384         if (offset & 0xF) {
1385                 LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-word alignment", offset);
1386                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
1387         }
1388
1389         /* If there's an odd number of words, the data has to be padded. Duplicate
1390          * the buffer and use the normal code path with a single block write since
1391          * it's probably cheaper than to special case the last odd write using
1392          * discrete accesses. */
1393
1394         int rem = count % 16;
1395         if (rem) {
1396                 new_buffer = malloc(count + 16 - rem);
1397                 if (new_buffer == NULL) {
1398                         LOG_ERROR("Odd number of words to write and no memory for padding buffer");
1399                         return ERROR_FAIL;
1400                 }
1401                 LOG_INFO("Odd number of words to write, padding with 0xFFFFFFFF");
1402                 buffer = memcpy(new_buffer, buffer, count);
1403                 while (rem < 16) {
1404                         new_buffer[count++] = 0xff;
1405                         rem++;
1406                 }
1407         }
1408
1409         int retval;
1410
1411         /* try using block write */
1412         retval = niietcm4_write_block(bank, buffer, offset, count/16);
1413         uint32_t flash_addr, flash_cmd, flash_data;
1414
1415         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
1416                 /* if block write failed (no sufficient working area),
1417                  * we use normal (slow) single halfword accesses */
1418                 LOG_WARNING("Can't use block writes, falling back to single memory accesses");
1419                 LOG_INFO("Please wait ..."); /* it`s quite a long process */
1420
1421                 /* chose between main bootflash and info bootflash */
1422                 if (niietcm4_info->bflash_info_remap)
1423                         flash_cmd = FMC_MAGIC_KEY | FMC_WRITE_IFB;
1424                 else
1425                         flash_cmd = FMC_MAGIC_KEY | FMC_WRITE;
1426
1427                 /* write 16 bytes per try */
1428                 for (unsigned int i = 0; i < count; i += 16) {
1429                         /* current addr */
1430                         LOG_INFO("%u byte of %" PRIu32, i, count);
1431                         flash_addr = offset + i;
1432                         retval = target_write_u32(target, FMA, flash_addr);
1433                         if (retval != ERROR_OK)
1434                                 goto free_buffer;
1435
1436                         /* Prepare data (4 words) */
1437                         uint32_t value[4];
1438                         memcpy(&value, buffer + i*16, 4*sizeof(uint32_t));
1439
1440                         /* place in reg 16 bytes of data */
1441                         flash_data = value[0];
1442                         retval = target_write_u32(target, FMD1, flash_data);
1443                         if (retval != ERROR_OK)
1444                                 goto free_buffer;
1445                         flash_data = value[1];
1446                         retval = target_write_u32(target, FMD2, flash_data);
1447                         if (retval != ERROR_OK)
1448                                 goto free_buffer;
1449                         flash_data = value[2];
1450                         retval = target_write_u32(target, FMD3, flash_data);
1451                         if (retval != ERROR_OK)
1452                                 goto free_buffer;
1453                         flash_data = value[3];
1454                         retval = target_write_u32(target, FMD4, flash_data);
1455                         if (retval != ERROR_OK)
1456                                 goto free_buffer;
1457
1458                         /* write start */
1459                         retval = target_write_u32(target, FMC, flash_cmd);
1460                         if (retval != ERROR_OK)
1461                                 goto free_buffer;
1462
1463                         /* status check */
1464                         retval = niietcm4_opstatus_check(bank);
1465                         if (retval != ERROR_OK)
1466                                 goto free_buffer;
1467                 }
1468
1469         }
1470
1471 free_buffer:
1472         free(new_buffer);
1473         return retval;
1474 }
1475
1476 static int niietcm4_probe_k1921vk01t(struct flash_bank *bank)
1477 {
1478         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1479         struct target *target = bank->target;
1480         int retval;
1481
1482         niietcm4_info->chip_name = "K1921VK01T";
1483
1484         /* check if we in service mode */
1485         uint32_t service_mode;
1486         retval = target_read_u32(target, 0x80017000, &service_mode);
1487         if (retval != ERROR_OK)
1488                 return retval;
1489         service_mode = (service_mode>>2) & 0x1;
1490
1491         if (!service_mode) {
1492                 niietcm4_info->uflash_width = 8;
1493                 niietcm4_info->uflash_size = 0x10000;
1494                 niietcm4_info->uflash_pagetotal = 256;
1495                 niietcm4_info->uflash_info_size = 0x200;
1496                 niietcm4_info->uflash_info_pagetotal = 2;
1497
1498                 uint32_t uflash_data[2];
1499                 uint32_t uflash_cmd = UFMC_MAGIC_KEY | UFMC_READ_IFB;
1500                 for (int i = 0; i < 2; i++) {
1501                         retval = target_write_u32(target, UFMA, i);
1502                         if (retval != ERROR_OK)
1503                                 return retval;
1504                         retval = target_write_u32(target, UFMC, uflash_cmd);
1505                         if (retval != ERROR_OK)
1506                                 return retval;
1507                         /* status check */
1508                         retval = niietcm4_uopstatus_check(bank);
1509                         if (retval != ERROR_OK)
1510                                 return retval;
1511                         retval = target_read_u32(target, UFMD, &uflash_data[i]);
1512                         if (retval != ERROR_OK)
1513                                 return retval;
1514                 }
1515
1516                 int boot_from_ifb = (uflash_data[0]>>INFOWORD0_BOOTFROM_IFB_POS) & 0x1;
1517                 int en_gpio = (uflash_data[0]>>INFOWORD0_EN_GPIO_POS) & 0x1;
1518                 int extmem_sel = (uflash_data[0]>>INFOWORD0_EXTMEM_SEL_POS) & 0x3;
1519                 int pinnum = (uflash_data[1]>>INFOWORD1_PINNUM_POS) & 0xF;
1520                 int portnum = (uflash_data[1]>>INFOWORD1_PORTNUM_POS) & 0x7;
1521
1522                 if (boot_from_ifb)
1523                         niietcm4_info->bflash_info_remap = false;
1524                 else
1525                         niietcm4_info->bflash_info_remap = true;
1526                 if (extmem_sel == 0x2)
1527                         niietcm4_info->extmem_boot_altfunc = 3;
1528                 else
1529                         niietcm4_info->extmem_boot_altfunc = 1;
1530                 if (portnum == 0x0)
1531                         niietcm4_info->extmem_boot_port = "GPIOA";
1532                 else if (portnum == 0x1)
1533                         niietcm4_info->extmem_boot_port = "GPIOB";
1534                 else if (portnum == 0x2)
1535                         niietcm4_info->extmem_boot_port = "GPIOC";
1536                 else if (portnum == 0x3)
1537                         niietcm4_info->extmem_boot_port = "GPIOD";
1538                 else if (portnum == 0x4)
1539                         niietcm4_info->extmem_boot_port = "GPIOE";
1540                 else if (portnum == 0x5)
1541                         niietcm4_info->extmem_boot_port = "GPIOF";
1542                 else if (portnum == 0x6)
1543                         niietcm4_info->extmem_boot_port = "GPIOG";
1544                 else if (portnum == 0x7)
1545                         niietcm4_info->extmem_boot_port = "GPIOH";
1546                 else
1547                         niietcm4_info->extmem_boot_port = "not defined";
1548                 if (en_gpio)
1549                         niietcm4_info->extmem_boot = false;
1550                 else
1551                         niietcm4_info->extmem_boot = true;
1552                 niietcm4_info->extmem_boot_pin = pinnum;
1553
1554                 /* check state of extmem boot en pin, if "high", extmem remapped to 0x00000000 */
1555                 uint32_t extmem_boot_port_data;
1556                 retval = target_read_u32(target, 0x80010000 + 0x1000*portnum, &extmem_boot_port_data);
1557                 if (retval != ERROR_OK)
1558                         return retval;
1559                 int extmem_boot_pin_data = (extmem_boot_port_data>>pinnum) & 0x1;
1560
1561                 uint32_t extmem_base;
1562                 uint32_t bflash_base;
1563                 if (extmem_boot_pin_data && niietcm4_info->extmem_boot) {
1564                         extmem_base = 0x00000000;
1565                         bflash_base = 0x40000000;
1566                 } else {
1567                         extmem_base = 0x40000000;
1568                         bflash_base = 0x00000000;
1569                 }
1570
1571                 uint32_t bflash_size = 0x100000;
1572                 uint32_t bflash_pages = 128;
1573                 uint32_t bflash_info_size = 0x2000;
1574                 uint32_t bflash_info_pages = 1;
1575                 if (niietcm4_info->bflash_info_remap) {
1576                         bflash_base += 0x2000;
1577                         bflash_size -= 0x2000;
1578                         bflash_pages--;
1579                         bank->size = bflash_info_size;
1580                         bank->num_sectors = bflash_info_pages;
1581                 } else {
1582                         bank->size = bflash_size;
1583                         bank->num_sectors = bflash_pages;
1584                 }
1585
1586                 char info_bootflash_addr_str[64];
1587                 if (niietcm4_info->bflash_info_remap)
1588                         snprintf(info_bootflash_addr_str, sizeof(info_bootflash_addr_str),
1589                                         TARGET_ADDR_FMT " base address", bank->base);
1590                 else
1591                         snprintf(info_bootflash_addr_str, sizeof(info_bootflash_addr_str),
1592                                         "not mapped to global address space");
1593
1594                 snprintf(niietcm4_info->chip_brief,
1595                                 sizeof(niietcm4_info->chip_brief),
1596                                 "\n"
1597                                 "MEMORY CONFIGURATION\n"
1598                                 "Bootflash :\n"
1599                                 "    %" PRIu32 " kB total\n"
1600                                 "    %" PRIu32 " pages %" PRIu32 " kB each\n"
1601                                 "    0x%08" PRIx32 " base address\n"
1602                                 "%s"
1603                                 "Info bootflash :\n"
1604                                 "    %" PRIu32 " kB total\n"
1605                                 "    %" PRIu32 " pages %" PRIu32 " kB each\n"
1606                                 "    %s\n"
1607                                 "%s"
1608                                 "Userflash :\n"
1609                                 "    %" PRIu32 " kB total\n"
1610                                 "    %" PRIu32 " pages %" PRIu32 " B each\n"
1611                                 "    %" PRIu32 " bit cells\n"
1612                                 "    not mapped to global address space\n"
1613                                 "Info userflash :\n"
1614                                 "    %" PRIu32 " B total\n"
1615                                 "    %" PRIu32 " pages of %" PRIu32 " B each\n"
1616                                 "    %" PRIu32 " bit cells\n"
1617                                 "    not mapped to global address space\n"
1618                                 "RAM :\n"
1619                                 "    192 kB total\n"
1620                                 "    0x20000000 base address\n"
1621                                 "External memory :\n"
1622                                 "    8/16 bit address space\n"
1623                                 "    0x%08" PRIx32 " base address\n"
1624                                 "\n"
1625                                 "INFOWORD STATUS\n"
1626                                 "Bootflash info region remap :\n"
1627                                 "    %s\n"
1628                                 "External memory boot port :\n"
1629                                 "    %s\n"
1630                                 "External memory boot pin :\n"
1631                                 "    %" PRIu32 "\n"
1632                                 "External memory interface alternative function :\n"
1633                                 "    %" PRIu32 "\n"
1634                                 "Option boot from external memory :\n"
1635                                 "    %s\n",
1636                                 bflash_size/1024,
1637                                 bflash_pages,
1638                                 (bflash_size/bflash_pages)/1024,
1639                                 bflash_base,
1640                                 niietcm4_info->bflash_info_remap ? "" : "    this flash will be used for debugging, writing and etc\n",
1641                                 bflash_info_size/1024,
1642                                 bflash_info_pages,
1643                                 (bflash_info_size/bflash_info_pages)/1024,
1644                                 info_bootflash_addr_str,
1645                                 niietcm4_info->bflash_info_remap ? "    this flash will be used for debugging, writing and etc\n" : "",
1646                                 niietcm4_info->uflash_size/1024,
1647                                 niietcm4_info->uflash_pagetotal,
1648                                 niietcm4_info->uflash_size/niietcm4_info->uflash_pagetotal,
1649                                 niietcm4_info->uflash_width,
1650                                 niietcm4_info->uflash_info_size,
1651                                 niietcm4_info->uflash_info_pagetotal,
1652                                 niietcm4_info->uflash_info_size/niietcm4_info->uflash_info_pagetotal,
1653                                 niietcm4_info->uflash_width,
1654                                 extmem_base,
1655                                 niietcm4_info->bflash_info_remap ? "enable" : "disable",
1656                                 niietcm4_info->extmem_boot_port,
1657                                 niietcm4_info->extmem_boot_pin,
1658                                 niietcm4_info->extmem_boot_altfunc,
1659                                 niietcm4_info->extmem_boot ? "enable" : "disable");
1660         } else {
1661                 bank->size = 0x100000;
1662                 bank->num_sectors = 128;
1663
1664                 sprintf(niietcm4_info->chip_brief,
1665                                 "\n"
1666                                 "H[2] was HIGH while startup. Device entered service mode.\n"
1667                                 "All flashes were locked.\n"
1668                                 "If you want to perform emergency erase (erase all flashes),\n"
1669                                 "please use \"service_mode_erase\" command and reset device.\n"
1670                                 "Do not forget to pull H[2] down while reset for returning to normal operation mode.\n"
1671                                 );
1672         }
1673
1674         return retval;
1675 }
1676
1677 static int niietcm4_probe(struct flash_bank *bank)
1678 {
1679         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1680         struct target *target = bank->target;
1681
1682         free(bank->sectors);
1683         bank->sectors = NULL;
1684
1685         uint32_t retval;
1686         uint32_t chipid;
1687
1688         retval = target_read_u32(target, CHIPID_ADDR, &chipid);
1689         if (retval != ERROR_OK) {
1690                 chipid = K1921VK01T_ID;
1691                 LOG_INFO("unknown chipid, assuming K1921VK01T");
1692         }
1693
1694         if (chipid == K1921VK01T_ID)
1695                 niietcm4_probe_k1921vk01t(bank);
1696
1697         int page_total = bank->num_sectors;
1698         int page_size = bank->size / page_total;
1699
1700         bank->sectors = malloc(sizeof(struct flash_sector) * page_total);
1701
1702         for (int i = 0; i < page_total; i++) {
1703                 bank->sectors[i].offset = i * page_size;
1704                 bank->sectors[i].size = page_size;
1705                 bank->sectors[i].is_erased = -1;
1706                 bank->sectors[i].is_protected = -1;
1707         }
1708
1709         niietcm4_info->probed = true;
1710
1711         return ERROR_OK;
1712 }
1713
1714 static int niietcm4_auto_probe(struct flash_bank *bank)
1715 {
1716         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1717         if (niietcm4_info->probed)
1718                 return ERROR_OK;
1719         return niietcm4_probe(bank);
1720 }
1721
1722 static int get_niietcm4_info(struct flash_bank *bank, struct command_invocation *cmd)
1723 {
1724         struct niietcm4_flash_bank *niietcm4_info = bank->driver_priv;
1725         command_print_sameline(cmd, "\nNIIET Cortex-M4F %s\n%s",
1726                         niietcm4_info->chip_name, niietcm4_info->chip_brief);
1727         return ERROR_OK;
1728 }
1729
1730
1731 const struct flash_driver niietcm4_flash = {
1732         .name = "niietcm4",
1733         .usage = "flash bank <name> niietcm4 <base> <size> 0 0 <target#>",
1734         .commands = niietcm4_command_handlers,
1735         .flash_bank_command = niietcm4_flash_bank_command,
1736         .erase = niietcm4_erase,
1737         .protect = niietcm4_protect,
1738         .write = niietcm4_write,
1739         .read = default_flash_read,
1740         .probe = niietcm4_probe,
1741         .auto_probe = niietcm4_auto_probe,
1742         .erase_check = default_flash_blank_check,
1743         .protect_check = niietcm4_protect_check,
1744         .info = get_niietcm4_info,
1745         .free_driver_priv = default_flash_free_driver_priv,
1746 };