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