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