git.gag.com Git - fw/openocd/blob - src/flash/nor/fm3.c

   1 /***************************************************************************
   2  *   Copyright (C) 2011 by Marc Willam, Holger Wech                        *
   3  *       openOCD.fseu(AT)de.fujitsu.com                                    *
   4  *   Copyright (C) 2011 Ronny Strutz                                       *
   5  *                                                                         *
   6  *   This program is free software; you can redistribute it and/or modify  *
   7  *   it under the terms of the GNU General Public License as published by  *
   8  *   the Free Software Foundation; either version 2 of the License, or     *
   9  *   (at your option) any later version.                                   *
  10  *                                                                         *
  11  *   This program is distributed in the hope that it will be useful,       *
  12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  14  *   GNU General Public License for more details.                          *
  15  *                                                                         *
  16  *   You should have received a copy of the GNU General Public License     *
  17  *   along with this program; if not, write to the                         *
  18  *   Free Software Foundation, Inc.,                                       *
  19  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
  20  ***************************************************************************/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include "config.h"
  24 #endif
  25
  26 #include "imp.h"
  27 #include <helper/binarybuffer.h>
  28 #include <target/algorithm.h>
  29 #include <target/armv7m.h>
  30
  31 #define FLASH_DQ6 0x00000040    /* Data toggle flag bit (TOGG) position */
  32 #define FLASH_DQ5 0x00000020    /* Time limit exceeding flag bit (TLOV) position */
  33
  34 enum fm3_variant
  35 {
  36         mb9bfxx1,       /* Flash Type '1' */
  37         mb9bfxx2,
  38         mb9bfxx3,
  39         mb9bfxx4,
  40         mb9bfxx5,
  41         mb9bfxx6,
  42         mb9afxx1,       /* Flash Type '2' */
  43         mb9afxx2,
  44         mb9afxx3,
  45         mb9afxx4,
  46         mb9afxx5,
  47         mb9afxx6
  48 };
  49
  50 enum fm3_flash_type
  51 {
  52         fm3_no_flash_type = 0,
  53         fm3_flash_type1   = 1,
  54         fm3_flash_type2   = 2
  55 };
  56
  57 struct fm3_flash_bank
  58 {
  59         struct working_area *write_algorithm;
  60         enum fm3_variant variant;
  61         enum fm3_flash_type flashtype;
  62         int probed;
  63 };
  64
  65 FLASH_BANK_COMMAND_HANDLER(fm3_flash_bank_command)
  66 {
  67         struct fm3_flash_bank *fm3_info;
  68
  69         if (CMD_ARGC < 6)
  70                 return ERROR_COMMAND_SYNTAX_ERROR;
  71
  72         fm3_info = malloc(sizeof(struct fm3_flash_bank));
  73         bank->driver_priv = fm3_info;
  74
  75         /* Flash type '1' */
  76         if (strcmp(CMD_ARGV[5], "mb9bfxx1.cpu") == 0) {
  77                 fm3_info->variant = mb9bfxx1;
  78                 fm3_info->flashtype = fm3_flash_type1;
  79         } else if (strcmp(CMD_ARGV[5], "mb9bfxx2.cpu") == 0) {
  80                 fm3_info->variant = mb9bfxx2;
  81                 fm3_info->flashtype = fm3_flash_type1;
  82         } else if (strcmp(CMD_ARGV[5], "mb9bfxx3.cpu") == 0) {
  83                 fm3_info->variant = mb9bfxx3;
  84                 fm3_info->flashtype = fm3_flash_type1;
  85         } else if (strcmp(CMD_ARGV[5], "mb9bfxx4.cpu") == 0) {
  86                 fm3_info->variant = mb9bfxx4;
  87                 fm3_info->flashtype = fm3_flash_type1;
  88         } else if (strcmp(CMD_ARGV[5], "mb9bfxx5.cpu") == 0) {
  89                 fm3_info->variant = mb9bfxx5;
  90                 fm3_info->flashtype = fm3_flash_type1;
  91         } else if (strcmp(CMD_ARGV[5], "mb9bfxx6.cpu") == 0) {
  92                 fm3_info->variant = mb9bfxx6;
  93                 fm3_info->flashtype = fm3_flash_type1;
  94         } else if (strcmp(CMD_ARGV[5], "mb9afxx1.cpu") == 0) {  /* Flash type '2' */
  95                 fm3_info->variant = mb9afxx1;
  96                 fm3_info->flashtype = fm3_flash_type2;
  97         } else if (strcmp(CMD_ARGV[5], "mb9afxx2.cpu") == 0) {
  98                 fm3_info->variant = mb9afxx2;
  99                 fm3_info->flashtype = fm3_flash_type2;
 100         } else if (strcmp(CMD_ARGV[5], "mb9afxx3.cpu") == 0) {
 101                 fm3_info->variant = mb9afxx3;
 102                 fm3_info->flashtype = fm3_flash_type2;
 103         } else if (strcmp(CMD_ARGV[5], "mb9afxx4.cpu") == 0) {
 104                 fm3_info->variant = mb9afxx4;
 105                 fm3_info->flashtype = fm3_flash_type2;
 106         } else if (strcmp(CMD_ARGV[5], "mb9afxx5.cpu") == 0) {
 107                 fm3_info->variant = mb9afxx5;
 108                 fm3_info->flashtype = fm3_flash_type2;
 109         } else if (strcmp(CMD_ARGV[5], "mb9afxx6.cpu") == 0) {
 110                 fm3_info->variant = mb9afxx6;
 111                 fm3_info->flashtype = fm3_flash_type2;
 112         }
 113
 114         /* unknown Flash type */
 115         else {
 116                 LOG_ERROR("unknown fm3 variant: %s", CMD_ARGV[5]);
 117                 free(fm3_info);
 118                 return ERROR_FLASH_BANK_INVALID;
 119         }
 120
 121         fm3_info->write_algorithm = NULL;
 122         fm3_info->probed = 0;
 123
 124         return ERROR_OK;
 125 }
 126
 127 /* Data polling algorithm */
 128 static int fm3_busy_wait(struct target *target, uint32_t offset, int timeout_ms)
 129 {
 130         int retval = ERROR_OK;
 131         uint16_t state1, state2;
 132         int ms = 0;
 133
 134         /* While(1) loop exit via "break" and "return" on error */
 135         while (1) {
 136                 /* dummy-read - see flash manual */
 137                 retval = target_read_u16(target, offset, &state1);
 138                 if (retval != ERROR_OK)
 139                         return retval;
 140
 141                 /* Data polling 1 */
 142                 retval = target_read_u16(target, offset, &state1);
 143                 if (retval != ERROR_OK)
 144                         return retval;
 145
 146                 /* Data polling 2 */
 147                 retval = target_read_u16(target, offset, &state2);
 148                 if (retval != ERROR_OK)
 149                         return retval;
 150
 151                 /* Flash command finished via polled data equal? */
 152                 if ((state1 & FLASH_DQ6) == (state2 & FLASH_DQ6))
 153                         break;
 154                 /* Timeout Flag? */
 155                 else if (state1 & FLASH_DQ5) {
 156                         /* Retry data polling */
 157
 158                         /* Data polling 1 */
 159                         retval = target_read_u16(target, offset, &state1);
 160                         if (retval != ERROR_OK)
 161                                 return retval;
 162
 163                         /* Data polling 2 */
 164                         retval = target_read_u16(target, offset, &state2);
 165                         if (retval != ERROR_OK)
 166                                 return retval;
 167
 168                         /* Flash command finished via polled data equal? */
 169                         if ((state1 & FLASH_DQ6) != (state2 & FLASH_DQ6))
 170                                 return ERROR_FLASH_OPERATION_FAILED;
 171
 172                         /* finish anyway */
 173                         break;
 174                 }
 175                 usleep(1000);
 176                 ++ms;
 177
 178                 /* Polling time exceeded? */
 179                 if (ms > timeout_ms) {
 180                         LOG_ERROR("Polling data reading timed out!");
 181                         return ERROR_FLASH_OPERATION_FAILED;
 182                 }
 183         }
 184
 185         if (retval == ERROR_OK)
 186                 LOG_DEBUG("fm3_busy_wait(%x) needs about %d ms", offset, ms);
 187
 188         return retval;
 189 }
 190
 191 static int fm3_erase(struct flash_bank *bank, int first, int last)
 192 {
 193         struct fm3_flash_bank *fm3_info = bank->driver_priv;
 194         struct target *target = bank->target;
 195         int retval = ERROR_OK;
 196         uint32_t u32DummyRead;
 197         int sector, odd;
 198         uint32_t u32FlashType;
 199         uint32_t u32FlashSeqAddress1;
 200         uint32_t u32FlashSeqAddress2;
 201
 202         u32FlashType = (uint32_t) fm3_info->flashtype;
 203
 204         if (u32FlashType == fm3_flash_type1) {
 205                 u32FlashSeqAddress1 = 0x00001550;
 206                 u32FlashSeqAddress2 = 0x00000AA8;
 207         } else if (u32FlashType == fm3_flash_type2) {
 208                 u32FlashSeqAddress1 = 0x00000AA8;
 209                 u32FlashSeqAddress2 = 0x00000554;
 210         } else {
 211                 LOG_ERROR("Flash/Device type unknown!");
 212                 return ERROR_FLASH_OPERATION_FAILED;
 213         }
 214
 215         if (target->state != TARGET_HALTED) {
 216                 LOG_ERROR("Target not halted");
 217                 return ERROR_TARGET_NOT_HALTED;
 218         }
 219
 220         LOG_INFO("Fujitsu MB9Bxxx: Sector Erase ... (%d to %d)", first, last);
 221
 222         /* FASZR = 0x01, Enables CPU Programming Mode (16-bit Flash acccess) */
 223         retval = target_write_u32(target, 0x40000000, 0x0001);
 224         if (retval != ERROR_OK)
 225                 return retval;
 226
 227         /* dummy read of FASZR */
 228         retval = target_read_u32(target, 0x40000000, &u32DummyRead);
 229         if (retval != ERROR_OK)
 230                 return retval;
 231
 232         for (sector = first ; sector <= last ; sector++) {
 233                 uint32_t offset = bank->sectors[sector].offset;
 234
 235                 for (odd = 0; odd < 2 ; odd++) {
 236                         if (odd)
 237                                 offset += 4;
 238
 239                         /* Flash unlock sequence */
 240                         retval = target_write_u16(target, u32FlashSeqAddress1, 0x00AA);
 241                         if (retval != ERROR_OK)
 242                                 return retval;
 243
 244                         retval = target_write_u16(target, u32FlashSeqAddress2, 0x0055);
 245                         if (retval != ERROR_OK)
 246                                 return retval;
 247
 248                         retval = target_write_u16(target, u32FlashSeqAddress1, 0x0080);
 249                         if (retval != ERROR_OK)
 250                                 return retval;
 251
 252                         retval = target_write_u16(target, u32FlashSeqAddress1, 0x00AA);
 253                         if (retval != ERROR_OK)
 254                                 return retval;
 255
 256                         retval = target_write_u16(target, u32FlashSeqAddress2, 0x0055);
 257                         if (retval != ERROR_OK)
 258                                 return retval;
 259
 260                         /* Sector erase command (0x0030) */
 261                         retval = target_write_u16(target, offset, 0x0030);
 262                         if (retval != ERROR_OK)
 263                                 return retval;
 264
 265                         retval = fm3_busy_wait(target, offset, 500);
 266                         if (retval != ERROR_OK)
 267                                 return retval;
 268                 }
 269                 bank->sectors[sector].is_erased = 1;
 270         }
 271
 272         /* FASZR = 0x02, Enables CPU Run Mode (32-bit Flash acccess) */
 273         retval = target_write_u32(target, 0x40000000, 0x0002);
 274         if (retval != ERROR_OK)
 275                 return retval;
 276
 277         retval = target_read_u32(target, 0x40000000, &u32DummyRead); /* dummy read of FASZR */
 278
 279         return retval;
 280 }
 281
 282 static int fm3_write_block(struct flash_bank *bank, uint8_t *buffer,
 283                 uint32_t offset, uint32_t count)
 284 {
 285         struct fm3_flash_bank *fm3_info = bank->driver_priv;
 286         struct target *target = bank->target;
 287         uint32_t buffer_size = 2048;            /* 8192 for MB9Bxx6! */
 288         struct working_area *source;
 289         uint32_t address = bank->base + offset;
 290         struct reg_param reg_params[6];
 291         struct armv7m_algorithm armv7m_info;
 292         int retval = ERROR_OK;
 293         uint32_t u32FlashType;
 294         uint32_t u32FlashSeqAddress1;
 295         uint32_t u32FlashSeqAddress2;
 296
 297         u32FlashType = (uint32_t) fm3_info->flashtype;
 298
 299         if (u32FlashType == fm3_flash_type1) {
 300                 u32FlashSeqAddress1 = 0x00001550;
 301                 u32FlashSeqAddress2 = 0x00000AA8;
 302         } else if (u32FlashType == fm3_flash_type2) {
 303                 u32FlashSeqAddress1 = 0x00000AA8;
 304                 u32FlashSeqAddress2 = 0x00000554;
 305         } else {
 306                 LOG_ERROR("Flash/Device type unknown!");
 307                 return ERROR_FLASH_OPERATION_FAILED;
 308         }
 309
 310         /* RAMCODE used for fm3 Flash programming:                 */
 311         /* R0 keeps source start address         (u32Source)       */
 312         /* R1 keeps target start address         (u32Target)       */
 313         /* R2 keeps number of halfwords to write (u32Count)        */
 314         /* R3 keeps Flash Sequence address 1     (u32FlashSeq1)    */
 315         /* R4 keeps Flash Sequence address 2     (u32FlashSeq2)    */
 316         /* R5 returns result value               (u32FlashResult)  */
 317
 318         const uint8_t fm3_flash_write_code[] = {
 319                                                                 /*    fm3_FLASH_IF->FASZ &= 0xFFFD;           */
 320         0x5F, 0xF0, 0x80, 0x45,         /*        MOVS.W   R5, #(fm3_FLASH_IF->FASZ)  */
 321         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 322         0x4F, 0xF6, 0xFD, 0x76,         /*        MOVW     R6, #0xFFFD                */
 323         0x35, 0x40,                                     /*        ANDS     R5, R5, R6                 */
 324         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 325         0x35, 0x60,                                     /*        STR      R5, [R6]                   */
 326                                                                 /*    fm3_FLASH_IF->FASZ |= 1;                */
 327         0x5F, 0xF0, 0x80, 0x45,         /*        MOVS.W   R5, #(fm3_FLASH_IF->FASZ)  */
 328         0x2D, 0x68,                                     /*        LDR      R5, [R3]                   */
 329         0x55, 0xF0, 0x01, 0x05,         /*        ORRS.W   R5, R5, #1                 */
 330         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 331         0x35, 0x60,                                     /*        STR      R5, [R6]                   */
 332                                                                 /*    u32DummyRead = fm3_FLASH_IF->FASZ;      */
 333         0x28, 0x4D,                                     /*        LDR.N    R5, ??u32DummyRead         */
 334         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 335         0x36, 0x68,                                     /*        LDR      R6, [R6]                   */
 336         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 337                                                                 /*    u32FlashResult = FLASH_WRITE_NO_RESULT  */
 338         0x26, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 339         0x00, 0x26,                                     /*        MOVS     R6, #0                     */
 340         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 341                                                                 /*    while ((u32Count > 0 )                  */
 342                                                                 /*      && (u32FlashResult                    */
 343                                                                 /*          == FLASH_WRITE_NO_RESULT))        */
 344         0x01, 0x2A,                                     /* L0:    CMP      R2, #1                     */
 345         0x2C, 0xDB,                                     /*        BLT.N    L1                         */
 346         0x24, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 347         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 348         0x00, 0x2D,                                     /*        CMP      R5, #0                     */
 349         0x28, 0xD1,                                     /*        BNE.N    L1                         */
 350                                                                 /*    *u32FlashSeq1 = FLASH_WRITE_1;          */
 351         0xAA, 0x25,                                     /*        MOVS     R5, #0xAA                  */
 352         0x1D, 0x60,                                     /*        STR      R5, [R3]                   */
 353                                                                 /*    *u32FlashSeq2 = FLASH_WRITE_2;          */
 354         0x55, 0x25,                                     /*        MOVS     R5, #0x55                  */
 355         0x25, 0x60,                                     /*        STR      R5, [R4]                   */
 356                                                                 /*    *u32FlashSeq1 = FLASH_WRITE_3;          */
 357         0xA0, 0x25,                                     /*        MOVS     R5, #0xA0                  */
 358         0x1D, 0x60,                                     /*        STRH     R5, [R3]                   */
 359                                                                 /*    *(volatile uint16_t*)u32Target          */
 360                                                                 /*      = *(volatile uint16_t*)u32Source;     */
 361         0x05, 0x88,                                     /*        LDRH     R5, [R0]                   */
 362         0x0D, 0x80,                                     /*        STRH     R5, [R1]                   */
 363                                                                 /*    while (u32FlashResult                   */
 364                                                                 /*           == FLASH_WRITE_NO_RESTULT)       */
 365         0x1E, 0x4D,                                     /* L2:    LDR.N    R5, ??u32FlashResult       */
 366         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 367         0x00, 0x2D,                                     /*        CMP      R5, #0                     */
 368         0x11, 0xD1,                                     /*        BNE.N    L3                         */
 369                                                                 /*    if ((*(volatile uint16_t*)u32Target     */
 370                                                                 /*        & FLASH_DQ5) == FLASH_DQ5)          */
 371         0x0D, 0x88,                                     /*        LDRH     R5, [R1]                   */
 372         0xAD, 0x06,                                     /*        LSLS     R5, R5, #0x1A              */
 373         0x02, 0xD5,                                     /*        BPL.N    L4                         */
 374                                                                 /*    u32FlashResult = FLASH_WRITE_TIMEOUT    */
 375         0x1A, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 376         0x02, 0x26,                                     /*        MOVS     R6, #2                     */
 377         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 378                                                                 /*    if ((*(volatile uint16_t *)u32Target    */
 379                                                                 /*         & FLASH_DQ7)                       */
 380                                                                 /*        == (*(volatile uint16_t*)u32Source  */
 381                                                                 /*            & FLASH_DQ7))                   */
 382         0x0D, 0x88,                                     /* L4:    LDRH     R5, [R1]                   */
 383         0x15, 0xF0, 0x80, 0x05,         /*        ANDS.W   R5, R5, #0x80              */
 384         0x06, 0x88,                                     /*        LDRH     R6, [R0]                   */
 385         0x16, 0xF0, 0x80, 0x06,         /*        ANDS.W   R6, R6, #0x80              */
 386         0xB5, 0x42,                                     /*        CMP      R5, R6                     */
 387         0xED, 0xD1,                                     /*        BNE.N    L2                         */
 388                                                                 /*    u32FlashResult = FLASH_WRITE_OKAY       */
 389         0x15, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 390         0x01, 0x26,                                     /*        MOVS     R6, #1                     */
 391         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 392         0xE9, 0xE7,                                     /*        B.N      L2                         */
 393                                                                 /*    if (u32FlashResult                      */
 394                                                                 /*        != FLASH_WRITE_TIMEOUT)             */
 395         0x13, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 396         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 397         0x02, 0x2D,                                     /*        CMP      R5, #2                     */
 398         0x02, 0xD0,                                     /*        BEQ.N    L5                         */
 399                                                                 /*    u32FlashResult = FLASH_WRITE_NO_RESULT  */
 400         0x11, 0x4D,                                     /*        LDR.N    R5, ??u32FlashResult       */
 401         0x00, 0x26,                                     /*        MOVS     R6, #0                     */
 402         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 403                                                                 /*    u32Count--;                             */
 404         0x52, 0x1E,                                     /* L5:    SUBS     R2, R2, #1                 */
 405                                                                 /*    u32Source += 2;                         */
 406         0x80, 0x1C,                                     /*        ADDS     R0, R0, #2                 */
 407                                                                 /*    u32Target += 2;                         */
 408         0x89, 0x1C,                                     /*        ADDS     R1, R1, #2                 */
 409         0xD0, 0xE7,                                     /*        B.N      L0                         */
 410                                                                 /*    fm3_FLASH_IF->FASZ &= 0xFFFE;           */
 411         0x5F, 0xF0, 0x80, 0x45,         /* L1:    MOVS.W   R5, #(fm3_FLASH_IF->FASZ)  */
 412         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 413         0x4F, 0xF6, 0xFE, 0x76,         /*        MOVW     R6, #0xFFFE                */
 414         0x35, 0x40,                                     /*        ANDS     R5, R5, R6                 */
 415         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 416         0x35, 0x60,                                     /*        STR      R5, [R6]                   */
 417                                                                 /*    fm3_FLASH_IF->FASZ |= 2;                */
 418         0x5F, 0xF0, 0x80, 0x45,         /*        MOVS.W   R5, #(fm3_FLASH_IF->FASZ)  */
 419         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 420         0x55, 0xF0, 0x02, 0x05,         /*        ORRS.W   R5, R5, #2                 */
 421         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 422         0x35, 0x60,                                     /*        STR      R5, [R6]                   */
 423                                                                 /*    u32DummyRead = fm3_FLASH_IF->FASZ;      */
 424         0x04, 0x4D,                                     /*        LDR.N    R5, ??u32DummyRead         */
 425         0x5F, 0xF0, 0x80, 0x46,         /*        MOVS.W   R6, #(fm3_FLASH_IF->FASZ)  */
 426         0x36, 0x68,                                     /*        LDR      R6, [R6]                   */
 427         0x2E, 0x60,                                     /*        STR      R6, [R5]                   */
 428                                                                 /*    copy u32FlashResult to R3 for return    */
 429                                                                 /*      value                                 */
 430         0xDF, 0xF8, 0x08, 0x50,         /*        LDR.W    R5, ??u32FlashResult       */
 431         0x2D, 0x68,                                     /*        LDR      R5, [R5]                   */
 432                                                                 /*    Breakpoint here                         */
 433         0x00, 0xBE,                                     /*        BKPT     #0                         */
 434
 435         /* The following address pointers assume, that the code is running from   */
 436         /* address 0x1FFF8008. These address pointers will be patched, if a       */
 437         /* different start address in RAM is used (e.g. for Flash type 2)!        */
 438         0x00, 0x80, 0xFF, 0x1F,     /* u32DummyRead address in RAM (0x1FFF8000)   */
 439         0x04, 0x80, 0xFF, 0x1F      /* u32FlashResult address in RAM (0x1FFF8004) */
 440         };
 441
 442         LOG_INFO("Fujitsu MB9B500: FLASH Write ...");
 443
 444         /* disable HW watchdog */
 445         retval = target_write_u32(target, 0x40011C00, 0x1ACCE551);
 446         if (retval != ERROR_OK)
 447                 return retval;
 448
 449         retval = target_write_u32(target, 0x40011C00, 0xE5331AAE);
 450         if (retval != ERROR_OK)
 451                 return retval;
 452
 453         retval = target_write_u32(target, 0x40011008, 0x00000000);
 454         if (retval != ERROR_OK)
 455                 return retval;
 456
 457         count = count / 2;              /* number bytes -> number halfwords */
 458
 459         /* check code alignment */
 460         if (offset & 0x1) {
 461                 LOG_WARNING("offset 0x%" PRIx32 " breaks required 2-byte alignment", offset);
 462                 return ERROR_FLASH_DST_BREAKS_ALIGNMENT;
 463         }
 464
 465         /* allocate working area with flash programming code */
 466         if (target_alloc_working_area(target, sizeof(fm3_flash_write_code),
 467                         &fm3_info->write_algorithm) != ERROR_OK) {
 468                 LOG_WARNING("no working area available, can't do block memory writes");
 469                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 470         }
 471
 472         retval = target_write_buffer(target, fm3_info->write_algorithm->address,
 473                 sizeof(fm3_flash_write_code), fm3_flash_write_code);
 474         if (retval != ERROR_OK)
 475                 return retval;
 476
 477
 478
 479         /* memory buffer */
 480         while (target_alloc_working_area(target, buffer_size, &source) != ERROR_OK) {
 481                 buffer_size /= 2;
 482                 if (buffer_size <= 256) {
 483                         /* free working area, if write algorithm already allocated */
 484                         if (fm3_info->write_algorithm)
 485                                 target_free_working_area(target, fm3_info->write_algorithm);
 486
 487                         LOG_WARNING("No large enough working area available, can't do block memory writes");
 488                         return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
 489                 }
 490         }
 491
 492         armv7m_info.common_magic = ARMV7M_COMMON_MAGIC;
 493         armv7m_info.core_mode = ARMV7M_MODE_ANY;
 494
 495         init_reg_param(&reg_params[0], "r0", 32, PARAM_OUT); /* source start address */
 496         init_reg_param(&reg_params[1], "r1", 32, PARAM_OUT); /* target start address */
 497         init_reg_param(&reg_params[2], "r2", 32, PARAM_OUT); /* number of halfwords to program */
 498         init_reg_param(&reg_params[3], "r3", 32, PARAM_OUT); /* Flash Sequence address 1 */
 499         init_reg_param(&reg_params[4], "r4", 32, PARAM_OUT); /* Flash Sequence address 1 */
 500         init_reg_param(&reg_params[5], "r5", 32, PARAM_IN);  /* result */
 501
 502         /* write code buffer and use Flash programming code within fm3           */
 503         /* Set breakpoint to 0 with time-out of 1000 ms                          */
 504         while (count > 0) {
 505                 uint32_t thisrun_count = (count > (buffer_size / 2)) ? (buffer_size / 2) : count;
 506
 507                 retval = target_write_buffer(target, fm3_info->write_algorithm->address, 8,
 508                                 fm3_flash_write_code);
 509                 if (retval != ERROR_OK)
 510                         break;
 511
 512                 /* Patching 'local variable address' for different RAM addresses */
 513                 if (fm3_info->write_algorithm->address != 0x1FFF8008) {
 514                         /* Algorithm: u32DummyRead: */
 515                         retval = target_write_u32(target, (fm3_info->write_algorithm->address)
 516                                 + sizeof(fm3_flash_write_code) - 8, (fm3_info->write_algorithm->address) - 8);
 517                         if (retval != ERROR_OK)
 518                                 break;
 519
 520                         /* Algorithm: u32FlashResult: */
 521                         retval = target_write_u32(target, (fm3_info->write_algorithm->address)
 522                                 + sizeof(fm3_flash_write_code) - 4, (fm3_info->write_algorithm->address) - 4);
 523                         if (retval != ERROR_OK)
 524                                 break;
 525                 }
 526
 527                 retval = target_write_buffer(target, source->address, thisrun_count * 2, buffer);
 528                 if (retval != ERROR_OK)
 529                         break;
 530
 531                 buf_set_u32(reg_params[0].value, 0, 32, source->address);
 532                 buf_set_u32(reg_params[1].value, 0, 32, address);
 533                 buf_set_u32(reg_params[2].value, 0, 32, thisrun_count);
 534                 buf_set_u32(reg_params[3].value, 0, 32, u32FlashSeqAddress1);
 535                 buf_set_u32(reg_params[4].value, 0, 32, u32FlashSeqAddress2);
 536
 537                 retval = target_run_algorithm(target, 0, NULL, 6, reg_params,
 538                                 fm3_info->write_algorithm->address, 0, 1000, &armv7m_info);
 539                 if (retval != ERROR_OK) {
 540                         LOG_ERROR("Error executing fm3 Flash programming algorithm");
 541                         retval = ERROR_FLASH_OPERATION_FAILED;
 542                         break;
 543                 }
 544
 545                 if (buf_get_u32(reg_params[5].value, 0, 32) != ERROR_OK) {
 546                         LOG_ERROR("Fujitsu MB9[A/B]FXXX: Flash programming ERROR (Timeout) -> Reg R3: %x",
 547                                 buf_get_u32(reg_params[5].value, 0, 32));
 548                         retval = ERROR_FLASH_OPERATION_FAILED;
 549                         break;
 550                 }
 551
 552                 buffer  += thisrun_count * 2;
 553                 address += thisrun_count * 2;
 554                 count   -= thisrun_count;
 555         }
 556
 557         target_free_working_area(target, source);
 558         target_free_working_area(target, fm3_info->write_algorithm);
 559
 560         destroy_reg_param(&reg_params[0]);
 561         destroy_reg_param(&reg_params[1]);
 562         destroy_reg_param(&reg_params[2]);
 563         destroy_reg_param(&reg_params[3]);
 564         destroy_reg_param(&reg_params[4]);
 565         destroy_reg_param(&reg_params[5]);
 566
 567         return retval;
 568 }
 569
 570 static int fm3_probe(struct flash_bank *bank)
 571 {
 572         struct fm3_flash_bank *fm3_info = bank->driver_priv;
 573         uint16_t num_pages;
 574
 575         if (bank->target->state != TARGET_HALTED) {
 576                 LOG_ERROR("Target not halted");
 577                 return ERROR_TARGET_NOT_HALTED;
 578         }
 579
 580         num_pages = 6;                          /* max number of Flash pages for malloc */
 581         fm3_info->probed = 0;
 582
 583         bank->sectors = malloc(sizeof(struct flash_sector) * num_pages);
 584         bank->base = 0x00000000;
 585         num_pages = 2;                          /* start with smallest Flash pages number */
 586         bank->size = 32 * 1024;         /* bytes */
 587
 588         bank->sectors[0].offset = 0;
 589         bank->sectors[0].size = 16 * 1024;
 590         bank->sectors[0].is_erased = -1;
 591         bank->sectors[0].is_protected = -1;
 592
 593         bank->sectors[1].offset = 0x4000;
 594         bank->sectors[1].size = 16 * 1024;
 595         bank->sectors[1].is_erased = -1;
 596         bank->sectors[1].is_protected = -1;
 597
 598         if ((fm3_info->variant == mb9bfxx1)
 599             || (fm3_info->variant == mb9afxx1)) {
 600                 num_pages = 3;
 601                 bank->size = 64 * 1024; /* bytes */
 602                 bank->num_sectors = num_pages;
 603
 604                 bank->sectors[2].offset = 0x8000;
 605                 bank->sectors[2].size = 32 * 1024;
 606                 bank->sectors[2].is_erased = -1;
 607                 bank->sectors[2].is_protected = -1;
 608         }
 609
 610         if ((fm3_info->variant == mb9bfxx2)
 611                 || (fm3_info->variant == mb9bfxx4)
 612                 || (fm3_info->variant == mb9bfxx5)
 613                 || (fm3_info->variant == mb9bfxx6)
 614                 || (fm3_info->variant == mb9afxx2)
 615                 || (fm3_info->variant == mb9afxx4)
 616                 || (fm3_info->variant == mb9afxx5)
 617                 || (fm3_info->variant == mb9afxx6)) {
 618                 num_pages = 3;
 619                 bank->size = 128 * 1024; /* bytes */
 620                 bank->num_sectors = num_pages;
 621
 622                 bank->sectors[2].offset = 0x8000;
 623                 bank->sectors[2].size = 96 * 1024;
 624                 bank->sectors[2].is_erased = -1;
 625                 bank->sectors[2].is_protected = -1;
 626         }
 627
 628         if ((fm3_info->variant == mb9bfxx4)
 629                 || (fm3_info->variant == mb9bfxx5)
 630                 || (fm3_info->variant == mb9bfxx6)
 631                 || (fm3_info->variant == mb9afxx4)
 632                 || (fm3_info->variant == mb9afxx5)
 633                 || (fm3_info->variant == mb9afxx6)) {
 634                 num_pages = 4;
 635                 bank->size = 256 * 1024; /* bytes */
 636                 bank->num_sectors = num_pages;
 637
 638                 bank->sectors[3].offset = 0x20000;
 639                 bank->sectors[3].size = 128 * 1024;
 640                 bank->sectors[3].is_erased = -1;
 641                 bank->sectors[3].is_protected = -1;
 642         }
 643
 644         if ((fm3_info->variant == mb9bfxx5)
 645                 || (fm3_info->variant == mb9bfxx6)
 646                 || (fm3_info->variant == mb9afxx5)
 647                 || (fm3_info->variant == mb9afxx6)) {
 648                 num_pages = 5;
 649                 bank->size = 384 * 1024; /* bytes */
 650                 bank->num_sectors = num_pages;
 651
 652                 bank->sectors[4].offset = 0x40000;
 653                 bank->sectors[4].size = 128 * 1024;
 654                 bank->sectors[4].is_erased = -1;
 655                 bank->sectors[4].is_protected = -1;
 656         }
 657
 658         if ((fm3_info->variant == mb9bfxx6)
 659                 || (fm3_info->variant == mb9afxx6)) {
 660                 num_pages = 6;
 661                 bank->size = 512 * 1024; /* bytes */
 662                 bank->num_sectors = num_pages;
 663
 664                 bank->sectors[5].offset = 0x60000;
 665                 bank->sectors[5].size = 128 * 1024;
 666                 bank->sectors[5].is_erased = -1;
 667                 bank->sectors[5].is_protected = -1;
 668         }
 669
 670         fm3_info->probed = 1;
 671
 672         return ERROR_OK;
 673 }
 674
 675 static int fm3_auto_probe(struct flash_bank *bank)
 676 {
 677         struct fm3_flash_bank *fm3_info = bank->driver_priv;
 678         if (fm3_info->probed)
 679                 return ERROR_OK;
 680         return fm3_probe(bank);
 681 }
 682
 683 static int fm3_info(struct flash_bank *bank, char *buf, int buf_size)
 684 {
 685         snprintf(buf, buf_size, "Fujitsu fm3 Device does not support Chip-ID (Type unknown)");
 686         return ERROR_OK;
 687 }
 688
 689 /* Chip erase */
 690 static int fm3_chip_erase(struct flash_bank *bank)
 691 {
 692         struct target *target = bank->target;
 693         struct fm3_flash_bank *fm3_info2 = bank->driver_priv;
 694         int retval = ERROR_OK;
 695         uint32_t u32DummyRead;
 696         uint32_t u32FlashType;
 697         uint32_t u32FlashSeqAddress1;
 698         uint32_t u32FlashSeqAddress2;
 699
 700         u32FlashType = (uint32_t) fm3_info2->flashtype;
 701
 702         if (u32FlashType == fm3_flash_type1) {
 703                 LOG_INFO("*** Erasing mb9bfxxx type");
 704                 u32FlashSeqAddress1 = 0x00001550;
 705                 u32FlashSeqAddress2 = 0x00000AA8;
 706         } else if (u32FlashType == fm3_flash_type2) {
 707                 LOG_INFO("*** Erasing mb9afxxx type");
 708                 u32FlashSeqAddress1 = 0x00000AA8;
 709                 u32FlashSeqAddress2 = 0x00000554;
 710         } else {
 711                 LOG_ERROR("Flash/Device type unknown!");
 712                 return ERROR_FLASH_OPERATION_FAILED;
 713         }
 714
 715         if (target->state != TARGET_HALTED) {
 716                 LOG_ERROR("Target not halted");
 717                 return ERROR_TARGET_NOT_HALTED;
 718         }
 719
 720         LOG_INFO("Fujitsu MB9[AB]xxx: Chip Erase ... (may take several seconds)");
 721
 722         /* Implement Flash chip erase (mass erase) completely on host */
 723
 724         /* FASZR = 0x01, Enables CPU Programming Mode (16-bit Flash access) */
 725         retval = target_write_u32(target, 0x40000000, 0x0001);
 726         if (retval != ERROR_OK)
 727                 return retval;
 728
 729         /* dummy read of FASZR */
 730         retval = target_read_u32(target, 0x40000000, &u32DummyRead);
 731         if (retval != ERROR_OK)
 732                 return retval;
 733
 734         /* Flash unlock sequence */
 735         retval = target_write_u16(target, u32FlashSeqAddress1, 0x00AA);
 736         if (retval != ERROR_OK)
 737                 return retval;
 738
 739         retval = target_write_u16(target, u32FlashSeqAddress2, 0x0055);
 740         if (retval != ERROR_OK)
 741                 return retval;
 742
 743         retval = target_write_u16(target, u32FlashSeqAddress1, 0x0080);
 744         if (retval != ERROR_OK)
 745                 return retval;
 746
 747         retval = target_write_u16(target, u32FlashSeqAddress1, 0x00AA);
 748         if (retval != ERROR_OK)
 749                 return retval;
 750
 751         retval = target_write_u16(target, u32FlashSeqAddress2, 0x0055);
 752         if (retval != ERROR_OK)
 753                 return retval;
 754
 755         /* Chip Erase command (0x0010) */
 756         retval = target_write_u16(target, u32FlashSeqAddress1, 0x0010);
 757         if (retval != ERROR_OK)
 758                 return retval;
 759
 760         retval = fm3_busy_wait(target, u32FlashSeqAddress2, 20000);     /* 20s timeout */
 761         if (retval != ERROR_OK)
 762                 return retval;
 763
 764         /* FASZR = 0x02, Re-enables CPU Run Mode (32-bit Flash access) */
 765         retval = target_write_u32(target, 0x40000000, 0x0002);
 766         if (retval != ERROR_OK)
 767                 return retval;
 768
 769         retval = target_read_u32(target, 0x40000000, &u32DummyRead); /* dummy read of FASZR */
 770
 771         return retval;
 772 }
 773
 774 COMMAND_HANDLER(fm3_handle_chip_erase_command)
 775 {
 776         int i;
 777
 778         if (CMD_ARGC < 1)
 779                 return ERROR_COMMAND_SYNTAX_ERROR;
 780
 781         struct flash_bank *bank;
 782         int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
 783         if (ERROR_OK != retval)
 784                 return retval;
 785
 786         if (fm3_chip_erase(bank) == ERROR_OK) {
 787                 /* set all sectors as erased */
 788                 for (i = 0; i < bank->num_sectors; i++)
 789                         bank->sectors[i].is_erased = 1;
 790
 791                 command_print(CMD_CTX, "fm3 chip erase complete");
 792         } else {
 793                 command_print(CMD_CTX, "fm3 chip erase failed");
 794         }
 795
 796         return ERROR_OK;
 797 }
 798
 799 static const struct command_registration fm3_exec_command_handlers[] = {
 800         {
 801                 .name = "chip_erase",
 802                 .usage = "<bank>",
 803                 .handler = fm3_handle_chip_erase_command,
 804                 .mode = COMMAND_EXEC,
 805                 .help = "Erase entire Flash device.",
 806         },
 807         COMMAND_REGISTRATION_DONE
 808 };
 809
 810 static const struct command_registration fm3_command_handlers[] = {
 811         {
 812                 .name = "fm3",
 813                 .mode = COMMAND_ANY,
 814                 .help = "fm3 Flash command group",
 815                 .usage = "",
 816                 .chain = fm3_exec_command_handlers,
 817         },
 818         COMMAND_REGISTRATION_DONE
 819 };
 820
 821 struct flash_driver fm3_flash = {
 822         .name = "fm3",
 823         .commands = fm3_command_handlers,
 824         .flash_bank_command = fm3_flash_bank_command,
 825         .erase = fm3_erase,
 826         .write = fm3_write_block,
 827         .probe = fm3_probe,
 828         .auto_probe = fm3_auto_probe,
 829         .erase_check = default_flash_mem_blank_check,
 830         .info = fm3_info,
 831 };