grub-lakitu/grub-core/gdb/cstub.c - cos/cobble - Git at Google

 /* cstub.c - machine independent portion of remote GDB stub */
 /*
  *  Copyright (C) 2006  Lubomir Kundrak
  *
  *  This program is free software; you can redistribute it and/or modify
  *  it under the terms of the GNU General Public License as published by
  *  the Free Software Foundation; either version 2 of the License, or
  *  (at your option) any later version.
  *
  *  This program is distributed in the hope that it will be useful,
  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  *  GNU General Public License for more details.
  *
  *  You should have received a copy of the GNU General Public License
  *  along with this program; if not, write to the Free Software
  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  */

 #include <grub/misc.h>
 #include <grub/cpu/gdb.h>
 #include <grub/gdb.h>
 #include <grub/serial.h>
 #include <grub/backtrace.h>

 static const char hexchars[] = "0123456789abcdef";
 int grub_gdb_regs[GRUB_MACHINE_NR_REGS];

 #define GRUB_GDB_COMBUF_SIZE 400	/* At least sizeof(grub_gdb_regs)*2 are needed for
 					   register packets.  */
 static char grub_gdb_inbuf[GRUB_GDB_COMBUF_SIZE + 1];
 static char grub_gdb_outbuf[GRUB_GDB_COMBUF_SIZE + 1];

 struct grub_serial_port *grub_gdb_port;

 static int
 hex (char ch)
 {
   if ((ch >= 'a') && (ch <= 'f'))
     return (ch - 'a' + 10);
   if ((ch >= '0') && (ch <= '9'))
     return (ch - '0');
   if ((ch >= 'A') && (ch <= 'F'))
     return (ch - 'A' + 10);
   return (-1);
 }

 /* Scan for the sequence $<data>#<checksum>.  */
 static char *
 grub_gdb_getpacket (void)
 {
   char *buffer = &grub_gdb_inbuf[0];
   unsigned char checksum;
   unsigned char xmitcsum;
   int count;
   int ch;

   while (1)
     {
       /* Wait around for the start character, ignore all other
          characters.  */
       while ((ch = grub_serial_port_fetch (grub_gdb_port)) != '$');

     retry:
       checksum = 0;
       xmitcsum = -1;
       count = 0;

       /* Now read until a # or end of buffer is found.  */
       while (count < GRUB_GDB_COMBUF_SIZE)
 	{
 	  do
 	    ch = grub_serial_port_fetch (grub_gdb_port);
 	  while (ch < 0);
 	  if (ch == '$')
 	    goto retry;
 	  if (ch == '#')
 	    break;
 	  checksum += ch;
 	  buffer[count] = ch;
 	  count = count + 1;
 	}
       buffer[count] = 0;
       if (ch == '#')
 	{
 	  do
 	    ch = grub_serial_port_fetch (grub_gdb_port);
 	  while (ch < 0);
 	  xmitcsum = hex (ch) << 4;
 	  do
 	    ch = grub_serial_port_fetch (grub_gdb_port);
 	  while (ch < 0);
 	  xmitcsum += hex (ch);

 	  if (checksum != xmitcsum)
 	    grub_serial_port_put (grub_gdb_port, '-');	/* Failed checksum.  */
 	  else
 	    {
 	      grub_serial_port_put (grub_gdb_port, '+');	/* Successful transfer.  */

 	      /* If a sequence char is present, reply the sequence ID.  */
 	      if (buffer[2] == ':')
 		{
 		  grub_serial_port_put (grub_gdb_port, buffer[0]);
 		  grub_serial_port_put (grub_gdb_port, buffer[1]);

 		  return &buffer[3];
 		}
 	      return &buffer[0];
 	    }
 	}
     }
 }

 /* Send the packet in buffer.  */
 static void
 grub_gdb_putpacket (char *buffer)
 {
   grub_uint8_t checksum;

   /* $<packet info>#<checksum>.  */
   do
     {
       char *ptr;
       grub_serial_port_put (grub_gdb_port, '$');
       checksum = 0;

       for (ptr = buffer; *ptr; ptr++)
 	{
 	  grub_serial_port_put (grub_gdb_port, *ptr);
 	  checksum += *ptr;
 	}

       grub_serial_port_put (grub_gdb_port, '#');
       grub_serial_port_put (grub_gdb_port, hexchars[checksum >> 4]);
       grub_serial_port_put (grub_gdb_port, hexchars[checksum & 0xf]);
     }
   while (grub_serial_port_fetch (grub_gdb_port) != '+');
 }

 /* Convert the memory pointed to by mem into hex, placing result in buf.
    Return a pointer to the last char put in buf (NULL).  */
 static char *
 grub_gdb_mem2hex (char *mem, char *buf, grub_size_t count)
 {
   grub_size_t i;
   unsigned char ch;

   for (i = 0; i < count; i++)
     {
       ch = *mem++;
       *buf++ = hexchars[ch >> 4];
       *buf++ = hexchars[ch % 16];
     }
   *buf = 0;
   return (buf);
 }

 /* Convert the hex array pointed to by buf into binary to be placed in mem.
    Return a pointer to the character after the last byte written.  */
 static char *
 grub_gdb_hex2mem (char *buf, char *mem, int count)
 {
   int i;
   unsigned char ch;

   for (i = 0; i < count; i++)
     {
       ch = hex (*buf++) << 4;
       ch = ch + hex (*buf++);
       *mem++ = ch;
     }
   return (mem);
 }

 /* Convert hex characters to int and return the number of characters
    processed.  */
 static int
 grub_gdb_hex2int (char **ptr, grub_uint64_t *int_value)
 {
   int num_chars = 0;
   int hex_value;

   *int_value = 0;

   while (**ptr)
     {
       hex_value = hex (**ptr);
       if (hex_value >= 0)
 	{
 	  *int_value = (*int_value << 4) | hex_value;
 	  num_chars++;
 	}
       else
 	break;

       (*ptr)++;
     }

   return (num_chars);
 }

 /* This function does all command procesing for interfacing to gdb.  */
 void __attribute__ ((regparm(3)))
 grub_gdb_trap (int trap_no)
 {
   unsigned int sig_no;
   int stepping;
   grub_uint64_t addr;
   grub_uint64_t length;
   char *ptr;

   if (!grub_gdb_port)
     {
       grub_printf ("Unhandled exception 0x%x at ", trap_no);
       grub_backtrace_print_address ((void *) grub_gdb_regs[PC]);
       grub_printf ("\n");
       grub_backtrace_pointer ((void *) grub_gdb_regs[EBP]);
       grub_fatal ("Unhandled exception");
     }

   sig_no = grub_gdb_trap2sig (trap_no);

   ptr = grub_gdb_outbuf;

   /* Reply to host that an exception has occurred.  */

   *ptr++ = 'T';	/* Notify gdb with signo, PC, FP and SP.  */

   *ptr++ = hexchars[sig_no >> 4];
   *ptr++ = hexchars[sig_no & 0xf];

   /* Stack pointer.  */
   *ptr++ = hexchars[SP];
   *ptr++ = ':';
   ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[ESP], ptr, 4);
   *ptr++ = ';';

   /* Frame pointer.  */
   *ptr++ = hexchars[FP];
   *ptr++ = ':';
   ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[EBP], ptr, 4);
   *ptr++ = ';';

   /* Program counter.  */
   *ptr++ = hexchars[PC];
   *ptr++ = ':';
   ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[PC], ptr, 4);
   *ptr++ = ';';

   *ptr = '\0';

   grub_gdb_putpacket (grub_gdb_outbuf);

   stepping = 0;

   while (1)
     {
       grub_gdb_outbuf[0] = 0;
       ptr = grub_gdb_getpacket ();

       switch (*ptr++)
 	{
 	case '?':
 	  grub_gdb_outbuf[0] = 'S';
 	  grub_gdb_outbuf[1] = hexchars[sig_no >> 4];
 	  grub_gdb_outbuf[2] = hexchars[sig_no & 0xf];
 	  grub_gdb_outbuf[3] = 0;
 	  break;

 	/* Return values of the CPU registers.  */
 	case 'g':
 	  grub_gdb_mem2hex ((char *) grub_gdb_regs, grub_gdb_outbuf,
 			    sizeof (grub_gdb_regs));
 	  break;

 	/* Set values of the CPU registers -- return OK.  */
 	case 'G':
 	  grub_gdb_hex2mem (ptr, (char *) grub_gdb_regs,
 			    sizeof (grub_gdb_regs));
 	  grub_strcpy (grub_gdb_outbuf, "OK");
 	  break;

 	/* Set the value of a single CPU register -- return OK.  */
 	case 'P':
 	  {
 	    grub_uint64_t regno;

 	    if (grub_gdb_hex2int (&ptr, &regno) && *ptr++ == '=')
 	      if (regno < GRUB_MACHINE_NR_REGS)
 		{
 		  grub_gdb_hex2mem (ptr, (char *) &grub_gdb_regs[regno], 4);
 		  grub_strcpy (grub_gdb_outbuf, "OK");
 		  break;
 		}
 	    /* FIXME: GDB requires setting orig_eax. I don't know what's
 	       this register is about. For now just simulate setting any
 	       registers.  */
 	    grub_strcpy (grub_gdb_outbuf, /*"E01"*/ "OK");
 	    break;
 	  }

 	/* mAA..AA,LLLL: Read LLLL bytes at address AA..AA.  */
 	case 'm':
 	  /* Try to read %x,%x.  Set ptr = 0 if successful.  */
 	  if (grub_gdb_hex2int (&ptr, &addr))
 	    if (*(ptr++) == ',')
 	      if (grub_gdb_hex2int (&ptr, &length))
 		{
 		  ptr = 0;
 		  grub_gdb_mem2hex ((char *) (grub_addr_t) addr,
 				    grub_gdb_outbuf, length);
 		}
 	  if (ptr)
 	    grub_strcpy (grub_gdb_outbuf, "E01");
 	  break;

 	/* MAA..AA,LLLL: Write LLLL bytes at address AA.AA -- return OK.  */
 	case 'M':
 	  /* Try to read %x,%x.  Set ptr = 0 if successful.  */
 	  if (grub_gdb_hex2int (&ptr, &addr))
 	    if (*(ptr++) == ',')
 	      if (grub_gdb_hex2int (&ptr, &length))
 		if (*(ptr++) == ':')
 		  {
 		    grub_gdb_hex2mem (ptr, (char *) (grub_addr_t) addr, length);
 		    grub_strcpy (grub_gdb_outbuf, "OK");
 		    ptr = 0;
 		  }
 	  if (ptr)
 	    {
 	      grub_strcpy (grub_gdb_outbuf, "E02");
 	    }
 	  break;

 	/* sAA..AA: Step one instruction from AA..AA(optional).  */
 	case 's':
 	  stepping = 1;
 	  /* FALLTHROUGH */

 	/* cAA..AA: Continue at address AA..AA(optional).  */
 	case 'c':
 	  /* try to read optional parameter, pc unchanged if no parm */
 	  if (grub_gdb_hex2int (&ptr, &addr))
 	    grub_gdb_regs[PC] = addr;

 	  /* Clear the trace bit.  */
 	  grub_gdb_regs[PS] &= 0xfffffeff;

 	  /* Set the trace bit if we're stepping.  */
 	  if (stepping)
 	    grub_gdb_regs[PS] |= 0x100;

 	  return;

 	/* Kill the program.  */
 	case 'k':
 	  /* Do nothing.  */
 	  return;
 	}

       /* Reply to the request.  */
       grub_gdb_putpacket (grub_gdb_outbuf);
     }
 }
	/* cstub.c - machine independent portion of remote GDB stub */
	/*
	* Copyright (C) 2006 Lubomir Kundrak
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License as published by
	* the Free Software Foundation; either version 2 of the License, or
	* (at your option) any later version.
	*
	* This program is distributed in the hope that it will be useful,
	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	* GNU General Public License for more details.
	*
	* You should have received a copy of the GNU General Public License
	* along with this program; if not, write to the Free Software
	* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
	*/

	#include <grub/misc.h>
	#include <grub/cpu/gdb.h>
	#include <grub/gdb.h>
	#include <grub/serial.h>
	#include <grub/backtrace.h>

	static const char hexchars[] = "0123456789abcdef";
	int grub_gdb_regs[GRUB_MACHINE_NR_REGS];

	#define GRUB_GDB_COMBUF_SIZE 400 /* At least sizeof(grub_gdb_regs)*2 are needed for
	register packets. */
	static char grub_gdb_inbuf[GRUB_GDB_COMBUF_SIZE + 1];
	static char grub_gdb_outbuf[GRUB_GDB_COMBUF_SIZE + 1];

	struct grub_serial_port *grub_gdb_port;

	static int
	hex (char ch)
	{
	if ((ch >= 'a') && (ch <= 'f'))
	return (ch - 'a' + 10);
	if ((ch >= '0') && (ch <= '9'))
	return (ch - '0');
	if ((ch >= 'A') && (ch <= 'F'))
	return (ch - 'A' + 10);
	return (-1);
	}

	/* Scan for the sequence $<data>#<checksum>. */
	static char *
	grub_gdb_getpacket (void)
	{
	char *buffer = &grub_gdb_inbuf[0];
	unsigned char checksum;
	unsigned char xmitcsum;
	int count;
	int ch;

	while (1)
	{
	/* Wait around for the start character, ignore all other
	characters. */
	while ((ch = grub_serial_port_fetch (grub_gdb_port)) != '$');

	retry:
	checksum = 0;
	xmitcsum = -1;
	count = 0;

	/* Now read until a # or end of buffer is found. */
	while (count < GRUB_GDB_COMBUF_SIZE)
	{
	do
	ch = grub_serial_port_fetch (grub_gdb_port);
	while (ch < 0);
	if (ch == '$')
	goto retry;
	if (ch == '#')
	break;
	checksum += ch;
	buffer[count] = ch;
	count = count + 1;
	}
	buffer[count] = 0;
	if (ch == '#')
	{
	do
	ch = grub_serial_port_fetch (grub_gdb_port);
	while (ch < 0);
	xmitcsum = hex (ch) << 4;
	do
	ch = grub_serial_port_fetch (grub_gdb_port);
	while (ch < 0);
	xmitcsum += hex (ch);

	if (checksum != xmitcsum)
	grub_serial_port_put (grub_gdb_port, '-'); /* Failed checksum. */
	else
	{
	grub_serial_port_put (grub_gdb_port, '+'); /* Successful transfer. */

	/* If a sequence char is present, reply the sequence ID. */
	if (buffer[2] == ':')
	{
	grub_serial_port_put (grub_gdb_port, buffer[0]);
	grub_serial_port_put (grub_gdb_port, buffer[1]);

	return &buffer[3];
	}
	return &buffer[0];
	}
	}
	}
	}

	/* Send the packet in buffer. */
	static void
	grub_gdb_putpacket (char *buffer)
	{
	grub_uint8_t checksum;

	/* $<packet info>#<checksum>. */
	do
	{
	char *ptr;
	grub_serial_port_put (grub_gdb_port, '$');
	checksum = 0;

	for (ptr = buffer; *ptr; ptr++)
	{
	grub_serial_port_put (grub_gdb_port, *ptr);
	checksum += *ptr;
	}

	grub_serial_port_put (grub_gdb_port, '#');
	grub_serial_port_put (grub_gdb_port, hexchars[checksum >> 4]);
	grub_serial_port_put (grub_gdb_port, hexchars[checksum & 0xf]);
	}
	while (grub_serial_port_fetch (grub_gdb_port) != '+');
	}

	/* Convert the memory pointed to by mem into hex, placing result in buf.
	Return a pointer to the last char put in buf (NULL). */
	static char *
	grub_gdb_mem2hex (char mem, char buf, grub_size_t count)
	{
	grub_size_t i;
	unsigned char ch;

	for (i = 0; i < count; i++)
	{
	ch = *mem++;
	*buf++ = hexchars[ch >> 4];
	*buf++ = hexchars[ch % 16];
	}
	*buf = 0;
	return (buf);
	}

	/* Convert the hex array pointed to by buf into binary to be placed in mem.
	Return a pointer to the character after the last byte written. */
	static char *
	grub_gdb_hex2mem (char buf, char mem, int count)
	{
	int i;
	unsigned char ch;

	for (i = 0; i < count; i++)
	{
	ch = hex (*buf++) << 4;
	ch = ch + hex (*buf++);
	*mem++ = ch;
	}
	return (mem);
	}

	/* Convert hex characters to int and return the number of characters
	processed. */
	static int
	grub_gdb_hex2int (char *ptr, grub_uint64_t int_value)
	{
	int num_chars = 0;
	int hex_value;

	*int_value = 0;

	while (**ptr)
	{
	hex_value = hex (**ptr);
	if (hex_value >= 0)
	{
	int_value = (int_value << 4) \| hex_value;
	num_chars++;
	}
	else
	break;

	(*ptr)++;
	}

	return (num_chars);
	}

	/* This function does all command procesing for interfacing to gdb. */
	void __attribute__ ((regparm(3)))
	grub_gdb_trap (int trap_no)
	{
	unsigned int sig_no;
	int stepping;
	grub_uint64_t addr;
	grub_uint64_t length;
	char *ptr;

	if (!grub_gdb_port)
	{
	grub_printf ("Unhandled exception 0x%x at ", trap_no);
	grub_backtrace_print_address ((void *) grub_gdb_regs[PC]);
	grub_printf ("\n");
	grub_backtrace_pointer ((void *) grub_gdb_regs[EBP]);
	grub_fatal ("Unhandled exception");
	}

	sig_no = grub_gdb_trap2sig (trap_no);

	ptr = grub_gdb_outbuf;

	/* Reply to host that an exception has occurred. */

	ptr++ = 'T'; / Notify gdb with signo, PC, FP and SP. */

	*ptr++ = hexchars[sig_no >> 4];
	*ptr++ = hexchars[sig_no & 0xf];

	/* Stack pointer. */
	*ptr++ = hexchars[SP];
	*ptr++ = ':';
	ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[ESP], ptr, 4);
	*ptr++ = ';';

	/* Frame pointer. */
	*ptr++ = hexchars[FP];
	*ptr++ = ':';
	ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[EBP], ptr, 4);
	*ptr++ = ';';

	/* Program counter. */
	*ptr++ = hexchars[PC];
	*ptr++ = ':';
	ptr = grub_gdb_mem2hex ((char *) &grub_gdb_regs[PC], ptr, 4);
	*ptr++ = ';';

	*ptr = '\0';

	grub_gdb_putpacket (grub_gdb_outbuf);

	stepping = 0;

	while (1)
	{
	grub_gdb_outbuf[0] = 0;
	ptr = grub_gdb_getpacket ();

	switch (*ptr++)
	{
	case '?':
	grub_gdb_outbuf[0] = 'S';
	grub_gdb_outbuf[1] = hexchars[sig_no >> 4];
	grub_gdb_outbuf[2] = hexchars[sig_no & 0xf];
	grub_gdb_outbuf[3] = 0;
	break;

	/* Return values of the CPU registers. */
	case 'g':
	grub_gdb_mem2hex ((char *) grub_gdb_regs, grub_gdb_outbuf,
	sizeof (grub_gdb_regs));
	break;

	/* Set values of the CPU registers -- return OK. */
	case 'G':
	grub_gdb_hex2mem (ptr, (char *) grub_gdb_regs,
	sizeof (grub_gdb_regs));
	grub_strcpy (grub_gdb_outbuf, "OK");
	break;

	/* Set the value of a single CPU register -- return OK. */
	case 'P':
	{
	grub_uint64_t regno;

	if (grub_gdb_hex2int (&ptr, &regno) && *ptr++ == '=')
	if (regno < GRUB_MACHINE_NR_REGS)
	{
	grub_gdb_hex2mem (ptr, (char *) &grub_gdb_regs[regno], 4);
	grub_strcpy (grub_gdb_outbuf, "OK");
	break;
	}
	/* FIXME: GDB requires setting orig_eax. I don't know what's
	this register is about. For now just simulate setting any
	registers. */
	grub_strcpy (grub_gdb_outbuf, /"E01"/ "OK");
	break;
	}

	/* mAA..AA,LLLL: Read LLLL bytes at address AA..AA. */
	case 'm':
	/* Try to read %x,%x. Set ptr = 0 if successful. */
	if (grub_gdb_hex2int (&ptr, &addr))
	if (*(ptr++) == ',')
	if (grub_gdb_hex2int (&ptr, &length))
	{
	ptr = 0;
	grub_gdb_mem2hex ((char *) (grub_addr_t) addr,
	grub_gdb_outbuf, length);
	}
	if (ptr)
	grub_strcpy (grub_gdb_outbuf, "E01");
	break;

	/* MAA..AA,LLLL: Write LLLL bytes at address AA.AA -- return OK. */
	case 'M':
	/* Try to read %x,%x. Set ptr = 0 if successful. */
	if (grub_gdb_hex2int (&ptr, &addr))
	if (*(ptr++) == ',')
	if (grub_gdb_hex2int (&ptr, &length))
	if (*(ptr++) == ':')
	{
	grub_gdb_hex2mem (ptr, (char *) (grub_addr_t) addr, length);
	grub_strcpy (grub_gdb_outbuf, "OK");
	ptr = 0;
	}
	if (ptr)
	{
	grub_strcpy (grub_gdb_outbuf, "E02");
	}
	break;

	/* sAA..AA: Step one instruction from AA..AA(optional). */
	case 's':
	stepping = 1;
	/* FALLTHROUGH */

	/* cAA..AA: Continue at address AA..AA(optional). */
	case 'c':
	/* try to read optional parameter, pc unchanged if no parm */
	if (grub_gdb_hex2int (&ptr, &addr))
	grub_gdb_regs[PC] = addr;

	/* Clear the trace bit. */
	grub_gdb_regs[PS] &= 0xfffffeff;

	/* Set the trace bit if we're stepping. */
	if (stepping)
	grub_gdb_regs[PS] \|= 0x100;

	return;

	/* Kill the program. */
	case 'k':
	/* Do nothing. */
	return;
	}

	/* Reply to the request. */
	grub_gdb_putpacket (grub_gdb_outbuf);
	}
	}