/* 
*        Monitor.c
*/

#include "Monitor.h"

struct {
ActionProc *a;
} MonRegistration[32];

static void action_display(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
char *p;
unsigned size, chunk;
size = data2-data1;
p = (char *) data1;
while (size>0) {
chunk = min(size, MSGSIZE);
(void) MonPutPacket(MONDISPLAY, data1, data1+chunk, p, chunk);
size = size - chunk;
p = p + chunk;
data1 = data1+ chunk;
}
(void) MonPutPacket(MONDISPLAY, 0, 0, (char *) 0, 0);
}

static void action_dump(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
unsigned i;
(void) MonPutPacket(MONDUMP, data1, 0, data1, 160);
}

static void fill8(first, last, val)
char *first, *last, val;
{
char *p;
for (p = first; p<last; ++p) *p = val;
}

static void fill16(first, last, val)
unsigned short *first, *last, val;
{
unsigned short *p;
for (p = first; p<last; ++p) *p = val;
}

static void fill32(first, last, val)
unsigned *first, *last, val;
{
unsigned *p;
for (p = first; p<last; ++p) *p = val;
}

static void action_fill(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
unsigned short sWord;
unsigned lWord;
switch (len) {
case 0:
(void) fill8((char *) data1, (char *) data2, 0);
break;
case 1:
(void) fill8((char *) data1, (char *) data2, chars[0]);
break;
case 2:
sWord = (chars[0]<<8)+chars[1];
(void) fill16((unsigned short *) data1, (unsigned short *) data2, sWord);
break;
case 4:
lWord = (chars[0]<<24)+(chars[1]<<16)+(chars[2]<<8)+chars[3];
(void) fill32((unsigned *) data1, (unsigned *) data2, lWord);
break;
}
}

static void action_open8(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
char *p;
p = (char *) data1;
*p = (char) data2;
}

static void action_open16(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
unsigned short *p;
p = (unsigned short *) data1;
*p = (unsigned short) data2;
}

static void action_open32(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
unsigned *p;
p = (unsigned *) data1;
*p = data2;
}

#define BREAKCODE 0
static void SetBrk(data)
unsigned data;
{
unsigned opcode, *address;
address= (unsigned*) data;
opcode=*address;
(void) MonPutPacket(MONBRK, data, opcode, (char *) 0, 0);
*address=BREAKCODE;
}

static void ClearBrk(data1, data2)
unsigned data1, data2;
{
unsigned opcode, *address;
address= (unsigned*) data1;
*address=data2;
}

#define SETBRK 0

static void action_brk(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
if (data2==SETBRK) SetBrk(data1);
else ClearBrk(data1, data2); 
}

static void action_call(chars, len, type, data1, data2)
char *chars;
unsigned len, type, data1, data2;
{
unsigned (*proc)();
unsigned args[5], i, retVal;
if (data2>5 || data2!=len/4) {MonPutPacket(MONCALL, 0, data1, (char *) 0, 0); return;};
proc = (unsigned (*)()) data1;
for (i = 0; i<data2; ++i) {
args[i] = (chars[0]<<24)+(chars[1]<<16)+(chars[2]<<8)+chars[3];
chars = chars+4;
}
switch (data2) {
case 0 : retVal = (*proc)(); break;
case 1 : retVal = (*proc)(args[0]); break;
case 2 : retVal = (*proc)(args[0], args[1]); break;
case 3 : retVal = (*proc)(args[0], args[1], args[2]); break;
case 4 : retVal = (*proc)(args[0], args[1], args[2], args[3]); break;
case 5 : retVal = (*proc)(args[0], args[1], args[2], args[3], args[4]); break;
}
MonPutPacket(MONCALL, data1, retVal, (char *) 0, 0);
}

void initMonitor()
{
MonRegistration[MONDISPLAY].a = action_display;
MonRegistration[MONFILL].a = action_fill;
MonRegistration[MONOPEN8].a = action_open8;
MonRegistration[MONOPEN16].a = action_open16;
MonRegistration[MONOPEN32].a = action_open32;
MonRegistration[MONDUMP].a = action_dump;
MonRegistration[MONBRK].a = action_brk;
MonRegistration[MONCALL].a = action_call;
}

/* error entry to the debugger */
XR_CallDebugger()
{
Trap0x40();
}

/* trap entry to the debugger */
debug(regs)
unsigned *regs;
{
initMonitor();
(void) MonPutPacket(MONENTER, 0, regs, (char *) 0, 0);
(void) MonWatchPacketsIn();
}