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