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

   1 /* SPDX-License-Identifier: GPL-2.0-or-later */
   2
   3 /***************************************************************************
   4  *   Copyright (C) 2005 by Dominic Rath                                    *
   5  *   Dominic.Rath@gmx.de                                                   *
   6  *                                                                         *
   7  *   Copyright (C) 2008 by Spencer Oliver                                  *
   8  *   spen@spen-soft.co.uk                                                  *
   9  *                                                                         *
  10  *   Copyright (C) 2011 by Andreas Fritiofson                              *
  11  *   andreas.fritiofson@gmail.com                                          *
  12  *                                                                         *
  13  *   Copyright (C) 2013 by Paul Fertser                                    *
  14  *   fercerpav@gmail.com                                                   *
  15  ***************************************************************************/
  16
  17 #ifdef HAVE_CONFIG_H
  18 #include "config.h"
  19 #endif
  20
  21 #include "imp.h"
  22 #include <helper/binarybuffer.h>
  23 #include <target/algorithm.h>
  24 #include <target/armv7m.h>
  25
  26 #define MD_RST_CLK              0x40020000
  27 #define MD_PER_CLOCK            (MD_RST_CLK + 0x1C)
  28 #define MD_PER_CLOCK_EEPROM     (1 << 3)
  29 #define MD_PER_CLOCK_RST_CLK    (1 << 4)
  30
  31 #define FLASH_REG_BASE  0x40018000
  32 #define FLASH_CMD       (FLASH_REG_BASE + 0x00)
  33 #define FLASH_ADR       (FLASH_REG_BASE + 0x04)
  34 #define FLASH_DI        (FLASH_REG_BASE + 0x08)
  35 #define FLASH_DO        (FLASH_REG_BASE + 0x0C)
  36 #define FLASH_KEY       (FLASH_REG_BASE + 0x10)
  37
  38 #define FLASH_NVSTR     (1 << 13)
  39 #define FLASH_PROG      (1 << 12)
  40 #define FLASH_MAS1      (1 << 11)
  41 #define FLASH_ERASE     (1 << 10)
  42 #define FLASH_IFREN     (1 << 9)
  43 #define FLASH_SE        (1 << 8)
  44 #define FLASH_YE        (1 << 7)
  45 #define FLASH_XE        (1 << 6)
  46 #define FLASH_RD        (1 << 2)
  47 #define FLASH_WR        (1 << 1)
  48 #define FLASH_CON       (1 << 0)
  49 #define FLASH_DELAY_MASK        (7 << 3)
  50
  51 #define KEY             0x8AAA5551
  52
  53 struct mdr_flash_bank {
  54         bool probed;
  55         unsigned int mem_type;
  56         unsigned int page_count;
  57         unsigned int sec_count;
  58 };
  59
  60 /* flash bank <name> mdr <base> <size> 0 0 <target#> <type> <page_count> <sec_count> */
  61 FLASH_BANK_COMMAND_HANDLER(mdr_flash_bank_command)
  62 {
  63         struct mdr_flash_bank *mdr_info;
  64
  65         if (CMD_ARGC < 9)
  66                 return ERROR_COMMAND_SYNTAX_ERROR;
  67
  68         mdr_info = malloc(sizeof(struct mdr_flash_bank));
  69
  70         bank->driver_priv = mdr_info;
  71         mdr_info->probed = false;
  72         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[6], mdr_info->mem_type);
  73         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[7], mdr_info->page_count);
  74         COMMAND_PARSE_NUMBER(uint, CMD_ARGV[8], mdr_info->sec_count);
  75         return ERROR_OK;
  76 }
  77
  78 static int mdr_mass_erase(struct flash_bank *bank)
  79 {
  80         struct target *target = bank->target;
  81         struct mdr_flash_bank *mdr_info = bank->driver_priv;
  82         uint32_t flash_cmd;
  83         int retval;
  84         unsigned int i;
  85
  86         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
  87         if (retval != ERROR_OK)
  88                 return retval;
  89
  90         for (i = 0; i < mdr_info->sec_count; i++) {
  91                 retval = target_write_u32(target, FLASH_ADR, i << 2);
  92                 if (retval != ERROR_OK)
  93                         return retval;
  94
  95                 flash_cmd |= FLASH_XE | FLASH_MAS1 | FLASH_ERASE;
  96                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
  97                 if (retval != ERROR_OK)
  98                         return retval;
  99                 flash_cmd |= FLASH_NVSTR;
 100                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 101                 if (retval != ERROR_OK)
 102                         return retval;
 103                 flash_cmd &= ~FLASH_ERASE;
 104                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 105                 if (retval != ERROR_OK)
 106                         return retval;
 107                 flash_cmd &= ~(FLASH_XE | FLASH_MAS1 | FLASH_NVSTR);
 108                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 109                 if (retval != ERROR_OK)
 110                         return retval;
 111         }
 112
 113         return retval;
 114 }
 115
 116 static int mdr_erase(struct flash_bank *bank, unsigned int first,
 117                 unsigned int last)
 118 {
 119         struct target *target = bank->target;
 120         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 121         int retval, retval2;
 122         unsigned int j;
 123         uint32_t flash_cmd, cur_per_clock;
 124
 125         if (bank->target->state != TARGET_HALTED) {
 126                 LOG_ERROR("Target not halted");
 127                 return ERROR_TARGET_NOT_HALTED;
 128         }
 129
 130         retval = target_read_u32(target, MD_PER_CLOCK, &cur_per_clock);
 131         if (retval != ERROR_OK)
 132                 return retval;
 133
 134         if (!(cur_per_clock & 0x10)) {
 135                 LOG_ERROR("Target needs reset before flash operations");
 136                 return ERROR_FLASH_OPERATION_FAILED;
 137         }
 138
 139         retval = target_write_u32(target, MD_PER_CLOCK, cur_per_clock | MD_PER_CLOCK_EEPROM);
 140         if (retval != ERROR_OK)
 141                 return retval;
 142
 143         retval = target_write_u32(target, FLASH_KEY, KEY);
 144         if (retval != ERROR_OK)
 145                 return retval;
 146
 147         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
 148         if (retval != ERROR_OK)
 149                 goto reset_pg_and_lock;
 150
 151         /* Switch on register access */
 152         flash_cmd = (flash_cmd & FLASH_DELAY_MASK) | FLASH_CON;
 153         if (mdr_info->mem_type)
 154                 flash_cmd |= FLASH_IFREN;
 155         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 156         if (retval != ERROR_OK)
 157                 goto reset_pg_and_lock;
 158
 159         if ((first == 0) && (last == (bank->num_sectors - 1)) &&
 160                 !mdr_info->mem_type) {
 161                 retval = mdr_mass_erase(bank);
 162                 goto reset_pg_and_lock;
 163         }
 164
 165         unsigned int page_size = bank->size / mdr_info->page_count;
 166         for (unsigned int i = first; i <= last; i++) {
 167                 for (j = 0; j < mdr_info->sec_count; j++) {
 168                         retval = target_write_u32(target, FLASH_ADR, (i * page_size) | (j << 2));
 169                         if (retval != ERROR_OK)
 170                                 goto reset_pg_and_lock;
 171
 172                         flash_cmd |= FLASH_XE | FLASH_ERASE;
 173                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 174                         if (retval != ERROR_OK)
 175                                 goto reset_pg_and_lock;
 176                         flash_cmd |= FLASH_NVSTR;
 177                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 178                         if (retval != ERROR_OK)
 179                                 goto reset_pg_and_lock;
 180                         flash_cmd &= ~FLASH_ERASE;
 181                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 182                         if (retval != ERROR_OK)
 183                                 goto reset_pg_and_lock;
 184                         flash_cmd &= ~(FLASH_XE | FLASH_NVSTR);
 185                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 186                         if (retval != ERROR_OK)
 187                                 goto reset_pg_and_lock;
 188                 }
 189         }
 190
 191 reset_pg_and_lock:
 192         flash_cmd &= FLASH_DELAY_MASK;
 193         retval2 = target_write_u32(target, FLASH_CMD, flash_cmd);
 194         if (retval == ERROR_OK)
 195                 retval = retval2;
 196
 197         retval2 = target_write_u32(target, FLASH_KEY, 0);
 198         if (retval == ERROR_OK)
 199                 retval = retval2;
 200
 201         return retval;
 202 }
 203
 204 static int mdr_write_block(struct flash_bank *bank, const uint8_t *buffer,
 205                 uint32_t offset, uint32_t count)
 206 {
 207         struct target *target = bank->target;
 208         uint32_t buffer_size = 16384;
 209         struct working_area *write_algorithm;
 210         struct working_area *source;
 211         uint32_t address = bank->base + offset;
 212         struct reg_param reg_params[5];
 213         struct armv7m_algorithm armv7m_info;
 214         int retval = ERROR_OK;
 215
 216         /* see contrib/loaders/flash/mdr32fx.S for src */
 217         static const uint8_t mdr32fx_flash_write_code[] = {
 218                 0x07, 0x68, 0x16, 0x68, 0x00, 0x2e, 0x2e, 0xd0, 0x55, 0x68, 0xb5, 0x42,
 219                 0xf9, 0xd0, 0x2e, 0x68, 0x44, 0x60, 0x86, 0x60, 0x17, 0x4e, 0x37, 0x43,
 220                 0x07, 0x60, 0x05, 0x26, 0x00, 0xf0, 0x25, 0xf8, 0x15, 0x4e, 0x37, 0x43,
 221                 0x07, 0x60, 0x0d, 0x26, 0x00, 0xf0, 0x1f, 0xf8, 0x80, 0x26, 0x37, 0x43,
 222                 0x07, 0x60, 0x3d, 0x26, 0x00, 0xf0, 0x19, 0xf8, 0x80, 0x26, 0xb7, 0x43,
 223                 0x07, 0x60, 0x0f, 0x4e, 0xb7, 0x43, 0x07, 0x60, 0x05, 0x26, 0x00, 0xf0,
 224                 0x10, 0xf8, 0x0d, 0x4e, 0xb7, 0x43, 0x07, 0x60, 0x04, 0x35, 0x04, 0x34,
 225                 0x9d, 0x42, 0x01, 0xd3, 0x15, 0x46, 0x08, 0x35, 0x55, 0x60, 0x01, 0x39,
 226                 0x00, 0x29, 0x00, 0xd0, 0xcd, 0xe7, 0x30, 0x46, 0x00, 0xbe, 0x01, 0x3e,
 227                 0x00, 0x2e, 0xfc, 0xd1, 0x70, 0x47, 0x00, 0x00, 0x40, 0x10, 0x00, 0x00,
 228                 0x00, 0x20, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x40, 0x20, 0x00, 0x00
 229         };
 230
 231         /* flash write code */
 232         if (target_alloc_working_area(target, sizeof(mdr32fx_flash_write_code),
 233                         &write_algorithm) != ERROR_OK) {
 234                 LOG_WARNING("no working area available, can't do block memory writes");
 235                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 236         }
 237
 238         retval = target_write_buffer(target, write_algorithm->address,
 239                         sizeof(mdr32fx_flash_write_code), mdr32fx_flash_write_code);
 240         if (retval != ERROR_OK)
 241                 return retval;
 242
 243         /* memory buffer */
 244         while (target_alloc_working_area_try(target, buffer_size, &source) != ERROR_OK) {
 245                 buffer_size /= 2;
 246                 buffer_size &= ~3UL; /* Make sure it's 4 byte aligned */
 247                 if (buffer_size <= 256) {
 248                         /* we already allocated the writing code, but failed to get a
 249                          * buffer, free the algorithm */
 250                         target_free_working_area(target, write_algorithm);
 251
 252                         LOG_WARNING("no large enough working area available, can't do block memory writes");
 253                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 254                 }
 255         }
 256
 257         init_reg_param(&reg_params[0], "r0", 32, PARAM_IN_OUT); /* flash base (in), status (out) */
 258         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT);    /* count (32bit) */
 259         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT);    /* buffer start */
 260         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT);    /* buffer end */
 261         init_reg_param(&reg_params[4], "r4", 32, PARAM_IN_OUT); /* target address */
 262
 263         buf_set_u32(reg_params[0].value, 0, 32, FLASH_REG_BASE);
 264         buf_set_u32(reg_params[1].value, 0, 32, count);
 265         buf_set_u32(reg_params[2].value, 0, 32, source->address);
 266         buf_set_u32(reg_params[3].value, 0, 32, source->address + source->size);
 267         buf_set_u32(reg_params[4].value, 0, 32, address);
 268
 269         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 270         armv7m_info.core_mode = ARM_MODE_THREAD;
 271
 272         retval = target_run_flash_async_algorithm(target, buffer, count, 4,
 273                         0, NULL,
 274                         5, reg_params,
 275                         source->address, source->size,
 276                         write_algorithm->address, 0,
 277                         &armv7m_info);
 278
 279         if (retval == ERROR_FLASH_OPERATION_FAILED)
 280                 LOG_ERROR("flash write failed at address 0x%"PRIx32,
 281                                 buf_get_u32(reg_params[4].value, 0, 32));
 282
 283         target_free_working_area(target, source);
 284         target_free_working_area(target, write_algorithm);
 285
 286         destroy_reg_param(&reg_params[0]);
 287         destroy_reg_param(&reg_params[1]);
 288         destroy_reg_param(&reg_params[2]);
 289         destroy_reg_param(&reg_params[3]);
 290         destroy_reg_param(&reg_params[4]);
 291
 292         return retval;
 293 }
 294
 295 static int mdr_write(struct flash_bank *bank, const uint8_t *buffer,
 296                 uint32_t offset, uint32_t count)
 297 {
 298         struct target *target = bank->target;
 299         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 300         uint8_t *new_buffer = NULL;
 301
 302         if (bank->target->state != TARGET_HALTED) {
 303                 LOG_ERROR("Target not halted");
 304                 return ERROR_TARGET_NOT_HALTED;
 305         }
 306
 307         if (offset & 0x3) {
 308                 LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte alignment", offset);
 309                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 310         }
 311
 312         /* If there's an odd number of bytes, the data has to be padded. Duplicate
 313          * the buffer and use the normal code path with a single block write since
 314          * it's probably cheaper than to special case the last odd write using
 315          * discrete accesses. */
 316         int rem = count % 4;
 317         if (rem) {
 318                 new_buffer = malloc(count + rem);
 319                 if (!new_buffer) {
 320                         LOG_ERROR("odd number of bytes to write and no memory for padding buffer");
 321                         return ERROR_FAIL;
 322                 }
 323                 LOG_INFO("odd number of bytes to write, padding with 0xff");
 324                 buffer = memcpy(new_buffer, buffer, count);
 325                 while (rem--)
 326                         new_buffer[count++] = 0xff;
 327         }
 328
 329         uint32_t flash_cmd, cur_per_clock;
 330         int retval, retval2;
 331
 332         retval = target_read_u32(target, MD_PER_CLOCK, &cur_per_clock);
 333         if (retval != ERROR_OK)
 334                 goto free_buffer;
 335
 336         if (!(cur_per_clock & MD_PER_CLOCK_RST_CLK)) {
 337                 /* Something's very wrong if the RST_CLK module is not clocked */
 338                 LOG_ERROR("Target needs reset before flash operations");
 339                 retval = ERROR_FLASH_OPERATION_FAILED;
 340                 goto free_buffer;
 341         }
 342
 343         retval = target_write_u32(target, MD_PER_CLOCK, cur_per_clock | MD_PER_CLOCK_EEPROM);
 344         if (retval != ERROR_OK)
 345                 goto free_buffer;
 346
 347         retval = target_write_u32(target, FLASH_KEY, KEY);
 348         if (retval != ERROR_OK)
 349                 goto free_buffer;
 350
 351         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
 352         if (retval != ERROR_OK)
 353                 goto reset_pg_and_lock;
 354
 355         /* Switch on register access */
 356         flash_cmd = (flash_cmd & FLASH_DELAY_MASK) | FLASH_CON;
 357         if (mdr_info->mem_type)
 358                 flash_cmd |= FLASH_IFREN;
 359         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 360         if (retval != ERROR_OK)
 361                 goto reset_pg_and_lock;
 362
 363         /* try using block write */
 364         retval = mdr_write_block(bank, buffer, offset, count/4);
 365
 366         if (retval == ERROR_TARGET_RESOURCE_NOT_AVAILABLE) {
 367                 /* if block write failed (no sufficient working area),
 368                  * we use normal (slow) single halfword accesses */
 369                 LOG_WARNING("Can't use block writes, falling back to single memory accesses");
 370
 371                 unsigned int page_size = bank->size / mdr_info->page_count;
 372                 unsigned int page_mask = page_size - 1;
 373                 while (count > 0) {
 374                         unsigned int i, j;
 375                         unsigned int cur_page = offset & ~page_mask;
 376                         unsigned int bytes_to_write = cur_page + page_size - offset;
 377                         if (count < bytes_to_write)
 378                                 bytes_to_write = count;
 379
 380                         /*LOG_INFO("Selecting next page: %08x", cur_page);*/
 381
 382                         for (i = 0; i < mdr_info->sec_count; i++) {
 383                                 retval = target_write_u32(target, FLASH_ADR, offset + i*4);
 384                                 if (retval != ERROR_OK)
 385                                         goto reset_pg_and_lock;
 386                                 /*LOG_INFO("Selecting page/sector: %08x", offset + i*4);*/
 387
 388                                 flash_cmd |= FLASH_XE | FLASH_PROG;
 389                                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 390                                 if (retval != ERROR_OK)
 391                                         goto reset_pg_and_lock;
 392
 393                                 flash_cmd |= FLASH_NVSTR;
 394                                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 395                                 if (retval != ERROR_OK)
 396                                         goto reset_pg_and_lock;
 397
 398                                 for (j = 0;
 399                                      (((offset + j + i*4) & ~page_mask) == cur_page) &&
 400                                              (j + i*4 < count);
 401                                      j += mdr_info->sec_count*4) {
 402                                         uint32_t value;
 403                                         memcpy(&value, buffer + j + i*4, sizeof(uint32_t));
 404                                         retval = target_write_u32(target, FLASH_DI, value);
 405                                         if (retval != ERROR_OK)
 406                                                 goto reset_pg_and_lock;
 407                                         /*LOG_INFO("Writing to addr %08x", offset + j + i*4);*/
 408                                         retval = target_write_u32(target, FLASH_ADR, offset + j + i*4);
 409                                         if (retval != ERROR_OK)
 410                                                 goto reset_pg_and_lock;
 411
 412                                         flash_cmd |= FLASH_YE;
 413                                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 414                                         if (retval != ERROR_OK)
 415                                                 goto reset_pg_and_lock;
 416                                         flash_cmd &= ~FLASH_YE;
 417                                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 418                                         if (retval != ERROR_OK)
 419                                                 goto reset_pg_and_lock;
 420                                 }
 421                                 flash_cmd &= ~FLASH_NVSTR;
 422                                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 423                                 if (retval != ERROR_OK)
 424                                         goto reset_pg_and_lock;
 425
 426                                 flash_cmd &= ~(FLASH_XE | FLASH_PROG);
 427                                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 428                                 if (retval != ERROR_OK)
 429                                         goto reset_pg_and_lock;
 430                         }
 431
 432                         buffer += bytes_to_write;
 433                         offset += bytes_to_write;
 434                         count -= bytes_to_write;
 435                 }
 436         }
 437
 438 reset_pg_and_lock:
 439         flash_cmd &= FLASH_DELAY_MASK;
 440         retval2 = target_write_u32(target, FLASH_CMD, flash_cmd);
 441         if (retval == ERROR_OK)
 442                 retval = retval2;
 443
 444         retval2 = target_write_u32(target, FLASH_KEY, 0);
 445         if (retval == ERROR_OK)
 446                 retval = retval2;
 447
 448 free_buffer:
 449         free(new_buffer);
 450
 451         /* read some bytes bytes to flush buffer in flash accelerator.
 452          * See errata for 1986VE1T and 1986VE3. Error 0007 */
 453         if ((retval == ERROR_OK) && (!mdr_info->mem_type)) {
 454                 uint32_t tmp;
 455                 target_checksum_memory(bank->target, bank->base, 64, &tmp);
 456         }
 457
 458         return retval;
 459 }
 460
 461 static int mdr_read(struct flash_bank *bank, uint8_t *buffer,
 462                     uint32_t offset, uint32_t count)
 463 {
 464         struct target *target = bank->target;
 465         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 466         int retval, retval2;
 467
 468         if (!mdr_info->mem_type)
 469                 return default_flash_read(bank, buffer, offset, count);
 470
 471         if (bank->target->state != TARGET_HALTED) {
 472                 LOG_ERROR("Target not halted");
 473                 return ERROR_TARGET_NOT_HALTED;
 474         }
 475
 476         if (offset & 0x3) {
 477                 LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte alignment", offset);
 478                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 479         }
 480
 481         if (count & 0x3) {
 482                 LOG_ERROR("count 0x%" PRIx32 " breaks required 4-byte alignment", count);
 483                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 484         }
 485
 486         uint32_t flash_cmd, cur_per_clock;
 487
 488         retval = target_read_u32(target, MD_PER_CLOCK, &cur_per_clock);
 489         if (retval != ERROR_OK)
 490                 goto err;
 491
 492         if (!(cur_per_clock & MD_PER_CLOCK_RST_CLK)) {
 493                 /* Something's very wrong if the RST_CLK module is not clocked */
 494                 LOG_ERROR("Target needs reset before flash operations");
 495                 retval = ERROR_FLASH_OPERATION_FAILED;
 496                 goto err;
 497         }
 498
 499         retval = target_write_u32(target, MD_PER_CLOCK, cur_per_clock | MD_PER_CLOCK_EEPROM);
 500         if (retval != ERROR_OK)
 501                 goto err;
 502
 503         retval = target_write_u32(target, FLASH_KEY, KEY);
 504         if (retval != ERROR_OK)
 505                 goto err;
 506
 507         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
 508         if (retval != ERROR_OK)
 509                 goto err_lock;
 510
 511         /* Switch on register access */
 512         flash_cmd = (flash_cmd & FLASH_DELAY_MASK) | FLASH_CON | FLASH_IFREN;
 513         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 514         if (retval != ERROR_OK)
 515                 goto reset_pg_and_lock;
 516
 517         for (uint32_t i = 0; i < count; i += 4) {
 518                 retval = target_write_u32(target, FLASH_ADR, offset + i);
 519                 if (retval != ERROR_OK)
 520                         goto reset_pg_and_lock;
 521
 522                 retval = target_write_u32(target, FLASH_CMD, flash_cmd |
 523                                           FLASH_XE | FLASH_YE | FLASH_SE);
 524                 if (retval != ERROR_OK)
 525                         goto reset_pg_and_lock;
 526
 527                 uint32_t buf;
 528                 retval = target_read_u32(target, FLASH_DO, &buf);
 529                 if (retval != ERROR_OK)
 530                         goto reset_pg_and_lock;
 531
 532                 buf_set_u32(buffer, i * 8, 32, buf);
 533
 534                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 535                 if (retval != ERROR_OK)
 536                         goto reset_pg_and_lock;
 537
 538         }
 539
 540 reset_pg_and_lock:
 541         flash_cmd &= FLASH_DELAY_MASK;
 542         retval2 = target_write_u32(target, FLASH_CMD, flash_cmd);
 543         if (retval == ERROR_OK)
 544                 retval = retval2;
 545
 546 err_lock:
 547         retval2 = target_write_u32(target, FLASH_KEY, 0);
 548         if (retval == ERROR_OK)
 549                 retval = retval2;
 550
 551 err:
 552         return retval;
 553 }
 554
 555 static int mdr_probe(struct flash_bank *bank)
 556 {
 557         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 558         unsigned int page_count, page_size, i;
 559
 560         page_count = mdr_info->page_count;
 561         page_size = bank->size / page_count;
 562
 563         free(bank->sectors);
 564
 565         bank->num_sectors = page_count;
 566         bank->sectors = malloc(sizeof(struct flash_sector) * page_count);
 567
 568         for (i = 0; i < page_count; i++) {
 569                 bank->sectors[i].offset = i * page_size;
 570                 bank->sectors[i].size = page_size;
 571                 bank->sectors[i].is_erased = -1;
 572                 bank->sectors[i].is_protected = 0;
 573         }
 574
 575         mdr_info->probed = true;
 576
 577         return ERROR_OK;
 578 }
 579
 580 static int mdr_auto_probe(struct flash_bank *bank)
 581 {
 582         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 583         if (mdr_info->probed)
 584                 return ERROR_OK;
 585         return mdr_probe(bank);
 586 }
 587
 588 static int get_mdr_info(struct flash_bank *bank, struct command_invocation *cmd)
 589 {
 590         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 591         command_print_sameline(cmd, "MDR32Fx - %s",
 592                         mdr_info->mem_type ? "info memory" : "main memory");
 593
 594         return ERROR_OK;
 595 }
 596
 597 const struct flash_driver mdr_flash = {
 598         .name = "mdr",
 599         .usage = "flash bank <name> mdr <base> <size> 0 0 <target#> <type> <page_count> <sec_count>\n"
 600         "<type>: 0 for main memory, 1 for info memory",
 601         .flash_bank_command = mdr_flash_bank_command,
 602         .erase = mdr_erase,
 603         .write = mdr_write,
 604         .read = mdr_read,
 605         .probe = mdr_probe,
 606         .auto_probe = mdr_auto_probe,
 607         .erase_check = default_flash_blank_check,
 608         .info = get_mdr_info,
 609         .free_driver_priv = default_flash_free_driver_priv,
 610 };