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_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, int first, int last)
 128 {
 129         struct target *target = bank->target;
 130         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 131         int i, retval, retval2;
 132         unsigned int j;
 133         uint32_t flash_cmd, cur_per_clock;
 134
 135         if (bank->target->state != TARGET_HALTED) {
 136                 LOG_ERROR("Target not halted");
 137                 return ERROR_TARGET_NOT_HALTED;
 138         }
 139
 140         retval = target_read_u32(target, MD_PER_CLOCK, &cur_per_clock);
 141         if (retval != ERROR_OK)
 142                 return retval;
 143
 144         if (!(cur_per_clock & 0x10)) {
 145                 LOG_ERROR("Target needs reset before flash operations");
 146                 return ERROR_FLASH_OPERATION_FAILED;
 147         }
 148
 149         retval = target_write_u32(target, MD_PER_CLOCK, cur_per_clock | MD_PER_CLOCK_EEPROM);
 150         if (retval != ERROR_OK)
 151                 return retval;
 152
 153         retval = target_write_u32(target, FLASH_KEY, KEY);
 154         if (retval != ERROR_OK)
 155                 return retval;
 156
 157         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
 158         if (retval != ERROR_OK)
 159                 goto reset_pg_and_lock;
 160
 161         /* Switch on register access */
 162         flash_cmd = (flash_cmd & FLASH_DELAY_MASK) | FLASH_CON;
 163         if (mdr_info->mem_type)
 164                 flash_cmd |= FLASH_IFREN;
 165         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 166         if (retval != ERROR_OK)
 167                 goto reset_pg_and_lock;
 168
 169         if ((first == 0) && (last == (bank->num_sectors - 1)) &&
 170                 !mdr_info->mem_type) {
 171                 retval = mdr_mass_erase(bank);
 172                 goto reset_pg_and_lock;
 173         }
 174
 175         unsigned int page_size = bank->size / mdr_info->page_count;
 176         for (i = first; i <= last; i++) {
 177                 for (j = 0; j < mdr_info->sec_count; j++) {
 178                         retval = target_write_u32(target, FLASH_ADR, (i * page_size) | (j << 2));
 179                         if (retval != ERROR_OK)
 180                                 goto reset_pg_and_lock;
 181
 182                         flash_cmd |= FLASH_XE | FLASH_ERASE;
 183                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 184                         if (retval != ERROR_OK)
 185                                 goto reset_pg_and_lock;
 186                         flash_cmd |= FLASH_NVSTR;
 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_ERASE;
 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_XE | FLASH_NVSTR);
 195                         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 196                         if (retval != ERROR_OK)
 197                                 goto reset_pg_and_lock;
 198                 }
 199                 bank->sectors[i].is_erased = 1;
 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 == NULL) {
 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         if (new_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         if (bank->sectors) {
 576                 free(bank->sectors);
 577                 bank->sectors = NULL;
 578         }
 579
 580         bank->num_sectors = page_count;
 581         bank->sectors = malloc(sizeof(struct flash_sector) * page_count);
 582
 583         for (i = 0; i < page_count; i++) {
 584                 bank->sectors[i].offset = i * page_size;
 585                 bank->sectors[i].size = page_size;
 586                 bank->sectors[i].is_erased = -1;
 587                 bank->sectors[i].is_protected = 0;
 588         }
 589
 590         mdr_info->probed = 1;
 591
 592         return ERROR_OK;
 593 }
 594
 595 static int mdr_auto_probe(struct flash_bank *bank)
 596 {
 597         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 598         if (mdr_info->probed)
 599                 return ERROR_OK;
 600         return mdr_probe(bank);
 601 }
 602
 603 static int get_mdr_info(struct flash_bank *bank, char *buf, int buf_size)
 604 {
 605         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 606         snprintf(buf, buf_size, "MDR32Fx - %s",
 607                  mdr_info->mem_type ? "info memory" : "main memory");
 608
 609         return ERROR_OK;
 610 }
 611
 612 struct flash_driver mdr_flash = {
 613         .name = "mdr",
 614         .usage = "flash bank <name> mdr <base> <size> 0 0 <target#> <type> <page_count> <sec_count>\n"
 615         "<type>: 0 for main memory, 1 for info memory",
 616         .flash_bank_command = mdr_flash_bank_command,
 617         .erase = mdr_erase,
 618         .write = mdr_write,
 619         .read = mdr_read,
 620         .probe = mdr_probe,
 621         .auto_probe = mdr_auto_probe,
 622         .erase_check = default_flash_blank_check,
 623         .info = get_mdr_info,
 624         .free_driver_priv = default_flash_free_driver_priv,
 625 };