#include <server.h>
#include <signal.h>
#include <stdio.h>

const char* REGISTERS[] = {
    "name:r0;bitsize:32;offset:0;encoding:uint;format:hex;set:General Purpose Registers;dwarf:0",
    "name:r1;bitsize:32;offset:4;encoding:uint;format:hex;set:General Purpose Registers;dwarf:1",
    "name:fp;alt-name:r2;bitsize:32;offset:8;encoding:uint;format:hex;set:General Purpose Registers;dwarf:2;generic:fp",
    "name:sp;alt-name:r3;bitsize:32;offset:12;encoding:uint;format:hex;set:General Purpose Registers;dwarf:3;generic:sp",
    "name:gp;alt-name:r4;bitsize:32;offset:16;encoding:uint;format:hex;set:General Purpose Registers;dwarf:4",
    "name:tp;alt-name:r5;bitsize:32;offset:20;encoding:uint;format:hex;set:General Purpose Registers;dwarf:5",
    "name:r6;bitsize:32;offset:24;encoding:uint;format:hex;set:General Purpose Registers;dwarf:6;generic:arg1",
    "name:r7;bitsize:32;offset:28;encoding:uint;format:hex;set:General Purpose Registers;dwarf:7;generic:arg2",
    "name:r8;bitsize:32;offset:32;encoding:uint;format:hex;set:General Purpose Registers;dwarf:8;generic:arg3",
    "name:r9;bitsize:32;offset:36;encoding:uint;format:hex;set:General Purpose Registers;dwarf:9;generic:arg4",
    "name:r10;bitsize:32;offset:40;encoding:uint;format:hex;set:General Purpose Registers;dwarf:10",
    "name:r11;bitsize:32;offset:44;encoding:uint;format:hex;set:General Purpose Registers;dwarf:11",
    "name:r12;bitsize:32;offset:48;encoding:uint;format:hex;set:General Purpose Registers;dwarf:12",
    "name:r13;bitsize:32;offset:52;encoding:uint;format:hex;set:General Purpose Registers;dwarf:13",
    "name:r14;bitsize:32;offset:56;encoding:uint;format:hex;set:General Purpose Registers;dwarf:14",
    "name:r15;bitsize:32;offset:60;encoding:uint;format:hex;set:General Purpose Registers;dwarf:15",
    "name:r16;bitsize:32;offset:64;encoding:uint;format:hex;set:General Purpose Registers;dwarf:16",
    "name:r17;bitsize:32;offset:68;encoding:uint;format:hex;set:General Purpose Registers;dwarf:17",
    "name:r18;bitsize:32;offset:72;encoding:uint;format:hex;set:General Purpose Registers;dwarf:18",
    "name:r19;bitsize:32;offset:76;encoding:uint;format:hex;set:General Purpose Registers;dwarf:19",
    "name:r20;bitsize:32;offset:80;encoding:uint;format:hex;set:General Purpose Registers;dwarf:20",
    "name:r21;bitsize:32;offset:84;encoding:uint;format:hex;set:General Purpose Registers;dwarf:21",
    "name:r22;bitsize:32;offset:88;encoding:uint;format:hex;set:General Purpose Registers;dwarf:22",
    "name:r23;bitsize:32;offset:92;encoding:uint;format:hex;set:General Purpose Registers;dwarf:23",
    "name:r24;bitsize:32;offset:96;encoding:uint;format:hex;set:General Purpose Registers;dwarf:24",
    "name:r25;bitsize:32;offset:100;encoding:uint;format:hex;set:General Purpose Registers;dwarf:25",
    "name:r26;bitsize:32;offset:104;encoding:uint;format:hex;set:General Purpose Registers;dwarf:26",
    "name:r27;bitsize:32;offset:108;encoding:uint;format:hex;set:General Purpose Registers;dwarf:27",
    "name:r28;bitsize:32;offset:112;encoding:uint;format:hex;set:General Purpose Registers;dwarf:28",
    "name:r29;bitsize:32;offset:116;encoding:uint;format:hex;set:General Purpose Registers;dwarf:29",
    "name:r30;bitsize:32;offset:120;encoding:uint;format:hex;set:General Purpose Registers;dwarf:30",
    "name:lp;alt-name:r31;bitsize:32;offset:124;encoding:uint;format:hex;set:General Purpose Registers;dwarf:31;generic:ra",
    "name:eipc;alt-name:sr0;bitsize:32;offset:128;encoding:uint;format:hex;set:Special Registers;dwarf:32",
    "name:eipsw;alt-name:sr1;bitsize:32;offset:132;encoding:uint;format:hex;set:Special Registers;dwarf:33",
    "name:fepc;alt-name:sr2;bitsize:32;offset:136;encoding:uint;format:hex;set:Special Registers;dwarf:34",
    "name:fepsw;alt-name:sr3;bitsize:32;offset:140;encoding:uint;format:hex;set:Special Registers;dwarf:35",
    "name:ecr;alt-name:sr4;bitsize:32;offset:144;encoding:uint;format:hex;set:Special Registers;dwarf:36",
    "name:psw;alt-name:sr5;bitsize:32;offset:148;encoding:uint;format:hex;set:Special Registers;dwarf:37;generic:flags",
    "name:pir;alt-name:sr6;bitsize:32;offset:152;encoding:uint;format:hex;set:Special Registers;dwarf:38",
    "name:tkcw;alt-name:sr7;bitsize:32;offset:156;encoding:uint;format:hex;set:Special Registers;dwarf:39",
    "name:chcw;alt-name:sr24;bitsize:32;offset:160;encoding:uint;format:hex;set:Special Registers;dwarf:40",
    "name:adtre;alt-name:sr25;bitsize:32;offset:164;encoding:uint;format:hex;set:Special Registers;dwarf:41",
    "name:sr29;bitsize:32;offset:168;encoding:uint;format:hex;set:Special Registers;dwarf:42",
    "name:sr30;bitsize:32;offset:172;encoding:uint;format:hex;set:Special Registers;dwarf:43",
    "name:sr31;bitsize:32;offset:176;encoding:uint;format:hex;set:Special Registers;dwarf:44",
    "name:pc;bitsize:32;offset:180;encoding:uint;format:hex;set:Special Registers;dwarf:45;generic:pc",
};

const uint32_t SYSTEM_REGISTERS[] = {
    VB_EIPC,
    VB_EIPSW,
    VB_FEPC,
    VB_FEPSW,
    VB_ECR,
    VB_PSW,
    VB_PIR,
    VB_TKCW,
    VB_CHCW,
    VB_ADTRE,
    29,
    30,
    31,
};

const uint32_t PC_INDEX = 32 + 13;

static int onExecute(VB *sim, uint32_t address, const uint16_t *code, int length) {
    uint32_t i;
    RdbServer *srv = (RdbServer *)vbGetUserData(sim);

    (void)sim;
    (void)code;
    (void)length;

    /* if we're stopped, just stop */
    if (srv->state == state_stopped) {
        return 1;
    }

    /* if we're stepping, we'll run this one instruction but no others */
    if (srv->state == state_stepping) {
        srv->state = state_stopped;
        srv->stopreason = stop_reason_trace;
        return 0;
    }

    for (i = 0; i < srv->brkslen; ++i) {
        if (srv->brks[i] == address) {
            srv->state = state_stopped;
            srv->stopreason = stop_reason_breakpoint;
            return 1;
        }
    }
    return 0;
}

static bool addBreakpoint(RdbServer *srv, uint32_t address) {
    uint32_t i;
    for (i = 0; i < srv->brkslen; ++i) {
        if (srv->brks[i] == address) {
            /* This breakpoint is already set */
            return true;
        }
    }
    if (i == RDB_SERVER_MAX_BREAKPOINTS) {
        /* We've added too many breakpoints */
        return false;
    }
    srv->brks[i] = address;
    ++srv->brkslen;
    return true;
}

static void removeBreakpoint(RdbServer *srv, uint32_t address) {
    uint32_t i;
    for (i = 0; i < srv->brkslen; ++i) {
        if (srv->brks[i] == address) {
            srv->brks[i] = srv->brks[srv->brkslen - 1];
            --srv->brkslen;
            return;
        }
    }
}

void rdbServerInit(RdbServer *srv, VB *sim) {
    srv->sim = sim;
    srv->brkslen = 0;
    srv->state = state_stopped;
    srv->stopreason = stop_reason_none;
    vbSetUserData(sim, srv);
    vbSetExecuteCallback(sim, onExecute);
}

int rdbServerHandleCommand(RdbServer *srv, CommandBuf *cmd, RdbResponse *res) {
    rdbResponseBeginPacket(res);

    if (cmdMatchStr(cmd, "QStartNoAckMode")) {
        /* The debugger is asking us to no longer ACK messages. */
        /* Note that we ack THIS response, because we already called rdbResponseBeginPacket. */
        res->should_ack = false;
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qSupported")) {
        /* The debugger is asking for a list of features we support. */
        rdbResponseWriteStr(res, "no-resumed+;multiprocess;vContSupported");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "QThreadSuffixSupported")) {
        /* The debugger is asking us to include the current thread as a suffix to some responses. */
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "QListThreadsInStopReply")) {
        /* The debugger is asking us to list all threads whenever we stop running. */
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qHostInfo")) {
        /* The debugger is asking us to describe the "host machine" getting debugged. */
        rdbResponseWriteStr(res, "triple:");
        rdbResponseWriteStrHex(res, "v810-unknown-vb");
        rdbResponseWriteStr(res, ";endian:little;ptrsize:4;");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qProcessInfo")) {
        /* The debugger is asking us to describe the "process" getting debugged. */
        /* We make up a process with id 1. */
        rdbResponseWriteStr(res, "pid:1;triple:");
        rdbResponseWriteStrHex(res, "v810-unknown-vb");
        rdbResponseWriteStr(res, "endian:little;ptrsize:4;");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qRegisterInfo")) {
        uint32_t reg_no;

        /* The debugger is asking for information about a specific register. */
        if (!cmdMatchHexNumber(cmd, &reg_no)) return 1;
        if (reg_no <= PC_INDEX) {
            rdbResponseWriteStr(res, REGISTERS[reg_no]);
        }
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qfThreadInfo")) {
        /* The debugger is asking us to list all threads. Return a list with "thread 1". */
        rdbResponseWriteStr(res, "mp1.t1");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "qsThreadInfo")) {
        /* The debugger is asking us to list all threads. */
        rdbResponseWriteStr(res, "l");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "vCont?")) {
        /* The debugger is asking which vCont commands we support. */
        rdbResponseWriteStr(res, "c;C;s;S");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "vCont;s:1")) {
        /* The debugger wants us to step */
        srv->state = state_stepping;
        return 0;
    }
    if (cmdMatchStr(cmd, "qC")) {
        /* The debugger is asking for the current thread id. Return "thread 1". */
        rdbResponseWriteStr(res, "QCp1.t1");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "p")) {
        uint32_t reg_no, reg_value;

        /* read a register. */
        if (!cmdMatchHexNumber(cmd, &reg_no)) return 1;
        if (reg_no > PC_INDEX) {
            return rdbResponseSendPacket(res);
        }

        if (reg_no == PC_INDEX) {
            reg_value = vbGetProgramCounter(srv->sim);
        } else if (reg_no > 31) {
            reg_value = vbGetSystemRegister(srv->sim, SYSTEM_REGISTERS[reg_no - 32]);
        } else {
            reg_value = vbGetProgramRegister(srv->sim, reg_no);
        }
        rdbResponseWriteI32Hex(res, reg_value);
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "P")) {
        uint32_t reg_no, reg_value;
        uint8_t reg_bytes[4];

        /* write a register. */
        if (!cmdMatchHexNumber(cmd, &reg_no)) return -1;
        if (!cmdMatchStr(cmd, "=")) return -1;
        if (!cmdMatchHexBytes(cmd, 4, reg_bytes)) return -1;

        reg_value = ((uint32_t) (reg_bytes[3]) << 24) |
                            ((uint32_t) (reg_bytes[2]) << 16) |
                            ((uint32_t) (reg_bytes[1]) << 8) |
                            ((uint32_t) reg_bytes[0]);

        if (reg_no > PC_INDEX) {
            return rdbResponseSendPacket(res);
        }
        if (reg_no == PC_INDEX) {
            vbSetProgramCounter(srv->sim, reg_value);
        } else if (reg_no > 31) {
            vbSetSystemRegister(srv->sim, SYSTEM_REGISTERS[reg_no - 32], reg_value);
        } else {
            vbSetProgramRegister(srv->sim, reg_no, reg_value);
        }
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "Hc-1")) {
        /* Set the "current thread" for future commands to all threads (thread -1). */
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "c")) {
        /* The debugger has told us to run until we are stopped. */
        /* Don't send a response to this until we receive an ETX (when the debugger pauses us). */
        srv->state = state_running;
        srv->stopreason = stop_reason_none;
        return 0;
    }
    if (cmdMatchStr(cmd, "\x03")) {
        /* Received an ETX, indicating that the server wants to cancel the "c" command from before. */
        srv->state = state_stopped;
        srv->stopreason = stop_reason_trap;
        /* Send the response to the "c" command from before. */
        return rdbServerSendStopPacket(srv, res);
    }
    if (cmdMatchStr(cmd, "?")) {
        /* The debugger has asked us why we stopped */
        return rdbServerSendStopPacket(srv, res);
    }
    if (cmdMatchStr(cmd, "m")) {
        /* read memory */
        uint32_t i, address, len;
        if (!cmdMatchHexNumber(cmd, &address)) return -1;
        if (!cmdMatchStr(cmd, ","))  {
            /* if the server asks for too much memory, just error */
            return rdbResponseSendPacket(res);
        };
        if (!cmdMatchHexNumber(cmd, &len)) return -1;

        for (i = 0; i < len; ++i) {
            uint8_t byte = vbRead(srv->sim, address + i, VB_U8);
            rdbResponseWriteI8Hex(res, byte);
        }
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "M")) {
        /* write memory */
        uint32_t i, address, len;
        if (!cmdMatchHexNumber(cmd, &address)) return -1;
        if (!cmdMatchStr(cmd, ",")) return -1;
        if (!cmdMatchHexNumber(cmd, &len)) return -1;
        if (!cmdMatchStr(cmd, ":")) return -1;

        for (i = 0; i < len; ++i) {
            uint8_t byte;
            if (!cmdMatchHexBytes(cmd, 1, &byte)) return -1;
            vbWrite(srv->sim, address + i, VB_U8, byte);
        }
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "Z0,")) {
        /* set a breakpoint */
        uint32_t address;
        if (!cmdMatchHexNumber(cmd, &address)) return -1;
        if (!cmdMatchStr(cmd, ",0")) return -1;

        if (!addBreakpoint(srv, address)) return -1;
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    if (cmdMatchStr(cmd, "z0,")) {
        /* remove a breakpoint */
        uint32_t address;
        if (!cmdMatchHexNumber(cmd, &address)) return -1;
        if (!cmdMatchStr(cmd, ",0")) return -1;

        removeBreakpoint(srv, address);
        rdbResponseWriteStr(res, "OK");
        return rdbResponseSendPacket(res);
    }
    fprintf(stderr, "Unrecognized command.\n");
    return rdbResponseSendPacket(res);
}

bool rdbServerIsRunning(RdbServer *srv) {
    /* stepping counts */
    return srv->state != state_stopped;
}

int rdbServerSendStopPacket(RdbServer *srv, RdbResponse *res) {
    rdbResponseBeginPacket(res);
    switch (srv->stopreason) {
    case stop_reason_trace:
    case stop_reason_none:
        rdbResponseWriteStr(res, "T00");
        break;
    default:
        rdbResponseWriteStr(res, "T05");
    }
    rdbResponseWriteStr(res, "thread:p1.t1;threads:p1.t1;");
    switch (srv->stopreason) {
    case stop_reason_trace:
        rdbResponseWriteStr(res, "reason:trace;");
        break;
    case stop_reason_breakpoint:
        rdbResponseWriteStr(res, "reason:breakpoint;");
        break;
    case stop_reason_trap:
        rdbResponseWriteStr(res, "reason:trap;");
        break;
    case stop_reason_not_implemented:
        rdbResponseWriteStr(res, "reason:exception;description:opcode not implemented;");
        break;
    default: break;
    }
    return rdbResponseSendPacket(res);
}