flash: cleanup/reformat fm3 flash driver
[fw/openocd] / 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 };