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

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