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