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         return retval;
 473 }
 474
 475 static int mdr_read(struct flash_bank *bank, uint8_t *buffer,
 476                     uint32_t offset, uint32_t count)
 477 {
 478         struct target *target = bank->target;
 479         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 480         int retval, retval2;
 481
 482         if (!mdr_info->mem_type)
 483                 return default_flash_read(bank, buffer, offset, count);
 484
 485         if (bank->target->state != TARGET_HALTED) {
 486                 LOG_ERROR("Target not halted");
 487                 return ERROR_TARGET_NOT_HALTED;
 488         }
 489
 490         if (offset & 0x3) {
 491                 LOG_ERROR("offset 0x%" PRIx32 " breaks required 4-byte alignment", offset);
 492                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 493         }
 494
 495         if (count & 0x3) {
 496                 LOG_ERROR("count 0x%" PRIx32 " breaks required 4-byte alignment", count);
 497                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 498         }
 499
 500         uint32_t flash_cmd, cur_per_clock;
 501
 502         retval = target_read_u32(target, MD_PER_CLOCK, &cur_per_clock);
 503         if (retval != ERROR_OK)
 504                 goto err;
 505
 506         if (!(cur_per_clock & MD_PER_CLOCK_RST_CLK)) {
 507                 /* Something's very wrong if the RST_CLK module is not clocked */
 508                 LOG_ERROR("Target needs reset before flash operations");
 509                 retval = ERROR_FLASH_OPERATION_FAILED;
 510                 goto err;
 511         }
 512
 513         retval = target_write_u32(target, MD_PER_CLOCK, cur_per_clock | MD_PER_CLOCK_EEPROM);
 514         if (retval != ERROR_OK)
 515                 goto err;
 516
 517         retval = target_write_u32(target, FLASH_KEY, KEY);
 518         if (retval != ERROR_OK)
 519                 goto err;
 520
 521         retval = target_read_u32(target, FLASH_CMD, &flash_cmd);
 522         if (retval != ERROR_OK)
 523                 goto err_lock;
 524
 525         /* Switch on register access */
 526         flash_cmd = (flash_cmd & FLASH_DELAY_MASK) | FLASH_CON | FLASH_IFREN;
 527         retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 528         if (retval != ERROR_OK)
 529                 goto reset_pg_and_lock;
 530
 531         for (uint32_t i = 0; i < count; i += 4) {
 532                 retval = target_write_u32(target, FLASH_ADR, offset + i);
 533                 if (retval != ERROR_OK)
 534                         goto reset_pg_and_lock;
 535
 536                 retval = target_write_u32(target, FLASH_CMD, flash_cmd |
 537                                           FLASH_XE | FLASH_YE | FLASH_SE);
 538                 if (retval != ERROR_OK)
 539                         goto reset_pg_and_lock;
 540
 541                 uint32_t buf;
 542                 retval = target_read_u32(target, FLASH_DO, &buf);
 543                 if (retval != ERROR_OK)
 544                         goto reset_pg_and_lock;
 545
 546                 buf_set_u32(buffer, i * 8, 32, buf);
 547
 548                 retval = target_write_u32(target, FLASH_CMD, flash_cmd);
 549                 if (retval != ERROR_OK)
 550                         goto reset_pg_and_lock;
 551
 552         }
 553
 554 reset_pg_and_lock:
 555         flash_cmd &= FLASH_DELAY_MASK;
 556         retval2 = target_write_u32(target, FLASH_CMD, flash_cmd);
 557         if (retval == ERROR_OK)
 558                 retval = retval2;
 559
 560 err_lock:
 561         retval2 = target_write_u32(target, FLASH_KEY, 0);
 562         if (retval == ERROR_OK)
 563                 retval = retval2;
 564
 565 err:
 566         return retval;
 567 }
 568
 569 static int mdr_probe(struct flash_bank *bank)
 570 {
 571         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 572         unsigned int page_count, page_size, i;
 573
 574         page_count = mdr_info->page_count;
 575         page_size = bank->size / page_count;
 576
 577         if (bank->sectors) {
 578                 free(bank->sectors);
 579                 bank->sectors = NULL;
 580         }
 581
 582         bank->num_sectors = page_count;
 583         bank->sectors = malloc(sizeof(struct flash_sector) * page_count);
 584
 585         for (i = 0; i < page_count; i++) {
 586                 bank->sectors[i].offset = i * page_size;
 587                 bank->sectors[i].size = page_size;
 588                 bank->sectors[i].is_erased = -1;
 589                 bank->sectors[i].is_protected = 0;
 590         }
 591
 592         mdr_info->probed = 1;
 593
 594         return ERROR_OK;
 595 }
 596
 597 static int mdr_auto_probe(struct flash_bank *bank)
 598 {
 599         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 600         if (mdr_info->probed)
 601                 return ERROR_OK;
 602         return mdr_probe(bank);
 603 }
 604
 605 static int get_mdr_info(struct flash_bank *bank, char *buf, int buf_size)
 606 {
 607         struct mdr_flash_bank *mdr_info = bank->driver_priv;
 608         snprintf(buf, buf_size, "MDR32Fx - %s",
 609                  mdr_info->mem_type ? "info memory" : "main memory");
 610
 611         return ERROR_OK;
 612 }
 613
 614 struct flash_driver mdr_flash = {
 615         .name = "mdr",
 616         .usage = "flash bank <name> mdr <base> <size> 0 0 <target#> <type> <page_count> <sec_count>\n"
 617         "<type>: 0 for main memory, 1 for info memory",
 618         .flash_bank_command = mdr_flash_bank_command,
 619         .erase = mdr_erase,
 620         .protect = mdr_protect,
 621         .write = mdr_write,
 622         .read = mdr_read,
 623         .probe = mdr_probe,
 624         .auto_probe = mdr_auto_probe,
 625         .erase_check = default_flash_blank_check,
 626         .protect_check = mdr_protect_check,
 627         .info = get_mdr_info,
 628 };