debugger.c

/* Emulator for LEGO RCX Brick, Copyright (C) 2003 Jochen Hoenicke.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 2, 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 Lesser General Public License
 * along with this program; see the file COPYING.LESSER.  If not, write to
 * the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA.
 *
 * $Id: debugger.c 112 2005-08-01 15:37:55Z hoenicke $
 */

#include <signal.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <sched.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>

#include "h8300.h"
#include "memory.h"
#include "peripherals.h"
#include "frame.h"
#include "socket.h"
//#define VERBOSE_DEBUG

#define BRICK_DEBUG_PORT 6789
#define SIGINT_EXCEPTION 7

#define NUM_REGS 13
#define NUM_REG_BYTES (NUM_REGS*sizeof(long))

int   debuggerfd;
int   monitorport;

static uint8 db_registers[NUM_REG_BYTES];
static int   serverfd, gdbfd;

static char db_in_buffer[2048];
static char db_out_buffer[2048];
static int db_len = 0, db_out_len = 0, db_cont;

static int remote_debug;

static int bptype2mask[6] = {
    MEMTYPE_BREAKPOINT, 
    MEMTYPE_BREAKPOINT, 
    MEMTYPE_WRITETRAP, 
    MEMTYPE_READTRAP,
    MEMTYPE_BREAKPOINT | MEMTYPE_WRITETRAP | MEMTYPE_READTRAP,
    MEMTYPE_BREAKPOINT, 
};
    
static const char hexchars[]="0123456789abcdef";
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);
}

/* 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* mem2hex(uint8 *mem, char *buf, int count) {
  int i;
  unsigned char ch;
  
  for (i=0;i<count;i++) {
      ch = *mem++;
      *buf++ = hexchars[ch >> 4];
      *buf++ = hexchars[ch % 16];
  }
  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 */
/*
 * This code also checks the data being written, and if it is gdb's
 * idea of a breakpoint (an h8/500 instruction, I think), it
 * translates it into 'bra #040ef' instruction.
 */
static char* hex2mem(char *buf, uint8 *mem, int count) {
  int i;
  unsigned char ch;

  if (count == 2) {
      char tbuff[5]; /* Drats . No memcmp, strncmp */
      memcpy(tbuff, buf, 4);
      tbuff[4] = 0;
      if (strcmp(tbuff, "5730") == 0) {
	memcpy(buf, "5f0c", 4);
      }
  }
  for (i=0;i<count;i++) {
    ch = hex(*buf++) << 4;
    ch = ch + hex(*buf++);
    *mem++ = ch;
  }
  return(mem);
}


static void db_send_packet() {
    struct sigaction sigact;
    struct sigaction oldsigact;

    if (gdbfd == -1) {
	db_out_len = 0;
	return;
    }

    sigact.sa_handler = SIG_IGN;
    sigemptyset(&sigact.sa_mask);
    sigact.sa_flags = SA_RESTART;
    sigaction(SIGPIPE, &sigact, &oldsigact);
    
#ifdef VERBOSE_DEBUG
    db_out_buffer[db_out_len] = 0;
    printf("Send db packet: '%s'\n", db_out_buffer);
#endif

    if (write(gdbfd, db_out_buffer, db_out_len) < 0) {
	printf ("Can't write packet :(\n");
	close(gdbfd);
	gdbfd = -1;
	debuggerfd = serverfd;
    }
    db_out_len = 0;

    sigaction(SIGPIPE, &oldsigact, NULL);
}

static void db_checksum_packet(int start) {
    unsigned char checksum = 0;
    int i;
    
    for (i = start; i < db_out_len; i++)
	checksum += db_out_buffer[i];
    
    db_out_buffer[db_out_len++] = '#';
    db_out_buffer[db_out_len++] = hexchars[checksum >> 4];
    db_out_buffer[db_out_len++] = hexchars[checksum & 0xf];
}

/**********************************************/
/* WHILE WE FIND NICE HEX CHARS, BUILD AN INT */
/* RETURN NUMBER OF CHARS PROCESSED           */
/**********************************************/
static int hexToInt(char **ptr, int *intValue) {
  int numChars = 0;
  int hexValue;
  
  *intValue = 0;
  while (**ptr) {
    hexValue = hex(**ptr);
    if (hexValue >=0) {
      *intValue = (*intValue <<4) | hexValue;
      numChars ++;
    }
    else
      break;
    
    (*ptr)++;
  }
  return (numChars);
}

static int sigval;

static void db_handle_packet(char* packet) {
    int start;
    int type, addr, length, i;

    db_out_buffer[db_out_len++] = '$';
    start = db_out_len;
    switch (*packet++) {
    case '?' :   
	db_out_buffer[db_out_len++] = 'S';
	db_out_buffer[db_out_len++] =  hexchars[sigval >> 4];
	db_out_buffer[db_out_len++] =  hexchars[sigval & 0xf];
	db_out_buffer[db_out_len++] =  0;
	break;
    case 'd' : 
	remote_debug = !(remote_debug);  /* toggle debug flag */
	break;
    case 'g' : /* return the value of the CPU registers */
	/*
	 * GDB expects 13 x 32 bit registers. The real register value is in
	 * the high 16 bits, the rest is 0 padded. The top 3 registers are
	 * not used by h8/300, and ignored (0 filled).
	 */
	
	memset(db_registers, 0, sizeof(db_registers));
	for (i = 0; i < 8; i++) {
	    db_registers[4*i]   = reg[i];
	    db_registers[4*i+1] = reg[i+8];
	}
	db_registers[33]  = ccr;
	db_registers[36]  = pc >> 8;
	db_registers[37]  = pc & 0xff;
	db_registers[40]  = (cycles >> 8) & 0xff;
	db_registers[41]  = cycles & 0xff;

	mem2hex(db_registers, 
		db_out_buffer + db_out_len, NUM_REG_BYTES);
	db_out_len += 2 * NUM_REG_BYTES;
	break;
    case 'G' : /* set the value of the CPU registers - return OK */
	hex2mem(packet, (char*) db_registers, NUM_REG_BYTES);
	db_out_buffer[db_out_len++] = 'O';
	db_out_buffer[db_out_len++] = 'K';
	break;
    case 'P' : /* set the value of a single CPU register - return OK */
	if (hexToInt (&packet, &addr) && *packet++ == '='
	    && addr >= 0 && addr < NUM_REGS) {
	    uint8 value[4];
	    hex2mem (packet, value, 4);
	    if (addr >= 0 && addr < 8) {
		reg[addr  ] = value[0];
		reg[addr+8] = value[1];
	    } else if (addr == 8) {
		ccr = value[1];
	    } else if (addr == 9) {
		pc = (value[0]<<8)+value[1];
	    }
	    db_out_buffer[db_out_len++] = 'O';
	    db_out_buffer[db_out_len++] = 'K';
	} else {
	    db_out_buffer[db_out_len++] = 'E';
	    db_out_buffer[db_out_len++] = '0';
	    db_out_buffer[db_out_len++] = '1';
	    break;
	}
	
	/* mAA..AA,LLLL  Read LLLL bytes at address AA..AA */
    case 'm' :

	if (hexToInt(&packet,&addr)
	    && *(packet++) == ','
	    && hexToInt(&packet,&length)) {

	    if (addr >= 0 && addr + length <= 0xff88) {
		mem2hex(&memory[addr], 
			db_out_buffer + db_out_len, length);
		db_out_len += 2 * length;
	    } else {
		db_out_buffer[db_out_len++] = 'E';
		db_out_buffer[db_out_len++] = '0';
		db_out_buffer[db_out_len++] = '3';
	    }
	} else {
	    db_out_buffer[db_out_len++] = 'E';
	    db_out_buffer[db_out_len++] = '0';
	    db_out_buffer[db_out_len++] = '1';
	}
	break;
	
	/* MAA..AA,LLLL: Write LLLL bytes at address AA.AA return OK */
    case 'M' :
	if (hexToInt(&packet,&addr) 
	    && *(packet++) == ','
	    && hexToInt(&packet,&length)
	    && *(packet++) == ':') {
	    
	    if (addr >= 0 && addr + length <= 0xff88) {
		/* we allow writing to ROM, but why not :) */
		hex2mem(packet, &memory[addr], length);
		db_out_buffer[db_out_len++] = 'O';
		db_out_buffer[db_out_len++] = 'K';
	    } else {
		db_out_buffer[db_out_len++] = 'E';
		db_out_buffer[db_out_len++] = '0';
		db_out_buffer[db_out_len++] = '3';
	    }
	    
	} else {
	    db_out_buffer[db_out_len++] = 'E';
	    db_out_buffer[db_out_len++] = '0';
	    db_out_buffer[db_out_len++] = '2';
	}
	break;
	
	/* cAA..AA    Continue at address AA..AA(optional) */
	/* sAA..AA   Step one instruction from AA..AA(optional) */
    case 's' :
    case 'c' :
	/* try to read optional parameter, pc unchanged if no parm */
	db_singlestep = *(packet - 1) == 's';
	db_trap = 0;
	if (hexToInt(&packet,&addr)) {
	    pc = addr;
	}
	db_out_len = start - 1;
	db_send_packet();
	return;

	/* detach */
    case 'D':
	db_cont = 1;
	break;

    case 'Z' :

	if (hexToInt(&packet,&type)
	    && *(packet++) == ','
	    && hexToInt(&packet,&addr)
	    && *(packet++) == ','
	    && hexToInt(&packet,&length)) {

	    if (addr >= 0 && addr + length <= 0xff88
		&& type >= 0 && type < 6) {
		int mask = bptype2mask[type];
		for (i = addr; i < addr + length; i++)
		    memtype[i] |= mask;
		db_out_buffer[db_out_len++] = 'O';
		db_out_buffer[db_out_len++] = 'K';
	    } else {
		db_out_buffer[db_out_len++] = 'E';
		db_out_buffer[db_out_len++] = '0';
		db_out_buffer[db_out_len++] = '3';
	    }
	} else {
	    db_out_buffer[db_out_len++] = 'E';
	    db_out_buffer[db_out_len++] = '0';
	    db_out_buffer[db_out_len++] = '1';
	}
	break;

    case 'z' :

	if (hexToInt(&packet,&type)
	    && *(packet++) == ','
	    && hexToInt(&packet,&addr)
	    && *(packet++) == ','
	    && hexToInt(&packet,&length)) {

	    if (addr >= 0 && addr + length <= 0xff88
		&& type >= 0 && type < 6) {
		int mask = bptype2mask[type];
		for (i = addr; i < addr + length; i++)
		    memtype[i] &= ~mask;
		db_out_buffer[db_out_len++] = 'O';
		db_out_buffer[db_out_len++] = 'K';
	    } else {
		db_out_buffer[db_out_len++] = 'E';
		db_out_buffer[db_out_len++] = '0';
		db_out_buffer[db_out_len++] = '3';
	    }
	} else {
	    db_out_buffer[db_out_len++] = 'E';
	    db_out_buffer[db_out_len++] = '0';
	    db_out_buffer[db_out_len++] = '1';
	}
	break;
	
	/* kill the program; ignore it */
    case 'k' :
	break;

    } /* switch */
    
    /* reply to the request */
    db_checksum_packet(start);
    db_send_packet();
}

static void db_parse_packet() {
    uint8 checksum;
    int i = 0;

    db_in_buffer[db_len] = 0;
    while (i < db_len) {
	while  (i < db_len && db_in_buffer[i] != '$') {
	    /* skip preceding garbage */
	    if (db_in_buffer[i] == '\003') {
		/* Ctrl-C pressed in debugger */
		db_trap = SIGINT_EXCEPTION;
	    }
#ifdef VERBOSE_DEBUG
	    else
		printf("preceding garbage: '%02x'\n", db_in_buffer[i]);
#endif
	    i++;
	}
	if (i >= db_len) {
	    db_len = 0;
	    return;
	}
	
	memcpy(db_in_buffer, db_in_buffer+i, db_len - i);
	db_len -= i;
	
	i = 1;
	checksum = 0;
	while (i < db_len - 2 && db_in_buffer[i] != '#') {
	    checksum += db_in_buffer[i++];
	}
	if (i == db_len - 2) {
	    return;
	}
	
	db_in_buffer[i+3] = 0;
	
	if (checksum != (hex(db_in_buffer[i+1]) << 4 | hex(db_in_buffer[i+2]))) {
	    printf("Illegal db packet: '%s'\n", db_in_buffer);
	    db_out_buffer[db_out_len++] = '-';
	    db_send_packet();
	} else {
#ifdef VERBOSE_DEBUG
	    printf("Got db packet: '%s'\n", db_in_buffer);
#endif
	    db_out_buffer[db_out_len++] = '+';
	    db_in_buffer[i] = 0;

	    /* if a sequence char is present, reply the sequence ID */
	    if (db_in_buffer[3] == ':') {
		db_out_buffer[db_out_len++] = db_in_buffer[1];
		db_out_buffer[db_out_len++] = db_in_buffer[2];
		db_in_buffer[0] = '#';
		db_handle_packet(db_in_buffer+4);
	    } else {
		db_handle_packet(db_in_buffer+1);
	    }
	    
	}
	i+=3;
	memcpy(db_in_buffer, db_in_buffer+i, db_len - i);
	db_len -= i;
    }
}

void db_handletrap() {
    int start;
    sigval = db_trap;

    if (gdbfd >= 0) {
	db_out_buffer[db_out_len++] = '$';
	start = db_out_len;
	db_out_buffer[db_out_len++] = 'S';
	db_out_buffer[db_out_len++] = hexchars[sigval >> 4];
	db_out_buffer[db_out_len++] = hexchars[sigval % 16];
	db_out_buffer[db_out_len++] = 0;
	db_checksum_packet(start);
	db_send_packet();
    } else {
	printf("Fault %d at %04x ... \n", db_trap, pc);
	dump_state();
    }
}

void db_handlefd() {
    int len;

#ifndef HMSMON_THREAD
    if (gdbfd < 0) {
	struct sockaddr addr;
	socklen_t addr_len = sizeof(addr);

	gdbfd = accept(serverfd, &addr, &addr_len);
	debuggerfd = gdbfd;
	printf("GDB connected!\n");
    }
#endif

    
    if ((len = read(gdbfd, db_in_buffer + db_len, 
		    sizeof(db_in_buffer) - db_len - 1)) == 0) {
	printf("debugger closed connection.\n");
	close(gdbfd);
	gdbfd = -1;
	debuggerfd = serverfd;
	/* try to rerun */
	db_trap = 0;
	return;
    }
    db_len += len;
    db_parse_packet();
}

static void sigint_handler(int sig) {
    db_trap = SIGINT_EXCEPTION;
}

#ifdef DB_THREAD
int db_thread(void *args) {
    struct sockaddr addr;
    socklen_t addr_len = sizeof(addr);

    while (1) {
	memset(&addr, 0, sizeof(addr));
	gdbfd = accept(serverfd, &addr, &addr_len);
	printf("GDB connected!\n");

	db_continueing = 0;
	db_trap = SIGINT_EXCEPTION;

	while (gdbfd >= 0) {
	    sleep(1);
	}
	printf("GDB connection closed.\n");
    }
}
#endif

void db_init() {
    struct sigaction sigact;

    gdbfd = -1;
    serverfd = create_anon_socket(&monitorport);
if (serverfd < 0) {
printf("Can't start debugging server\n");
		abort();
		return;
	    }
	printf("Debugging-Server started on port %d.\n", monitorport);
    debuggerfd = serverfd;

#ifdef DB_THREAD
    clone(db_thread, malloc(16*1024)+16*1024 - sizeof(long), 
	  CLONE_FS | CLONE_FILES | CLONE_VM,
	  NULL);
#endif

    sigact.sa_handler = sigint_handler;
    sigemptyset(&sigact.sa_mask);
    sigact.sa_flags = SA_RESTART;
    sigaction(SIGQUIT, &sigact, NULL);
}