/* This file implements support code for remote gdb vxshell debugger.
 * This file contains the VxWorks target side code.
 * Basically this file implements VxWorks task which communicates
 * with the host side GDB program for debugging remotely.  It
 * accomplishes this by communicating over TCP/IP socket, via custom
 * remote debugging "protocol" and handling requests sent by the
 * host side GDB program by performing target side debugging related
 * work, such as actually inserting breakpoints, continuing from a
 * stopped breakpoint, etc.  The actual work is done via use of
 * the VxWorks target resident shell debugger (dbgLib).
 *
 * 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., 59
 * Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

/* Written by Hwa-Jin Bae (Email: bae@mail.com)
 * This file is distributed under GNU GPL.
 */

#include "vxWorks.h"
#include "stdio.h"
#include "sys/types.h"
#include "stdarg.h"
#include "unistd.h"
#include "string.h"
#include "taskLib.h"
#include "ctype.h"
#include "sockLib.h"
#include "socket.h"
#include "selectLib.h"
#include "taskLib.h"
#include "ioLib.h"
#include "taskArchLib.h"
#include "regs.h"
#include "private/taskLibP.h"
#include "private/windLibP.h"
#include "sigLib.h"
#include "taskHookLib.h"
#include "errnoLib.h"
#include "config.h"
#include "telnetLib.h"
#include "in.h"
#include "if.h"
#include "if_ether.h"
#include "ioctl.h"
#include "ctype.h"
#include "inetLib.h"
#include "strLib.h"
#include "in_var.h"
#include "errno.h"
#include "ifLib.h"
#include "regs.h"
#include "assert.h"
#include "dbgLib.h"
#include "semLib.h"

extern int      execute();
extern int      taskIdFigure(int);
extern int      sp();
extern int      sysClkRateGet();

#define GDB_PORT  9999
#define GDB_PROMPT "gdb-synch\r\n"
#define GDB_BREAK_PROMPT "gdb-break\r\n"
#define GDB_MAX_BREAK 100

SEMAPHORE       gdb_sem_data;
SEM_ID		gdb_sem	= &gdb_sem_data;

struct gdb_bs_info {
    int val;
    int addr;
};

struct gdb_bs_info      gdb_break_shadow[GDB_MAX_BREAK];

int		gdb_debug = 0;
int		gdb_sock = 0;
WIND_TCB       *gdb_work_task = 0;
WIND_TCB       *gdb_break_tcb = 0;
WIND_TCB       *gdb_server_task = 0;
FILE	       *gdb_fp = 0;
int		gdb_server_sock	= 0;
int		gdb_curtid_init	= 0;
int		gdb_curtid = 0;

char		gdb_pad[1024];
char		gdb_buf[1024];

#define NUM_REGS 18
struct myregs {
    unsigned int    regs[NUM_REGS];     /* d0-7/a0-7/sr/pc */
};

extern int      sysMemTop();

int
gdb_validate_addr(addr)
{
    /* system RAM or flash area (assume both flash parts are present ) XXX */

    if (((unsigned int) addr >= (unsigned int) LOCAL_MEM_LOCAL_ADRS &&
	 (unsigned int)	addr < (unsigned int) sysMemTop()) ||
        ((unsigned int) addr >= 0 &&
	 (unsigned int)	addr < (unsigned int) 0x200000)) {
        return 0;
    }

    logMsg("bad addr 0x%x\n", addr);
    return -1;
}

int
gdb_get_tid()
{
    int		    curtid;
    
    curtid = gdb_curtid;
    if (taskIdVerify((int) curtid) != 0)
        curtid = taskIdDefault(0);
    
    return curtid;
}

int
gdb_set_tid(int id)
{
    int		    tid;
    
    if (gdb_curtid_init == 0)
        tid = (int)gdb_break_tcb;
    else
        tid = gdb_curtid;
    
    if (tid == (int) gdb_work_task) {
        logMsg("error, attempted to debug gdb work task!\n");
        return 0;
    }
    gdb_curtid_init++;  /* mark initialized */
    
    return 0;
}

int
gdb_rget(int reg_id)
{
    REG_SET	    reg_set;
    int		    tid;
    struct myregs  *rp;
    
    tid = gdb_get_tid();
    taskRegsGet(tid, &reg_set);
    rp = (struct myregs *) & reg_set;
    
    sprintf(gdb_pad, "%d = %x\n", reg_id, rp->regs[reg_id]);
    fputs(gdb_pad, gdb_fp);
    if (gdb_debug)
        fputs(gdb_pad, stdout);
    
    return 0;
}

int
gdb_rset(int reg_id, int val)
{
    REG_SET	    reg_set;
    int		    tid;
    struct myregs  *rp;
    
    tid = gdb_get_tid();
    taskRegsGet(tid, &reg_set);
    
    rp = (struct myregs *) & reg_set;
    rp->regs[reg_id] = val;
    taskRegsSet(tid, &reg_set);
    
    return 0;
}

int
gdb_mdump_b(int addr, int len)
{
    int		    i;
    char	   *cp = gdb_pad;
    
    if (gdb_validate_addr(addr) < 0)
        return 0;
    
    for (i = 0; i < len; i++) {
        cp += sprintf(cp, " %02x", *((unsigned char *) addr));
        addr++;
    }
    
    sprintf(cp, " end-of-getmem\n");
    fputs(gdb_pad, gdb_fp);
    if (gdb_debug)
        fputs(gdb_pad, stdout);
    
    return 0;
}

int
gdb_mdump_w(int addr, int len)
{
    int		    i;
    char	   *cp = gdb_pad;
    
    if (gdb_validate_addr(addr) < 0)
        return 0;
    
    for (i = 0; i < len; i += 2) {
        cp += sprintf(cp, " %04x", *((unsigned short *) addr));
        addr += 2;
    }
    
    sprintf(cp, " end-of-getmem\n");
    fputs(gdb_pad, gdb_fp);
    if (gdb_debug)
        fputs(gdb_pad, stdout);
    
    return 0;
}

int
gdb_mdump_l(int addr, int len)
{
    int		    i;
    char	   *cp = gdb_pad;
    
    if (gdb_validate_addr(addr) < 0)
        return 0;
    
    for (i = 0; i < len; i += 4) {
        cp += sprintf(cp, " %08x", *((unsigned int *) addr));
        addr += 4;
    }
    
    sprintf(cp, " end-of-getmem\n");
    fputs(gdb_pad, gdb_fp);
    if (gdb_debug)
        fputs(gdb_pad, stdout);
    
    return 0;
}

int
gdb_mset_b(int addr, int val)
{
    if (gdb_validate_addr(addr) < 0)
        return 0;

    *((unsigned char *) addr) = (unsigned char) val;

    return 0;
}

int
gdb_mset_w(int addr, int val)
{
    if (gdb_validate_addr(addr) < 0)
        return 0;

    *((unsigned short *) addr) = (unsigned short) val;

    return 0;
}

int
gdb_mset_l(int addr, int val)
{
    if (gdb_validate_addr(addr) < 0)
        return 0;

    *((unsigned int *) addr) = (unsigned int) val;

    return 0;
}

int
gdb_rdump()
{
    int		    tid;
    REG_SET	    reg_set;
    struct myregs  *rp;
    int		    i;
    char	   *cp = gdb_pad;

    tid = gdb_get_tid();
    taskRegsGet(tid, &reg_set);

    rp = (struct myregs *) & reg_set;

    for (i = 0; i < NUM_REGS; i++)
        cp += sprintf(cp, "%d = %x ", i, rp->regs[i]);

    sprintf(cp, " \n");
    fputs(gdb_pad, gdb_fp);
    if (gdb_debug)
        fputs(gdb_pad, stdout);

    return 0;
}

int
gdb_break(int addr)
{
    int i;

    for (i = 0; i < GDB_MAX_BREAK; i++)
        if (gdb_break_shadow[i].addr == addr)
	    break;

    if (i < GDB_MAX_BREAK)      /* already in the shadow */
        return 0;

    for (i = 0; i < GDB_MAX_BREAK; i++)
        if (gdb_break_shadow[i].addr == 0)
	    break;

    if (i == GDB_MAX_BREAK) {
        logMsg("gdb break, too many breakpoints\n");
        return 0;
    }

    gdb_break_shadow[i].addr = addr;
    bcopy((char *)addr, (char *)&gdb_break_shadow[i].val, 4);

    if (gdb_debug)
        logMsg("insert break @ 0x%x shadow 0x%x\n", 
	       gdb_break_shadow[i].addr, gdb_break_shadow[i].val);

    sprintf(gdb_pad, "b 0x%x", addr);

    execute(gdb_pad);

    return 0;
}

int
gdb_delete_break(int addr)
{
    int i;

    for (i = 0; i < GDB_MAX_BREAK; i++)
        if (gdb_break_shadow[i].addr == addr)
	    break;
        
    if (i == GDB_MAX_BREAK) {
        logMsg("gdb delete break, can't find breakpoint at 0x%x\n",
	       addr);
        return 0;
    }

    sprintf(gdb_pad, "bd 0x%x", addr);
    execute(gdb_pad);

    if (gdb_debug)
        logMsg("del break @ 0x%x shadow 0x%x\n", 
	       gdb_break_shadow[i].addr, gdb_break_shadow[i].val);

    bcopy((char *)&gdb_break_shadow[i].val, (char *)addr, 4);
    gdb_break_shadow[i].addr = 0;

    return 0;
}

int
gdb_delete_all_breaks()
{
    int i;

    sprintf(gdb_pad, "bdall");
    execute(gdb_pad);

    for (i = 0; i < GDB_MAX_BREAK; i++) {
        if (gdb_break_shadow[i].addr != 0) {

	    if (gdb_debug)
		logMsg("del break @ 0x%x shadow	0x%x\n", 
		       gdb_break_shadow[i].addr, 
		       gdb_break_shadow[i].val);

	    bcopy((char	*)&gdb_break_shadow[i].val, 
		  (char	*)&gdb_break_shadow[i].addr, 4);
	    gdb_break_shadow[i].addr = 0;
        }
    }

    return 0;
}

int
gdb_step()
{
    sprintf(gdb_pad, "dbgStepQuiet");
    execute(gdb_pad);

    return 0;
}

int
gdb_continue()
{
    sprintf(gdb_pad, "c");
    execute(gdb_pad);

    return 0;
}

/*
 * delay for 'delay' seconds and spawn a task for 'func'. useful for spawning
 * a task from a remote gdb -- to give the user enough time to issue continue
 * command at a command prompt right afterwards.
 */
int
del_sp(char *params)
{
    char buf[200];

    sprintf(buf, "sp %s", params);

    taskDelay(6 * sysClkRateGet());
    execute(buf);

    return 0;
}

int
gdb_kill()
{
    int		    tmp;

    if (gdb_sock) {
        logMsg("shutting down gdb socket %d\n", gdb_sock);
        shutdown(gdb_sock, 2);
        close(gdb_sock);
        gdb_sock = 0;
    }
    if (gdb_work_task != 0) {
        logMsg("killing gdb_work_task 0x%x\n", (int) gdb_work_task);
        taskDelay(2 * sysClkRateGet());
        tmp = (int) gdb_work_task;
        gdb_work_task = 0;
        taskDelete((int) tmp);
    }
    return 0;
}

char	       *
gdb_params(char *cp)
{
    if (cp == 0)
        return 0;

    while (!isspace(*cp))
        cp++;

    return cp;
}

char	       *
gdb_strip_whites(char *cp)
{
    char	   *np = gdb_buf;

    if (cp == 0)
        return 0;

    assert(cp != np);

    while (*cp != '\0') {
        if (*cp == '\r' || *cp == '\n') {
	    cp++;
	    continue;
        }
        *np++ = *cp++;
    }

    *np = '\0';	        /* null terminate */
    return gdb_buf;
}

int
gdb_synch()
{
    fputs(GDB_PROMPT, gdb_fp);
    if (gdb_debug)
        fputs(GDB_PROMPT, stdout);

    return 0;
}

/*
 * all the commands/protocols for remote gdb interfacing goes here
 */
int
gdb_process_input(char *cp)
{
    int		    (*func) ();
    int		    addr, val;
    int		    len;
    int		    i;
    int		    desc[3];

    cp = gdb_strip_whites(cp);

    if (cp == 0) {
        logMsg("null input\n");
        return 0;
    }
    if (strlen(cp) == 0) {
        gdb_synch();
        return 0;
    }
    if (strncmp(cp, "gdb", 3) == 0) {
        gdb_synch();
        return 0;
    }
    if (strncmp(cp, "dummy", 5) == 0) {
        gdb_synch();
        return 0;
    }
    if (strncmp(cp, "mdump", 5) == 0) {
        switch (cp[6]) {
        case 'b':
	    func = gdb_mdump_b;
	    break;

        case 'w':
	    func = gdb_mdump_w;
	    break;

        case 'l':
	    func = gdb_mdump_l;
	    break;

        default:
	    goto bad_cmd;
        }

        cp = gdb_params(cp);
        sscanf(cp, "%x %x", &addr, &len);
        (*func) (addr, len);

        return 0;
    }
    if (strncmp(cp, "mset", 4) == 0) {
        switch (cp[5]) {
        case 'b':
	    func = gdb_mset_b;
	    break;

        case 'w':
	    func = gdb_mset_w;
	    break;

        case 'l':
	    func = gdb_mset_l;
	    break;

        default:
	    goto bad_cmd;
        }

        cp = gdb_params(cp);
        sscanf(cp, "%x %x", &addr, &val);
        (*func) (addr, val);

        return 0;
    }
    if (strncmp(cp, "!", 1) == 0) {
        cp++;

        for (i = 0; i < 3; i++)
	    desc[i] = ioGlobalStdGet(i);

        for (i = 0; i < 3; i++)
	    ioGlobalStdSet(i, gdb_sock);

        execute(cp);

        for (i = 0; i < 3; i++)
	    ioGlobalStdSet(i, desc[i]);

        return 0;
    }
    if (strncmp(cp, "rget", 4) == 0) {
        cp = gdb_params(cp);
        sscanf(cp, "%d", &val);
        gdb_rget(val);
        return 0;
    }
    if (strncmp(cp, "rset", 4) == 0) {
        cp = gdb_params(cp);
        sscanf(cp, "%d %x", &addr, &val);
        gdb_rset(addr, val);
        return 0;
    }
    if (strncmp(cp, "step", 4) == 0) {
        gdb_step();
        return 0;
    }
    if (strncmp(cp, "cont", 4) == 0) {
        gdb_continue();
        return 0;
    }
    if (strncmp(cp, "brk", 3) == 0) {
        cp = gdb_params(cp);
        sscanf(cp, "%x", &addr);
        gdb_break(addr);
        return 0;
    }
    if (strncmp(cp, "dbrk-real", 9) == 0) {
        cp = gdb_params(cp);
        sscanf(cp, "%x", &addr);
        gdb_delete_break(addr);
        return 0;
    }
    if (strncmp(cp, "dball", 5) == 0) {
        gdb_delete_all_breaks();
        return 0;
    }
    if (strncmp(cp, "rdump", 5) == 0) {
        gdb_rdump();
        return 0;
    }
    if (strncmp(cp, "set_tid", 7) == 0) {
        cp = gdb_params(cp);
        sscanf(cp, "%x", &val);
        gdb_set_tid(val);
        return 0;
    }
    if (strncmp(cp, "del_sp", 6) == 0) {
        cp = gdb_params(cp);
        taskSpawn(cp, 250, 0, 10000, del_sp,
		  cp, 0, 0, 0, 0, 0, 0,	0, 0, 0);
        return 0;
    }
    if (strncmp(cp, "get_tid", 7) == 0) {
        val = gdb_get_tid();
        sprintf(gdb_pad, "task %x\n", val);
        fputs(gdb_pad, gdb_fp);
        if (gdb_debug)
	    fputs(gdb_pad, stdout);
        return 0;
    }
bad_cmd:
    logMsg("gdb bad command <%s> received from remote client.\n", cp);
    return -1;
}

int
gdb_break_notify(int task)
{
    semGive(gdb_sem);
    return 0;
}

/*
 * this is main body of a gdb work task.  it accepts remote gdb commands sent
 * by the gdb internal machinery and does the work
 */
int
gdb_work_loop(int sock)
{
    struct sockaddr_in sin;
    int		    len	= sizeof(sin);
    char	   *cp;
    fd_set	    read_fds;
    int		    i;

    /* verify connection */
    if (getpeername(sock, (struct sockaddr *) & sin, &len) < 0) {
        logMsg("can't getpeername\n");
        gdb_kill();
    }
    gdb_fp = fdopen(sock, "r+");
    if (gdb_fp == NULL) {
        logMsg("can't fdopen\n");
        gdb_kill();
    }
    setvbuf(gdb_fp, 0, _IONBF, 0);      /* no buffering */

    dbgBreakNotifyInstall(gdb_break_notify);

    logMsg("gdb work loop starting.\n");

    gdb_curtid = 0;
    bzero((char *)gdb_break_shadow, sizeof gdb_break_shadow);

    for (;;) {
        bzero((char *) &read_fds, sizeof read_fds);
        FD_SET(sock, &read_fds);
        i = select(64, &read_fds, 0, 0, 0);

        if (i < 0) {
	    logMsg("gdb	work loop, select error\n");
	    goto end;
        }
        if (i == 0)
	    continue;

        if (!FD_ISSET(sock, &read_fds))
	    continue;

        cp = fgets(gdb_pad, sizeof gdb_pad, gdb_fp);
        if (gdb_debug) {
	    putchar('$');
	    fputs(gdb_pad, stdout);
        }
        if (cp == NULL) {
	    logMsg("fgets returns null.	remote gdb may be going down.\n");
	    gdb_kill();
        }
#if 0
        /* echo it back */
        fputs(cp, gdb_fp);
#endif

        gdb_process_input(cp);
    }

end:
    gdb_kill();
    return (0);
}

/*
 * gdb server's main loop.  it waits for incoming connections from remote gdb
 * and accepts them.  then it spawns a task to handle the accepted
 * connection.
 */
int
gdb_main_loop(int sock)
{
    int		    client;
    struct sockaddr_in sin;
    struct sockaddr_in *sinptr = &sin;
    int		    optval;
    int		    len;

    for (;;) {
        len = sizeof(*sinptr);
        client = accept(sock, (struct sockaddr *) sinptr, &len);
        if (client < 0) {
	    logMsg("gdb	server error in accept");
	    continue;
        }
        logMsg("gdb server accepted new connection\n");

        if (gdb_work_task) {
	    logMsg("no multiple	connections allowed.\n");
	    logMsg("use	gdb_kill() to kill existing session.\n");
	    shutdown(client, 2);
	    close(client);
	    continue;
        }
        gdb_sock = client;

        /*
	 * turn	on KEEPALIVE so if the client crashes, we'll know
	 * about it
	 */

        optval = 1;
        setsockopt(client, SOL_SOCKET,
		   SO_KEEPALIVE, (char *) &optval, sizeof(optval));

        gdb_work_task = (WIND_TCB *)
	    taskSpawn("gdbwork", 70, 0,	20000, gdb_work_loop,
		      gdb_sock,	0, 0, 0, 0, 0, 0, 0, 0, 0);

        logMsg("new gdb_work_task tcb 0x%x\n", (int) gdb_work_task);
    }

    return 0;
}

int
gdb_break_task()
{
    for (;;) {
        semTake(gdb_sem, WAIT_FOREVER);

        fputs(GDB_BREAK_PROMPT, gdb_fp);
        if (gdb_debug)
	    fputs(GDB_BREAK_PROMPT, stdout);
    }
}

/*
 * start gdb server task
 */
int
gdb_start()
{
    int		    sock;
    struct sockaddr_in sin;
    struct sockaddr_in *sinptr = &sin;
    int		    true = 1;
    WIND_TCB	   *tcb;

    sock = socket(AF_INET, SOCK_STREAM, 0);
    if (sock < 0) {
        logMsg("gdb start, error socket\n");
        return -1;
    }
    gdb_server_sock = sock;

    if ((setsockopt(sock, SOL_SOCKET, SO_REUSEADDR,
		    (void *) &true, sizeof(true))) == -1) {
        logMsg("gdb start, error setsockopt\n");
        goto err;
    }
    bzero((char *) gdb_buf, sizeof gdb_buf);
    bzero((char *) gdb_pad, sizeof gdb_pad);
    bzero((char *) sinptr, sizeof(*sinptr));

    sinptr->sin_family = AF_INET;
    sinptr->sin_port = htons(GDB_PORT);
    sinptr->sin_addr.s_addr = htonl(INADDR_ANY);

    if (bind(sock, (struct sockaddr *) sinptr, sizeof(*sinptr)) < 0) {
        logMsg("gdb start, bind\n");
        goto err;
    }
    if (listen(sock, 5) < 0) {
        logMsg("gdb start, listen\n");
        goto err;
    }
    tcb = (WIND_TCB *) taskSpawn("gdbserv", 69, 0, 5000, gdb_main_loop,
				 gdb_server_sock, 0, 0,	0, 0, 0, 0, 0, 0, 0);

    semBInit(gdb_sem, SEM_Q_PRIORITY, SEM_EMPTY);

    gdb_break_tcb = (WIND_TCB *)
        taskSpawn("gdbbreak", 71, 0, 1000, gdb_break_task,
		  0, 0,	0, 0, 0, 0, 0, 0, 0, 0);

    logMsg("gdb start, server tcb 0x%x\n", (int) tcb);
    gdb_server_task = tcb;

    return 0;

err:
    close(gdb_server_sock);
    gdb_server_sock = 0;
    return -1;
}