1/* Debug stub for Z80.
2
3   Copyright (C) 2021-2023 Free Software Foundation, Inc.
4
5   This file is part of GDB.
6
7   This program is free software; you can redistribute it and/or modify
8   it under the terms of the GNU General Public License as published by
9   the Free Software Foundation; either version 3 of the License, or
10   (at your option) any later version.
11
12   This program is distributed in the hope that it will be useful,
13   but WITHOUT ANY WARRANTY; without even the implied warranty of
14   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15   GNU General Public License for more details.
16
17   You should have received a copy of the GNU General Public License
18   along with this program.  If not, see <http://www.gnu.org/licenses/>.  */
19
20/* Usage:
21  1. Copy this file to project directory
22  2. Configure it commenting/uncommenting macros below or define DBG_CONFIGURED
23     and all required macros and then include this file to one of your C-source
24     files.
25  3. Implement getDebugChar() and putDebugChar(), functions must not return
26     until data received or sent.
27  4. Implement all optional functions used to toggle breakpoints/watchpoints,
28     if supported. Do not write fuctions to toggle software breakpoints if
29     you unsure (GDB will do itself).
30  5. Implement serial port initialization routine called at program start.
31  6. Add necessary debugger entry points to your program, for example:
32	.org 0x08	;RST 8 handler
33	jp _debug_swbreak
34	...
35	.org	0x66	;NMI handler
36	jp	_debug_nmi
37	...
38	main_loop:
39	halt
40	call	isDbgInterrupt
41	jr	z,101$
42	ld	hl, 2	;EX_SIGINT
43	push	hl
44	call	_debug_exception
45	101$:
46	...
47  7. Compile file using SDCC (supported ports are: z80, z180, z80n, gbz80 and
48     ez80_z80), do not use --peep-asm option. For example:
49	$ sdcc -mz80 --opt-code-size --max-allocs-per-node 50000 z80-stub.c
50*/
51/******************************************************************************\
52			     Configuration
53\******************************************************************************/
54#ifndef DBG_CONFIGURED
55/* Uncomment this line, if stub size is critical for you */
56//#define DBG_MIN_SIZE
57
58/* Comment this line out if software breakpoints are unsupported.
59   If you have special function to toggle software breakpoints, then provide
60   here name of these function. Expected prototype:
61       int toggle_swbreak(int set, void *addr);
62   function must return 0 on success. */
63//#define DBG_SWBREAK toggle_swbreak
64#define DBG_SWBREAK
65
66/* Define if one of standard RST handlers is used as software
67   breakpoint entry point */
68//#define DBG_SWBREAK_RST 0x08
69
70/* if platform supports hardware breakpoints then define following macro
71   by name of function. Fuction must have next prototype:
72     int toggle_hwbreak(int set, void *addr);
73   function must return 0 on success. */
74//#define DBG_HWBREAK toggle_hwbreak
75
76/* if platform supports hardware watchpoints then define all or some of
77   following macros by names of functions. Fuctions prototypes:
78     int toggle_watch(int set, void *addr, size_t size);  // memory write watch
79     int toggle_rwatch(int set, void *addr, size_t size); // memory read watch
80     int toggle_awatch(int set, void *addr, size_t size); // memory access watch
81   function must return 0 on success. */
82//#define DBG_WWATCH toggle_watch
83//#define DBG_RWATCH toggle_rwatch
84//#define DBG_AWATCH toggle_awatch
85
86/* Size of hardware breakpoint. Required to correct PC. */
87#define DBG_HWBREAK_SIZE 0
88
89/* Define following macro if you need custom memory read/write routine.
90   Function should return non-zero on success, and zero on failure
91   (for example, write to ROM area).
92   Useful with overlays (bank switching).
93   Do not forget to define:
94   _ovly_table - overlay table
95   _novlys - number of items in _ovly_table
96   or
97   _ovly_region_table - overlay regions table
98   _novly_regions - number of items in _ovly_region_table
99
100   _ovly_debug_prepare - function is called before overlay mapping
101   _ovly_debug_event - function is called after overlay mapping
102 */
103//#define DBG_MEMCPY memcpy
104
105/* define dedicated stack size if required */
106//#define DBG_STACK_SIZE 256
107
108/* max GDB packet size
109   should be much less that DBG_STACK_SIZE because it will be allocated on stack
110*/
111#define DBG_PACKET_SIZE 150
112
113/* Uncomment if required to use trampoline when resuming operation.
114   Useful with dedicated stack when stack pointer do not point to the stack or
115   stack is not writable */
116//#define DBG_USE_TRAMPOLINE
117
118/* Uncomment following macro to enable debug printing to debugger console */
119//#define DBG_PRINT
120
121#define DBG_NMI_EX EX_HWBREAK
122#define DBG_INT_EX EX_SIGINT
123
124/* Define following macro to statement, which will be exectuted after entering to
125   stub_main function. Statement should include semicolon. */
126//#define DBG_ENTER debug_enter();
127
128/* Define following macro to instruction(s), which will be execute before return
129   control to the program. It is useful when gdb-stub is placed in one of overlays.
130   This procedure must not change any register. On top of stack before invocation
131   will be return address of the program. */
132//#define DBG_RESUME jp _restore_bank
133
134/* Define following macro to the string containing memory map definition XML.
135   GDB will use it to select proper breakpoint type (HW or SW). */
136/*#define DBG_MEMORY_MAP "\
137<memory-map>\
138	<memory type=\"rom\" start=\"0x0000\" length=\"0x4000\"/>\
139<!--	<memory type=\"flash\" start=\"0x4000\" length=\"0x4000\">\
140		<property name=\"blocksize\">128</property>\
141	</memory> -->\
142	<memory type=\"ram\" start=\"0x8000\" length=\"0x8000\"/>\
143</memory-map>\
144"
145*/
146#endif /* DBG_CONFIGURED */
147/******************************************************************************\
148			     Public Interface
149\******************************************************************************/
150
151/* Enter to debug mode from software or hardware breakpoint.
152   Assume address of next instruction after breakpoint call is on top of stack.
153   Do JP _debug_swbreak or JP _debug_hwbreak from RST handler, for example.
154 */
155void debug_swbreak (void);
156void debug_hwbreak (void);
157
158/* Jump to this function from NMI handler. Just replace RETN instruction by
159   JP _debug_nmi
160   Use if NMI detects request to enter to debug mode.
161 */
162void debug_nmi (void);
163
164/* Jump to this function from INT handler. Just replace EI+RETI instructions by
165   JP _debug_int
166   Use if INT detects request to enter to debug mode.
167 */
168void debug_int (void);
169
170#define EX_SWBREAK	0	/* sw breakpoint */
171#define EX_HWBREAK	-1	/* hw breakpoint */
172#define EX_WWATCH	-2	/* memory write watch */
173#define EX_RWATCH	-3	/* memory read watch */
174#define EX_AWATCH	-4	/* memory access watch */
175#define EX_SIGINT	2
176#define EX_SIGTRAP	5
177#define EX_SIGABRT	6
178#define EX_SIGBUS	10
179#define EX_SIGSEGV	11
180/* or any standard *nix signal value */
181
182/* Enter to debug mode (after receiving BREAK from GDB, for example)
183 * Assume:
184 *   program PC in (SP+0)
185 *   caught signal in (SP+2)
186 *   program SP is SP+4
187 */
188void debug_exception (int ex);
189
190/* Prints to debugger console. */
191void debug_print(const char *str);
192/******************************************************************************\
193			      Required functions
194\******************************************************************************/
195
196extern int getDebugChar (void);
197extern void putDebugChar (int ch);
198
199#ifdef DBG_SWBREAK
200#define DO_EXPAND(VAL)  VAL ## 123456
201#define EXPAND(VAL)     DO_EXPAND(VAL)
202
203#if EXPAND(DBG_SWBREAK) != 123456
204#define DBG_SWBREAK_PROC DBG_SWBREAK
205extern int DBG_SWBREAK(int set, void *addr);
206#endif
207
208#undef EXPAND
209#undef DO_EXPAND
210#endif /* DBG_SWBREAK */
211
212#ifdef DBG_HWBREAK
213extern int DBG_HWBREAK(int set, void *addr);
214#endif
215
216#ifdef DBG_MEMCPY
217extern void* DBG_MEMCPY (void *dest, const void *src, unsigned n);
218#endif
219
220#ifdef DBG_WWATCH
221extern int DBG_WWATCH(int set, void *addr, unsigned size);
222#endif
223
224#ifdef DBG_RWATCH
225extern int DBG_RWATCH(int set, void *addr, unsigned size);
226#endif
227
228#ifdef DBG_AWATCH
229extern int DBG_AWATCH(int set, void *addr, unsigned size);
230#endif
231
232/******************************************************************************\
233			       IMPLEMENTATION
234\******************************************************************************/
235
236#include <string.h>
237
238#ifndef NULL
239# define NULL (void*)0
240#endif
241
242typedef unsigned char byte;
243typedef unsigned short word;
244
245/* CPU state */
246#ifdef __SDCC_ez80_adl
247# define REG_SIZE 3
248#else
249# define REG_SIZE 2
250#endif /* __SDCC_ez80_adl */
251
252#define R_AF    (0*REG_SIZE)
253#define R_BC    (1*REG_SIZE)
254#define R_DE    (2*REG_SIZE)
255#define R_HL    (3*REG_SIZE)
256#define R_SP    (4*REG_SIZE)
257#define R_PC    (5*REG_SIZE)
258
259#ifndef __SDCC_gbz80
260#define R_IX    (6*REG_SIZE)
261#define R_IY    (7*REG_SIZE)
262#define R_AF_   (8*REG_SIZE)
263#define R_BC_   (9*REG_SIZE)
264#define R_DE_   (10*REG_SIZE)
265#define R_HL_   (11*REG_SIZE)
266#define R_IR    (12*REG_SIZE)
267
268#ifdef __SDCC_ez80_adl
269#define R_SPS   (13*REG_SIZE)
270#define NUMREGBYTES (14*REG_SIZE)
271#else
272#define NUMREGBYTES (13*REG_SIZE)
273#endif /* __SDCC_ez80_adl */
274#else
275#define NUMREGBYTES (6*REG_SIZE)
276#define FASTCALL
277#endif /*__SDCC_gbz80 */
278static byte state[NUMREGBYTES];
279
280#if DBG_PACKET_SIZE < (NUMREGBYTES*2+5)
281#error "Too small DBG_PACKET_SIZE"
282#endif
283
284#ifndef FASTCALL
285#define FASTCALL __z88dk_fastcall
286#endif
287
288/* dedicated stack */
289#ifdef DBG_STACK_SIZE
290
291#define LOAD_SP	ld	sp, #_stack + DBG_STACK_SIZE
292
293static char stack[DBG_STACK_SIZE];
294
295#else
296
297#undef DBG_USE_TRAMPOLINE
298#define LOAD_SP
299
300#endif
301
302#ifndef DBG_ENTER
303#define DBG_ENTER
304#endif
305
306#ifndef DBG_RESUME
307#define DBG_RESUME ret
308#endif
309
310static signed char sigval;
311
312static void stub_main (int sigval, int pc_adj);
313static char high_hex (byte v) FASTCALL;
314static char low_hex (byte v) FASTCALL;
315static char put_packet_info (const char *buffer) FASTCALL;
316static void save_cpu_state (void);
317static void rest_cpu_state (void);
318
319/******************************************************************************/
320#ifdef DBG_SWBREAK
321#ifdef DBG_SWBREAK_RST
322#define DBG_SWBREAK_SIZE 1
323#else
324#define DBG_SWBREAK_SIZE 3
325#endif
326void
327debug_swbreak (void) __naked
328{
329  __asm
330	ld	(#_state + R_SP), sp
331	LOAD_SP
332	call	_save_cpu_state
333	ld	hl, #-DBG_SWBREAK_SIZE
334	push	hl
335	ld	hl, #EX_SWBREAK
336	push	hl
337	call	_stub_main
338	.globl	_break_handler
339#ifdef DBG_SWBREAK_RST
340_break_handler = DBG_SWBREAK_RST
341#else
342_break_handler = _debug_swbreak
343#endif
344  __endasm;
345}
346#endif /* DBG_SWBREAK */
347/******************************************************************************/
348#ifdef DBG_HWBREAK
349#ifndef DBG_HWBREAK_SIZE
350#define DBG_HWBREAK_SIZE 0
351#endif /* DBG_HWBREAK_SIZE */
352void
353debug_hwbreak (void) __naked
354{
355  __asm
356	ld	(#_state + R_SP), sp
357	LOAD_SP
358	call	_save_cpu_state
359	ld	hl, #-DBG_HWBREAK_SIZE
360	push	hl
361	ld	hl, #EX_HWBREAK
362	push	hl
363	call	_stub_main
364  __endasm;
365}
366#endif /* DBG_HWBREAK_SET */
367/******************************************************************************/
368void
369debug_exception (int ex) __naked
370{
371  __asm
372	ld	(#_state + R_SP), sp
373	LOAD_SP
374	call	_save_cpu_state
375	ld	hl, #0
376	push	hl
377#ifdef __SDCC_gbz80
378	ld	hl, #_state + R_SP
379	ld	a, (hl+)
380	ld	h, (hl)
381	ld	l, a
382#else
383	ld	hl, (#_state + R_SP)
384#endif
385	inc	hl
386	inc	hl
387	ld	e, (hl)
388	inc	hl
389	ld	d, (hl)
390	push	de
391	call	_stub_main
392  __endasm;
393  (void)ex;
394}
395/******************************************************************************/
396#ifndef __SDCC_gbz80
397void
398debug_nmi(void) __naked
399{
400  __asm
401	ld	(#_state + R_SP), sp
402	LOAD_SP
403	call	_save_cpu_state
404	ld	hl, #0	;pc_adj
405	push	hl
406	ld	hl, #DBG_NMI_EX
407	push	hl
408	ld	hl, #_stub_main
409	push	hl
410	push	hl
411	retn
412  __endasm;
413}
414#endif
415/******************************************************************************/
416void
417debug_int(void) __naked
418{
419  __asm
420	ld	(#_state + R_SP), sp
421	LOAD_SP
422	call	_save_cpu_state
423	ld	hl, #0	;pc_adj
424	push	hl
425	ld	hl, #DBG_INT_EX
426	push	hl
427	ld	hl, #_stub_main
428	push	hl
429	push	hl
430	ei
431	reti
432  __endasm;
433}
434/******************************************************************************/
435#ifdef DBG_PRINT
436void
437debug_print(const char *str)
438{
439  putDebugChar ('$');
440  putDebugChar ('O');
441  char csum = 'O';
442  for (; *str != '\0'; )
443    {
444      char c = high_hex (*str);
445      csum += c;
446      putDebugChar (c);
447      c = low_hex (*str++);
448      csum += c;
449      putDebugChar (c);
450    }
451  putDebugChar ('#');
452  putDebugChar (high_hex (csum));
453  putDebugChar (low_hex (csum));
454}
455#endif /* DBG_PRINT */
456/******************************************************************************/
457static void store_pc_sp (int pc_adj) FASTCALL;
458#define get_reg_value(mem) (*(void* const*)(mem))
459#define set_reg_value(mem,val) do { (*(void**)(mem) = (val)); } while (0)
460static char* byte2hex(char *buf, byte val);
461static int hex2int (const char **buf) FASTCALL;
462static char* int2hex (char *buf, int v);
463static void get_packet (char *buffer);
464static void put_packet (const char *buffer);
465static char process (char *buffer) FASTCALL;
466static void rest_cpu_state (void);
467
468static void
469stub_main (int ex, int pc_adj)
470{
471  char buffer[DBG_PACKET_SIZE+1];
472  sigval = (signed char)ex;
473  store_pc_sp (pc_adj);
474
475  DBG_ENTER
476
477  /* after starting gdb_stub must always return stop reason */
478  *buffer = '?';
479  for (; process (buffer);)
480    {
481      put_packet (buffer);
482      get_packet (buffer);
483    }
484  put_packet (buffer);
485  rest_cpu_state ();
486}
487
488static void
489get_packet (char *buffer)
490{
491  byte csum;
492  char ch;
493  char *p;
494  byte esc;
495#if DBG_PACKET_SIZE <= 256
496  byte count; /* it is OK to use up to 256 here */
497#else
498  unsigned count;
499#endif
500  for (;; putDebugChar ('-'))
501    {
502      /* wait for packet start character */
503      while (getDebugChar () != '$');
504retry:
505      csum = 0;
506      esc = 0;
507      p = buffer;
508      count = DBG_PACKET_SIZE;
509      do
510	{
511	  ch = getDebugChar ();
512	  switch (ch)
513	    {
514	    case '$':
515	      goto retry;
516	    case '#':
517	      goto finish;
518	    case '}':
519	      esc = 0x20;
520	      break;
521	    default:
522	      *p++ = ch ^ esc;
523	      esc = 0;
524	      --count;
525	    }
526	  csum += ch;
527	}
528      while (count != 0);
529finish:
530      *p = '\0';
531      if (ch != '#') /* packet is too large */
532	continue;
533      ch = getDebugChar ();
534      if (ch != high_hex (csum))
535	continue;
536      ch = getDebugChar ();
537      if (ch != low_hex (csum))
538	continue;
539      break;
540    }
541  putDebugChar ('+');
542}
543
544static void
545put_packet (const char *buffer)
546{
547  /*  $<packet info>#<checksum>. */
548  for (;;)
549    {
550      putDebugChar ('$');
551      char checksum = put_packet_info (buffer);
552      putDebugChar ('#');
553      putDebugChar (high_hex(checksum));
554      putDebugChar (low_hex(checksum));
555      for (;;)
556	{
557	  char c = getDebugChar ();
558	  switch (c)
559	    {
560	    case '+': return;
561	    case '-': break;
562	    default:
563	      putDebugChar (c);
564	      continue;
565	    }
566	  break;
567	}
568    }
569}
570
571static char
572put_packet_info (const char *src) FASTCALL
573{
574  char ch;
575  char checksum = 0;
576  for (;;)
577    {
578      ch = *src++;
579      if (ch == '\0')
580	break;
581      if (ch == '}' || ch == '*' || ch == '#' || ch == '$')
582	{
583	  /* escape special characters */
584	  putDebugChar ('}');
585	  checksum += '}';
586	  ch ^= 0x20;
587	}
588      putDebugChar (ch);
589      checksum += ch;
590    }
591  return checksum;
592}
593
594static void
595store_pc_sp (int pc_adj) FASTCALL
596{
597  byte *sp = get_reg_value (&state[R_SP]);
598  byte *pc = get_reg_value (sp);
599  pc += pc_adj;
600  set_reg_value (&state[R_PC], pc);
601  set_reg_value (&state[R_SP], sp + REG_SIZE);
602}
603
604static char *mem2hex (char *buf, const byte *mem, unsigned bytes);
605static char *hex2mem (byte *mem, const char *buf, unsigned bytes);
606
607/* Command processors. Takes pointer to buffer (begins from command symbol),
608   modifies buffer, returns: -1 - empty response (ignore), 0 - success,
609   positive: error code. */
610
611#ifdef DBG_MIN_SIZE
612static signed char
613process_question (char *p) FASTCALL
614{
615  signed char sig;
616  *p++ = 'S';
617  sig = sigval;
618  if (sig <= 0)
619    sig = EX_SIGTRAP;
620  p = byte2hex (p, (byte)sig);
621  *p = '\0';
622  return 0;
623}
624#else /* DBG_MIN_SIZE */
625static char *format_reg_value (char *p, unsigned reg_num, const byte *value);
626
627static signed char
628process_question (char *p) FASTCALL
629{
630  signed char sig;
631  *p++ = 'T';
632  sig = sigval;
633  if (sig <= 0)
634    sig = EX_SIGTRAP;
635  p = byte2hex (p, (byte)sig);
636  p = format_reg_value(p, R_AF/REG_SIZE, &state[R_AF]);
637  p = format_reg_value(p, R_SP/REG_SIZE, &state[R_SP]);
638  p = format_reg_value(p, R_PC/REG_SIZE, &state[R_PC]);
639#if defined(DBG_SWBREAK_PROC) || defined(DBG_HWBREAK) || defined(DBG_WWATCH) || defined(DBG_RWATCH) || defined(DBG_AWATCH)
640  const char *reason;
641  unsigned addr = 0;
642  switch (sigval)
643    {
644#ifdef DBG_SWBREAK_PROC
645    case EX_SWBREAK:
646      reason = "swbreak";
647      break;
648#endif
649#ifdef DBG_HWBREAK
650    case EX_HWBREAK:
651      reason = "hwbreak";
652      break;
653#endif
654#ifdef DBG_WWATCH
655    case EX_WWATCH:
656      reason = "watch";
657      addr = 1;
658      break;
659#endif
660#ifdef DBG_RWATCH
661    case EX_RWATCH:
662      reason = "rwatch";
663      addr = 1;
664      break;
665#endif
666#ifdef DBG_AWATCH
667    case EX_AWATCH:
668      reason = "awatch";
669      addr = 1;
670      break;
671#endif
672    default:
673      goto finish;
674    }
675  while ((*p++ = *reason++))
676    ;
677  --p;
678  *p++ = ':';
679  if (addr != 0)
680    p = int2hex(p, addr);
681  *p++ = ';';
682finish:
683#endif /* DBG_HWBREAK, DBG_WWATCH, DBG_RWATCH, DBG_AWATCH */
684  *p++ = '\0';
685  return 0;
686}
687#endif /* DBG_MINSIZE */
688
689#define STRING2(x) #x
690#define STRING1(x) STRING2(x)
691#define STRING(x) STRING1(x)
692#ifdef DBG_MEMORY_MAP
693static void read_memory_map (char *buffer, unsigned offset, unsigned length);
694#endif
695
696static signed char
697process_q (char *buffer) FASTCALL
698{
699  char *p;
700  if (memcmp (buffer + 1, "Supported", 9) == 0)
701    {
702      memcpy (buffer, "PacketSize=", 11);
703      p = int2hex (&buffer[11], DBG_PACKET_SIZE);
704#ifndef DBG_MIN_SIZE
705#ifdef DBG_SWBREAK_PROC
706      memcpy (p, ";swbreak+", 9);
707      p += 9;
708#endif
709#ifdef DBG_HWBREAK
710      memcpy (p, ";hwbreak+", 9);
711      p += 9;
712#endif
713#endif /* DBG_MIN_SIZE */
714
715#ifdef DBG_MEMORY_MAP
716      memcpy (p, ";qXfer:memory-map:read+", 23);
717      p += 23;
718#endif
719      *p = '\0';
720      return 0;
721    }
722#ifdef DBG_MEMORY_MAP
723  if (memcmp (buffer + 1, "Xfer:memory-map:read:", 21) == 0)
724    {
725      p = strchr (buffer + 1 + 21, ':');
726      if (p == NULL)
727	return 1;
728      ++p;
729      unsigned offset = hex2int (&p);
730      if (*p++ != ',')
731	return 2;
732      unsigned length = hex2int (&p);
733      if (length == 0)
734	return 3;
735      if (length > DBG_PACKET_SIZE)
736	return 4;
737      read_memory_map (buffer, offset, length);
738      return 0;
739    }
740#endif
741#ifndef DBG_MIN_SIZE
742  if (memcmp (&buffer[1], "Attached", 9) == 0)
743    {
744      /* Just report that GDB attached to existing process
745	 if it is not applicable for you, then send patches */
746      memcpy(buffer, "1", 2);
747      return 0;
748    }
749#endif /* DBG_MIN_SIZE */
750  *buffer = '\0';
751  return -1;
752}
753
754static signed char
755process_g (char *buffer) FASTCALL
756{
757  mem2hex (buffer, state, NUMREGBYTES);
758  return 0;
759}
760
761static signed char
762process_G (char *buffer) FASTCALL
763{
764  hex2mem (state, &buffer[1], NUMREGBYTES);
765  /* OK response */
766  *buffer = '\0';
767  return 0;
768}
769
770static signed char
771process_m (char *buffer) FASTCALL
772{/* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
773  char *p = &buffer[1];
774  byte *addr = (void*)hex2int(&p);
775  if (*p++ != ',')
776    return 1;
777  unsigned len = (unsigned)hex2int(&p);
778  if (len == 0)
779    return 2;
780  if (len > DBG_PACKET_SIZE/2)
781    return 3;
782  p = buffer;
783#ifdef DBG_MEMCPY
784  do
785    {
786      byte tmp[16];
787      unsigned tlen = sizeof(tmp);
788      if (tlen > len)
789	tlen = len;
790      if (!DBG_MEMCPY(tmp, addr, tlen))
791	return 4;
792      p = mem2hex (p, tmp, tlen);
793      addr += tlen;
794      len -= tlen;
795    }
796  while (len);
797#else
798  p = mem2hex (p, addr, len);
799#endif
800  return 0;
801}
802
803static signed char
804process_M (char *buffer) FASTCALL
805{/* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
806  char *p = &buffer[1];
807  byte *addr = (void*)hex2int(&p);
808  if (*p != ',')
809    return 1;
810  ++p;
811  unsigned len = (unsigned)hex2int(&p);
812  if (*p++ != ':')
813    return 2;
814  if (len == 0)
815    goto end;
816  if (len*2 + (p - buffer) > DBG_PACKET_SIZE)
817    return 3;
818#ifdef DBG_MEMCPY
819  do
820    {
821      byte tmp[16];
822      unsigned tlen = sizeof(tmp);
823      if (tlen > len)
824	tlen = len;
825      p = hex2mem (tmp, p, tlen);
826      if (!DBG_MEMCPY(addr, tmp, tlen))
827	return 4;
828      addr += tlen;
829	len -= tlen;
830    }
831  while (len);
832#else
833  hex2mem (addr, p, len);
834#endif
835end:
836  /* OK response */
837  *buffer = '\0';
838  return 0;
839}
840
841#ifndef DBG_MIN_SIZE
842static signed char
843process_X (char *buffer) FASTCALL
844{/* XAA..AA,LLLL: Write LLLL binary bytes at address AA.AA return OK */
845  char *p = &buffer[1];
846  byte *addr = (void*)hex2int(&p);
847  if (*p != ',')
848    return 1;
849  ++p;
850  unsigned len = (unsigned)hex2int(&p);
851  if (*p++ != ':')
852    return 2;
853  if (len == 0)
854    goto end;
855  if (len + (p - buffer) > DBG_PACKET_SIZE)
856    return 3;
857#ifdef DBG_MEMCPY
858  if (!DBG_MEMCPY(addr, p, len))
859    return 4;
860#else
861  memcpy (addr, p, len);
862#endif
863end:
864  /* OK response */
865  *buffer = '\0';
866  return 0;
867}
868#else /* DBG_MIN_SIZE */
869static signed char
870process_X (char *buffer) FASTCALL
871{
872  (void)buffer;
873  return -1;
874}
875#endif /* DBG_MIN_SIZE */
876
877static signed char
878process_c (char *buffer) FASTCALL
879{/* 'cAAAA' - Continue at address AAAA(optional) */
880  const char *p = &buffer[1];
881  if (*p != '\0')
882    {
883      void *addr = (void*)hex2int(&p);
884      set_reg_value (&state[R_PC], addr);
885    }
886  rest_cpu_state ();
887  return 0;
888}
889
890static signed char
891process_D (char *buffer) FASTCALL
892{/* 'D' - detach the program: continue execution */
893  *buffer = '\0';
894  return -2;
895}
896
897static signed char
898process_k (char *buffer) FASTCALL
899{/* 'k' - Kill the program */
900  set_reg_value (&state[R_PC], 0);
901  rest_cpu_state ();
902  (void)buffer;
903  return 0;
904}
905
906static signed char
907process_v (char *buffer) FASTCALL
908{
909#ifndef DBG_MIN_SIZE
910  if (memcmp (&buffer[1], "Cont", 4) == 0)
911    {
912      if (buffer[5] == '?')
913	{
914	  /* result response will be "vCont;c;C"; C action must be
915	     supported too, because GDB reguires at lease both of them */
916	  memcpy (&buffer[5], ";c;C", 5);
917	  return 0;
918	}
919      buffer[0] = '\0';
920      if (buffer[5] == ';' && (buffer[6] == 'c' || buffer[6] == 'C'))
921	return -2; /* resume execution */
922      return 1;
923  }
924#endif /* DBG_MIN_SIZE */
925  return -1;
926}
927
928static signed char
929process_zZ (char *buffer) FASTCALL
930{ /* insert/remove breakpoint */
931#if defined(DBG_SWBREAK_PROC) || defined(DBG_HWBREAK) || \
932    defined(DBG_WWATCH) || defined(DBG_RWATCH) || defined(DBG_AWATCH)
933  const byte set = (*buffer == 'Z');
934  const char *p = &buffer[3];
935  void *addr = (void*)hex2int(&p);
936  if (*p != ',')
937    return 1;
938  p++;
939  int kind = hex2int(&p);
940  *buffer = '\0';
941  switch (buffer[1])
942    {
943#ifdef DBG_SWBREAK_PROC
944    case '0': /* sw break */
945      return DBG_SWBREAK_PROC(set, addr);
946#endif
947#ifdef DBG_HWBREAK
948    case '1': /* hw break */
949      return DBG_HWBREAK(set, addr);
950#endif
951#ifdef DBG_WWATCH
952    case '2': /* write watch */
953      return DBG_WWATCH(set, addr, kind);
954#endif
955#ifdef DBG_RWATCH
956    case '3': /* read watch */
957      return DBG_RWATCH(set, addr, kind);
958#endif
959#ifdef DBG_AWATCH
960    case '4': /* access watch */
961      return DBG_AWATCH(set, addr, kind);
962#endif
963    default:; /* not supported */
964    }
965#endif
966  (void)buffer;
967  return -1;
968}
969
970static signed char
971do_process (char *buffer) FASTCALL
972{
973  switch (*buffer)
974    {
975    case '?': return process_question (buffer);
976    case 'G': return process_G (buffer);
977    case 'k': return process_k (buffer);
978    case 'M': return process_M (buffer);
979    case 'X': return process_X (buffer);
980    case 'Z': return process_zZ (buffer);
981    case 'c': return process_c (buffer);
982    case 'D': return process_D (buffer);
983    case 'g': return process_g (buffer);
984    case 'm': return process_m (buffer);
985    case 'q': return process_q (buffer);
986    case 'v': return process_v (buffer);
987    case 'z': return process_zZ (buffer);
988    default:  return -1; /* empty response */
989    }
990}
991
992static char
993process (char *buffer) FASTCALL
994{
995  signed char err = do_process (buffer);
996  char *p = buffer;
997  char ret = 1;
998  if (err == -2)
999    {
1000      ret = 0;
1001      err = 0;
1002    }
1003  if (err > 0)
1004    {
1005      *p++ = 'E';
1006      p = byte2hex (p, err);
1007      *p = '\0';
1008    }
1009  else if (err < 0)
1010    {
1011      *p = '\0';
1012    }
1013  else if (*p == '\0')
1014    memcpy(p, "OK", 3);
1015  return ret;
1016}
1017
1018static char *
1019byte2hex (char *p, byte v)
1020{
1021  *p++ = high_hex (v);
1022  *p++ = low_hex (v);
1023  return p;
1024}
1025
1026static signed char
1027hex2val (unsigned char hex) FASTCALL
1028{
1029  if (hex <= '9')
1030    return hex - '0';
1031  hex &= 0xdf; /* make uppercase */
1032  hex -= 'A' - 10;
1033  return (hex >= 10 && hex < 16) ? hex : -1;
1034}
1035
1036static int
1037hex2byte (const char *p) FASTCALL
1038{
1039  signed char h = hex2val (p[0]);
1040  signed char l = hex2val (p[1]);
1041  if (h < 0 || l < 0)
1042    return -1;
1043  return (byte)((byte)h << 4) | (byte)l;
1044}
1045
1046static int
1047hex2int (const char **buf) FASTCALL
1048{
1049  word r = 0;
1050  for (;; (*buf)++)
1051    {
1052      signed char a = hex2val(**buf);
1053      if (a < 0)
1054	break;
1055      r <<= 4;
1056      r += (byte)a;
1057    }
1058  return (int)r;
1059}
1060
1061static char *
1062int2hex (char *buf, int v)
1063{
1064  buf = byte2hex(buf, (word)v >> 8);
1065  return byte2hex(buf, (byte)v);
1066}
1067
1068static char
1069high_hex (byte v) FASTCALL
1070{
1071  return low_hex(v >> 4);
1072}
1073
1074static char
1075low_hex (byte v) FASTCALL
1076{
1077/*
1078  __asm
1079	ld	a, l
1080	and	a, #0x0f
1081	add	a, #0x90
1082	daa
1083	adc	a, #0x40
1084	daa
1085	ld	l, a
1086  __endasm;
1087  (void)v;
1088*/
1089  v &= 0x0f;
1090  v += '0';
1091  if (v < '9'+1)
1092    return v;
1093  return v + 'a' - '0' - 10;
1094}
1095
1096/* convert the memory, pointed to by mem into hex, placing result in buf */
1097/* return a pointer to the last char put in buf (null) */
1098static char *
1099mem2hex (char *buf, const byte *mem, unsigned bytes)
1100{
1101  char *d = buf;
1102  if (bytes != 0)
1103    {
1104      do
1105	{
1106	  d = byte2hex (d, *mem++);
1107	}
1108      while (--bytes);
1109    }
1110  *d = 0;
1111  return d;
1112}
1113
1114/* convert the hex array pointed to by buf into binary, to be placed in mem
1115   return a pointer to the character after the last byte written */
1116
1117static const char *
1118hex2mem (byte *mem, const char *buf, unsigned bytes)
1119{
1120  if (bytes != 0)
1121    {
1122      do
1123	{
1124	  *mem++ = hex2byte (buf);
1125	  buf += 2;
1126	}
1127      while (--bytes);
1128    }
1129  return buf;
1130}
1131
1132#ifdef DBG_MEMORY_MAP
1133static void
1134read_memory_map (char *buffer, unsigned offset, unsigned length)
1135{
1136  const char *map = DBG_MEMORY_MAP;
1137  const unsigned map_sz = strlen(map);
1138  if (offset >= map_sz)
1139    {
1140      buffer[0] = 'l';
1141      buffer[1] = '\0';
1142      return;
1143    }
1144  if (offset + length > map_sz)
1145    length = map_sz - offset;
1146  buffer[0] = 'm';
1147  memcpy (&buffer[1], &map[offset], length);
1148  buffer[1+length] = '\0';
1149}
1150#endif
1151
1152/* write string like " nn:0123" and return pointer after it */
1153#ifndef DBG_MIN_SIZE
1154static char *
1155format_reg_value (char *p, unsigned reg_num, const byte *value)
1156{
1157  char *d = p;
1158  unsigned char i;
1159  d = byte2hex(d, reg_num);
1160  *d++ = ':';
1161  value += REG_SIZE;
1162  i = REG_SIZE;
1163  do
1164    {
1165      d = byte2hex(d, *--value);
1166    }
1167  while (--i != 0);
1168  *d++ = ';';
1169  return d;
1170}
1171#endif /* DBG_MIN_SIZE */
1172
1173#ifdef __SDCC_gbz80
1174/* saves all state.except PC and SP */
1175static void
1176save_cpu_state() __naked
1177{
1178  __asm
1179	push	af
1180	ld	a, l
1181	ld	(#_state + R_HL + 0), a
1182	ld	a, h
1183	ld	(#_state + R_HL + 1), a
1184	ld	hl, #_state + R_HL - 1
1185	ld	(hl), d
1186	dec	hl
1187	ld	(hl), e
1188	dec	hl
1189	ld	(hl), b
1190	dec	hl
1191	ld	(hl), c
1192	dec	hl
1193	pop	bc
1194	ld	(hl), b
1195	dec	hl
1196	ld	(hl), c
1197	ret
1198  __endasm;
1199}
1200
1201/* restore CPU state and continue execution */
1202static void
1203rest_cpu_state() __naked
1204{
1205  __asm
1206;restore SP
1207	ld	a, (#_state + R_SP + 0)
1208	ld	l,a
1209	ld	a, (#_state + R_SP + 1)
1210	ld	h,a
1211	ld	sp, hl
1212;push PC value as return address
1213	ld	a, (#_state + R_PC + 0)
1214	ld	l, a
1215	ld	a, (#_state + R_PC + 1)
1216	ld	h, a
1217	push	hl
1218;restore registers
1219	ld	hl, #_state + R_AF
1220	ld	c, (hl)
1221	inc	hl
1222	ld	b, (hl)
1223	inc	hl
1224	push	bc
1225	ld	c, (hl)
1226	inc	hl
1227	ld	b, (hl)
1228	inc	hl
1229	ld	e, (hl)
1230	inc	hl
1231	ld	d, (hl)
1232	inc	hl
1233	ld	a, (hl)
1234	inc	hl
1235	ld	h, (hl)
1236	ld	l, a
1237	pop	af
1238	ret
1239  __endasm;
1240}
1241#else
1242/* saves all state.except PC and SP */
1243static void
1244save_cpu_state() __naked
1245{
1246  __asm
1247	ld	(#_state + R_HL), hl
1248	ld	(#_state + R_DE), de
1249	ld	(#_state + R_BC), bc
1250	push	af
1251	pop	hl
1252	ld	(#_state + R_AF), hl
1253	ld	a, r	;R is increased by 7 or by 8 if called via RST
1254	ld	l, a
1255	sub	a, #7
1256	xor	a, l
1257	and	a, #0x7f
1258	xor	a, l
1259#ifdef __SDCC_ez80_adl
1260	ld	hl, i
1261	ex	de, hl
1262	ld	hl, #_state + R_IR
1263	ld	(hl), a
1264	inc	hl
1265	ld	(hl), e
1266	inc	hl
1267	ld	(hl), d
1268	ld	a, MB
1269	ld	(#_state + R_AF+2), a
1270#else
1271	ld	l, a
1272	ld	a, i
1273	ld	h, a
1274	ld	(#_state + R_IR), hl
1275#endif /* __SDCC_ez80_adl */
1276	ld	(#_state + R_IX), ix
1277	ld	(#_state + R_IY), iy
1278	ex	af, af'	;'
1279	exx
1280	ld	(#_state + R_HL_), hl
1281	ld	(#_state + R_DE_), de
1282	ld	(#_state + R_BC_), bc
1283	push	af
1284	pop	hl
1285	ld	(#_state + R_AF_), hl
1286	ret
1287  __endasm;
1288}
1289
1290/* restore CPU state and continue execution */
1291static void
1292rest_cpu_state() __naked
1293{
1294  __asm
1295#ifdef DBG_USE_TRAMPOLINE
1296	ld	sp, _stack + DBG_STACK_SIZE
1297	ld	hl, (#_state + R_PC)
1298	push	hl	/* resume address */
1299#ifdef __SDCC_ez80_adl
1300	ld	hl, 0xc30000 ; use 0xc34000 for jp.s
1301#else
1302	ld	hl, 0xc300
1303#endif
1304	push	hl	/* JP opcode */
1305#endif /* DBG_USE_TRAMPOLINE */
1306	ld	hl, (#_state + R_AF_)
1307	push	hl
1308	pop	af
1309	ld	bc, (#_state + R_BC_)
1310	ld	de, (#_state + R_DE_)
1311	ld	hl, (#_state + R_HL_)
1312	exx
1313	ex	af, af'	;'
1314	ld	iy, (#_state + R_IY)
1315	ld	ix, (#_state + R_IX)
1316#ifdef __SDCC_ez80_adl
1317	ld	a, (#_state + R_AF + 2)
1318	ld	MB, a
1319	ld	hl, (#_state + R_IR + 1) ;I register
1320	ld	i, hl
1321	ld	a, (#_state + R_IR + 0) ; R register
1322	ld	l, a
1323#else
1324	ld	hl, (#_state + R_IR)
1325	ld	a, h
1326	ld	i, a
1327	ld	a, l
1328#endif /* __SDCC_ez80_adl */
1329	sub	a, #10	;number of M1 cycles after ld r,a
1330	xor	a, l
1331	and	a, #0x7f
1332	xor	a, l
1333	ld	r, a
1334	ld	de, (#_state + R_DE)
1335	ld	bc, (#_state + R_BC)
1336	ld	hl, (#_state + R_AF)
1337	push	hl
1338	pop	af
1339	ld	sp, (#_state + R_SP)
1340#ifndef DBG_USE_TRAMPOLINE
1341	ld	hl, (#_state + R_PC)
1342	push	hl
1343	ld	hl, (#_state + R_HL)
1344	DBG_RESUME
1345#else
1346	ld	hl, (#_state + R_HL)
1347#ifdef __SDCC_ez80_adl
1348	jp	#_stack + DBG_STACK_SIZE - 4
1349#else
1350	jp	#_stack + DBG_STACK_SIZE - 3
1351#endif
1352#endif /* DBG_USE_TRAMPOLINE */
1353  __endasm;
1354}
1355#endif /* __SDCC_gbz80 */
1356