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