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