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