arch/m68k-amiga/c/gdbstub.c

   1 /*
   2     Copyright © 1995-2010, The AROS Development Team. All rights reserved.
   3     $Id$
   4
   5     Desc: m68k-amiga gdb stub
   6     Lang: english
   7  */
   8 /* Adapted from public domain code from:
   9  * ftp://ftp.jyu.fi/pub/PalmOS/ryeham/ALPHA/m68k-gdbstub.c
  10  *
  11  *  vvvvvvv The below are the original comments from m68k-gdbstub.c vvvvvvv
  12  */
  13 /****************************************************************************
  14  *  Header: remcom.c,v 1.34 91/03/09 12:29:49 glenne Exp $
  15  *
  16  *  Module name: remcom.c $
  17  *  Revision: 1.34 $
  18  *  Date: 91/03/09 12:29:49 $
  19  *  Contributor:     Lake Stevens Instrument Division$
  20  *
  21  *  Description:     low level support for gdb debugger. $
  22  *
  23  *  Considerations:  only works on target hardware $
  24  *
  25  *  Written by:      Glenn Engel $
  26  *  ModuleState:     Experimental $
  27  *
  28  *  NOTES:           See Below $
  29  *
  30  *  To enable debugger support, two things need to happen.  One, a
  31  *  call to set_debug_traps() is necessary in order to allow any breakpoints
  32  *  or error conditions to be properly intercepted and reported to gdb.
  33  *  Two, a breakpoint needs to be generated to begin communication.  This
  34  *  is most easily accomplished by a call to breakpoint().  Breakpoint()
  35  *  simulates a breakpoint by executing a trap #1.  The breakpoint instruction
  36  *  is hardwired to trap #1 because not to do so is a compatibility problem--
  37  *  there either should be a standard breakpoint instruction, or the protocol
  38  *  should be extended to provide some means to communicate which breakpoint
  39  *  instruction is in use (or have the stub insert the breakpoint).
  40  *
  41  *  Some explanation is probably necessary to explain how exceptions are
  42  *  handled.  When an exception is encountered the 68000 pushes the current
  43  *  program counter and status register onto the supervisor stack and then
  44  *  transfers execution to a location specified in it's vector table.
  45  *  The handlers for the exception vectors are hardwired to jmp to an address
  46  *  given by the relation:  (exception - 256) * 6.  These are decending
  47  *  addresses starting from -6, -12, -18, ...  By allowing 6 bytes for
  48  *  each entry, a jsr, jmp, bsr, ... can be used to enter the exception
  49  *  handler.  Using a jsr to handle an exception has an added benefit of
  50  *  allowing a single handler to service several exceptions and use the
  51  *  return address as the key differentiation.  The vector number can be
  52  *  computed from the return address by [ exception = (addr + 1530) / 6 ].
  53  *  The sole purpose of the routine catchException is to compute the
  54  *  exception number and push it on the stack in place of the return address.
  55  *  The external function exceptionHandler() is
  56  *  used to attach a specific handler to a specific m68k exception.
  57  *  For 68020 machines, the ability to have a return address around just
  58  *  so the vector can be determined is not necessary because the '020 pushes an
  59  *  extra word onto the stack containing the vector offset
  60  *
  61  *  Because gdb will sometimes write to the stack area to execute function
  62  *  calls, this program cannot rely on using the supervisor stack so it
  63  *  uses it's own stack area reserved in the int array remcomStack.
  64  *
  65  *************
  66  *
  67  *    The following gdb commands are supported:
  68  *
  69  * command          function                               Return value
  70  *
  71  *    g             return the value of the CPU registers  hex data or ENN
  72  *    G             set the value of the CPU registers     OK or ENN
  73  *
  74  *    mAA..AA,LLLL  Read LLLL bytes at address AA..AA      hex data or ENN
  75  *    MAA..AA,LLLL: Write LLLL bytes at address AA.AA      OK or ENN
  76  *
  77  *    c             Resume at current address              SNN   ( signal NN)
  78  *    cAA..AA       Continue at address AA..AA             SNN
  79  *
  80  *    s             Step one instruction                   SNN
  81  *    sAA..AA       Step one instruction from AA..AA       SNN
  82  *
  83  *    k             kill
  84  *
  85  *    ?             What was the last sigval ?             SNN   (signal NN)
  86  *
  87  * All commands and responses are sent with a packet which includes a
  88  * checksum.  A packet consists of
  89  *
  90  * $<packet info>#<checksum>.
  91  *
  92  * where
  93  * <packet info> :: <characters representing the command or response>
  94  * <checksum>    :: < two hex digits computed as modulo 256 sum of <packetinfo>>
  95  *
  96  * When a packet is received, it is first acknowledged with either '+' or '-'.
  97  * '+' indicates a successful transfer.  '-' indicates a failed transfer.
  98  *
  99  * Example:
 100  *
 101  * Host:                  Reply:
 102  * $m0,10#2a               +$00010203040506070809101112131415#42
 103  *
 104  ****************************************************************************/
 105
 106 #include <stdio.h>
 107 #include <string.h>
 108 #include <setjmp.h>
 109
 110 /************************************************************************
 111  *
 112  * external low-level support routines
 113  */
 114 typedef void (*ExceptionHook)(int);   /* pointer to function with int parm */
 115 typedef void (*Function)();           /* pointer to a function */
 116
 117 extern int DebugPutChar(register int x);   /* write a single character      */
 118 extern int DebugGetChar();   /* read and return a single char */
 119
 120 void exceptionHandler(int n, ExceptionHook a);  /* assign an exception handler */
 121
 122 /************************/
 123 /* FORWARD DECLARATIONS */
 124 /************************/
 125
 126 /************************************************************************/
 127 /* BUFMAX defines the maximum number of characters in inbound/outbound buffers*/
 128 /* at least NUMREGBYTES*2 are needed for register packets */
 129 #define BUFMAX 400
 130
 131 static char initialized;  /* boolean flag. != 0 means we've been initialized */
 132
 133 int     remote_debug;
 134 /*  debug >  0 prints ill-formed commands in valid packets & checksum errors */
 135
 136 static const char hexchars[]="0123456789abcdef";
 137
 138 /* there are 180 bytes of registers on a 68020 w/68881      */
 139 /* many of the fpa registers are 12 byte (96 bit) registers */
 140 #define NUMREGBYTES 180
 141 enum regnames {D0,D1,D2,D3,D4,D5,D6,D7,
 142                A0,A1,A2,A3,A4,A5,A6,A7,
 143                PS,PC,
 144                FP0,FP1,FP2,FP3,FP4,FP5,FP6,FP7,
 145                FPCONTROL,FPSTATUS,FPIADDR
 146               };
 147
 148 /*
 149  * these should not be static cuz they can be used outside this module
 150  */
 151 int registers[NUMREGBYTES/4];
 152 int superStack;
 153
 154 #define STACKSIZE 1024
 155 int remcomStack[STACKSIZE/sizeof(int)];
 156
 157 #ifdef mc68020
 158 /* the size of the exception stack on the 68020 varies with the type of
 159  * exception.  The following table is the number of WORDS used
 160  * for each exception format.
 161  */
 162 const short exceptionSize[] = { 4,4,6,4,4,4,4,4,29,10,16,46,12,4,4,4 };
 163 #endif
 164
 165 #ifdef mc68332
 166 const short exceptionSize[] = { 4,4,6,4,4,4,4,4,4,4,4,4,16,4,4,4 };
 167 #endif
 168
 169 /************* jump buffer used for setjmp/longjmp **************************/
 170 jmp_buf remcomEnv;
 171
 172 /***************************  ASSEMBLY CODE MACROS *************************/
 173 /*                                                                         */
 174
 175 #define BREAKPOINT() asm("   trap #1");
 176
 177 int hex(ch)
 178 char ch;
 179 {
 180   if ((ch >= 'a') && (ch <= 'f')) return (ch-'a'+10);
 181   if ((ch >= '0') && (ch <= '9')) return (ch-'0');
 182   if ((ch >= 'A') && (ch <= 'F')) return (ch-'A'+10);
 183   return (-1);
 184 }
 185
 186
 187 /* scan for the sequence $<data>#<checksum>     */
 188 void getpacket(buffer)
 189 char * buffer;
 190 {
 191   unsigned char checksum;
 192   unsigned char xmitcsum;
 193   int  i;
 194   int  count;
 195   char ch;
 196
 197   do {
 198     /* wait around for the start character, ignore all other characters */
 199     while ((ch = (DebugGetChar() & 0x7f)) != '$');
 200     checksum = 0;
 201     xmitcsum = -1;
 202
 203     count = 0;
 204
 205     /* now, read until a # or end of buffer is found */
 206     while (count < BUFMAX) {
 207       ch = DebugGetChar() & 0x7f;
 208       if (ch == '#') break;
 209       checksum = checksum + ch;
 210       buffer[count] = ch;
 211       count = count + 1;
 212       }
 213     buffer[count] = 0;
 214
 215     if (ch == '#') {
 216       xmitcsum = hex(DebugGetChar() & 0x7f) << 4;
 217       xmitcsum += hex(DebugGetChar() & 0x7f);
 218 #ifdef HAVE_STDIO
 219       if ((remote_debug ) && (checksum != xmitcsum)) {
 220         fprintf (stderr,"bad checksum.  My count = 0x%x, sent=0x%x. buf=%s\n",
 221                                                      checksum,xmitcsum,buffer);
 222       }
 223 #endif
 224
 225       if (checksum != xmitcsum) DebugPutChar('-');  /* failed checksum */
 226       else {
 227          DebugPutChar('+');  /* successful transfer */
 228          /* if a sequence char is present, reply the sequence ID */
 229          if (buffer[2] == ':') {
 230             DebugPutChar( buffer[0] );
 231             DebugPutChar( buffer[1] );
 232             /* remove sequence chars from buffer */
 233             count = strlen(buffer);
 234             for (i=3; i <= count; i++) buffer[i-3] = buffer[i];
 235          }
 236       }
 237     }
 238   } while (checksum != xmitcsum);
 239
 240 }
 241
 242 /* send the packet in buffer.  The host get's one chance to read it.
 243    This routine does not wait for a positive acknowledge.  */
 244
 245
 246 void putpacket(buffer)
 247 char * buffer;
 248 {
 249   unsigned char checksum;
 250   int  count;
 251   char ch;
 252
 253   /*  $<packet info>#<checksum>. */
 254   do {
 255   DebugPutChar('$');
 256   checksum = 0;
 257   count    = 0;
 258
 259   while ((ch=buffer[count]) != 0) {
 260     if (! DebugPutChar(ch)) return;
 261     checksum += ch;
 262     count += 1;
 263   }
 264
 265   DebugPutChar('#');
 266   DebugPutChar(hexchars[checksum >> 4]);
 267   DebugPutChar(hexchars[checksum % 16]);
 268
 269   } while (DebugGetChar() != '+');
 270
 271 }
 272
 273 char  remcomInBuffer[BUFMAX];
 274 char  remcomOutBuffer[BUFMAX];
 275 static short error;
 276
 277
 278 void debug_error(format, parm)
 279 char * format;
 280 char * parm;
 281 {
 282 #ifdef HAVE_STDIO
 283   if (remote_debug) fprintf (stderr,format,parm);
 284 #endif
 285 }
 286
 287 /* convert the memory pointed to by mem into hex, placing result in buf */
 288 /* return a pointer to the last char put in buf (null) */
 289 char* mem2hex(mem, buf, count)
 290 char* mem;
 291 char* buf;
 292 int   count;
 293 {
 294       int i;
 295       unsigned char ch;
 296       for (i=0;i<count;i++) {
 297           ch = *mem++;
 298           *buf++ = hexchars[ch >> 4];
 299           *buf++ = hexchars[ch % 16];
 300       }
 301       *buf = 0;
 302       return(buf);
 303 }
 304
 305 /* convert the hex array pointed to by buf into binary to be placed in mem */
 306 /* return a pointer to the character AFTER the last byte written */
 307 char* hex2mem(buf, mem, count)
 308 char* buf;
 309 char* mem;
 310 int   count;
 311 {
 312       int i;
 313       unsigned char ch;
 314       for (i=0;i<count;i++) {
 315           ch = hex(*buf++) << 4;
 316           ch = ch + hex(*buf++);
 317           *mem++ = ch;
 318       }
 319       return(mem);
 320 }
 321
 322 /* a bus error has occurred, perform a longjmp
 323    to return execution and allow handling of the error */
 324
 325 void handle_buserror()
 326 {
 327   longjmp(remcomEnv,1);
 328 }
 329
 330 /* this function takes the 68000 exception number and attempts to
 331    translate this number into a unix compatible signal value */
 332 int computeSignal( exceptionVector )
 333 int exceptionVector;
 334 {
 335   int sigval;
 336   switch (exceptionVector) {
 337     case 2 : sigval = 10; break; /* bus error           */
 338     case 3 : sigval = 10; break; /* address error       */
 339     case 4 : sigval = 4;  break; /* illegal instruction */
 340     case 5 : sigval = 8;  break; /* zero divide         */
 341     case 6 : sigval = 8; break; /* chk instruction     */
 342     case 7 : sigval = 8; break; /* trapv instruction   */
 343     case 8 : sigval = 11; break; /* privilege violation */
 344     case 9 : sigval = 5;  break; /* trace trap          */
 345     case 10: sigval = 4;  break; /* line 1010 emulator  */
 346     case 11: sigval = 4;  break; /* line 1111 emulator  */
 347
 348       /* Coprocessor protocol violation.  Using a standard MMU or FPU
 349          this cannot be triggered by software.  Call it a SIGBUS.  */
 350     case 13: sigval = 10;  break;
 351
 352     case 31: sigval = 2;  break; /* interrupt           */
 353     case 32+1: sigval = 5;  break; /* trap #1 breakpoint  */
 354     case 32+15: sigval = 5;  break; /* trap #15 breakpoint  */
 355
 356     case 48: sigval = 8;  break; /* floating point err  */
 357     case 49: sigval = 8;  break; /* floating point err  */
 358     case 50: sigval = 8;  break; /* zero divide         */
 359     case 51: sigval = 8;  break; /* underflow           */
 360     case 52: sigval = 8;  break; /* operand error       */
 361     case 53: sigval = 8;  break; /* overflow            */
 362     case 54: sigval = 8;  break; /* NAN                 */
 363     default:
 364       sigval = 7;         /* "software generated"*/
 365   }
 366   return (sigval);
 367 }
 368
 369 /**********************************************/
 370 /* WHILE WE FIND NICE HEX CHARS, BUILD AN INT */
 371 /* RETURN NUMBER OF CHARS PROCESSED           */
 372 /**********************************************/
 373 int hexToInt(char **ptr, int *intValue)
 374 {
 375     int numChars = 0;
 376     int hexValue;
 377
 378     *intValue = 0;
 379
 380     while (**ptr)
 381     {
 382         hexValue = hex(**ptr);
 383         if (hexValue >=0)
 384         {
 385             *intValue = (*intValue <<4) | hexValue;
 386             numChars ++;
 387         }
 388         else
 389             break;
 390
 391         (*ptr)++;
 392     }
 393
 394     return (numChars);
 395 }
 396
 397 /*
 398  * This function does all command procesing for interfacing to gdb.
 399  */
 400 void handle_exception(int exceptionVector)
 401 {
 402   int    sigval;
 403   int    addr, length;
 404   char * ptr;
 405
 406   if ((exceptionVector == (32+1)) ||
 407       (exceptionVector == (32+15)))
 408       registers[ PC ] -= 2;
 409
 410 #ifdef HAVE_STDIO
 411   if (remote_debug) printf("vector=%d, sr=0x%x, pc=0x%x\n",
 412                             exceptionVector,
 413                             registers[ PS ],
 414                             registers[ PC ]);
 415 #endif
 416   /* reply to host that an exception has occurred */
 417   sigval = computeSignal( exceptionVector );
 418   remcomOutBuffer[0] = 'S';
 419   remcomOutBuffer[1] =  hexchars[sigval >> 4];
 420   remcomOutBuffer[2] =  hexchars[sigval % 16];
 421   remcomOutBuffer[3] = 0;
 422
 423   putpacket(remcomOutBuffer);
 424
 425   while (1==1) {
 426     error = 0;
 427     remcomOutBuffer[0] = 0;
 428     getpacket(remcomInBuffer);
 429     switch (remcomInBuffer[0]) {
 430       case '?' :   remcomOutBuffer[0] = 'S';
 431                    remcomOutBuffer[1] =  hexchars[sigval >> 4];
 432                    remcomOutBuffer[2] =  hexchars[sigval % 16];
 433                    remcomOutBuffer[3] = 0;
 434                  break;
 435       case 'd' : remote_debug = !(remote_debug);  /* toggle debug flag */
 436                  break;
 437       case 'g' : /* return the value of the CPU registers */
 438                 mem2hex((char*) registers, remcomOutBuffer, NUMREGBYTES);
 439                 break;
 440       case 'G' : /* set the value of the CPU registers - return OK */
 441                 hex2mem(&remcomInBuffer[1], (char*) registers, NUMREGBYTES);
 442                 strcpy(remcomOutBuffer,"OK");
 443                 break;
 444
 445       /* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
 446       case 'm' :
 447                 if (setjmp(remcomEnv) == 0)
 448                 {
 449                     exceptionHandler(2,handle_buserror);
 450
 451                     /* TRY TO READ %x,%x.  IF SUCCEED, SET PTR = 0 */
 452                     ptr = &remcomInBuffer[1];
 453                     if (hexToInt(&ptr,&addr))
 454                         if (*(ptr++) == ',')
 455                             if (hexToInt(&ptr,&length))
 456                             {
 457                                 ptr = 0;
 458                                 mem2hex((char*) addr, remcomOutBuffer, length);
 459                             }
 460
 461                     if (ptr)
 462                     {
 463                       strcpy(remcomOutBuffer,"E01");
 464                       debug_error("malformed read memory command: %s",remcomInBuffer);
 465                   }
 466                 }
 467                 else {
 468                   exceptionHandler(2,handle_exception);
 469                   strcpy(remcomOutBuffer,"E03");
 470                   debug_error("bus error");
 471                   }
 472
 473                 /* restore handler for bus error */
 474                 exceptionHandler(2,handle_exception);
 475                 break;
 476
 477       /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
 478       case 'M' :
 479                 if (setjmp(remcomEnv) == 0) {
 480                     exceptionHandler(2,handle_buserror);
 481
 482                     /* TRY TO READ '%x,%x:'.  IF SUCCEED, SET PTR = 0 */
 483                     ptr = &remcomInBuffer[1];
 484                     if (hexToInt(&ptr,&addr))
 485                         if (*(ptr++) == ',')
 486                             if (hexToInt(&ptr,&length))
 487                                 if (*(ptr++) == ':')
 488                                 {
 489                                     hex2mem(ptr, (char*) addr, length);
 490                                     ptr = 0;
 491                                     strcpy(remcomOutBuffer,"OK");
 492                                 }
 493                     if (ptr)
 494                     {
 495                       strcpy(remcomOutBuffer,"E02");
 496                       debug_error("malformed write memory command: %s",remcomInBuffer);
 497                       }
 498                 }
 499                 else {
 500                   exceptionHandler(2,handle_exception);
 501                   strcpy(remcomOutBuffer,"E03");
 502                   debug_error("bus error");
 503                   }
 504
 505                 /* restore handler for bus error */
 506                 exceptionHandler(2,handle_exception);
 507                 break;
 508
 509      /* cAA..AA    Continue at address AA..AA(optional) */
 510      /* sAA..AA   Step one instruction from AA..AA(optional) */
 511      case 'c' :
 512      case 's' :
 513           /* try to read optional parameter, pc unchanged if no parm */
 514          ptr = &remcomInBuffer[1];
 515          if (hexToInt(&ptr,&addr))
 516              registers[ PC ] = addr;
 517
 518          /* clear the trace bit */
 519          registers[ PS ] &= 0x7fff;
 520
 521          /* set the trace bit if we're stepping */
 522          if (remcomInBuffer[0] == 's') registers[ PS ] |= 0x8000;
 523
 524          /* Don't send a reply. The trap will signal GDB. */
 525          return;
 526
 527       /* kill the program */
 528       case 'k' :  /* do nothing */
 529                 break;
 530       } /* switch */
 531
 532     /* reply to the request */
 533     putpacket(remcomOutBuffer);
 534     }
 535 }
 536
 537 /* this function is used to set up exception handlers for tracing and
 538    breakpoints */
 539 void set_debug_traps()
 540 {
 541   initialized = 1;
 542
 543   DebugPutChar('\n');
 544   DebugPutChar('\r');
 545   DebugPutChar('g');
 546   DebugPutChar('d');
 547   DebugPutChar('b');
 548   DebugPutChar(' ');
 549   DebugPutChar('s');
 550   DebugPutChar('t');
 551   DebugPutChar('u');
 552   DebugPutChar('b');
 553 }
 554
 555 /* This function will generate a breakpoint exception.  It is used at the
 556    beginning of a program to sync up with a debugger and can be used
 557    otherwise as a quick means to stop program execution and "break" into
 558    the debugger. */
 559
 560 void breakpoint()
 561 {
 562   if (initialized) BREAKPOINT();
 563 }
 564
 565 void gdbstub()
 566 {
 567   set_debug_traps();
 568 }
 569
 570 /**************** end of gdbstub.c, rest is AROS interface ***********/
 571
 572 #include <aros/detach.h>
 573 #include <proto/dos.h>
 574 #include <proto/exec.h>
 575 #include <exec/execbase.h>
 576
 577 /* write a single character      */
 578 int DebugPutChar(register int x)
 579 {
 580     RawPutChar(x);
 581
 582     return 1;
 583 }
 584
 585 /* read and return a single char */
 586 int DebugGetChar()
 587 {
 588     LONG c;
 589     while ((c = RawMayGetChar()) < 0);
 590
 591     return (int)c;
 592 }
 593
 594 static ExceptionHook exceptionTable[256];
 595
 596 void exceptionHandler(int n, ExceptionHook a)
 597 {
 598     exceptionTable[n] = a;
 599 }
 600
 601 union M68K_Exception_Frame {
 602     struct {
 603         UWORD status;
 604         ULONG access;
 605         UWORD sr;
 606         ULONG pc;
 607     } m68000_bus;
 608     struct {
 609         UWORD sr;
 610         ULONG pc;
 611     } m68000;
 612     struct {
 613         UWORD sr;
 614         ULONG pc;
 615         UWORD vector;
 616         UWORD data[0];
 617     } m68010;
 618 };
 619
 620 void trapHandler_(union M68K_Exception_Frame *frame, ULONG id)
 621 {
 622     if (SysBase->AttnFlags & AFF_68010) {
 623         /* M68010+, any trap */
 624         registers[PS] = frame->m68010.sr;
 625         registers[PC] = frame->m68010.pc;
 626     } else if (id == 2 || id == 3) {
 627         /* M68000 Bus/Address trap */
 628         registers[PS] = frame->m68000_bus.sr;
 629         registers[PC] = frame->m68000_bus.pc;
 630     } else {
 631         /* M68000 other traps */
 632         registers[PS] = frame->m68000.sr;
 633         registers[PC] = frame->m68000.pc;
 634     }
 635
 636     /* TODO: Save FPU state */
 637
 638     if (id > 256 || exceptionTable[id] == NULL) {
 639         handle_exception(id);
 640     } else {
 641         (exceptionTable[id])(id);
 642     }
 643
 644     /* Restore registers */
 645     if (SysBase->AttnFlags & AFF_68010) {
 646         /* M68010+, any trap */
 647         frame->m68010.sr = registers[PS];
 648         frame->m68010.pc = registers[PC];
 649     } else if (id == 2 || id == 3) {
 650         /* M68000 Bus/Address trap */
 651         frame->m68000_bus.sr = registers[PS];
 652         frame->m68000_bus.pc = registers[PC];
 653     } else {
 654         /* M68000 other traps */
 655         frame->m68000.sr = registers[PS];
 656         frame->m68000.pc = registers[PC];
 657     }
 658 }
 659
 660 /* Wrapper for the trapHandler_ */
 661 extern void trapHandler(void);
 662 asm (
 663         "       .text\n"
 664         "       .global trapHandler\n"
 665         "trapHandler:\n"
 666         "       oriw    #0x0700,%sr\n"      /* Disable interrupts */
 667         "       movem.l %d0-%d7/%a0-%a6,registers\n"
 668         "       move.l  %usp,%a0\n"        /* registers[A7] = USP */
 669         "       lea.l   registers,%a1\n"
 670         "       move.l  %a0,%a1@(15*4)\n"
 671         "       lea.l   %sp@(+4),%a0\n"    /* A0 = exception frame */
 672         "       move.l  %a0,%sp@-\n"       /* Stack = Frame *, ID */
 673         "       jsr     trapHandler_\n"    /* Call C routine */
 674         "       addq    #8,%sp\n"          /* Pop off stack args */
 675         "       lea.l   registers,%a1\n"
 676         "       move.l  %a1@(15*4),%a0\n"
 677         "       move.l  %a0,%usp\n"        /* Save new USP */
 678         "       movem.l registers,%d0-%d7/%a0-%a6\n"    /* Restore regs */
 679         "       rte\n"                     /* Done! */
 680 );
 681
 682 static APTR oldAlert;
 683 AROS_UFH2(void, myAlert,
 684         AROS_UFHA(ULONG, alertNum, D7),
 685         AROS_UFHA(struct ExecBase *, SysBase, A6))
 686 {
 687     AROS_USERFUNC_INIT
 688
 689     breakpoint();
 690
 691     AROS_USERFUNC_EXIT
 692 }
 693
 694 static APTR oldAddTask;
 695 AROS_UFH4(APTR, myAddTask,
 696         AROS_UFHA(struct Task *,     task,      A1),
 697         AROS_UFHA(APTR,              initialPC, A2),
 698         AROS_UFHA(APTR,              finalPC,   A3),
 699         AROS_UFHA(struct ExecBase *, SysBase,   A6))
 700 {
 701     AROS_USERFUNC_INIT
 702
 703     APTR ret;
 704
 705     ret = AROS_UFC4(APTR, oldAddTask,
 706         AROS_UFCA(struct Task *,     task,      A1),
 707         AROS_UFCA(APTR,              initialPC, A2),
 708         AROS_UFCA(APTR,              finalPC,   A3),
 709         AROS_UFCA(struct ExecBase *, SysBase,   A6));
 710
 711     /* Set us up as the trap handler */
 712     task->tc_TrapCode = trapHandler;
 713
 714     return ret;
 715
 716     AROS_USERFUNC_EXIT
 717 }
 718
 719 static APTR UpdateTrapCode(APTR newHandler)
 720 {
 721     APTR oldHandler;
 722     struct Task *task;
 723
 724     /* Update SysBase default */
 725     oldHandler = SysBase->TaskTrapCode;
 726     SysBase->TaskTrapCode = newHandler;
 727
 728     /* And ourselves. */
 729     task = FindTask(NULL);
 730     task->tc_TrapCode = newHandler;
 731
 732     /* And any other tasks in the system.
 733      */
 734     ForeachNode(&SysBase->TaskReady, task) {
 735         if (task->tc_TrapCode == oldHandler)
 736             task->tc_TrapCode = newHandler;
 737     }
 738     ForeachNode(&SysBase->TaskWait, task) {
 739         if (task->tc_TrapCode == oldHandler)
 740             task->tc_TrapCode = newHandler;
 741     }
 742
 743     return oldHandler;
 744 }
 745
 746 int main(int argc, char **argv)
 747 {
 748     APTR DOSBase;
 749     APTR oldTaskTrapCode;
 750
 751     if ((DOSBase = OpenLibrary("dos.library",36))) {
 752         const char msg[] = "GDB trapping enabled on the serial port\n";
 753         /* We need to patch AddTask() to set
 754          * us up as the default stub for tc_TrapCode.
 755          *
 756          * Although this is not needed (yet) on AROS,
 757          * the Shell or Workbench may set tc_TrapCode
 758          * before AddTask() on AOS.
 759          *
 760          * So instead of modifying SysBase->TrapCode,
 761          * we have to inject this into the Exec Library.
 762          */
 763         /* Set up new handler
 764          */
 765         Disable();
 766         *(UWORD *)0 = 0x4e41;
 767         oldTaskTrapCode = UpdateTrapCode(trapHandler);
 768         oldAddTask  = SetFunction((struct Library *)SysBase, -47 * LIB_VECTSIZE, myAddTask);
 769         /* Patch Alert() to generate a breakpoint */
 770         oldAlert = SetFunction((struct Library *)SysBase, -18 * LIB_VECTSIZE, myAlert);
 771         Enable();
 772
 773         gdbstub();
 774         Write(Output(), msg, sizeof(msg)-1);
 775         Detach();
 776         Wait(SIGBREAKF_CTRL_C);
 777
 778         /* Restore traps. Not really safe, but better than nothing
 779          */
 780         Disable();
 781         SetFunction((struct Library *)SysBase, -47 * LIB_VECTSIZE, oldAddTask);
 782         SetFunction((struct Library *)SysBase, -18 * LIB_VECTSIZE, oldAlert);
 783         UpdateTrapCode(oldTaskTrapCode);
 784         Enable();
 785
 786         CloseLibrary(DOSBase);
 787     } else {
 788         return RETURN_ERROR;
 789     }
 790     return RETURN_OK;
 791 }