mips32_pracc.c 34.2 KB
Newer Older
ntfreak's avatar
ntfreak committed
1
2
3
4
5
6
/***************************************************************************
 *   Copyright (C) 2008 by Spencer Oliver                                  *
 *   spen@spen-soft.co.uk                                                  *
 *                                                                         *
 *   Copyright (C) 2008 by David T.L. Wong                                 *
 *                                                                         *
7
8
 *   Copyright (C) 2009 by David N. Claffey <dnclaffey@gmail.com>          *
 *                                                                         *
ntfreak's avatar
ntfreak committed
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
24
25
26
27
28
29
30

/*
This version has optimized assembly routines for 32 bit operations:
- read word
- write word
- write array of words

31
One thing to be aware of is that the MIPS32 cpu will execute the
32
33
34
35
36
37
38
39
instruction after a branch instruction (one delay slot).

For example:

    LW $2, ($5 +10)
    B foo
    LW $1, ($2 +100)

40
The LW $1, ($2 +100) instruction is also executed. If this is
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
not wanted a NOP can be inserted:

    LW $2, ($5 +10)
    B foo
    NOP
    LW $1, ($2 +100)

or the code can be changed to:

    B foo
    LW $2, ($5 +10)
    LW $1, ($2 +100)

The original code contained NOPs. I have removed these and moved
the branches.

I also moved the PRACC_STACK to 0xFF204000. This allows
the use of 16 bits offsets to get pointers to the input
and output area relative to the stack. Note that the stack
isn't really a stack (the stack pointer is not 'moving')
but a FIFO simulated in software.

These changes result in a 35% speed increase when programming an
external flash.

More improvement could be gained if the registers do no need
to be preserved but in that case the routines should be aware
OpenOCD is used as a flash programmer or as a debug tool.

Nico Coesel
*/

ntfreak's avatar
ntfreak committed
73
74
75
76
77
78
79
#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include "mips32.h"
#include "mips32_pracc.h"

Zachary T Welch's avatar
Zachary T Welch committed
80
81
struct mips32_pracc_context
{
82
	uint32_t *local_iparam;
ntfreak's avatar
ntfreak committed
83
	int num_iparam;
84
	uint32_t *local_oparam;
ntfreak's avatar
ntfreak committed
85
	int num_oparam;
86
	const uint32_t *code;
ntfreak's avatar
ntfreak committed
87
	int code_len;
88
	uint32_t stack[32];
ntfreak's avatar
ntfreak committed
89
	int stack_offset;
90
	struct mips_ejtag *ejtag_info;
Zachary T Welch's avatar
Zachary T Welch committed
91
};
ntfreak's avatar
ntfreak committed
92

93
static int wait_for_pracc_rw(struct mips_ejtag *ejtag_info, uint32_t *ctrl)
ntfreak's avatar
ntfreak committed
94
{
95
	uint32_t ejtag_ctrl;
96
97

	while (1)
ntfreak's avatar
ntfreak committed
98
99
	{
		mips_ejtag_set_instr(ejtag_info, EJTAG_INST_CONTROL, NULL);
100
		ejtag_ctrl = ejtag_info->ejtag_ctrl;
ntfreak's avatar
ntfreak committed
101
102
103
104
105
106
		mips_ejtag_drscan_32(ejtag_info, &ejtag_ctrl);
		if (ejtag_ctrl & EJTAG_CTRL_PRACC)
			break;
		LOG_DEBUG("DEBUGMODULE: No memory access in progress!\n");
		return ERROR_JTAG_DEVICE_ERROR;
	}
107

ntfreak's avatar
ntfreak committed
108
109
110
111
	*ctrl = ejtag_ctrl;
	return ERROR_OK;
}

Zachary T Welch's avatar
Zachary T Welch committed
112
static int mips32_pracc_exec_read(struct mips32_pracc_context *ctx, uint32_t address)
ntfreak's avatar
ntfreak committed
113
{
114
	struct mips_ejtag *ejtag_info = ctx->ejtag_info;
ntfreak's avatar
ntfreak committed
115
	int offset;
116
	uint32_t ejtag_ctrl, data;
ntfreak's avatar
ntfreak committed
117
118
119
120
121
122
123
124
125
126
127
128
129
130

	if ((address >= MIPS32_PRACC_PARAM_IN)
		&& (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
	{
		offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
		data = ctx->local_iparam[offset];
	}
	else if ((address >= MIPS32_PRACC_PARAM_OUT)
		&& (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
	{
		offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
		data = ctx->local_oparam[offset];
	}
	else if ((address >= MIPS32_PRACC_TEXT)
Spencer Oliver's avatar
Spencer Oliver committed
131
		&& (address <= MIPS32_PRACC_TEXT + ctx->code_len * 4))
ntfreak's avatar
ntfreak committed
132
133
134
135
136
137
138
139
140
141
142
143
144
	{
		offset = (address - MIPS32_PRACC_TEXT) / 4;
		data = ctx->code[offset];
	}
	else if (address == MIPS32_PRACC_STACK)
	{
		/* save to our debug stack */
		data = ctx->stack[--ctx->stack_offset];
	}
	else
	{
		/* TODO: send JMP 0xFF200000 instruction. Hopefully processor jump back
		 * to start of debug vector */
145

ntfreak's avatar
ntfreak committed
146
		data = 0;
Spencer Oliver's avatar
Spencer Oliver committed
147
		LOG_ERROR("Error reading unexpected address 0x%8.8" PRIx32 "", address);
ntfreak's avatar
ntfreak committed
148
149
		return ERROR_JTAG_DEVICE_ERROR;
	}
150

ntfreak's avatar
ntfreak committed
151
152
153
154
155
	/* Send the data out */
	mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA, NULL);
	mips_ejtag_drscan_32(ctx->ejtag_info, &data);

	/* Clear the access pending bit (let the processor eat!) */
156
	ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
ntfreak's avatar
ntfreak committed
157
	mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL, NULL);
158
	mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
159
160
161

	jtag_add_clocks(5);
	jtag_execute_queue();
162

ntfreak's avatar
ntfreak committed
163
164
165
	return ERROR_OK;
}

Zachary T Welch's avatar
Zachary T Welch committed
166
static int mips32_pracc_exec_write(struct mips32_pracc_context *ctx, uint32_t address)
ntfreak's avatar
ntfreak committed
167
{
168
	uint32_t ejtag_ctrl,data;
ntfreak's avatar
ntfreak committed
169
	int offset;
170
	struct mips_ejtag *ejtag_info = ctx->ejtag_info;
171

ntfreak's avatar
ntfreak committed
172
173
	mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_DATA, NULL);
	mips_ejtag_drscan_32(ctx->ejtag_info, &data);
174

ntfreak's avatar
ntfreak committed
175
	/* Clear access pending bit */
176
	ejtag_ctrl = ejtag_info->ejtag_ctrl & ~EJTAG_CTRL_PRACC;
ntfreak's avatar
ntfreak committed
177
	mips_ejtag_set_instr(ctx->ejtag_info, EJTAG_INST_CONTROL, NULL);
178
	mips_ejtag_drscan_32(ctx->ejtag_info, &ejtag_ctrl);
179
180
181

	jtag_add_clocks(5);
	jtag_execute_queue();
182

ntfreak's avatar
ntfreak committed
183
184
185
186
187
188
	if ((address >= MIPS32_PRACC_PARAM_IN)
		&& (address <= MIPS32_PRACC_PARAM_IN + ctx->num_iparam * 4))
	{
		offset = (address - MIPS32_PRACC_PARAM_IN) / 4;
		ctx->local_iparam[offset] = data;
	}
189
	else if ((address >= MIPS32_PRACC_PARAM_OUT)
ntfreak's avatar
ntfreak committed
190
191
192
193
194
195
196
197
198
199
200
201
		&& (address <= MIPS32_PRACC_PARAM_OUT + ctx->num_oparam * 4))
	{
		offset = (address - MIPS32_PRACC_PARAM_OUT) / 4;
		ctx->local_oparam[offset] = data;
	}
	else if (address == MIPS32_PRACC_STACK)
	{
		/* save data onto our stack */
		ctx->stack[ctx->stack_offset++] = data;
	}
	else
	{
Spencer Oliver's avatar
Spencer Oliver committed
202
		LOG_ERROR("Error writing unexpected address 0x%8.8" PRIx32 "", address);
ntfreak's avatar
ntfreak committed
203
204
		return ERROR_JTAG_DEVICE_ERROR;
	}
205

ntfreak's avatar
ntfreak committed
206
207
208
	return ERROR_OK;
}

209
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)
ntfreak's avatar
ntfreak committed
210
{
211
212
	uint32_t ejtag_ctrl;
	uint32_t address, data;
Zachary T Welch's avatar
Zachary T Welch committed
213
	struct mips32_pracc_context ctx;
ntfreak's avatar
ntfreak committed
214
215
	int retval;
	int pass = 0;
216

ntfreak's avatar
ntfreak committed
217
218
219
220
221
222
223
224
	ctx.local_iparam = param_in;
	ctx.local_oparam = param_out;
	ctx.num_iparam = num_param_in;
	ctx.num_oparam = num_param_out;
	ctx.code = code;
	ctx.code_len = code_len;
	ctx.ejtag_info = ejtag_info;
	ctx.stack_offset = 0;
225

ntfreak's avatar
ntfreak committed
226
227
	while (1)
	{
228
		if ((retval = wait_for_pracc_rw(ejtag_info, &ejtag_ctrl)) != ERROR_OK)
ntfreak's avatar
ntfreak committed
229
			return retval;
230

ntfreak's avatar
ntfreak committed
231
232
233
		address = data = 0;
		mips_ejtag_set_instr(ejtag_info, EJTAG_INST_ADDRESS, NULL);
		mips_ejtag_drscan_32(ejtag_info, &address);
234

ntfreak's avatar
ntfreak committed
235
		/* Check for read or write */
236
		if (ejtag_ctrl & EJTAG_CTRL_PRNW)
ntfreak's avatar
ntfreak committed
237
238
239
240
241
242
243
244
245
246
247
248
249
		{
			if ((retval = mips32_pracc_exec_write(&ctx, address)) != ERROR_OK)
				return retval;
		}
		else
		{
			/* Check to see if its reading at the debug vector. The first pass through
			 * the module is always read at the vector, so the first one we allow.  When
			 * the second read from the vector occurs we are done and just exit. */
			if ((address == MIPS32_PRACC_TEXT) && (pass++))
			{
				break;
			}
250

ntfreak's avatar
ntfreak committed
251
252
253
			if ((retval = mips32_pracc_exec_read(&ctx, address)) != ERROR_OK)
				return retval;
		}
254

ntfreak's avatar
ntfreak committed
255
256
257
		if (cycle == 0)
			break;
	}
258

ntfreak's avatar
ntfreak committed
259
260
261
262
263
	/* stack sanity check */
	if (ctx.stack_offset != 0)
	{
		LOG_DEBUG("Pracc Stack not zero");
	}
264

ntfreak's avatar
ntfreak committed
265
266
267
	return ERROR_OK;
}

268
int mips32_pracc_read_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
ntfreak's avatar
ntfreak committed
269
270
271
272
{
	switch (size)
	{
		case 1:
273
			return mips32_pracc_read_mem8(ejtag_info, addr, count, (uint8_t*)buf);
ntfreak's avatar
ntfreak committed
274
		case 2:
zwelch's avatar
zwelch committed
275
			return mips32_pracc_read_mem16(ejtag_info, addr, count, (uint16_t*)buf);
ntfreak's avatar
ntfreak committed
276
		case 4:
zwelch's avatar
zwelch committed
277
			if (count == 1)
278
				return mips32_pracc_read_u32(ejtag_info, addr, (uint32_t*)buf);
279
			else
280
				return mips32_pracc_read_mem32(ejtag_info, addr, count, (uint32_t*)buf);
ntfreak's avatar
ntfreak committed
281
	}
282

ntfreak's avatar
ntfreak committed
283
284
285
	return ERROR_OK;
}

286
int mips32_pracc_read_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
287
{
288
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
289
290
291
292
293
294
295
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
296
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
297

ntfreak's avatar
ntfreak committed
298
299
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),		/* $8 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
zwelch's avatar
zwelch committed
300
		MIPS32_LW(9,0,8),									/* $9 = mem[$8]; read addr */
zwelch's avatar
zwelch committed
301
		MIPS32_LW(10,4,8),									/* $10 = mem[$8 + 4]; read count */
zwelch's avatar
zwelch committed
302
		MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)), 	/* $11 = MIPS32_PRACC_PARAM_OUT */
ntfreak's avatar
ntfreak committed
303
304
		MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
															/* loop: */
Spencer Oliver's avatar
Spencer Oliver committed
305
		MIPS32_BEQ(0,10,8),									/* beq 0, $10, end */
ntfreak's avatar
ntfreak committed
306
		MIPS32_NOP,
307

308
309
		MIPS32_LW(8,0,9), 									/* lw $8,0($9), Load $8 with the word @mem[$9] */
		MIPS32_SW(8,0,11), 									/* sw $8,0($11) */
310

ntfreak's avatar
ntfreak committed
311
		MIPS32_ADDI(10,10,NEG16(1)), 						/* $10-- */
zwelch's avatar
zwelch committed
312
313
		MIPS32_ADDI(9,9,4), 								/* $1 += 4 */
		MIPS32_ADDI(11,11,4), 								/* $11 += 4 */
314

Spencer Oliver's avatar
Spencer Oliver committed
315
		MIPS32_B(NEG16(8)),									/* b loop */
ntfreak's avatar
ntfreak committed
316
317
		MIPS32_NOP,
															/* end: */
318
319
320
321
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
ntfreak's avatar
ntfreak committed
322
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
323
		MIPS32_B(NEG16(28)),								/* b start */
ntfreak's avatar
ntfreak committed
324
325
		MIPS32_NOP,
	};
326

327
	int retval = ERROR_OK;
ntfreak's avatar
ntfreak committed
328
329
	int blocksize;
	int bytesread;
330
	uint32_t param_in[2];
331

ntfreak's avatar
ntfreak committed
332
	bytesread = 0;
333

ntfreak's avatar
ntfreak committed
334
	while (count > 0)
335
	{
ntfreak's avatar
ntfreak committed
336
337
338
		blocksize = count;
		if (count > 0x400)
			blocksize = 0x400;
339

ntfreak's avatar
ntfreak committed
340
341
		param_in[0] = addr;
		param_in[1] = blocksize;
342

Zachary T Welch's avatar
Zachary T Welch committed
343
344
		if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
			ARRAY_SIZE(param_in), param_in, blocksize, &buf[bytesread], 1)) != ERROR_OK)
oharboe's avatar
oharboe committed
345
		{
ntfreak's avatar
ntfreak committed
346
			return retval;
oharboe's avatar
oharboe committed
347
		}
348

ntfreak's avatar
ntfreak committed
349
350
351
352
353
354
355
356
		count -= blocksize;
		addr += blocksize;
		bytesread += blocksize;
	}

	return retval;
}

357
int mips32_pracc_read_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
358
{
359
	static const uint32_t code[] = {
360
361
362
363
364
365
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */

Spencer Oliver's avatar
Spencer Oliver committed
366
		MIPS32_LW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN),15), /* load R8 @ param_in[0] = address */
367
368

		MIPS32_LW(8,0,8), 									/* lw $8,0($8), Load $8 with the word @mem[$8] */
Spencer Oliver's avatar
Spencer Oliver committed
369
		MIPS32_SW(8,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_OUT),15), /* store R8 @ param_out[0] */
370
371

		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
Spencer Oliver's avatar
Spencer Oliver committed
372
373
		MIPS32_B(NEG16(9)),									/* b start */
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
374
375
376
	};

	int retval = ERROR_OK;
377
	uint32_t param_in[1];
378
379
380

	param_in[0] = addr;

Zachary T Welch's avatar
Zachary T Welch committed
381
	if ((retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code,
Spencer Oliver's avatar
Spencer Oliver committed
382
		ARRAY_SIZE(param_in), param_in, 1, buf, 1)) != ERROR_OK)
383
384
385
386
387
388
389
	{
		return retval;
	}

	return retval;
}

390
int mips32_pracc_read_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
ntfreak's avatar
ntfreak committed
391
{
392
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
393
394
395
396
397
398
399
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
400
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
401

ntfreak's avatar
ntfreak committed
402
403
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)),		/* $8 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
zwelch's avatar
zwelch committed
404
		MIPS32_LW(9,0,8),									/* $9 = mem[$8]; read addr */
zwelch's avatar
zwelch committed
405
		MIPS32_LW(10,4,8),									/* $10 = mem[$8 + 4]; read count */
zwelch's avatar
zwelch committed
406
		MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)),		/* $11 = MIPS32_PRACC_PARAM_OUT */
ntfreak's avatar
ntfreak committed
407
408
		MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
															/* loop: */
Spencer Oliver's avatar
Spencer Oliver committed
409
		MIPS32_BEQ(0,10,8), 								/* beq 0, $10, end */
ntfreak's avatar
ntfreak committed
410
		MIPS32_NOP,
411

412
413
		MIPS32_LHU(8,0,9), 									/* lw $8,0($9), Load $8 with the halfword @mem[$9] */
		MIPS32_SW(8,0,11), 									/* sw $8,0($11) */
414

ntfreak's avatar
ntfreak committed
415
		MIPS32_ADDI(10,10,NEG16(1)), 						/* $10-- */
zwelch's avatar
zwelch committed
416
417
		MIPS32_ADDI(9,9,2), 								/* $9 += 2 */
		MIPS32_ADDI(11,11,4), 								/* $11 += 4 */
Spencer Oliver's avatar
Spencer Oliver committed
418
		MIPS32_B(NEG16(8)),									/* b loop */
ntfreak's avatar
ntfreak committed
419
		MIPS32_NOP,
Spencer Oliver's avatar
Spencer Oliver committed
420
															/* end: */
421
422
423
424
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
ntfreak's avatar
ntfreak committed
425
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
426
		MIPS32_B(NEG16(28)),								/* b start */
ntfreak's avatar
ntfreak committed
427
428
429
		MIPS32_NOP,
	};

Spencer Oliver's avatar
Spencer Oliver committed
430
	/* TODO remove array */
431
	uint32_t *param_out = malloc(count * sizeof(uint32_t));
ntfreak's avatar
ntfreak committed
432
	int i;
433

ntfreak's avatar
ntfreak committed
434
435
436
//	int retval;
	int blocksize;
	int bytesread;
437
	uint32_t param_in[2];
438

ntfreak's avatar
ntfreak committed
439
	bytesread = 0;
440

ntfreak's avatar
ntfreak committed
441
	//while (count > 0)
442
	{
ntfreak's avatar
ntfreak committed
443
444
445
		blocksize = count;
		if (count > 0x400)
			blocksize = 0x400;
446

ntfreak's avatar
ntfreak committed
447
448
		param_in[0] = addr;
		param_in[1] = blocksize;
449

Zachary T Welch's avatar
Zachary T Welch committed
450
451
		mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
			ARRAY_SIZE(param_in), param_in, count, param_out, 1);
452

ntfreak's avatar
ntfreak committed
453
454
455
456
//		count -= blocksize;
//		addr += blocksize;
//		bytesread += blocksize;
	}
457

ntfreak's avatar
ntfreak committed
458
459
460
461
	for (i = 0; i < count; i++)
	{
		buf[i] = param_out[i];
	}
462

463
464
	free(param_out);

ntfreak's avatar
ntfreak committed
465
466
467
	return ERROR_OK;
}

468
int mips32_pracc_read_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
ntfreak's avatar
ntfreak committed
469
{
470
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
471
472
473
474
475
476
477
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
478
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
479

ntfreak's avatar
ntfreak committed
480
481
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)), 		/* $8 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
zwelch's avatar
zwelch committed
482
		MIPS32_LW(9,0,8), 									/* $9 = mem[$8]; read addr */
zwelch's avatar
zwelch committed
483
		MIPS32_LW(10,4,8), 									/* $10 = mem[$8 + 4]; read count */
zwelch's avatar
zwelch committed
484
		MIPS32_LUI(11,UPPER16(MIPS32_PRACC_PARAM_OUT)), 	/* $11 = MIPS32_PRACC_PARAM_OUT */
ntfreak's avatar
ntfreak committed
485
486
		MIPS32_ORI(11,11,LOWER16(MIPS32_PRACC_PARAM_OUT)),
															/* loop: */
Spencer Oliver's avatar
Spencer Oliver committed
487
		MIPS32_BEQ(0,10,8), 								/* beq 0, $10, end */
ntfreak's avatar
ntfreak committed
488
		MIPS32_NOP,
489

490
491
		MIPS32_LBU(8,0,9), 									/* lw $8,0($9), Load t4 with the byte @mem[t1] */
		MIPS32_SW(8,0,11), 									/* sw $8,0($11) */
492

ntfreak's avatar
ntfreak committed
493
		MIPS32_ADDI(10,10,NEG16(1)), 						/* $10-- */
zwelch's avatar
zwelch committed
494
495
		MIPS32_ADDI(9,9,1), 								/* $9 += 1 */
		MIPS32_ADDI(11,11,4), 								/* $11 += 4 */
Spencer Oliver's avatar
Spencer Oliver committed
496
		MIPS32_B(NEG16(8)),									/* b loop */
ntfreak's avatar
ntfreak committed
497
498
		MIPS32_NOP,
															/* end: */
499
500
501
502
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
ntfreak's avatar
ntfreak committed
503
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
504
		MIPS32_B(NEG16(28)),								/* b start */
ntfreak's avatar
ntfreak committed
505
506
		MIPS32_NOP,
	};
507

Spencer Oliver's avatar
Spencer Oliver committed
508
	/* TODO remove array */
509
	uint32_t *param_out = malloc(count * sizeof(uint32_t));
ntfreak's avatar
ntfreak committed
510
	int i;
511

ntfreak's avatar
ntfreak committed
512
513
514
//	int retval;
	int blocksize;
	int bytesread;
515
	uint32_t param_in[2];
516

ntfreak's avatar
ntfreak committed
517
	bytesread = 0;
518

ntfreak's avatar
ntfreak committed
519
//	while (count > 0)
520
	{
ntfreak's avatar
ntfreak committed
521
522
523
		blocksize = count;
		if (count > 0x400)
			blocksize = 0x400;
524

ntfreak's avatar
ntfreak committed
525
526
		param_in[0] = addr;
		param_in[1] = blocksize;
527

Zachary T Welch's avatar
Zachary T Welch committed
528
529
		mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
			ARRAY_SIZE(param_in), param_in, count, param_out, 1);
530

ntfreak's avatar
ntfreak committed
531
532
533
534
//		count -= blocksize;
//		addr += blocksize;
//		bytesread += blocksize;
	}
535

ntfreak's avatar
ntfreak committed
536
537
538
539
540
	for (i = 0; i < count; i++)
	{
		buf[i] = param_out[i];
	}

541
542
	free(param_out);

ntfreak's avatar
ntfreak committed
543
544
545
	return ERROR_OK;
}

546
int mips32_pracc_write_mem(struct mips_ejtag *ejtag_info, uint32_t addr, int size, int count, void *buf)
ntfreak's avatar
ntfreak committed
547
548
549
550
{
	switch (size)
	{
		case 1:
551
			return mips32_pracc_write_mem8(ejtag_info, addr, count, (uint8_t*)buf);
ntfreak's avatar
ntfreak committed
552
		case 2:
zwelch's avatar
zwelch committed
553
			return mips32_pracc_write_mem16(ejtag_info, addr, count,(uint16_t*)buf);
ntfreak's avatar
ntfreak committed
554
		case 4:
zwelch's avatar
zwelch committed
555
			if (count == 1)
556
				return mips32_pracc_write_u32(ejtag_info, addr, (uint32_t*)buf);
557
			else
558
				return mips32_pracc_write_mem32(ejtag_info, addr, count, (uint32_t*)buf);
ntfreak's avatar
ntfreak committed
559
	}
560

ntfreak's avatar
ntfreak committed
561
562
563
	return ERROR_OK;
}

564
int mips32_pracc_write_mem32(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint32_t *buf)
ntfreak's avatar
ntfreak committed
565
{
566
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
567
568
569
570
571
572
573
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
574
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
575

Spencer Oliver's avatar
Spencer Oliver committed
576
		MIPS32_ADDI(8,15,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)),  /* $8= MIPS32_PRACC_PARAM_IN */
ntfreak's avatar
ntfreak committed
577
		MIPS32_LW(9,0,8), 									/* Load write addr to $9 */
Spencer Oliver's avatar
Spencer Oliver committed
578
579
		MIPS32_LW(10,4,8),									/* Load write count to $10 */
		MIPS32_ADDI(8,8,8),									/* $8 += 8 beginning of data */
580

Spencer Oliver's avatar
Spencer Oliver committed
581
															/* loop: */
ntfreak's avatar
ntfreak committed
582
583
		MIPS32_LW(11,0,8), 									/* lw $11,0($8), Load $11 with the word @mem[$8] */
		MIPS32_SW(11,0,9), 									/* sw $11,0($9) */
584

zwelch's avatar
zwelch committed
585
		MIPS32_ADDI(9,9,4), 								/* $9 += 4 */
Spencer Oliver's avatar
Spencer Oliver committed
586
587
588
		MIPS32_BNE(10,9,NEG16(4)),							/* bne $10, $9, loop */
		MIPS32_ADDI(8,8,4),									/* $8 += 4 */

ntfreak's avatar
ntfreak committed
589
															/* end: */
590
591
592
593
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
Spencer Oliver's avatar
Spencer Oliver committed
594
		MIPS32_B(NEG16(21)),								/* b start */
ntfreak's avatar
ntfreak committed
595
596
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
	};
597

ntfreak's avatar
ntfreak committed
598
	/* TODO remove array */
599
	uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
ntfreak's avatar
ntfreak committed
600
	param_in[0] = addr;
Spencer Oliver's avatar
Spencer Oliver committed
601
	param_in[1] = addr + (count * sizeof(uint32_t));	/* last address */
602

603
	memcpy(&param_in[2], buf, count * sizeof(uint32_t));
604

Zachary T Welch's avatar
Zachary T Welch committed
605
	mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
606
		count + 2, param_in, 0, NULL, 1);
ntfreak's avatar
ntfreak committed
607

608
609
	free(param_in);

ntfreak's avatar
ntfreak committed
610
611
612
	return ERROR_OK;
}

613
int mips32_pracc_write_u32(struct mips_ejtag *ejtag_info, uint32_t addr, uint32_t *buf)
614
{
615
	static const uint32_t code[] = {
616
617
618
619
620
621
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
622

Spencer Oliver's avatar
Spencer Oliver committed
623
624
		MIPS32_LW(8,NEG16((MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN)-4), 15),	/* load R8 @ param_in[1] = data */
		MIPS32_LW(9,NEG16(MIPS32_PRACC_STACK-MIPS32_PRACC_PARAM_IN), 15),		/* load R9 @ param_in[0] = address */
625
626
627
628
629

		MIPS32_SW(8,0,9), 									/* sw $8,0($9) */

		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
630
		MIPS32_B(NEG16(11)),								/* b start */
Spencer Oliver's avatar
Spencer Oliver committed
631
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
632
633
634
	};

	/* TODO remove array */
zwelch's avatar
zwelch committed
635
	uint32_t param_in[1 + 1];
636
637
638
	param_in[0] = addr;
	param_in[1] = *buf;

Zachary T Welch's avatar
Zachary T Welch committed
639
	mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
Spencer Oliver's avatar
Spencer Oliver committed
640
		ARRAY_SIZE(param_in), param_in, 0, NULL, 1);
641
642
643
644

	return ERROR_OK;
}

645
int mips32_pracc_write_mem16(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint16_t *buf)
ntfreak's avatar
ntfreak committed
646
{
647
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
648
649
650
651
652
653
654
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
655
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
656

ntfreak's avatar
ntfreak committed
657
658
659
660
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)), 		/* $8 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
		MIPS32_LW(9,0,8), 									/* Load write addr to $9 */
		MIPS32_LW(10,4,8), 									/* Load write count to $10 */
zwelch's avatar
zwelch committed
661
		MIPS32_ADDI(8,8,8), 								/* $8 += 8 */
ntfreak's avatar
ntfreak committed
662
															/* loop: */
Spencer Oliver's avatar
Spencer Oliver committed
663
		MIPS32_BEQ(0,10,8),									/* beq $0, $10, end */
ntfreak's avatar
ntfreak committed
664
		MIPS32_NOP,
665

ntfreak's avatar
ntfreak committed
666
667
		MIPS32_LW(11,0,8), 									/* lw $11,0($8), Load $11 with the word @mem[$8] */
		MIPS32_SH(11,0,9), 									/* sh $11,0($9) */
668

ntfreak's avatar
ntfreak committed
669
		MIPS32_ADDI(10,10,NEG16(1)), 						/* $10-- */
zwelch's avatar
zwelch committed
670
671
		MIPS32_ADDI(9,9,2), 								/* $9 += 2 */
		MIPS32_ADDI(8,8,4), 								/* $8 += 4 */
672

Spencer Oliver's avatar
Spencer Oliver committed
673
		MIPS32_B(NEG16(8)),									/* b loop */
ntfreak's avatar
ntfreak committed
674
675
		MIPS32_NOP,
															/* end: */
676
677
678
679
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
ntfreak's avatar
ntfreak committed
680
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
681
		MIPS32_B(NEG16(27)),								/* b start */
ntfreak's avatar
ntfreak committed
682
683
		MIPS32_NOP,
	};
684

ntfreak's avatar
ntfreak committed
685
	/* TODO remove array */
686
	uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
ntfreak's avatar
ntfreak committed
687
688
689
	int i;
	param_in[0] = addr;
	param_in[1] = count;
690

ntfreak's avatar
ntfreak committed
691
692
	for (i = 0; i < count; i++)
	{
zwelch's avatar
zwelch committed
693
		param_in[i + 2] = buf[i];
ntfreak's avatar
ntfreak committed
694
	}
695

Zachary T Welch's avatar
Zachary T Welch committed
696
	mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
697
		count + 2, param_in, 0, NULL, 1);
ntfreak's avatar
ntfreak committed
698

699
700
	free(param_in);

ntfreak's avatar
ntfreak committed
701
702
703
	return ERROR_OK;
}

704
int mips32_pracc_write_mem8(struct mips_ejtag *ejtag_info, uint32_t addr, int count, uint8_t *buf)
ntfreak's avatar
ntfreak committed
705
{
706
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
707
708
709
710
711
712
713
															/* start: */
		MIPS32_MTC0(15,31,0),								/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 		/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(8,0,15), 									/* sw $8,($15) */
		MIPS32_SW(9,0,15), 									/* sw $9,($15) */
		MIPS32_SW(10,0,15), 								/* sw $10,($15) */
ntfreak's avatar
ntfreak committed
714
		MIPS32_SW(11,0,15), 								/* sw $11,($15) */
715

ntfreak's avatar
ntfreak committed
716
717
718
719
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_PARAM_IN)), 		/* $8 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_PARAM_IN)),
		MIPS32_LW(9,0,8), 									/* Load write addr to $9 */
		MIPS32_LW(10,4,8), 									/* Load write count to $10 */
zwelch's avatar
zwelch committed
720
		MIPS32_ADDI(8,8,8), 								/* $8 += 8 */
ntfreak's avatar
ntfreak committed
721
															/* loop: */
Spencer Oliver's avatar
Spencer Oliver committed
722
		MIPS32_BEQ(0,10,8),									/* beq $0, $10, end */
ntfreak's avatar
ntfreak committed
723
		MIPS32_NOP,
724

ntfreak's avatar
ntfreak committed
725
726
		MIPS32_LW(11,0,8), 									/* lw $11,0($8), Load $11 with the word @mem[$8] */
		MIPS32_SB(11,0,9), 									/* sb $11,0($9) */
727

ntfreak's avatar
ntfreak committed
728
		MIPS32_ADDI(10,10,NEG16(1)), 						/* $10-- */
zwelch's avatar
zwelch committed
729
730
		MIPS32_ADDI(9,9,1), 								/* $9 += 1 */
		MIPS32_ADDI(8,8,4), 								/* $8 += 4 */
731

Spencer Oliver's avatar
Spencer Oliver committed
732
		MIPS32_B(NEG16(8)),									/* b loop */
ntfreak's avatar
ntfreak committed
733
734
		MIPS32_NOP,
															/* end: */
735
736
737
738
		MIPS32_LW(11,0,15), 								/* lw $11,($15) */
		MIPS32_LW(10,0,15), 								/* lw $10,($15) */
		MIPS32_LW(9,0,15), 									/* lw $9,($15) */
		MIPS32_LW(8,0,15), 									/* lw $8,($15) */
ntfreak's avatar
ntfreak committed
739
		MIPS32_MFC0(15,31,0),								/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
740
		MIPS32_B(NEG16(27)),								/* b start */
ntfreak's avatar
ntfreak committed
741
742
		MIPS32_NOP,
	};
743

ntfreak's avatar
ntfreak committed
744
	/* TODO remove array */
745
	uint32_t *param_in = malloc((count + 2) * sizeof(uint32_t));
ntfreak's avatar
ntfreak committed
746
747
748
749
	int retval;
	int i;
	param_in[0] = addr;
	param_in[1] = count;
750

ntfreak's avatar
ntfreak committed
751
752
	for (i = 0; i < count; i++)
	{
zwelch's avatar
zwelch committed
753
		param_in[i + 2] = buf[i];
ntfreak's avatar
ntfreak committed
754
	}
755

Zachary T Welch's avatar
Zachary T Welch committed
756
	retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
Spencer Oliver's avatar
Spencer Oliver committed
757
		count + 2, param_in, 0, NULL, 1);
ntfreak's avatar
ntfreak committed
758

759
760
	free(param_in);

ntfreak's avatar
ntfreak committed
761
762
763
	return retval;
}

764
int mips32_pracc_write_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
ntfreak's avatar
ntfreak committed
765
{
766
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
767
768
769
770
														/* start: */
		MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_IN)), 	/* $2 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_IN)),
		MIPS32_LW(1,1*4,2), 							/* lw $1,1*4($2) */
771
772
773
774
775
		MIPS32_LW(15,15*4,2), 							/* lw $15,15*4($2) */
		MIPS32_MTC0(15,31,0),							/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 	/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(1,0,15), 								/* sw $1,($15) */
ntfreak's avatar
ntfreak committed
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
		MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_IN)), 	/* $1 = MIPS32_PRACC_PARAM_IN */
		MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_IN)),
		MIPS32_LW(3,3*4,1), 							/* lw $3,3*4($1) */
		MIPS32_LW(4,4*4,1), 							/* lw $4,4*4($1) */
		MIPS32_LW(5,5*4,1), 							/* lw $5,5*4($1) */
		MIPS32_LW(6,6*4,1), 							/* lw $6,6*4($1) */
		MIPS32_LW(7,7*4,1), 							/* lw $7,7*4($1) */
		MIPS32_LW(8,8*4,1), 							/* lw $8,8*4($1) */
		MIPS32_LW(9,9*4,1), 							/* lw $9,9*4($1) */
		MIPS32_LW(10,10*4,1), 							/* lw $10,10*4($1) */
		MIPS32_LW(11,11*4,1), 							/* lw $11,11*4($1) */
		MIPS32_LW(12,12*4,1), 							/* lw $12,12*4($1) */
		MIPS32_LW(13,13*4,1), 							/* lw $13,13*4($1) */
		MIPS32_LW(14,14*4,1), 							/* lw $14,14*4($1) */
		MIPS32_LW(16,16*4,1), 							/* lw $16,16*4($1) */
		MIPS32_LW(17,17*4,1), 							/* lw $17,17*4($1) */
		MIPS32_LW(18,18*4,1), 							/* lw $18,18*4($1) */
		MIPS32_LW(19,19*4,1), 							/* lw $19,19*4($1) */
		MIPS32_LW(20,20*4,1), 							/* lw $20,20*4($1) */
		MIPS32_LW(21,21*4,1), 							/* lw $21,21*4($1) */
		MIPS32_LW(22,22*4,1), 							/* lw $22,22*4($1) */
		MIPS32_LW(23,23*4,1), 							/* lw $23,23*4($1) */
		MIPS32_LW(24,24*4,1), 							/* lw $24,24*4($1) */
		MIPS32_LW(25,25*4,1), 							/* lw $25,25*4($1) */
		MIPS32_LW(26,26*4,1), 							/* lw $26,26*4($1) */
		MIPS32_LW(27,27*4,1), 							/* lw $27,27*4($1) */
		MIPS32_LW(28,28*4,1), 							/* lw $28,28*4($1) */
		MIPS32_LW(29,29*4,1), 							/* lw $29,29*4($1) */
		MIPS32_LW(30,30*4,1), 							/* lw $30,30*4($1) */
		MIPS32_LW(31,31*4,1), 							/* lw $31,31*4($1) */
806

807
808
809
810
811
812
813
814
815
816
817
818
		MIPS32_LW(2,32*4,1), 							/* lw $2,32*4($1) */
		MIPS32_MTC0(2,12,0),							/* move $2 to status */
		MIPS32_LW(2,33*4,1), 							/* lw $2,33*4($1) */
		MIPS32_MTLO(2),									/* move $2 to lo */
		MIPS32_LW(2,34*4,1), 							/* lw $2,34*4($1) */
		MIPS32_MTHI(2),									/* move $2 to hi */
		MIPS32_LW(2,35*4,1), 							/* lw $2,35*4($1) */
		MIPS32_MTC0(2,8,0),								/* move $2 to badvaddr */
		MIPS32_LW(2,36*4,1), 							/* lw $2,36*4($1) */
		MIPS32_MTC0(2,13,0),							/* move $2 to cause*/
		MIPS32_LW(2,37*4,1), 							/* lw $2,37*4($1) */
		MIPS32_MTC0(2,24,0),							/* move $2 to pc */
819

820
821
822
		MIPS32_LW(2,2*4,1), 							/* lw $2,2*4($1) */
		MIPS32_LW(1,0,15), 								/* lw $1,($15) */
		MIPS32_MFC0(15,31,0),							/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
823
		MIPS32_B(NEG16(54)),							/* b start */
ntfreak's avatar
ntfreak committed
824
825
		MIPS32_NOP,
	};
826

ntfreak's avatar
ntfreak committed
827
	int retval;
828

Zachary T Welch's avatar
Zachary T Welch committed
829
	retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
Spencer Oliver's avatar
Spencer Oliver committed
830
			MIPS32NUMCOREREGS, regs, 0, NULL, 1);
831

ntfreak's avatar
ntfreak committed
832
833
834
	return retval;
}

835
int mips32_pracc_read_regs(struct mips_ejtag *ejtag_info, uint32_t *regs)
ntfreak's avatar
ntfreak committed
836
{
837
	static const uint32_t code[] = {
ntfreak's avatar
ntfreak committed
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
														/* start: */
		MIPS32_MTC0(2,31,0),							/* move $2 to COP0 DeSave */
		MIPS32_LUI(2,UPPER16(MIPS32_PRACC_PARAM_OUT)), 	/* $2 = MIPS32_PRACC_PARAM_OUT */
		MIPS32_ORI(2,2,LOWER16(MIPS32_PRACC_PARAM_OUT)),
		MIPS32_SW(0,0*4,2),								/* sw $0,0*4($2) */
		MIPS32_SW(1,1*4,2), 							/* sw $1,1*4($2) */
		MIPS32_SW(15,15*4,2), 							/* sw $15,15*4($2) */
		MIPS32_MFC0(2,31,0),							/* move COP0 DeSave to $2 */
		MIPS32_MTC0(15,31,0),							/* move $15 to COP0 DeSave */
		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_STACK)), 	/* $15 = MIPS32_PRACC_STACK */
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_STACK)),
		MIPS32_SW(1,0,15), 								/* sw $1,($15) */
		MIPS32_SW(2,0,15), 								/* sw $2,($15) */
		MIPS32_LUI(1,UPPER16(MIPS32_PRACC_PARAM_OUT)), 	/* $1 = MIPS32_PRACC_PARAM_OUT */
		MIPS32_ORI(1,1,LOWER16(MIPS32_PRACC_PARAM_OUT)),
		MIPS32_SW(2,2*4,1), 							/* sw $2,2*4($1) */
		MIPS32_SW(3,3*4,1), 							/* sw $3,3*4($1) */
		MIPS32_SW(4,4*4,1), 							/* sw $4,4*4($1) */
		MIPS32_SW(5,5*4,1), 							/* sw $5,5*4($1) */
		MIPS32_SW(6,6*4,1), 							/* sw $6,6*4($1) */
		MIPS32_SW(7,7*4,1), 							/* sw $7,7*4($1) */
		MIPS32_SW(8,8*4,1), 							/* sw $8,8*4($1) */
		MIPS32_SW(9,9*4,1), 							/* sw $9,9*4($1) */
		MIPS32_SW(10,10*4,1), 							/* sw $10,10*4($1) */
		MIPS32_SW(11,11*4,1), 							/* sw $11,11*4($1) */
		MIPS32_SW(12,12*4,1), 							/* sw $12,12*4($1) */
		MIPS32_SW(13,13*4,1), 							/* sw $13,13*4($1) */
		MIPS32_SW(14,14*4,1), 							/* sw $14,14*4($1) */
		MIPS32_SW(16,16*4,1), 							/* sw $16,16*4($1) */
		MIPS32_SW(17,17*4,1), 							/* sw $17,17*4($1) */
		MIPS32_SW(18,18*4,1), 							/* sw $18,18*4($1) */
		MIPS32_SW(19,19*4,1), 							/* sw $19,19*4($1) */
		MIPS32_SW(20,20*4,1), 							/* sw $20,20*4($1) */
		MIPS32_SW(21,21*4,1), 							/* sw $21,21*4($1) */
		MIPS32_SW(22,22*4,1), 							/* sw $22,22*4($1) */
		MIPS32_SW(23,23*4,1), 							/* sw $23,23*4($1) */
		MIPS32_SW(24,24*4,1), 							/* sw $24,24*4($1) */
		MIPS32_SW(25,25*4,1), 							/* sw $25,25*4($1) */
		MIPS32_SW(26,26*4,1), 							/* sw $26,26*4($1) */
		MIPS32_SW(27,27*4,1), 							/* sw $27,27*4($1) */
		MIPS32_SW(28,28*4,1), 							/* sw $28,28*4($1) */
		MIPS32_SW(29,29*4,1), 							/* sw $29,29*4($1) */
		MIPS32_SW(30,30*4,1), 							/* sw $30,30*4($1) */
		MIPS32_SW(31,31*4,1), 							/* sw $31,31*4($1) */
882

ntfreak's avatar
ntfreak committed
883
884
		MIPS32_MFC0(2,12,0),							/* move status to $2 */
		MIPS32_SW(2,32*4,1), 							/* sw $2,32*4($1) */
885
		MIPS32_MFLO(2),									/* move lo to $2 */
ntfreak's avatar
ntfreak committed
886
		MIPS32_SW(2,33*4,1), 							/* sw $2,33*4($1) */
887
		MIPS32_MFHI(2),									/* move hi to $2 */
ntfreak's avatar
ntfreak committed
888
889
890
891
892
893
894
		MIPS32_SW(2,34*4,1), 							/* sw $2,34*4($1) */
		MIPS32_MFC0(2,8,0),								/* move badvaddr to $2 */
		MIPS32_SW(2,35*4,1), 							/* sw $2,35*4($1) */
		MIPS32_MFC0(2,13,0),							/* move cause to $2 */
		MIPS32_SW(2,36*4,1), 							/* sw $2,36*4($1) */
		MIPS32_MFC0(2,24,0),							/* move pc to $2 */
		MIPS32_SW(2,37*4,1), 							/* sw $2,37*4($1) */
895

896
897
		MIPS32_LW(2,0,15), 								/* lw $2,($15) */
		MIPS32_LW(1,0,15), 								/* lw $1,($15) */
ntfreak's avatar
ntfreak committed
898
		MIPS32_MFC0(15,31,0),							/* move COP0 DeSave to $15 */
Spencer Oliver's avatar
Spencer Oliver committed
899
		MIPS32_B(NEG16(59)),							/* b start */
ntfreak's avatar
ntfreak committed
900
901
		MIPS32_NOP,
	};
902

ntfreak's avatar
ntfreak committed
903
	int retval;
904

Zachary T Welch's avatar
Zachary T Welch committed
905
	retval = mips32_pracc_exec(ejtag_info, ARRAY_SIZE(code), code, \
Spencer Oliver's avatar
Spencer Oliver committed
906
		0, NULL, MIPS32NUMCOREREGS, regs, 1);
907

ntfreak's avatar
ntfreak committed
908
909
	return retval;
}
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975

/* fastdata upload/download requires an initialized working area
 * to load the download code; it should not be called otherwise
 * fetch order from the fastdata area
 * 1. start addr
 * 2. end addr
 * 3. data ...
 */
int mips32_pracc_fastdata_xfer(struct mips_ejtag *ejtag_info, struct working_area *source,
								int write, uint32_t addr, int count, uint32_t *buf)
{
	uint32_t handler_code[] = {
		/* caution when editing, table is modified below */
		/* r15 points to the start of this code */
		MIPS32_SW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
		MIPS32_SW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
		MIPS32_SW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
		MIPS32_SW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),
		/* start of fastdata area in t0 */
		MIPS32_LUI(8,UPPER16(MIPS32_PRACC_FASTDATA_AREA)),
		MIPS32_ORI(8,8,LOWER16(MIPS32_PRACC_FASTDATA_AREA)),
		MIPS32_LW(9,0,8),								/* start addr in t1 */
		MIPS32_LW(10,0,8),								/* end addr to t2 */
														/* loop: */
		/* 8 */ MIPS32_LW(11,0,0),						/* lw t3,[t8 | r9] */
		/* 9 */ MIPS32_SW(11,0,0),						/* sw t3,[r9 | r8] */
		MIPS32_BNE(10,9,NEG16(3)),						/* bne $t2,t1,loop */
		MIPS32_ADDI(9,9,4),								/* addi t1,t1,4 */

		MIPS32_LW(8,MIPS32_FASTDATA_HANDLER_SIZE - 4,15),
		MIPS32_LW(9,MIPS32_FASTDATA_HANDLER_SIZE - 8,15),
		MIPS32_LW(10,MIPS32_FASTDATA_HANDLER_SIZE - 12,15),
		MIPS32_LW(11,MIPS32_FASTDATA_HANDLER_SIZE - 16,15),

		MIPS32_LUI(15,UPPER16(MIPS32_PRACC_TEXT)),
		MIPS32_ORI(15,15,LOWER16(MIPS32_PRACC_TEXT)),
		MIPS32_JR(15),									/* jr start */
		MIPS32_MFC0(15,31,0),							/* move COP0 DeSave to $15 */
	};

	uint32_t jmp_code[] = {
		MIPS32_MTC0(15,31,0),			/* move $15 to COP0 DeSave */
		/* 1 */ MIPS32_LUI(15,0),		/* addr of working area added below */
		/* 2 */ MIPS32_ORI(15,15,0),	/* addr of working area added below */
		MIPS32_JR(15),					/* jump to ram program */
		MIPS32_NOP,
	};

	int retval, i;
	uint32_t val, ejtag_ctrl, address;

	if (source->size < MIPS32_FASTDATA_HANDLER_SIZE)
		return ERROR_TARGET_RESOURCE_NOT_AVAILABLE;

	if (write)
	{
		handler_code[8] = MIPS32_LW(11,0,8);	/* load data from probe at fastdata area */
		handler_code[9] = MIPS32_SW(11,0,9);	/* store data to RAM @ r9 */
	}
	else
	{
		handler_code[8] = MIPS32_LW(11,0,9);	/* load data from RAM @ r9 */
		handler_code[9] = MIPS32_SW(11,0,8);	/* store data to probe at fastdata area */
	}

	/* write program into RAM */
Spencer Oliver's avatar
Spencer Oliver committed
976
	mips32_pracc_write_mem32(ejtag_info, source->a