arch/nios2nommu/kernel/nios_gdb_stub.c

   1 // Modified for uClinux - Vic - Apr 2002
   2 // From:
   3
   4 // File: nios_gdb_stub.c
   5 // Date: 2000 June 20
   6 // Author dvb \ Altera Santa Cruz
   7
   8 #ifndef __KERNEL__
   9 #include "nios.h"
  10 #else
  11 #include <linux/kernel.h>
  12 #include <linux/sched.h>
  13 #include <asm/nios.h>
  14 #endif
  15
  16 #include "nios_gdb_stub.h"
  17
  18 #define na_debug_peripheral_irq 8
  19
  20 enum
  21 {
  22   na_BreakpointTrap     = 3,
  23   na_SingleStepTrap     = 4,
  24   na_StartGDBTrap       = 5
  25 };
  26
  27
  28 #ifdef __KERNEL__
  29
  30 extern int _etext;
  31
  32 static void puts( unsigned char *s )
  33 {
  34         while(*s) {
  35                 while (!(nasys_printf_uart->np_uartstatus & np_uartstatus_trdy_mask));
  36                 nasys_printf_uart->np_uarttxdata = *s++;
  37         }
  38 }
  39
  40 #endif  // __KERNEL__
  41
  42 // --------------------------------
  43 // Local Prototypes
  44
  45 #if GDB_DEBUG_PRINT
  46
  47 static void StringFit(char *s,int w);
  48
  49 // --------------------------------
  50 // Debugging The Debugger
  51
  52 void GDB_RawMessage(char *s)
  53         {
  54         StringFit(s,32);
  55         nr_pio_lcdwritescreen(s);
  56         }
  57 #else
  58         #define GDB_RawMessage(a,b,c)   // define away to nothing
  59 #endif
  60
  61 #if GDB_DEBUG_PRINT
  62 void GDB_Print2(char *s,int n1,int n2)
  63         {
  64         char st[1000];
  65
  66         sprintf(st,s,n1,n2);
  67         GDB_RawMessage(st);
  68         }
  69 #else
  70         #define GDB_Print2(a,b,c)       // define away to nothing
  71 #endif
  72
  73 // If string is longer than w, cut out the middle.
  74
  75 #if GDB_DEBUG_PRINT
  76 int StringLen(char *s)
  77         {
  78         int l = 0;
  79
  80         while(*s++)
  81                 l++;
  82         return l;
  83         }
  84
  85 static void StringFit(char *s,int w)
  86         {
  87         if(StringLen(s) > w)
  88                 {
  89                 int i;
  90
  91
  92                 w = w / 2;
  93
  94                 for(i = 0; i < w; i++)
  95                         {
  96                         s[i + w] = s[StringLen(s) - w + i];
  97                         }
  98                 s[w + w] = 0;
  99                 }
 100         }
 101 #endif
 102
 103 // ---------------------------------------------
 104 // Generic routines for dealing with
 105 // hex input, output, and parsing
 106 // (Adapted from other stubs.)
 107
 108 NiosGDBGlobals gdb = {0};               // not static: the ISR uses it!
 109
 110 static char dHexChars[16] = "0123456789abcdef";
 111
 112 /*
 113  * HexCharToValue -- convert a characters
 114  *                   to its hex value, or -1 if not.
 115  */
 116 char HexCharToValue(char c)
 117 {
 118         char result=0;
 119
 120         if(c >= '0' && c <= '9')
 121                 result = c - '0';
 122         else if(c >= 'a' && c <= 'f')
 123                 result = c - 'a' + 10;
 124         else if(c >= 'A' && c <= 'F')
 125                 result = c - 'A' + 10;
 126         else
 127                 result = -1;
 128         return result;
 129 }
 130
 131 /*
 132  * HexStringToValue -- convert a 2*byte_width string of characters
 133  *                   to its little endian hex value,
 134  *                   or -1 if not.
 135  *              This routine is for strings of hex values
 136  */
 137 unsigned long HexStringToValue(char *c, int byte_width)
 138 {
 139         unsigned long result=0;
 140         unsigned char a,b;
 141         int i=0;
 142
 143         while (i < byte_width)
 144         {
 145                 a = HexCharToValue(*c++);
 146                 if (a & 0x80) return a;
 147                 b = HexCharToValue(*c++);
 148                 if (b & 0x80) return b;
 149                 b = (a<<4) | (b&0x0f);
 150                 result |= b << (i*8);
 151                 i++;
 152         }
 153         return result;
 154 }
 155
 156 /*
 157  * Hex2Value -- convert a non-hex char delimited string
 158  *              to its big endian hex value.
 159  *              This routine is for address and byte count values
 160  */
 161
 162 char *Hex2Value(char *hexIn, int *valueOut)
 163         {
 164         char c;
 165         int digitValue;
 166         int value = 0;
 167
 168         while(1)
 169                 {
 170                 c = *hexIn;
 171                 digitValue = HexCharToValue(c);
 172                 if(digitValue < 0)
 173                         {
 174                         *valueOut = value;
 175                         return hexIn;
 176                         }
 177                 hexIn++;
 178                 value = (value << 4) + digitValue;
 179                 }
 180         }
 181
 182 /*
 183  * HexToMem -- convert a string to a specified
 184  *             number of bytes in memory.
 185  *
 186  *              JMB -- make this thing a bit smarter so
 187  *                         that it selects the byte width to
 188  *                         write based on the number of bytes
 189  *                         and the destination address alignment.
 190  *                         This is to support writes to non-byte enabled
 191  *                         peripheral registers...I don't like it.
 192  *                         Beware! there are cases where it wont work
 193  */
 194 char *HexToMem(char *hexIn, char *memOut, int memByteCount)
 195 {
 196         int i;
 197         unsigned long x;
 198         short *memOutS=0;
 199         long *memOutL=0;
 200         int byte_width;
 201
 202         //determine maximum byte width
 203         if (((memByteCount%2) != 0) || (((unsigned int)memOut%2) != 0))
 204                 byte_width = 1;
 205         else if (((memByteCount % 4) != 0) || (((unsigned int)memOut % 4) != 0))
 206         {
 207                 byte_width = 2;
 208                 memOutS = (short *)memOut;
 209         }
 210         else
 211         {
 212                 byte_width = 4;
 213                 memOutL = (long *)memOut;
 214         }
 215         for(i = 0; i < memByteCount; i+=byte_width)
 216         {
 217                 x = HexStringToValue(hexIn,byte_width);
 218                 hexIn += byte_width*2;
 219                 switch (byte_width)
 220                 {
 221                 case 1:
 222                         *memOut++ = (unsigned char) 0x000000ff & x;
 223                         break;
 224                 case 2:
 225                         *memOutS++ = (unsigned short) 0x0000ffff & x;
 226                         break;
 227                 case 4:
 228                         *memOutL++ = x;
 229                         break;
 230                 default:
 231                         //How could this ever happen???
 232                         break;
 233                 }
 234         }
 235
 236         return hexIn;
 237 }
 238
 239 char *MemToHex(char *memIn, char *hexOut, int memByteCount)
 240 {
 241         int i,j;
 242         int byte_width;
 243         unsigned long x=0;
 244         unsigned short *memInS=0;
 245         unsigned long *memInL=0;
 246
 247         //determine maximum byte width
 248         if (((memByteCount % 2) != 0) || (((unsigned int)memIn % 2) != 0))
 249                 byte_width = 1;
 250         else if (((memByteCount % 4) != 0) || (((unsigned int)memIn % 4) != 0))
 251         {
 252                 byte_width = 2;
 253                 memInS = (short *)memIn;
 254         }
 255         else
 256         {
 257                 byte_width = 4;
 258                 memInL = (long *)memIn;
 259         }
 260
 261         for(i = 0; i < memByteCount; i+=byte_width)
 262         {
 263                 switch (byte_width)
 264                 {
 265                 case 1:
 266                         x = *memIn++;
 267                         break;
 268                 case 2:
 269                         x = *memInS++;
 270                         break;
 271                 case 4:
 272                         x = *memInL++;
 273                         break;
 274                 default:
 275                         //How would we get here?
 276                         break;
 277                 }
 278
 279                 for (j=0; j<byte_width; j++)
 280                 {
 281                         *hexOut++ = dHexChars[(x&0x000000f0)>>4];
 282                         *hexOut++ = dHexChars[x&0x0000000f];
 283                         x = x>>8;
 284                 }
 285         }
 286
 287         *hexOut = 0;
 288
 289         return hexOut;
 290 }
 291
 292 //Send just the + or - to indicate
 293 //ACK or NACK
 294 void GDBPutAck (char ack)
 295 {
 296         if (gdb.comlink == ne_gdb_serial)
 297                 GDBPutChar (ack);
 298 #ifdef ETHER_DEBUG
 299 #ifdef ethernet_exists
 300         else
 301         {
 302                 if (gdb.host_ip_address != 0)
 303                         nr_plugs_send_to (gdb.gdb_eth_plug, &ack, 1, 0,
 304                                 gdb.host_ip_address,
 305                                 gdb.host_port_number);
 306         }
 307 #endif
 308 #endif
 309 }
 310
 311 /*
 312  * Once a $ comes in, use GetGDBPacket to
 313  * retrieve a full gdb packet, and verify
 314  * checksum, and reply + or -.
 315  */
 316 int GetGDBPacket(char *aBuffer)
 317 {
 318         int checksum=0;
 319         int length=0;
 320         char c;
 321         int x=0;
 322
 323         if (gdb.comlink == ne_gdb_serial)
 324         {
 325                 while ((c = GDBGetChar ()) != '$') ;
 326
 327 startPacket:
 328                 length = 0;
 329                 checksum = 0;
 330                 while(((c = GDBGetChar()) != '#') && (length < kTextBufferSize))
 331                 {
 332                         if(c == '$')
 333                                 goto startPacket;
 334                         checksum += c;
 335                         aBuffer[length++] = c;
 336                         aBuffer[length] = 0;
 337                 }
 338
 339                 c = GDBGetChar();
 340                 x = HexCharToValue(c) << 4;
 341                 c = GDBGetChar();
 342                 x += HexCharToValue(c);
 343
 344
 345                 checksum &= 0xff;
 346
 347                 GDB_Print2("GetPacket %d",length,0);
 348         }
 349 #ifdef ETHER_DEBUG
 350 #ifdef ethernet_exists
 351         else
 352         {
 353                 int srcidx;
 354                 // wait till beginning of packet
 355                 while (gdb.textBuffer[0] != '$') nr_plugs_idle();
 356 startEPacket:
 357                 length = 0;
 358                 checksum = 0;
 359                 srcidx = 1;
 360
 361                 //loop until packet terminator
 362                 //leave enough room for the checksum at the end
 363                 while (((c = gdb.textBuffer[srcidx++]) != '#') && (srcidx < kTextBufferSize-2))
 364                 {
 365                         if (c == '$')
 366                                 goto startEPacket;
 367
 368                         checksum += c;
 369                         aBuffer[length++] = c;
 370                 }
 371
 372                 c = gdb.textBuffer[srcidx++];
 373                 x = HexCharToValue(c) << 4;
 374                 c = gdb.textBuffer[srcidx++];
 375                 x += HexCharToValue (c);
 376
 377                 aBuffer[length++] = 0;
 378
 379                 checksum &= 0xff;
 380
 381                 GDB_Print2("GetPacket %d",length,0);
 382         }
 383 #endif
 384 #endif
 385
 386         if(checksum != x)
 387         {
 388                 GDBPutAck('-');
 389                 length = 0;
 390         }
 391         else
 392         {
 393                 GDBPutAck('+');
 394         }
 395         return length;
 396 }
 397
 398 //Wait for acknowledgement
 399 //Should we have some way of timing out???
 400 //return TRUE if ACK
 401 //return FALSE if NACK
 402 int GDBGetACK (void)
 403 {
 404         char c;
 405         if (gdb.comlink == ne_gdb_serial)
 406         {
 407                 while (1)
 408                 {
 409                         c = GDBGetChar ();
 410                         if (c == '+') return (1);
 411                         else if (c == '-') return (0);
 412                 }
 413
 414         }
 415 #ifdef ETHER_DEBUG
 416 #ifdef ethernet_exists
 417         else
 418         {
 419                 gdb.ACKstatus = ne_gdb_ack_waiting;
 420                 while (1)
 421                 {
 422                         nr_plugs_idle ();
 423                         if (gdb.ACKstatus == ne_gdb_ack_acked)
 424                         {
 425                                 gdb.ACKstatus = ne_gdb_ack_notwaiting;
 426                                 return (1);
 427                         }
 428                         else if (gdb.ACKstatus == ne_gdb_ack_nacked)
 429                         {
 430                                 gdb.ACKstatus = ne_gdb_ack_notwaiting;
 431                                 return (0);
 432                         }
 433                 }
 434         }
 435 #endif
 436 #endif
 437         return(0);
 438 }
 439
 440 /*
 441  * Send a packet, preceded by $,
 442  * and followed by #checksum.
 443  */
 444 void PutGDBPacket(char *aBuffer)
 445 {
 446         int checksum;
 447         char c;
 448         char *origPtr;
 449         int cnt=0;
 450
 451         origPtr = aBuffer; // Remember in case we get a NACK
 452         if (gdb.comlink == ne_gdb_serial)
 453         {
 454 startPutSerial:
 455                 GDBPutChar('$');
 456                 checksum = 0;
 457                 while((c = *aBuffer++) != 0)
 458                 {
 459                         checksum += c;
 460                         GDBPutChar(c);
 461                 }
 462                 GDBPutChar('#');
 463                 GDBPutChar(dHexChars[(checksum >> 4) & 15]);
 464                 GDBPutChar(dHexChars[checksum & 15]);
 465
 466                 if (!GDBGetACK ())
 467                 {
 468                   aBuffer = origPtr;
 469                         if (++cnt < GDB_RETRY_CNT) goto startPutSerial;
 470                 }
 471         }
 472 #ifdef ETHER_DEBUG
 473 #ifdef ethernet_exists
 474         else
 475         {
 476                 if (gdb.host_ip_address != 0)
 477                 {
 478                         int i;
 479                         int result;
 480                         char c1;
 481
 482                         i = 0;
 483                         c = aBuffer[i];
 484                         if (c==0) return; //there is no data in packet, so why bother sending
 485                         aBuffer[i++] = '$';
 486                         checksum = 0;
 487                         do
 488                         {
 489                                 checksum += c;
 490                                 c1 = aBuffer[i];
 491                                 aBuffer[i++] = c;
 492                                 c = c1;
 493                         } while (c != 0);
 494
 495                         aBuffer[i++] = '#';
 496                         aBuffer[i++] = dHexChars[(checksum >> 4) & 15];
 497                         aBuffer[i++] = dHexChars[checksum & 15];
 498                         aBuffer[i++] = 0;
 499 startPutEth:
 500                         result = nr_plugs_send_to (gdb.gdb_eth_plug, aBuffer, i, 0,
 501                                 gdb.host_ip_address,
 502                                 gdb.host_port_number);
 503
 504                         if (!GDBGetACK ())
 505                         {
 506                                 if (++cnt < GDB_RETRY_CNT) goto startPutEth;
 507                         }
 508                         aBuffer[0] = 0; //clear packet to
 509                 }
 510         }
 511 #endif
 512 #endif
 513 }
 514
 515 int PutTracePacket(char *aBuffer, int size)
 516 {
 517         int checksum;
 518 #ifdef ethernet_exists
 519         char c;
 520 #endif
 521         int i;
 522         int cnt=0;
 523
 524         if (gdb.comlink == ne_gdb_serial)
 525         {
 526 startPutSerial:
 527                 GDBPutChar('$');
 528                 checksum = 0;
 529                 for (i=0; i<size; i++)
 530                 {
 531                         checksum += aBuffer[i];
 532                         GDBPutChar (aBuffer[i]);
 533                 }
 534                 GDBPutChar('#');
 535                 GDBPutChar(dHexChars[(checksum >> 4) & 15]);
 536                 GDBPutChar(dHexChars[checksum & 15]);
 537
 538                 if (!GDBGetACK ())
 539                 {
 540                         if (++cnt < GDB_RETRY_CNT) goto startPutSerial;
 541                 }
 542         }
 543 #ifdef ETHER_DEBUG
 544 #ifdef ethernet_exists
 545         else
 546         {
 547                 int result;
 548                 char c1;
 549
 550                 checksum = 0;
 551                 c = '$';
 552                 for (i=0; i<size; i++)
 553                 {
 554                         checksum += aBuffer[i];
 555                         c1 = aBuffer[i];
 556                         aBuffer[i] = c;
 557                         c = c1;
 558                 }
 559                 aBuffer[i++] = c;
 560
 561                 aBuffer[i++] = '#';
 562                 aBuffer[i++] = dHexChars[(checksum >> 4) & 15];
 563                 aBuffer[i++] = dHexChars[checksum & 15];
 564                 aBuffer[i++] = 0;
 565 ethResend:
 566                 if (gdb.host_ip_address != 0)
 567                 {
 568                         result = nr_plugs_send_to (gdb.gdb_eth_plug, aBuffer, i, 0,
 569                                 gdb.host_ip_address,
 570                                 gdb.host_port_number);
 571                 }
 572                 if (!GDBGetACK ())
 573                 {
 574                         if (++cnt < GDB_RETRY_CNT) goto ethResend;
 575                 }
 576                 aBuffer[0]=0;
 577         }
 578 #endif
 579 #endif
 580         if (cnt < GDB_RETRY_CNT) return 1;
 581         else return 0;
 582 }
 583
 584 void PutGDBOKPacket(char *aBuffer)
 585         {
 586         aBuffer[0] = 'O';
 587         aBuffer[1] = 'K';
 588         aBuffer[2] = 0;
 589         PutGDBPacket(aBuffer);
 590         }
 591
 592 #if nasys_debug_core
 593
 594 //some defines used exclusively for TRACE data xfer
 595 //stepsize is the ascii hex step value i.e. twice the binary length
 596 #define stepsize (2*(2*sizeof(int) + sizeof (char)))
 597 #define MAX_TRACE_BYTES (((int)((2*MAX_DATA_SIZE-2)/stepsize))*stepsize)
 598
 599 int Trace_Read_Intercept (char *aBuffer)
 600 {
 601         int cnt=0;
 602         unsigned int data;
 603         unsigned char code;
 604         int byteCount;
 605         unsigned char *w;
 606         unsigned short dataAccumulate;
 607         int status;
 608
 609         w = aBuffer;
 610         w++;    //skip past the m
 611         if (*w++ == 't')  //see if this is a special "memory trace" packet
 612         {
 613                 w = Hex2Value(w,&byteCount); //get the number of bytes to transfer
 614
 615                 //turn byteCount to a multiple of stepsize
 616                 byteCount = ((int)(byteCount/stepsize))*stepsize;
 617
 618                 //wait until fifo empties
 619                 nm_debug_get_reg(status, np_debug_write_status);
 620                 while (status&np_debug_write_status_writing_mask) nm_debug_get_reg(status,np_debug_write_status);
 621
 622                 // loop through total size
 623                 while (byteCount > 0)
 624                 {
 625                         w=aBuffer;      //reset w to beginning of buffer
 626
 627                         //calculate the number of bytes in this packet
 628                         if (byteCount > MAX_TRACE_BYTES) dataAccumulate = MAX_TRACE_BYTES;
 629                         else dataAccumulate = byteCount;
 630
 631                         //insert data size at beginning of packet
 632                         w = MemToHex((char *)&dataAccumulate, w, sizeof (dataAccumulate));
 633
 634                         byteCount -= dataAccumulate; //decrement byteCount
 635
 636                         // accumulate a full buffer
 637                         for (cnt=0; cnt<dataAccumulate; cnt+=stepsize)
 638                         {
 639                                 int valid;
 640                                 nm_debug_set_reg (1, np_debug_read_sample); //begin transaction
 641
 642                                 //wait until data is ready
 643                                 nm_debug_get_reg (valid, np_debug_data_valid);
 644                                 while (!valid) nm_debug_get_reg(valid,np_debug_data_valid) ;
 645
 646                                 nm_debug_get_reg (data, np_debug_trace_address);
 647                                 w = MemToHex ((char *)&data, w, sizeof (int));
 648
 649                                 nm_debug_get_reg (data, np_debug_trace_data);
 650                                 w = MemToHex ((char *)&data, w, sizeof (int));
 651
 652                                 nm_debug_get_reg (data, np_debug_trace_code);
 653                                 w = MemToHex ((char *)&data, w, sizeof (char));
 654                         }
 655
 656                         //if one of our data packets doesn't make it, stop sending them
 657                         //if (PutTracePacket (aBuffer,dataAccumulate+4) != 1) //+4 for size filed
 658                         //      byteCount = 0;
 659                         /* kenw - My module can't handle the incoming data fast enough.  So
 660                          * send this one packet, and wait for another mt command.
 661                          */
 662                         PutTracePacket (aBuffer,dataAccumulate+4);
 663                         byteCount = 0;
 664                 }
 665                 return 1;
 666         }
 667         return 0;
 668 }
 669
 670 /*
 671 #undef stepsize
 672 #undef MAX_TRACE_BYTES
 673 */
 674
 675 #endif
 676
 677 void DoGDBCommand_m(char *aBuffer)
 678         {
 679         char *w;
 680         int startAddr,byteCount;
 681
 682 #if nasys_debug_core
 683         /* intercept some access to the dbg peripheral */
 684         if (Trace_Read_Intercept (aBuffer)) return;
 685 #endif
 686
 687         w = aBuffer;
 688         w++;                            // past 'm'
 689         w = Hex2Value(w,&startAddr);
 690         w++;                            // past ','
 691         w = Hex2Value(w,&byteCount);
 692
 693         if (byteCount > MAX_DATA_SIZE) byteCount = MAX_DATA_SIZE;
 694
 695         // mA,L -- request memory
 696         w = aBuffer;
 697         w = MemToHex((char *)startAddr,w,byteCount);
 698         PutGDBPacket(aBuffer);
 699         }
 700
 701 void DoGDBCommand_M(char *aBuffer)
 702         {
 703         char *w;
 704         int startAddr,byteCount;
 705
 706         w = aBuffer;
 707         w++;                            // past 'M'
 708         w = Hex2Value(w,&startAddr);
 709         w++;                            // past ','
 710         w = Hex2Value(w,&byteCount);
 711         w++;                            // past ':'
 712
 713         GDB_Print2("M from %x to %x",startAddr,byteCount);
 714
 715         // MA,L:values -- write to memory
 716
 717         w = HexToMem(w,(char *)startAddr,byteCount);
 718
 719         // Send "OK"
 720         PutGDBOKPacket(aBuffer);
 721         }
 722
 723 int Debug_Read_Intercept (char *aBuffer)
 724 {
 725         unsigned int data;
 726         int index;
 727         unsigned char *w;
 728
 729         w = aBuffer;
 730         w++;    //skip past the g
 731         if (*w++ == 'g')  //see if this is a special "register read" packet
 732         {
 733             w = Hex2Value(w,&index); //get the index of the register to be read
 734
 735             nm_debug_get_reg (data, index);
 736
 737             //assemble the output packet
 738             w=aBuffer;  //reset w to beginning of buffer
 739             w = MemToHex((char *)&data, w, sizeof (data));
 740             *w++ = 0;
 741
 742             //now send it
 743             PutTracePacket (aBuffer,sizeof (data) * 2);
 744
 745             return 1;
 746         }
 747         return 0;
 748 }
 749
 750 // Return the values of all the registers
 751 void DoGDBCommand_g(NiosGDBGlobals *g)
 752         {
 753         char *w;
 754
 755         if (Debug_Read_Intercept (g->textBuffer)) return;
 756
 757         w = g->textBuffer;
 758
 759         w = MemToHex((char *)(&g->registers),w,sizeof(g->registers));
 760         PutGDBPacket(g->textBuffer);
 761         GDB_Print2("Sent            Registers",0,0);
 762         }
 763
 764 int Debug_Write_Intercept (char *aBuffer)
 765 {
 766         unsigned int data;
 767         int index;
 768         unsigned char *w;
 769
 770         w = aBuffer;
 771         w++;    //skip past the g
 772         if (*w++ == 'g')  //see if this is a special "register read" packet
 773         {
 774             w = Hex2Value(w,&index); //get the index of the register to be written
 775             w++;                                // past ','
 776             w = Hex2Value(w,&data);
 777
 778             nm_debug_set_reg (data, index);
 779
 780             //now send it
 781             // Send "OK"
 782             PutGDBOKPacket(aBuffer);
 783
 784             return 1;
 785         }
 786         return 0;
 787 }
 788
 789 void DoGDBCommand_G(NiosGDBGlobals *g)
 790         {
 791         char *w;
 792
 793         if (Debug_Write_Intercept (g->textBuffer)) return;
 794
 795         w = g->textBuffer;
 796         w++;    // skip past 'G'
 797         w = HexToMem(w,(char *)(&g->registers), sizeof(g->registers) );
 798
 799         // Send "OK"
 800         PutGDBOKPacket(g->textBuffer);
 801
 802         GDB_Print2("Received        Registers",0,0);
 803         }
 804
 805 // Return last signal value
 806 void DoGDBCommand_qm(NiosGDBGlobals *g)
 807         {
 808         char *w;
 809
 810         w = g->textBuffer;
 811
 812         *w++ = 'S';
 813         *w++ = '2';
 814         *w++ = '3';     // make up a signal for now...
 815         *w++ = 0;
 816         PutGDBPacket(g->textBuffer);
 817         }
 818
 819 void DoGDBCommand_q(NiosGDBGlobals *g)
 820 {
 821 #ifdef na_ssram_detect_in
 822         short int* ssram_exists;
 823 #endif
 824         char *w;
 825         w = g->textBuffer;
 826
 827         w++;    /* skip past the q */
 828         switch (*w) {
 829                 case ('A'):
 830                         w = g->textBuffer;
 831
 832                         /* handle intialization information */
 833                         /* is nios_ocd available? */
 834 #ifdef nasys_debug_core
 835                         *w++ = nasys_debug_core + '0';
 836 #else
 837                         *w++ = '0';
 838 #endif
 839                         *w++ = ',';
 840
 841                         /* determine if the SSRAM debugger board is
 842                          * physically present */
 843 #ifdef na_ssram_detect_in
 844                         ssram_exists = (short int*) na_ssram_detect_in;
 845                         *w++ = !(*ssram_exists) + '0';
 846 #else
 847                         *w++ = '0';
 848 #endif
 849                         *w++ = ',';
 850
 851                         /* print out the max size of a trace packet */
 852 #if nasys_debug_core
 853                         sprintf (w, "%04x", MAX_TRACE_BYTES);
 854 #else
 855                         sprintf (w, "0000");
 856 #endif
 857
 858                         break;
 859                 case ('B'):
 860                         w = g->textBuffer;
 861
 862                         /* returns 1 if it was an OCD interrupt
 863                          * returns 0 if it was software breakpoint */
 864                         if (gdb.trapNumber == nasys_debug_core_irq) {
 865                                 *w++ = '1';
 866                         } else {
 867                                 *w++ = '0';
 868                         }
 869
 870                         *w++ = 0;
 871                         break;
 872                 default:
 873                         w = g->textBuffer;
 874
 875                         *w = 0;
 876                         break;
 877         }
 878
 879         PutGDBPacket(g->textBuffer);
 880 }
 881
 882
 883 void GDBInsertBreakpoint(NiosGDBGlobals *g,short *address)
 884         {
 885         NiosGDBBreakpoint *b;
 886
 887         GDB_Print2("breakpoint 0x%x",(int)address,0);
 888         if(g->breakpointCount < kMaximumBreakpoints)
 889                 {
 890                 b = &g->breakpoint[g->breakpointCount++];
 891                 b->address = address;
 892                 b->oldContents = *b->address;
 893                 *b->address = 0x7904;
 894                 }
 895         }
 896
 897 void GDBRemoveBreakpoints(NiosGDBGlobals *g)
 898         {
 899         NiosGDBBreakpoint *b;
 900         int i;
 901
 902         for(i = 0; i < g->breakpointCount; i++)
 903                 {
 904                 b = &g->breakpoint[i];
 905                 *b->address = b->oldContents;
 906                 b->address = 0;
 907                 }
 908
 909         g->breakpointCount = 0;
 910         }
 911
 912 int NiosInstructionIsTrap5(unsigned short instruction)
 913         {
 914         return instruction == 0x7905;
 915         }
 916
 917 int NiosInstructionIsPrefix(unsigned short instruction)
 918         {
 919         return (instruction >> 11) == 0x13;
 920         }
 921
 922 int NiosInstructionIsSkip(unsigned short instruction)
 923         {
 924         int op6;
 925         int op11;
 926
 927         op6 = (instruction >> 10);
 928         op11 = (instruction >> 5);
 929
 930         return (op6 == 0x14             // SKP0
 931                 || op6 == 0x15          // SKP1
 932                 || op11 == 0x3f6        // SKPRz
 933                 || op11 == 0x3f7        // SKPS
 934                 || op11 == 0x3fa);      // SKPRnz
 935         }
 936
 937 int NiosInstructionIsBranch(unsigned short instruction,short *pc,short **branchTargetOut)
 938         {
 939         int op4;
 940         int op7;
 941         int op10;
 942         short *branchTarget = 0;
 943         int result = 0;
 944
 945         op4 = (instruction >> 12);
 946         op7 = (instruction >> 9);
 947         op10 = (instruction >> 6);
 948
 949         if(op4 == 0x08)         // BR, BSR
 950                 {
 951                 int offset;
 952
 953                 result = 1;
 954                 offset = instruction & 0x07ff;
 955                 if(offset & 0x400)      // sign extend
 956                         offset |= 0xffffF800;
 957                 branchTarget = pc + offset + 1; // short * gets x2 scaling automatically
 958                 }
 959         else if(op10 == 0x1ff)  // JMP, CALL
 960                 {
 961                 result = 1;
 962                 branchTarget = (short *)(gdb.registers.r[instruction & 31] * 2);
 963                 }
 964         else if(op7 == 0x3d)    // JMPC, CALLC
 965                 {
 966                 result = 1;
 967                 branchTarget = pc + 1 + (instruction & 0x0ffff);
 968 #ifdef __nios32__
 969                 branchTarget = (short *)((int)branchTarget & 0xffffFFFc);       // align 32...
 970 #else
 971                 branchTarget = (short *)((int)branchTarget & 0xFFFe);           // align 16...
 972 #endif
 973                 branchTarget = (short *)(*(int *)branchTarget);
 974                 }
 975
 976         if(branchTargetOut)
 977                 *branchTargetOut = branchTarget;
 978
 979         return result;
 980         }
 981
 982 // -------------------------
 983 // Step at address
 984 //
 985 // "stepping" involves inserting a
 986 // breakpoint at some reasonable
 987 // spot later than the current program
 988 // counter
 989 //
 990 // On the Nios processor, this is
 991 // nontrivial. For example, we should
 992 // not break up a PFX instruction.
 993
 994 void DoGDBCommand_s(NiosGDBGlobals *g)
 995         {
 996         char *w;
 997         int x;
 998         short *pc;
 999         short *branchTarget;
1000         unsigned short instruction;
1001         int stepType;
1002
1003         /*
1004          * First, if there's an argument to the packet,
1005          * set the new program-counter value
1006          */
1007
1008         w = g->textBuffer;
1009         w++;
1010         if(HexCharToValue(*w) >= 0)
1011                 {
1012                 w = Hex2Value(w,&x);
1013                 g->registers.pc = x;
1014                 }
1015
1016         /*
1017          * Scan forward to see what the
1018          * most appropriate location(s) for
1019          * a breakpoint will be.
1020          *
1021          * The rules are:
1022          *  1. If *pc == PFX, break after modified instruction.
1023          *  2. If *pc == BR,BSR,JMP,CALL, break at destination
1024          *  3. If *pc == SKIP, break right after SKIP AND after optional instruction,
1025                          which might, of course, be prefixed.
1026          *  4. Anything else, just drop in the breakpoint.
1027          */
1028
1029         pc = (short *)(int)g->registers.pc;
1030
1031         instruction = *pc;
1032         stepType = 0;
1033
1034         if(NiosInstructionIsPrefix(instruction))
1035                 {
1036                 /*
1037                  * PFX instruction: skip til after it
1038                  */
1039                 while(NiosInstructionIsPrefix(instruction))
1040                         {
1041                         pc++;
1042                         instruction = *pc;
1043                         }
1044
1045                 GDBInsertBreakpoint(g,pc + 1);
1046                 stepType = 1;
1047                 }
1048         else if(NiosInstructionIsBranch(instruction,pc,&branchTarget))
1049                 {
1050                 GDBInsertBreakpoint(g,branchTarget);
1051                 stepType = 2;
1052                 }
1053         else if(NiosInstructionIsSkip(instruction))
1054                 {
1055                 short *pc2;
1056                 stepType = 3;
1057
1058                 /*
1059                  * Skip gets to breaks: one after the skippable instruction,
1060                  * and the skippable instruction itself.
1061                  *
1062                  * Since Skips know how to skip over PFX's, we have to, too.
1063                  */
1064                 pc2 = pc;       // the Skip instruction
1065                 do
1066                         {
1067                         pc2++;
1068                         } while(NiosInstructionIsPrefix(*pc2));
1069                 // pc2 now points to first non-PFX after Skip
1070                 GDBInsertBreakpoint(g,pc2+1);
1071                 GDBInsertBreakpoint(g,pc+1);
1072                 }
1073         else
1074                 GDBInsertBreakpoint(g,pc+1);            // the genericest case
1075
1076         GDB_Print2("Program Steppingat 0x%x (%d)",g->registers.pc,stepType);
1077         }
1078
1079 // -----------------------------
1080 // Continue at address
1081
1082 void DoGDBCommand_c(NiosGDBGlobals *g)
1083         {
1084         char *w;
1085         int x;
1086         w = g->textBuffer;
1087
1088         w++;            // past command
1089
1090         // Anything in the packet? if so,
1091         // use it to set the PC value
1092
1093         if(HexCharToValue(*w) >= 0)
1094                 {
1095                 w = Hex2Value(w,&x);
1096                 g->registers.pc = x;
1097                 }
1098
1099         GDB_Print2("Program Running at 0x%x",g->registers.pc,0);
1100         }
1101
1102 // ----------------------
1103 // Kill
1104
1105 void DoGDBCommand_k(NiosGDBGlobals *g)
1106         {
1107         return;
1108         }
1109
1110
1111 /*
1112  * If we've somehow skidded
1113  * to a stop just after a PFX instruction
1114  * back up the program counter by one.
1115  *
1116  * That way, we can't end up with an accidentally-unprefixed
1117  * instruction.
1118  *
1119  * We do this just before we begin running
1120  * again, so that when the host queries our
1121  * registers, we report the place we actually
1122  * stopped.
1123  */
1124
1125 void MaybeAdjustProgramCounter(NiosGDBGlobals *g)
1126         {
1127         short instruction;
1128         if(g->registers.pc)
1129                 {
1130                 instruction = *(short *)(int)(g->registers.pc - 2);
1131                 if(NiosInstructionIsPrefix(instruction))
1132                         g->registers.pc -= 2;
1133                 else
1134                         {
1135                         // If the *current* instruction is Trap5, we must skip it!
1136                         instruction = *(short *)(int)(g->registers.pc);
1137                         if(NiosInstructionIsTrap5(instruction))
1138                                 g->registers.pc += 2;
1139                         }
1140                 }
1141         }
1142
1143 /*
1144  * GDBMainLoop - this is the main processing loop
1145  * for the GDB stub.
1146  */
1147 void GDBMainLoop (void)
1148 {
1149         while(1)
1150         {
1151                 if (GetGDBPacket(gdb.textBuffer) > 0)
1152                 {
1153
1154                         GDB_Print2(gdb.textBuffer,0,0);
1155                         switch(gdb.textBuffer[0])
1156                         {
1157                         case 's':
1158                                 DoGDBCommand_s(&gdb);
1159                                 goto startRunning;
1160                                 break;
1161
1162                         case 'c':       // continue
1163                                 DoGDBCommand_c(&gdb);
1164
1165                                 // if the PC is something other than 0, it's
1166                                 // probably ok to exit and go there
1167
1168                         startRunning:
1169                                 if(gdb.registers.pc)
1170                                 {
1171                                         MaybeAdjustProgramCounter(&gdb);
1172                                         return;
1173                                 }
1174                                 break;
1175
1176                         case 'm':       // memory read
1177                                 DoGDBCommand_m(gdb.textBuffer);
1178                                 break;
1179
1180                         case 'M':       // memory set
1181                                 DoGDBCommand_M(gdb.textBuffer);
1182                                 break;
1183
1184                         case 'g':       // registers read
1185                                 DoGDBCommand_g(&gdb);
1186                                 break;
1187
1188                         case 'G':       //registers set
1189                                 DoGDBCommand_G(&gdb);
1190                                 break;
1191
1192                         case 'k':       //kill process
1193                                 DoGDBCommand_k(&gdb);
1194                                 break;
1195
1196                         case '?':       // last exception value
1197                                 DoGDBCommand_qm(&gdb);
1198                                 break;
1199
1200                         case 'q':
1201                                 DoGDBCommand_q(&gdb);
1202                                 break;
1203
1204                         default:        // return empty packet, means "yeah yeah".
1205                                 gdb.textBuffer[0] = 0;
1206                                 PutGDBPacket(gdb.textBuffer);
1207                         break;
1208                         }
1209                 }
1210         }
1211
1212 }
1213
1214 // ----------main------------
1215 void GDBMain(void)
1216 {
1217         int i;
1218
1219         for(i = 0; i < kTextBufferSize; i++)
1220                 gdb.textBuffer[i] = i;
1221
1222         GDBRemoveBreakpoints(&gdb);
1223
1224 #ifdef __KERNEL__
1225 /*
1226  * Inform the user that they need to add the symbol file for the application
1227  * that is just starting up.  Display the  .text  .data  .bss  regions.
1228  */
1229         if (gdb.trapNumber == 5) {
1230                 extern struct task_struct *_current_task;
1231                 sprintf(gdb.textBuffer,
1232                                 "\r\n\nGDB: trap 5 at 0x%08lX", gdb.registers.pc);
1233                 puts(gdb.textBuffer);
1234                 if (_current_task) {
1235                         if ( _current_task->mm->start_code > _etext )
1236                                 sprintf(gdb.textBuffer,
1237                                         "\r\nGDB: Enter the following command in the nios-elf-gdb Console Window:"
1238                                         "\r\nGDB:    add-symbol-file %s.abself 0x%08lX 0x%08lX 0x%08lX\r\n\n",
1239                                         _current_task->comm,
1240                                         (unsigned long)_current_task->mm->start_code,
1241                                         (unsigned long)_current_task->mm->start_data,
1242                                         (unsigned long)_current_task->mm->end_data );
1243                         else
1244                                 sprintf(gdb.textBuffer,
1245                                         ", kernel process: %s\r\n", _current_task->comm );
1246                 } else
1247                         sprintf(gdb.textBuffer,
1248                                 ", kernel process unknown\r\n" );
1249                 puts(gdb.textBuffer);
1250         }
1251 #endif
1252
1253         // Send trapnumber for breakpoint encountered. No other signals.
1254
1255         gdb.textBuffer[0] = 'S';
1256         gdb.textBuffer[1] = '0';
1257
1258 #if nasys_debug_core
1259         if (gdb.trapNumber == nasys_debug_core_irq)
1260         {
1261             /* gdb.textBuffer[2] = '8'; */
1262             gdb.textBuffer[2] = '5';
1263         }
1264         else
1265         {
1266             gdb.textBuffer[2] = '5';
1267         }
1268 #else
1269         gdb.textBuffer[2] = '5';
1270 #endif
1271         gdb.textBuffer[3] = 0;
1272         PutGDBPacket(gdb.textBuffer);
1273
1274         GDB_Print2("Trap %2d         At 0x%x",
1275                 gdb.trapNumber,gdb.registers.pc);
1276 //      printf ("Trap %d at 0x%x\n",gdb.trapNumber,gdb.registers.pc);
1277 //      for (i=0;i<32;i++) printf ("    register[%d] = 0x%x\n",i,gdb.registers.r[i]);
1278
1279         GDBMainLoop ();
1280 }
1281
1282 // +----------------------------------
1283 // | gdb_eth_proc -- gets called for udp packets
1284 // | from the host bound for gdb stub
1285 #ifdef ETHER_DEBUG
1286 #ifdef ethernet_exists
1287 int gdb_eth_proc(int plug_handle,
1288                 void *context,
1289                 ns_plugs_packet *p,
1290                 void *payload,
1291                 int payload_length)
1292 {
1293         int i;
1294         char *buf = (char *)payload;
1295         // if this is a stop request, set a flag to stop after nr_plugs_idle
1296         // leave it up to the host to prevent stops from being sent while stub is running???
1297
1298         if (*buf == 3) gdb.stop = 1;
1299
1300         // if we're waiting for an ack, check that here
1301         if (gdb.ACKstatus == ne_gdb_ack_waiting)
1302         {
1303                 if (buf[0] == '+')
1304                 {
1305                         gdb.ACKstatus = ne_gdb_ack_acked;
1306                         return 0;
1307                 }
1308                 else if (buf[0] == '-')
1309                 {
1310                         gdb.ACKstatus = ne_gdb_ack_nacked;
1311                         return 0;
1312                 }
1313         }
1314         strcpy (gdb.textBuffer, buf);   //all commands should be zero terminated strings
1315
1316         gdb.textBuffer[payload_length] = 0;     //terminate string
1317
1318         gdb.host_ip_address=((ns_plugs_ip_packet *)(p[ne_plugs_ip].header))->source_ip_address;
1319         gdb.host_port_number=((ns_plugs_udp_packet *)(p[ne_plugs_udp].header))->source_port;
1320
1321         return 0;
1322 }
1323
1324 int nr_dbg_plugs_idle (void)
1325 {
1326         int result;
1327
1328         result = nr_plugs_idle ();
1329         if (gdb.stop)
1330         {
1331                 gdb.stop = 0;
1332 //;dgt2;tmp;            asm ("TRAP #5");
1333         }
1334         return result;
1335 }
1336 #endif
1337 #endif
1338
1339
1340 /*
1341  * int main(void)
1342  *
1343  * All we really do here is install our trap # 3,
1344  * and call it once, so that we're living down in
1345  * the GDBMain, trap handler.
1346  */
1347
1348 extern int StubBreakpointHandler;
1349 extern int StubHarmlessHandler;
1350 #if nasys_debug_core
1351 extern int StubHWBreakpointHandler;
1352 #endif
1353 #ifdef nasys_debug_uart
1354 extern int StubUartHandler;
1355 #endif
1356
1357 void gdb_local_install(int active)
1358 {
1359         unsigned int *vectorTable;
1360         unsigned int stubBreakpointHandler;
1361         unsigned int stubHarmlessHandler;
1362 #if nasys_debug_core
1363         unsigned int stubHWBreakpointHandler;
1364 #endif
1365
1366         gdb.breakpointCount = 0;
1367         gdb.textBuffer[0] = 0;
1368
1369         vectorTable = (int *)nasys_vector_table;
1370         stubBreakpointHandler = ( (unsigned int)(&StubBreakpointHandler) ) >> 1;
1371         stubHarmlessHandler = ( (unsigned int)(&StubHarmlessHandler) ) >> 1;
1372 #if nasys_debug_core
1373         stubHWBreakpointHandler = ( (unsigned int)(&StubHWBreakpointHandler) ) >> 1;
1374 #endif
1375
1376         /*
1377          * Breakpoint & single step both go here
1378          */
1379         vectorTable[na_BreakpointTrap] = stubBreakpointHandler;
1380         vectorTable[na_SingleStepTrap] = stubBreakpointHandler;
1381         vectorTable[na_StartGDBTrap] = active ? stubBreakpointHandler : stubHarmlessHandler;
1382         /*
1383          * If it exists, Hardware Breakpoint has a different entry point
1384          */
1385 #if nasys_debug_core
1386         vectorTable[na_debug_peripheral_irq] = stubHWBreakpointHandler;
1387 #endif
1388
1389 #ifndef __KERNEL__
1390 #ifdef nasys_debug_uart
1391         if (gdb.comlink == ne_gdb_serial)
1392         {
1393                 np_uart *uart = (np_uart *)nasys_debug_uart;
1394                 unsigned int stubUartHandler = ((unsigned int)(&StubUartHandler)) >> 1;
1395
1396                 vectorTable[nasys_debug_uart_irq] = stubUartHandler;      //set Uart int vector
1397                 uart->np_uartcontrol = np_uartcontrol_irrdy_mask; //enable Rx intr
1398         }
1399 #endif
1400 #endif
1401 }
1402
1403 void nios_gdb_install(int active)
1404 {
1405         gdb.comlink = ne_gdb_serial;
1406         gdb_local_install (active);
1407 }
1408
1409 #ifdef ETHER_DEBUG
1410 #ifdef ethernet_exists
1411 void nios_gdb_install_ethernet (int active)
1412 {
1413         int result;
1414         host_16 host_port = GDB_ETH_PORT;
1415
1416         gdb.comlink = ne_gdb_ethernet;
1417         gdb_local_install (active);
1418
1419         result = nr_plugs_create (&gdb.gdb_eth_plug, ne_plugs_udp, host_port, gdb_eth_proc, 0, 0);
1420         //if unabled to open ethernet plug, switch back to default serial interface
1421         if (result)
1422         {
1423                 printf ("nr_plugs_create failed %d\n",result);
1424                 gdb.comlink = ne_gdb_serial;
1425                 return;
1426         }
1427         result = nr_plugs_connect (gdb.gdb_eth_plug, 0, -1, -1);
1428         if (result)
1429         {
1430                 printf ("nr_plugs_connect fialed %d\n",result);
1431                 gdb.comlink = ne_gdb_serial;
1432                 return;
1433         }
1434 }
1435 #endif
1436 #endif
1437
1438 #ifdef nios_gdb_breakpoint
1439         #undef nios_gdb_breakpoint
1440 #endif
1441
1442 void nios_gdb_breakpoint(void)
1443         {
1444         /*
1445          * If you arrived here, you didn't include
1446          * the file "nios_peripherals.h", which
1447          * defines nios_gdb_breakpoint as a
1448          * macro that expands to TRAP 5.
1449          *
1450          * (No problem, you can step out
1451          * of this routine.)
1452          */
1453 //;dgt2;tmp;    asm("TRAP 5");
1454         }
1455
1456 // end of file