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