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