Promote Fuzev2 to unstable
[kugel-rb.git] / gdb / arm-stub.c
blob33181acab63a06701ce44052d105768188375aa4
1 /***************************************************************************
2 * __________ __ ___.
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
7 * \/ \/ \/ \/ \/
8 * $Id$
10 * Copyright (C) 2006 by Tomasz Malesinski
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
20 ****************************************************************************/
22 #include <stdbool.h>
23 #include <string.h>
24 #include "ifp_usb_serial.h"
25 #include "sscanf.h"
26 #include "pnx0101.h"
27 #include "gdb_api.h"
29 #define BUFMAX 1024
31 #define VEC_UND 1
32 #define VEC_SWI 2
33 #define VEC_PABT 3
34 #define VEC_DABT 4
36 static char packet_buf[BUFMAX];
37 static char reply_buf[BUFMAX];
39 static const char hexchars[] = "0123456789abcdef";
40 static int gdb_exception_no, gdb_mem_access;
41 static unsigned char watchdog_enabled;
42 static unsigned long registers[17];
44 void gdb_api_breakpoint(void);
45 static void gdb_api_log(char *msg);
47 __attribute__((section(".gdbapi"))) struct gdb_api gdb_api =
49 GDB_API_MAGIC,
50 {gdb_api_breakpoint, gdb_api_log}
53 static void watchdog_enable(int on)
55 (*(volatile unsigned long *)0x80002804) = on;
56 watchdog_enabled = on;
59 static void watchdog_service(void)
61 if (watchdog_enabled)
63 (*(volatile unsigned long *)0x80002804) = 0;
64 (*(volatile unsigned long *)0x80002808) = 0;
65 (*(volatile unsigned long *)0x80002804) = 1;
69 static inline bool isxdigit(char c)
71 return ((c >= '0') && (c <= '9'))
72 || ((c >= 'a') && (c <= 'f')) || ((c >= 'A') && (c <= 'F'));
75 static int hex(char ch) {
76 if ((ch >= 'a') && (ch <= 'f'))
77 return ch - 'a' + 10;
78 if ((ch >= '0') && (ch <= '9'))
79 return ch - '0';
80 if ((ch >= 'A') && (ch <= 'F'))
81 return ch - 'A' + 10;
82 return -1;
85 static void hex_byte(char *s, int byte) {
86 s[0] = hexchars[(byte >> 4) & 0xf];
87 s[1] = hexchars[byte & 0xf];
90 static void hex_word(char *s, unsigned long val) {
91 int i;
92 for (i = 0; i < 4; i++)
93 hex_byte(s + i * 2, (val >> (i * 8)) & 0xff);
96 static void hex_string(char *d, char *s) {
97 while (*s) {
98 hex_byte(d, *s++);
99 d += 2;
101 *d = 0;
104 static int get_hex_byte(char *s) {
105 return (hex(s[0]) << 4) + hex(s[1]);
108 static unsigned long get_hex_word(char *s) {
109 int i;
110 unsigned long r = 0;
111 for (i = 3; i >= 0; i--)
112 r = (r << 8) + get_hex_byte(s + i * 2);
113 return r;
116 static void reply_error(int n, char *reply) {
117 reply[0] = 'E';
118 hex_byte(reply + 1, n);
119 reply[3] = 0;
122 static void reply_signal(int n, char *reply) {
123 int signal;
124 reply[0] = 'S';
125 switch (n)
127 case VEC_UND:
128 signal = 4;
129 break;
130 case VEC_PABT:
131 case VEC_DABT:
132 signal = 7;
133 break;
134 default:
135 signal = 5;
136 break;
138 hex_byte(reply + 1, signal);
139 reply[3] = 0;
142 static void reply_ok(char *reply) {
143 strcpy(reply, "OK");
146 static int get_byte(void) {
147 int b;
148 while ((b = usb_serial_try_get_byte()) < 0)
149 watchdog_service();
150 watchdog_service();
151 return b;
154 static void put_byte(unsigned char ch) {
155 while (usb_serial_try_put_byte(ch) < 0)
156 watchdog_service();
157 watchdog_service();
160 static void serial_write(unsigned char *buf, int len) {
161 int i;
162 for (i = 0; i < len; i++)
163 put_byte(buf[i]);
166 static void get_packet(char *buf, int len) {
167 int count, checksum, escaped;
168 int ch;
170 while (1) {
171 do {
172 ch = get_byte();
173 } while (ch != '$');
175 checksum = 0;
176 count = 0;
177 escaped = 0;
178 while (count < len) {
179 ch = get_byte();
180 if (!escaped) {
181 if (ch == '$') {
182 checksum = 0;
183 count = 0;
184 } else if (ch == '#')
185 break;
186 else if (ch == 0x7d) {
187 escaped = 1;
188 checksum += ch;
189 } else {
190 checksum += ch;
191 buf[count] = ch;
192 count++;
194 } else {
195 escaped = 0;
196 checksum += ch;
197 buf[count] = ch ^ 0x20;
198 count++;
201 buf[count] = 0;
203 if (ch == '#') {
204 int rchksum;
206 ch = get_byte();
207 rchksum = hex(ch) << 4;
208 ch = get_byte();
209 rchksum += hex(ch);
211 if ((checksum & 0xff) != rchksum)
212 put_byte('-');
213 else {
214 put_byte('+');
215 return;
221 static void put_packet(char *buf) {
222 int i, checksum;
223 int ch;
224 char tmp[3];
226 do {
227 put_byte('$');
229 checksum = 0;
230 for (i = 0; buf[i]; i++)
231 checksum += buf[i];
233 serial_write(buf, i);
235 tmp[0] = '#';
236 hex_byte(tmp + 1, checksum & 0xff);
237 serial_write(tmp, 3);
239 ch = get_byte();
241 } while (ch != '+');
244 static inline unsigned long get_general_reg(int n)
246 return registers[n + 1];
249 static inline void set_general_reg(int n, unsigned long v)
251 registers[n + 1] = v;
254 static inline unsigned long get_cpsr(void)
256 return registers[0];
259 static inline void set_cpsr(unsigned long v)
261 registers[0] = v;
264 static void g_reply(char *buf) {
265 int i;
266 char *p;
268 p = buf;
269 for (i = 0; i < 16; i++) {
270 hex_word(p, get_general_reg(i));
271 p += 8;
274 for (i = 0; i < 8; i++) {
275 memset(p, '0', 16);
276 p += 16;
279 hex_word(p, 0);
280 p += 8;
281 hex_word(p, get_cpsr());
282 p[8] = 0;
285 static void cmd_get_register(char *args, char *reply) {
286 int r;
288 if (sscanf(args, "%x", &r) != 1) {
289 reply_error(0, reply);
290 return;
293 if (r >= 0 && r < 16) {
294 hex_word(reply, get_general_reg(r));
295 reply[8] = 0;
296 } else if (r == 25) {
297 hex_word(reply, get_cpsr());
298 reply[8] = 0;
299 } else {
300 hex_word(reply, 0);
301 reply[8] = 0;
305 static void cmd_set_register(char *args, char *reply) {
306 int r, p;
307 unsigned long v;
309 p = -1;
310 sscanf(args, "%x=%n", &r, &p);
311 if (p == -1) {
312 reply_error(0, reply);
313 return;
316 v = get_hex_word(args + p);
317 if (r >= 0 && r < 16)
318 set_general_reg(r, v);
319 else if (r == 25)
320 set_cpsr(v);
321 reply_ok(reply);
324 static void cmd_set_registers(char *args, char *reply) {
325 char *p;
326 int i, len;
328 len = strlen(args);
330 p = args;
331 for (i = 0; i < 16 && len >= (i + 1) * 8; i++) {
332 set_general_reg(i, get_hex_word(p));
333 p += 8;
336 if (len >= 16 * 8 + 8 * 16 + 2 * 8)
338 p += 8 * 16 + 8;
339 set_cpsr(get_hex_word(p));
342 reply_ok(reply);
345 static void cmd_get_memory(char *args, char *reply) {
346 unsigned long addr, len, i;
348 if (sscanf(args, "%lx,%lx", &addr, &len) != 2) {
349 reply_error(0, reply);
350 return;
353 if (len > (BUFMAX - 16) / 2) {
354 reply_error(1, reply);
355 return;
358 gdb_mem_access = 1;
359 for (i = 0; i < len; i++)
360 hex_byte(reply + i * 2, *((unsigned char *)(addr + i)));
361 gdb_mem_access = 0;
363 reply[len * 2] = 0;
366 static void cmd_put_memory(char *args, char *reply) {
367 unsigned long addr, len, i;
368 int pos;
370 pos = -1;
371 sscanf(args, "%lx,%lx:%n", &addr, &len, &pos);
372 if (pos == -1) {
373 reply_error(0, reply);
374 return;
377 gdb_mem_access = 1;
378 for (i = 0; i < len; i++)
379 *((unsigned char *)(addr + i)) = get_hex_byte(args + pos + i * 2);
380 gdb_mem_access = 0;
382 reply_ok(reply);
385 static void cmd_put_memory_binary(char *args, char *reply) {
386 unsigned long addr, len, i;
387 int pos;
389 pos = -1;
390 sscanf(args, "%lx,%lx:%n", &addr, &len, &pos);
391 if (pos == -1) {
392 reply_error(0, reply);
393 return;
396 gdb_mem_access = 1;
397 for (i = 0; i < len; i++)
398 *((unsigned char *)(addr + i)) = args[pos + i];
399 gdb_mem_access = 0;
401 reply_ok(reply);
404 static void parse_continue_args(char *args) {
405 int sig;
406 unsigned long addr;
408 if (sscanf(args, "%x;%lx", &sig, &addr) == 2) {
409 set_general_reg(15, addr);
410 } else if (sscanf(args, "%lx", &addr) == 1) {
411 set_general_reg(15, addr);
415 static void cmd_go(char *args) {
416 parse_continue_args(args);
418 asm volatile(
419 " mov r1, %0\n"
420 " ldr r12, [r1], #4\n"
421 " mov r0, r12\n"
422 " and r0, r0, #0x1f\n"
423 " cmp r0, #0x10\n"
424 " bne 1f\n"
425 " ldr r14, [r1, #60]\n"
426 " msr spsr_fsxc, r12\n"
427 " ldmia r1, {r0-r14}^\n"
428 " movs r15, r14\n"
429 "1:\n"
430 " msr cpsr_fsxc, r12\n"
431 " ldmia r1, {r0-r15}\n"
432 : : "r" (registers));
435 static void remote_cmd(char *cmd, char *reply) {
436 int i, err;
437 i = 0;
438 err = 0;
439 while ((cmd[i] >= 'a' && cmd[i] <= 'z') || cmd[i] == '_')
440 i++;
441 if (!strncmp(cmd, "reboot", i))
443 reply_ok(reply);
444 put_packet(reply);
445 watchdog_enable(1);
446 (*(volatile unsigned long *)0x80002804) = 1;
447 while (1);
449 else if (!strncmp(cmd, "power_off", i))
451 reply_ok(reply);
452 put_packet(reply);
453 GPIO1_CLR = 1 << 16;
454 GPIO2_SET = 1;
455 while (1);
457 else if (!strncmp(cmd, "watchdog", i))
459 int t;
460 if (sscanf(cmd + i, "%d", &t) == 1)
461 watchdog_enable(t != 0);
462 else
463 err = 1;
464 reply_ok(reply);
466 else
467 hex_string(reply, "Unrecognized command\n");
468 if (err)
469 reply_error(err, reply);
472 static void cmd_query(char *args, char *reply) {
473 if (!strncmp(args, "Rcmd,", 5)) {
474 unsigned i = 0;
475 char *s = args + 5;
476 char cmd[200];
477 while (isxdigit(s[0]) && isxdigit(s[1]) && i < sizeof(cmd) - 1) {
478 cmd[i++] = get_hex_byte(s);
479 s += 2;
481 cmd[i] = 0;
482 remote_cmd(cmd, reply);
483 } else
484 reply[0] = 0;
487 void gdb_loop(void) {
488 int no_reply;
490 gdb_mem_access = 0;
492 while (1) {
493 get_packet(packet_buf, sizeof(packet_buf) - 1);
495 no_reply = 0;
496 switch (packet_buf[0]) {
497 case '?':
498 reply_signal(gdb_exception_no, reply_buf);
499 break;
501 case 'p':
502 cmd_get_register(packet_buf + 1, reply_buf);
503 break;
505 case 'P':
506 cmd_set_register(packet_buf + 1, reply_buf);
507 break;
509 case 'g':
510 g_reply(reply_buf);
511 break;
513 case 'G':
514 cmd_set_registers(packet_buf + 1, reply_buf);
515 break;
517 case 'm':
518 cmd_get_memory(packet_buf + 1, reply_buf);
519 break;
521 case 'M':
522 cmd_put_memory(packet_buf + 1, reply_buf);
523 break;
525 case 'X':
526 cmd_put_memory_binary(packet_buf + 1, reply_buf);
527 break;
529 case 'q':
530 cmd_query(packet_buf + 1, reply_buf);
531 break;
533 case 'c':
534 cmd_go(packet_buf + 1);
535 reply_error(1, reply_buf);
536 break;
538 /* case 's': */
539 /* cmd_go(packet_buf + 1); */
540 /* break; */
542 default:
543 reply_buf[0] = 0;
546 if (!no_reply)
547 put_packet(reply_buf);
551 extern void *vectors[];
553 static void gdb_set_vector(int n, void *p)
555 vectors[n] = p;
558 void gdb_und_exc(void);
559 void gdb_swi_exc(void);
560 void gdb_pabt_exc(void);
561 void gdb_dabt_exc(void);
563 static void gdb_set_vectors(void)
565 gdb_set_vector(VEC_UND, gdb_und_exc);
566 gdb_set_vector(VEC_SWI, gdb_swi_exc);
567 gdb_set_vector(VEC_PABT, gdb_pabt_exc);
568 gdb_set_vector(VEC_DABT, gdb_dabt_exc);
571 void gdb_loop_from_exc(void)
573 if (gdb_mem_access)
574 reply_error(1, reply_buf);
575 else
576 reply_signal(gdb_exception_no, reply_buf);
577 put_packet(reply_buf);
578 gdb_loop();
581 #define IRQ_REG(reg) (*(volatile unsigned long *)(0x80300000 + (reg)))
583 static inline unsigned long irq_read(int reg)
585 unsigned long v, v2;
588 v = IRQ_REG(reg);
589 v2 = IRQ_REG(reg);
590 } while (v != v2);
591 return v;
594 #define IRQ_WRITE_WAIT(reg, val, cond) \
595 do { unsigned long v, v2; \
596 do { \
597 IRQ_REG(reg) = (val); \
598 v = IRQ_REG(reg); \
599 v2 = IRQ_REG(reg); \
600 } while ((v != v2) || !(cond)); \
601 } while (0);
603 void fiq(void)
607 static void system_init(void)
609 int i;
611 watchdog_enable(0);
613 for (i = 0; i < 0x1c; i++)
615 IRQ_WRITE_WAIT(0x404 + i * 4, 0x1e000001, (v & 0x3010f) == 1);
616 IRQ_WRITE_WAIT(0x404 + i * 4, 0x4000000, (v & 0x10000) == 0);
617 IRQ_WRITE_WAIT(0x404 + i * 4, 0x10000001, (v & 0xf) == 1);
620 GPIO3_CLR = 1;
623 static void gdb_api_log(char *msg)
625 int i;
627 reply_buf[0] = 'O';
628 i = 1;
629 while (*msg && i + 2 <= BUFMAX - 1)
631 hex_byte(reply_buf + i, *msg++);
632 i += 2;
634 reply_buf[i] = 0;
635 put_packet(reply_buf);
638 void main(void)
640 system_init();
641 usb_serial_init();
642 gdb_mem_access = 0;
643 gdb_set_vectors();
644 gdb_api_breakpoint();
645 while (1);
648 #define str(s) #s
649 #define xstr(s) str(s)
651 asm (".text\n"
652 "gdb_und_exc:\n"
653 " ldr sp, =_stub_stack\n"
654 " sub r14, r14, #4\n"
655 " stmfd sp!, {r0-r3, r12, r14}\n"
656 " mov r0, #" xstr(VEC_UND) "\n"
657 " b gdb_handle_exception\n"
658 "gdb_swi_exc:\n"
659 " ldr sp, =_stub_stack\n"
660 " stmfd sp!, {r0-r3, r12, r14}\n"
661 " mov r0, #" xstr(VEC_SWI) "\n"
662 " b gdb_handle_exception\n"
663 "gdb_pabt_exc:\n"
664 " ldr sp, =_stub_stack\n"
665 " stmfd sp!, {r0-r3, r12, r14}\n"
666 " mov r0, #" xstr(VEC_PABT) "\n"
667 " b gdb_handle_exception\n"
668 "gdb_dabt_exc:\n"
669 " ldr sp, =_stub_stack\n"
670 " sub r14, r14, #4\n"
671 " stmfd sp!, {r0-r3, r12, r14}\n"
672 " ldr r0, =gdb_mem_access\n"
673 " ldr r0, [r0]\n"
674 " tst r0, r0\n"
675 " bne gdb_data_abort\n"
676 " mov r0, #" xstr(VEC_DABT) "\n"
677 " b gdb_handle_exception\n"
678 "gdb_handle_exception:\n"
679 " ldr r1, =gdb_exception_no\n"
680 " str r0, [r1]\n"
681 " ldr r0, =registers\n"
682 " mrs r12, spsr\n"
683 " str r12, [r0], #4\n"
684 " ldmfd sp!, {r2, r3}\n"
685 " stmia r0!, {r2, r3}\n"
686 " ldmfd sp!, {r2, r3, r12, r14}\n"
687 " str r14, [r0, #52]\n"
688 " stmia r0!, {r2-r12}\n"
689 " mrs r1, spsr\n"
690 " and r2, r1, #0x1f\n"
691 " cmp r2, #0x10\n"
692 " bne 1f\n"
693 " stmia r0, {r13, r14}^\n"
694 " b gdb_data_abort\n"
695 "1:\n"
696 " msr cpsr_c, r1\n"
697 " stmia r0, {r13, r14}\n"
698 "gdb_data_abort:\n"
699 " msr cpsr_c, #0xd3\n"
700 " ldr sp, =_stub_stack\n"
701 " b gdb_loop_from_exc\n"
702 "gdb_api_breakpoint:\n"
703 " stmfd sp!, {r0-r1}\n"
704 " ldr r0, =registers\n"
705 " mrs r1, cpsr\n"
706 " str r1, [r0], #4\n"
707 " ldmfd sp!, {r1}\n"
708 " str r1, [r0], #4\n"
709 " ldmfd sp!, {r1}\n"
710 " str r1, [r0], #4\n"
711 " stmia r0!, {r2-r14}\n"
712 " str r14, [r0]\n"
713 " msr cpsr_c, #0xd3\n"
714 " ldr sp, =_stub_stack\n"
715 " ldr r0, =gdb_exception_no\n"
716 " mov r1, #5\n"
717 " str r1, [r0]\n"
718 " b gdb_loop_from_exc\n");