]> git.sur5r.net Git - openocd/blob - src/target/mips32_pracc.c
0ee5715dd299811cbc7d991053068f709f9635d6
[openocd] / src / target / mips32_pracc.c
1 /***************************************************************************
2  *   Copyright (C) 2008 by Spencer Oliver                                  *
3  *   spen@spen-soft.co.uk                                                  *
4  *                                                                         *
5  *   Copyright (C) 2008 by David T.L. Wong                                 *
6  *                                                                         *
7  *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
8  *                                                                         *
9  *   This program is free software; you can redistribute it and/or modify  *
10  *   it under the terms of the GNU General Public License as published by  *
11  *   the Free Software Foundation; either version 2 of the License, or     *
12  *   (at your option) any later version.                                   *
13  *                                                                         *
14  *   This program is distributed in the hope that it will be useful,       *
15  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
16  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
17  *   GNU General Public License for more details.                          *
18  *                                                                         *
19  *   You should have received a copy of the GNU General Public License     *
20  *   along with this program; if not, write to the                         *
21  *   Free Software Foundation, Inc.,                                       *
22  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
23  ***************************************************************************/
24
25 /*
26 This version has optimized assembly routines for 32 bit operations:
27 - read word
28 - write word
29 - write array of words
30
31 One thing to be aware of is that the MIPS32 cpu will execute the
32 instruction after a branch instruction (one delay slot).
33
34 For example:
35
36     LW $2, ($5 +10)
37     B foo
38     LW $1, ($2 +100)
39
40 The LW $1, ($2 +100) instruction is also executed. If this is
41 not wanted a NOP can be inserted:
42
43     LW $2, ($5 +10)
44     B foo
45     NOP
46     LW $1, ($2 +100)
47
48 or the code can be changed to:
49
50     B foo
51     LW $2, ($5 +10)
52     LW $1, ($2 +100)
53
54 The original code contained NOPs. I have removed these and moved
55 the branches.
56
57 I also moved the PRACC_STACK to 0xFF204000. This allows
58 the use of 16 bits offsets to get pointers to the input
59 and output area relative to the stack. Note that the stack
60 isn't really a stack (the stack pointer is not 'moving')
61 but a FIFO simulated in software.
62
63 These changes result in a 35% speed increase when programming an
64 external flash.
65
66 More improvement could be gained if the registers do no need
67 to be preserved but in that case the routines should be aware
68 OpenOCD is used as a flash programmer or as a debug tool.
69
70 Nico Coesel
71 */
72
73 #ifdef HAVE_CONFIG_H
74 #include "config.h"
75 #endif
76
77 #include "mips32.h"
78 #include "mips32_pracc.h"
79
80 struct mips32_pracc_context
81 {
82         uint32_t *local_iparam;
83         int num_iparam;
84         uint32_t *local_oparam;
85         int num_oparam;
86         const uint32_t *code;
87         int code_len;
88         uint32_t stack[32];
89         int stack_offset;
90         struct mips_ejtag *ejtag_info;
91 };
92
93 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info,
94                 uint32_t addr, int count, uint8_t *buf);
95 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info,
96                 uint32_t addr, int count, uint16_t *buf);
97 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info,
98                 uint32_t addr, int count, uint32_t *buf);
99 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info,
100                 uint32_t addr, uint32_t *buf);
101
102 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info,
103                 uint32_t addr, int count, uint8_t *buf);
104 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info,
105                 uint32_t addr, int count, uint16_t *buf);
106 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info,
107                 uint32_t addr, int count, uint32_t *buf);
108 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info,
109                 uint32_t addr, uint32_t *buf);
110
111 static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
112 {
113         uint32_t ejtag_ctrl;
114
115         while (1)
116         {
117                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
118                 ejtag_ctrl = ejtag_info->ejtag_ctrl;
119                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
120                 if (ejtag_ctrl & EJTAG_CTRL_PRACC)
121                         break;
122                 LOG_DEBUG("DEBUGMODULE: No memory access in progress!");
123                 return ERROR_JTAG_DEVICE_ERROR;
124         }
125
126         *ctrl = ejtag_ctrl;
127         return ERROR_OK;
128 }
129
130 static int mips32_pracc_exec_read(struct mips32_pracc_context *ctx, uint32_t address)
131 {
132         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
133         int offset;
134         uint32_t ejtag_ctrl, data;
135
136         if ((address >= MIPS32_PRACC_PARAM_IN)
137                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
138         {
139                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
140                 data = ctx->local_iparam[offset];
141         }
142         else if ((address >= MIPS32_PRACC_PARAM_OUT)
143                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
144         {
145                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
146                 data = ctx->local_oparam[offset];
147         }
148         else if ((address >= MIPS32_PRACC_TEXT)
149                 && (address <= MIPS32_PRACC_TEXT + ctx->code_len * 4))
150         {
151                 offset = (address - MIPS32_PRACC_TEXT) / 4;
152                 data = ctx->code[offset];
153         }
154         else if (address == MIPS32_PRACC_STACK)
155         {
156                 /* save to our debug stack */
157                 data = ctx->stack[--ctx->stack_offset];
158         }
159         else
160         {
161                 /* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
162                  * to start of debug vector */
163
164                 data = 0;
165                 LOG_ERROR("Error reading unexpected address 0x%8.8" PRIx32 "", address);
166                 return ERROR_JTAG_DEVICE_ERROR;
167         }
168
169         /* Send the data out */
170         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
171         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
172
173         /* Clear the access pending bit (let the processor eat!) */
174         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
175         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
176         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
177
178         jtag_add_clocks(5);
179         return jtag_execute_queue();
180 }
181
182 static int mips32_pracc_exec_write(struct mips32_pracc_context *ctx, uint32_t address)
183 {
184         uint32_t ejtag_ctrl,data;
185         int offset;
186         struct mips_ejtag *ejtag_info = ctx->ejtag_info;
187
188         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA);
189         mips_ejtag_drscan_32(ctx->ejtag_info, &data);
190
191         /* Clear access pending bit */
192         ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
193         mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL);
194         mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
195
196         jtag_add_clocks(5);
197         return jtag_execute_queue();
198
199         if ((address >= MIPS32_PRACC_PARAM_IN)
200                 && (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
201         {
202                 offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
203                 ctx->local_iparam[offset] = data;
204         }
205         else if ((address >= MIPS32_PRACC_PARAM_OUT)
206                 && (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
207         {
208                 offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
209                 ctx->local_oparam[offset] = data;
210         }
211         else if (address == MIPS32_PRACC_STACK)
212         {
213                 /* save data onto our stack */
214                 ctx->stack[ctx->stack_offset++] = data;
215         }
216         else
217         {
218                 LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
219                 return ERROR_JTAG_DEVICE_ERROR;
220         }
221
222         return ERROR_OK;
223 }
224
225 int mips32_pracc_exec(struct mips_ejtag *ejtag_info, int code_len, const uint32_t *code,
226                 int num_param_in, uint32_t *param_in, int num_param_out, uint32_t *param_out, int cycle)
227 {
228         uint32_t ejtag_ctrl;
229         uint32_t address, data;
230         struct mips32_pracc_context ctx;
231         int retval;
232         int pass = 0;
233
234         ctx.local_iparam = param_in;
235         ctx.local_oparam = param_out;
236         ctx.num_iparam = num_param_in;
237         ctx.num_oparam = num_param_out;
238         ctx.code = code;
239         ctx.code_len = code_len;
240         ctx.ejtag_info = ejtag_info;
241         ctx.stack_offset = 0;
242
243         while (1)
244         {
245                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
246                         return retval;
247
248                 address = data = 0;
249                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
250                 mips_ejtag_drscan_32(ejtag_info, &address);
251
252                 /* Check for read or write */
253                 if (ejtag_ctrl & EJTAG_CTRL_PRNW)
254                 {
255                         if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
256                                 return retval;
257                 }
258                 else
259                 {
260                         /* Check to see if its reading at the debug vector. The first pass through
261                          * the module is always read at the vector, so the first one we allow.  When
262                          * the second read from the vector occurs we are done and just exit. */
263                         if ((address == MIPS32_PRACC_TEXT) && (pass++))
264                         {
265                                 break;
266                         }
267
268                         if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
269                                 return retval;
270                 }
271
272                 if (cycle == 0)
273                         break;
274         }
275
276         /* stack sanity check */
277         if (ctx.stack_offset != 0)
278         {
279                 LOG_DEBUG("Pracc Stack not zero");
280         }
281
282         return ERROR_OK;
283 }
284
285 int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
286 {
287         switch (size)
288         {
289                 case 1:
290                         return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
291                 case 2:
292                         return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
293                 case 4:
294                         if (count == 1)
295                                 return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
296                         else
297                                 return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
298         }
299
300         return ERROR_OK;
301 }
302
303 static int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
304 {
305         static const uint32_t code[] = {
306                                                                                                                         /* start: */
307                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
308                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
309                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
310                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
311                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
312                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
313                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
314
315                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
316                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
317                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
318                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
319                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
320                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
321                                                                                                                         /* loop: */
322                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
323                 MIPS32_NOP,
324
325                 MIPS32_LW(8,0,9),                                                                       /* lw $8,0($9), Load $8 with the word @mem[$9] */
326                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
327
328                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
329                 MIPS32_ADDI(9,9,4),                                                                     /* $1 += 4 */
330                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
331
332                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
333                 MIPS32_NOP,
334                                                                                                                         /* end: */
335                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
336                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
337                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
338                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
339                 MIPS32_B(NEG16(27)),                                                            /* b start */
340                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
341         };
342
343         int retval = ERROR_OK;
344         int blocksize;
345         int bytesread;
346         uint32_t param_in[2];
347
348         bytesread = 0;
349
350         while (count > 0)
351         {
352                 blocksize = count;
353                 if (count > 0x400)
354                         blocksize = 0x400;
355
356                 param_in[0] = addr;
357                 param_in[1] = blocksize;
358
359                 if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
360                         ARRAY_SIZE(param_in), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
361                 {
362                         return retval;
363                 }
364
365                 count -= blocksize;
366                 addr += blocksize;
367                 bytesread += blocksize;
368         }
369
370         return retval;
371 }
372
373 static int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
374 {
375         static const uint32_t code[] = {
376                                                                                                                         /* start: */
377                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
378                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
379                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
380                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
381
382                 MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN),15), /* load R8 @ param_in[0] = address */
383
384                 MIPS32_LW(8,0,8),                                                                       /* lw $8,0($8), Load $8 with the word @mem[$8] */
385                 MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15), /* store R8 @ param_out[0] */
386
387                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
388                 MIPS32_B(NEG16(9)),                                                                     /* b start */
389                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
390         };
391
392         int retval = ERROR_OK;
393         uint32_t param_in[1];
394
395         param_in[0] = addr;
396
397         if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
398                 ARRAY_SIZE(param_in), param_in, 1, buf, 1)) != ERROR_OK)
399         {
400                 return retval;
401         }
402
403         return retval;
404 }
405
406 static int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
407 {
408         static const uint32_t code[] = {
409                                                                                                                         /* start: */
410                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
411                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
412                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
413                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
414                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
415                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
416                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
417
418                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
419                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
420                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
421                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
422                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
423                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
424                                                                                                                         /* loop: */
425                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
426                 MIPS32_NOP,
427
428                 MIPS32_LHU(8,0,9),                                                                      /* lw $8,0($9), Load $8 with the halfword @mem[$9] */
429                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
430
431                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
432                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
433                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
434                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
435                 MIPS32_NOP,
436                                                                                                                         /* end: */
437                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
438                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
439                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
440                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
441                 MIPS32_B(NEG16(27)),                                                            /* b start */
442                 MIPS32_MFC0(15,30,0),                                                           /* move COP0 DeSave to $15 */
443         };
444
445         /* TODO remove array */
446         uint32_t *param_out = malloc(count * sizeof(uint32_t));
447         int i;
448
449 //      int retval;
450         int blocksize;
451         int bytesread;
452         uint32_t param_in[2];
453
454         bytesread = 0;
455
456         //while (count > 0)
457         {
458                 blocksize = count;
459                 if (count > 0x400)
460                         blocksize = 0x400;
461
462                 param_in[0] = addr;
463                 param_in[1] = blocksize;
464
465                 mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
466                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
467
468 //              count -= blocksize;
469 //              addr += blocksize;
470 //              bytesread += blocksize;
471         }
472
473         for (i = 0; i < count; i++)
474         {
475                 buf[i] = param_out[i];
476         }
477
478         free(param_out);
479
480         return ERROR_OK;
481 }
482
483 static int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
484 {
485         static const uint32_t code[] = {
486                                                                                                                         /* start: */
487                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
488                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
489                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
490                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
491                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
492                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
493                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
494
495                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
496                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
497                 MIPS32_LW(9,0,8),                                                                       /* $9 = mem[$8]; read addr */
498                 MIPS32_LW(10,4,8),                                                                      /* $10 = mem[$8 + 4]; read count */
499                 MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),         /* $11 = MIPS32_PRACC_PARAM_OUT */
500                 MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
501                                                                                                                         /* loop: */
502                 MIPS32_BEQ(0,10,8),                                                                     /* beq 0, $10, end */
503                 MIPS32_NOP,
504
505                 MIPS32_LBU(8,0,9),                                                                      /* lw $8,0($9), Load t4 with the byte @mem[t1] */
506                 MIPS32_SW(8,0,11),                                                                      /* sw $8,0($11) */
507
508                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
509                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
510                 MIPS32_ADDI(11,11,4),                                                           /* $11 += 4 */
511                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
512                 MIPS32_NOP,
513                                                                                                                         /* end: */
514                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
515                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
516                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
517                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
518                 MIPS32_B(NEG16(27)),                                                            /* b start */
519                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
520         };
521
522         /* TODO remove array */
523         uint32_t *param_out = malloc(count * sizeof(uint32_t));
524         int i;
525
526 //      int retval;
527         int blocksize;
528         int bytesread;
529         uint32_t param_in[2];
530
531         bytesread = 0;
532
533 //      while (count > 0)
534         {
535                 blocksize = count;
536                 if (count > 0x400)
537                         blocksize = 0x400;
538
539                 param_in[0] = addr;
540                 param_in[1] = blocksize;
541
542                 mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
543                         ARRAY_SIZE(param_in), param_in, count, param_out, 1);
544
545 //              count -= blocksize;
546 //              addr += blocksize;
547 //              bytesread += blocksize;
548         }
549
550         for (i = 0; i < count; i++)
551         {
552                 buf[i] = param_out[i];
553         }
554
555         free(param_out);
556
557         return ERROR_OK;
558 }
559
560 int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
561 {
562         switch (size)
563         {
564                 case 1:
565                         return mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
566                 case 2:
567                         return mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
568                 case 4:
569                         if (count == 1)
570                                 return mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
571                         else
572                                 return mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
573         }
574
575         return ERROR_OK;
576 }
577
578 static int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
579 {
580         static const uint32_t code[] = {
581                                                                                                                         /* start: */
582                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
583                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
584                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
585                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
586                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
587                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
588                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
589
590                 MIPS32_ADDI(8,15,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)),  /* $8= MIPS32_PRACC_PARAM_IN */
591                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
592                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
593                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 beginning of data */
594
595                                                                                                                         /* loop: */
596                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
597                 MIPS32_SW(11,0,9),                                                                      /* sw $11,0($9) */
598
599                 MIPS32_ADDI(9,9,4),                                                                     /* $9 += 4 */
600                 MIPS32_BNE(10,9,NEG16(4)),                                                      /* bne $10, $9, loop */
601                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
602
603                                                                                                                         /* end: */
604                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
605                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
606                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
607                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
608                 MIPS32_B(NEG16(21)),                                                            /* b start */
609                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
610         };
611
612         /* TODO remove array */
613         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
614         param_in[0] = addr;
615         param_in[1] = addr + (count * sizeof(uint32_t));        /* last address */
616
617         memcpy(&param_in[2], buf, count * sizeof(uint32_t));
618
619         mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
620                 count + 2, param_in, 0, NULL, 1);
621
622         free(param_in);
623
624         return ERROR_OK;
625 }
626
627 static int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
628 {
629         static const uint32_t code[] = {
630                                                                                                                         /* start: */
631                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
632                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
633                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
634                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
635                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
636
637                 MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),   /* load R8 @ param_in[1] = data */
638                 MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),               /* load R9 @ param_in[0] = address */
639
640                 MIPS32_SW(8,0,9),                                                                       /* sw $8,0($9) */
641
642                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
643                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
644                 MIPS32_B(NEG16(11)),                                                            /* b start */
645                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
646         };
647
648         /* TODO remove array */
649         uint32_t param_in[1 + 1];
650         param_in[0] = addr;
651         param_in[1] = *buf;
652
653         mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
654                 ARRAY_SIZE(param_in), param_in, 0, NULL, 1);
655
656         return ERROR_OK;
657 }
658
659 static int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
660 {
661         static const uint32_t code[] = {
662                                                                                                                         /* start: */
663                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
664                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
665                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
666                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
667                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
668                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
669                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
670
671                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
672                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
673                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
674                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
675                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
676                                                                                                                         /* loop: */
677                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
678                 MIPS32_NOP,
679
680                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
681                 MIPS32_SH(11,0,9),                                                                      /* sh $11,0($9) */
682
683                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
684                 MIPS32_ADDI(9,9,2),                                                                     /* $9 += 2 */
685                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
686
687                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
688                 MIPS32_NOP,
689                                                                                                                         /* end: */
690                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
691                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
692                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
693                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
694                 MIPS32_B(NEG16(26)),                                                            /* b start */
695                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
696         };
697
698         /* TODO remove array */
699         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
700         int i;
701         param_in[0] = addr;
702         param_in[1] = count;
703
704         for (i = 0; i < count; i++)
705         {
706                 param_in[i + 2] = buf[i];
707         }
708
709         mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
710                 count + 2, param_in, 0, NULL, 1);
711
712         free(param_in);
713
714         return ERROR_OK;
715 }
716
717 static int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
718 {
719         static const uint32_t code[] = {
720                                                                                                                         /* start: */
721                 MIPS32_MTC0(15,31,0),                                                           /* move $15 to COP0 DeSave */
722                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),                     /* $15 = MIPS32_PRACC_STACK */
723                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
724                 MIPS32_SW(8,0,15),                                                                      /* sw $8,($15) */
725                 MIPS32_SW(9,0,15),                                                                      /* sw $9,($15) */
726                 MIPS32_SW(10,0,15),                                                                     /* sw $10,($15) */
727                 MIPS32_SW(11,0,15),                                                                     /* sw $11,($15) */
728
729                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),           /* $8 = MIPS32_PRACC_PARAM_IN */
730                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
731                 MIPS32_LW(9,0,8),                                                                       /* Load write addr to $9 */
732                 MIPS32_LW(10,4,8),                                                                      /* Load write count to $10 */
733                 MIPS32_ADDI(8,8,8),                                                                     /* $8 += 8 */
734                                                                                                                         /* loop: */
735                 MIPS32_BEQ(0,10,8),                                                                     /* beq $0, $10, end */
736                 MIPS32_NOP,
737
738                 MIPS32_LW(11,0,8),                                                                      /* lw $11,0($8), Load $11 with the word @mem[$8] */
739                 MIPS32_SB(11,0,9),                                                                      /* sb $11,0($9) */
740
741                 MIPS32_ADDI(10,10,NEG16(1)),                                            /* $10-- */
742                 MIPS32_ADDI(9,9,1),                                                                     /* $9 += 1 */
743                 MIPS32_ADDI(8,8,4),                                                                     /* $8 += 4 */
744
745                 MIPS32_B(NEG16(8)),                                                                     /* b loop */
746                 MIPS32_NOP,
747                                                                                                                         /* end: */
748                 MIPS32_LW(11,0,15),                                                                     /* lw $11,($15) */
749                 MIPS32_LW(10,0,15),                                                                     /* lw $10,($15) */
750                 MIPS32_LW(9,0,15),                                                                      /* lw $9,($15) */
751                 MIPS32_LW(8,0,15),                                                                      /* lw $8,($15) */
752                 MIPS32_B(NEG16(26)),                                                            /* b start */
753                 MIPS32_MFC0(15,31,0),                                                           /* move COP0 DeSave to $15 */
754         };
755
756         /* TODO remove array */
757         uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
758         int retval;
759         int i;
760         param_in[0] = addr;
761         param_in[1] = count;
762
763         for (i = 0; i < count; i++)
764         {
765                 param_in[i + 2] = buf[i];
766         }
767
768         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
769                 count + 2, param_in, 0, NULL, 1);
770
771         free(param_in);
772
773         return retval;
774 }
775
776 int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
777 {
778         static const uint32_t code[] = {
779                                                                                                                 /* start: */
780                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $2 = MIPS32_PRACC_PARAM_IN */
781                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_IN)),
782                 MIPS32_LW(1,1*4,2),                                                             /* lw $1,1*4($2) */
783                 MIPS32_LW(15,15*4,2),                                                   /* lw $15,15*4($2) */
784                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
785                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
786                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
787                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
788                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_IN)),   /* $1 = MIPS32_PRACC_PARAM_IN */
789                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_IN)),
790                 MIPS32_LW(3,3*4,1),                                                             /* lw $3,3*4($1) */
791                 MIPS32_LW(4,4*4,1),                                                             /* lw $4,4*4($1) */
792                 MIPS32_LW(5,5*4,1),                                                             /* lw $5,5*4($1) */
793                 MIPS32_LW(6,6*4,1),                                                             /* lw $6,6*4($1) */
794                 MIPS32_LW(7,7*4,1),                                                             /* lw $7,7*4($1) */
795                 MIPS32_LW(8,8*4,1),                                                             /* lw $8,8*4($1) */
796                 MIPS32_LW(9,9*4,1),                                                             /* lw $9,9*4($1) */
797                 MIPS32_LW(10,10*4,1),                                                   /* lw $10,10*4($1) */
798                 MIPS32_LW(11,11*4,1),                                                   /* lw $11,11*4($1) */
799                 MIPS32_LW(12,12*4,1),                                                   /* lw $12,12*4($1) */
800                 MIPS32_LW(13,13*4,1),                                                   /* lw $13,13*4($1) */
801                 MIPS32_LW(14,14*4,1),                                                   /* lw $14,14*4($1) */
802                 MIPS32_LW(16,16*4,1),                                                   /* lw $16,16*4($1) */
803                 MIPS32_LW(17,17*4,1),                                                   /* lw $17,17*4($1) */
804                 MIPS32_LW(18,18*4,1),                                                   /* lw $18,18*4($1) */
805                 MIPS32_LW(19,19*4,1),                                                   /* lw $19,19*4($1) */
806                 MIPS32_LW(20,20*4,1),                                                   /* lw $20,20*4($1) */
807                 MIPS32_LW(21,21*4,1),                                                   /* lw $21,21*4($1) */
808                 MIPS32_LW(22,22*4,1),                                                   /* lw $22,22*4($1) */
809                 MIPS32_LW(23,23*4,1),                                                   /* lw $23,23*4($1) */
810                 MIPS32_LW(24,24*4,1),                                                   /* lw $24,24*4($1) */
811                 MIPS32_LW(25,25*4,1),                                                   /* lw $25,25*4($1) */
812                 MIPS32_LW(26,26*4,1),                                                   /* lw $26,26*4($1) */
813                 MIPS32_LW(27,27*4,1),                                                   /* lw $27,27*4($1) */
814                 MIPS32_LW(28,28*4,1),                                                   /* lw $28,28*4($1) */
815                 MIPS32_LW(29,29*4,1),                                                   /* lw $29,29*4($1) */
816                 MIPS32_LW(30,30*4,1),                                                   /* lw $30,30*4($1) */
817                 MIPS32_LW(31,31*4,1),                                                   /* lw $31,31*4($1) */
818
819                 MIPS32_LW(2,32*4,1),                                                    /* lw $2,32*4($1) */
820                 MIPS32_MTC0(2,12,0),                                                    /* move $2 to status */
821                 MIPS32_LW(2,33*4,1),                                                    /* lw $2,33*4($1) */
822                 MIPS32_MTLO(2),                                                                 /* move $2 to lo */
823                 MIPS32_LW(2,34*4,1),                                                    /* lw $2,34*4($1) */
824                 MIPS32_MTHI(2),                                                                 /* move $2 to hi */
825                 MIPS32_LW(2,35*4,1),                                                    /* lw $2,35*4($1) */
826                 MIPS32_MTC0(2,8,0),                                                             /* move $2 to badvaddr */
827                 MIPS32_LW(2,36*4,1),                                                    /* lw $2,36*4($1) */
828                 MIPS32_MTC0(2,13,0),                                                    /* move $2 to cause*/
829                 MIPS32_LW(2,37*4,1),                                                    /* lw $2,37*4($1) */
830                 MIPS32_MTC0(2,24,0),                                                    /* move $2 to depc (pc) */
831
832                 MIPS32_LW(2,2*4,1),                                                             /* lw $2,2*4($1) */
833                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
834                 MIPS32_B(NEG16(53)),                                                    /* b start */
835                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
836         };
837
838         int retval;
839
840         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
841                         MIPS32NUMCOREREGS, regs, 0, NULL, 1);
842
843         return retval;
844 }
845
846 int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
847 {
848         static const uint32_t code[] = {
849                                                                                                                 /* start: */
850                 MIPS32_MTC0(2,31,0),                                                    /* move $2 to COP0 DeSave */
851                 MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $2 = MIPS32_PRACC_PARAM_OUT */
852                 MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_OUT)),
853                 MIPS32_SW(0,0*4,2),                                                             /* sw $0,0*4($2) */
854                 MIPS32_SW(1,1*4,2),                                                             /* sw $1,1*4($2) */
855                 MIPS32_SW(15,15*4,2),                                                   /* sw $15,15*4($2) */
856                 MIPS32_MFC0(2,31,0),                                                    /* move COP0 DeSave to $2 */
857                 MIPS32_MTC0(15,31,0),                                                   /* move $15 to COP0 DeSave */
858                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)),             /* $15 = MIPS32_PRACC_STACK */
859                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
860                 MIPS32_SW(1,0,15),                                                              /* sw $1,($15) */
861                 MIPS32_SW(2,0,15),                                                              /* sw $2,($15) */
862                 MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_OUT)),  /* $1 = MIPS32_PRACC_PARAM_OUT */
863                 MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_OUT)),
864                 MIPS32_SW(2,2*4,1),                                                             /* sw $2,2*4($1) */
865                 MIPS32_SW(3,3*4,1),                                                             /* sw $3,3*4($1) */
866                 MIPS32_SW(4,4*4,1),                                                             /* sw $4,4*4($1) */
867                 MIPS32_SW(5,5*4,1),                                                             /* sw $5,5*4($1) */
868                 MIPS32_SW(6,6*4,1),                                                             /* sw $6,6*4($1) */
869                 MIPS32_SW(7,7*4,1),                                                             /* sw $7,7*4($1) */
870                 MIPS32_SW(8,8*4,1),                                                             /* sw $8,8*4($1) */
871                 MIPS32_SW(9,9*4,1),                                                             /* sw $9,9*4($1) */
872                 MIPS32_SW(10,10*4,1),                                                   /* sw $10,10*4($1) */
873                 MIPS32_SW(11,11*4,1),                                                   /* sw $11,11*4($1) */
874                 MIPS32_SW(12,12*4,1),                                                   /* sw $12,12*4($1) */
875                 MIPS32_SW(13,13*4,1),                                                   /* sw $13,13*4($1) */
876                 MIPS32_SW(14,14*4,1),                                                   /* sw $14,14*4($1) */
877                 MIPS32_SW(16,16*4,1),                                                   /* sw $16,16*4($1) */
878                 MIPS32_SW(17,17*4,1),                                                   /* sw $17,17*4($1) */
879                 MIPS32_SW(18,18*4,1),                                                   /* sw $18,18*4($1) */
880                 MIPS32_SW(19,19*4,1),                                                   /* sw $19,19*4($1) */
881                 MIPS32_SW(20,20*4,1),                                                   /* sw $20,20*4($1) */
882                 MIPS32_SW(21,21*4,1),                                                   /* sw $21,21*4($1) */
883                 MIPS32_SW(22,22*4,1),                                                   /* sw $22,22*4($1) */
884                 MIPS32_SW(23,23*4,1),                                                   /* sw $23,23*4($1) */
885                 MIPS32_SW(24,24*4,1),                                                   /* sw $24,24*4($1) */
886                 MIPS32_SW(25,25*4,1),                                                   /* sw $25,25*4($1) */
887                 MIPS32_SW(26,26*4,1),                                                   /* sw $26,26*4($1) */
888                 MIPS32_SW(27,27*4,1),                                                   /* sw $27,27*4($1) */
889                 MIPS32_SW(28,28*4,1),                                                   /* sw $28,28*4($1) */
890                 MIPS32_SW(29,29*4,1),                                                   /* sw $29,29*4($1) */
891                 MIPS32_SW(30,30*4,1),                                                   /* sw $30,30*4($1) */
892                 MIPS32_SW(31,31*4,1),                                                   /* sw $31,31*4($1) */
893
894                 MIPS32_MFC0(2,12,0),                                                    /* move status to $2 */
895                 MIPS32_SW(2,32*4,1),                                                    /* sw $2,32*4($1) */
896                 MIPS32_MFLO(2),                                                                 /* move lo to $2 */
897                 MIPS32_SW(2,33*4,1),                                                    /* sw $2,33*4($1) */
898                 MIPS32_MFHI(2),                                                                 /* move hi to $2 */
899                 MIPS32_SW(2,34*4,1),                                                    /* sw $2,34*4($1) */
900                 MIPS32_MFC0(2,8,0),                                                             /* move badvaddr to $2 */
901                 MIPS32_SW(2,35*4,1),                                                    /* sw $2,35*4($1) */
902                 MIPS32_MFC0(2,13,0),                                                    /* move cause to $2 */
903                 MIPS32_SW(2,36*4,1),                                                    /* sw $2,36*4($1) */
904                 MIPS32_MFC0(2,24,0),                                                    /* move depc (pc) to $2 */
905                 MIPS32_SW(2,37*4,1),                                                    /* sw $2,37*4($1) */
906
907                 MIPS32_LW(2,0,15),                                                              /* lw $2,($15) */
908                 MIPS32_LW(1,0,15),                                                              /* lw $1,($15) */
909                 MIPS32_B(NEG16(58)),                                                    /* b start */
910                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
911         };
912
913         int retval;
914
915         retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
916                 0, NULL, MIPS32NUMCOREREGS, regs, 1);
917
918         return retval;
919 }
920
921 /* fastdata upload/download requires an initialized working area
922  * to load the download code; it should not be called otherwise
923  * fetch order from the fastdata area
924  * 1. start addr
925  * 2. end addr
926  * 3. data ...
927  */
928 int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
929                                                                 int write_t, uint32_t addr, int count, uint32_t *buf)
930 {
931         uint32_t handler_code[] = {
932                 /* caution when editing, table is modified below */
933                 /* r15 points to the start of this code */
934                 MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
935                 MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
936                 MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
937                 MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
938                 /* start of fastdata area in t0 */
939                 MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
940                 MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
941                 MIPS32_LW(9,0,8),                                                               /* start addr in t1 */
942                 MIPS32_LW(10,0,8),                                                              /* end addr to t2 */
943                                                                                                                 /* loop: */
944                 /* 8 */ MIPS32_LW(11,0,0),                                              /* lw t3,[t8 | r9] */
945                 /* 9 */ MIPS32_SW(11,0,0),                                              /* sw t3,[r9 | r8] */
946                 MIPS32_BNE(10,9,NEG16(3)),                                              /* bne $t2,t1,loop */
947                 MIPS32_ADDI(9,9,4),                                                             /* addi t1,t1,4 */
948
949                 MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
950                 MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
951                 MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
952                 MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
953
954                 MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
955                 MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
956                 MIPS32_JR(15),                                                                  /* jr start */
957                 MIPS32_MFC0(15,31,0),                                                   /* move COP0 DeSave to $15 */
958         };
959
960         uint32_t jmp_code[] = {
961                 MIPS32_MTC0(15,31,0),                   /* move $15 to COP0 DeSave */
962                 /* 1 */ MIPS32_LUI(15,0),               /* addr of working area added below */
963                 /* 2 */ MIPS32_ORI(15,15,0),    /* addr of working area added below */
964                 MIPS32_JR(15),                                  /* jump to ram program */
965                 MIPS32_NOP,
966         };
967
968         int retval, i;
969         uint32_t val, ejtag_ctrl, address;
970
971         if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
972                 return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;
973
974         if (write_t)
975         {
976                 handler_code[8] = MIPS32_LW(11,0,8);    /* load data from probe at fastdata area */
977                 handler_code[9] = MIPS32_SW(11,0,9);    /* store data to RAM @ r9 */
978         }
979         else
980         {
981                 handler_code[8] = MIPS32_LW(11,0,9);    /* load data from RAM @ r9 */
982                 handler_code[9] = MIPS32_SW(11,0,8);    /* store data to probe at fastdata area */
983         }
984
985         /* write program into RAM */
986         if (write_t != ejtag_info->fast_access_save)
987         {
988                 mips32_pracc_write_mem32(ejtag_info, source->address, ARRAY_SIZE(handler_code), handler_code);
989                 /* save previous operation to speed to any consecutive read/writes */
990                 ejtag_info->fast_access_save = write_t;
991         }
992
993         LOG_DEBUG("%s using 0x%.8" PRIx32 " for write handler", __func__, source->address);
994
995         jmp_code[1] |= UPPER16(source->address);
996         jmp_code[2] |= LOWER16(source->address);
997
998         for (i = 0; i < (int) ARRAY_SIZE(jmp_code); i++)
999         {
1000                 if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1001                         return retval;
1002
1003                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_DATA);
1004                 mips_ejtag_drscan_32(ejtag_info, &jmp_code[i]);
1005
1006                 /* Clear the access pending bit (let the processor eat!) */
1007                 ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
1008                 mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL);
1009                 mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
1010         }
1011
1012         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1013                 return retval;
1014
1015         /* next fetch to dmseg should be in FASTDATA_AREA, check */
1016         address = 0;
1017         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1018         mips_ejtag_drscan_32(ejtag_info, &address);
1019
1020         if (address != MIPS32_PRACC_FASTDATA_AREA)
1021                 return ERROR_FAIL;
1022
1023         /* Send the load start address */
1024         val = addr;
1025         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1026         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1027
1028         /* Send the load end address */
1029         val = addr + (count - 1) * 4;
1030         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_FASTDATA);
1031         mips_ejtag_fastdata_scan(ejtag_info, 1, &val);
1032
1033         for (i = 0; i < count; i++)
1034         {
1035                 /* Send the data out using fastdata (clears the access pending bit) */
1036                 if ((retval = mips_ejtag_fastdata_scan(ejtag_info, write_t, buf++)) != ERROR_OK)
1037                         return retval;
1038         }
1039
1040         if ((retval = jtag_execute_queue()) != ERROR_OK)
1041         {
1042                 LOG_ERROR("fastdata load failed");
1043                 return retval;
1044         }
1045
1046         if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
1047                 return retval;
1048
1049         address = 0;
1050         mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS);
1051         mips_ejtag_drscan_32(ejtag_info, &address);
1052
1053         if (address != MIPS32_PRACC_TEXT)
1054                 LOG_ERROR("mini program did not return to start");
1055
1056         return retval;
1057 }