/*
 *  CONTROL.C
 *
 *  DNET (c)Copyright 1988, Matthew Dillon, All Rights Reserved.
 *
 */

#include "dnet.h"
#include <stdio.h>

static ubyte Dctl;

/*
 *  RTO:    read timeout.   Timeout occured while waiting for the rest of
 *	    a packet.  Reset state and restart read.
 *
 *  called from iosink loop, ok if signal mask cleared
 */

do_rto(ior)
IOT *ior;
{
    NetAbortRead();
    NetStartRead(&Raux->sync, 3);
    if (RState == RS_DATA && (Dctl & PKF_MASK) == PKCMD_WRITE)
	replywindow(Dctl >> 5);     /* NAK the packet */
    RState = 0;
}

/*
 *  WTO:    Write timeout (unresolved output windows exist).  Resend a CHECK
 *	    command for all unresolved (READY) output windows
 */

void
do_wto(ior)
{
    register short i;
    register PKT *pkt;

    if (Restart) {
	NetWrite(RestartPkt, 3, 1);
	return;
    }
    for (i = 0; i < WPUsed; ++i) {
	pkt = WPak[i];
	if (pkt->state == READY)    /*  send check  */
	    NetWrite(CheckPkt[(WPStart+i)&7], 3, 1);
    }
}

/*
 *  RNET:   Receved data ready from network.  The RNet request will
 *	    automagically be reissued on return.
 *
 *  NOTE:   The buffer the data is contained in can be anything, and
 *	    is not restricted to the Auxillary request this routine uses.
 *
 *	    called from iosink loop, signal mask can be cleared.
 */

do_rnet(ior)
IOR *ior;
{
    register ubyte *ptr = (ubyte *)ior->io_Data;
    register long len  = ior->io_Actual;
    long n;
    static uword dlen;
    static uword dblen;

    if (len <= 0) {
	len = 0;
	RState = RS_SYNC;
    }
    while (len) {
	switch(RState) {
	case RS_SYNC:			/*  SYNC BYTE	    */
	    --len;
	    if (*ptr++ == SYNC) {
		RState = RS_CTL;
	    } else {
		++GarbageIn;
		if (DDebug)
		    printf("Garbage: '%c'\n", *(ptr-1));
	    }
	    break;
	case RS_CTL:			/*  CONTROL BYTE    */
	    --len;
	    Dctl = *ptr++;
	    RState = RS_CCHK;
	    break;
	case RS_CCHK:			/*  CHECK BYTE	    */
	    if ((ubyte)(((SYNC<<1)^Dctl)) == *ptr) {
		++ptr;
		--len;
		if (Dctl & PKF_DATA) {  /*  long packet     */
		    RState = RS_LEN1;
		    break;
		}
		RState = RS_SYNC;	/*  short packet    */
		do_cmd(Dctl, NULL, 0);
	    } else {
		if (len) {              /*  error           */
		    ++len;
		    --ptr;
		}
	    }
	    RState = RS_SYNC;
	    break;
	case RS_LEN1:			/*  first byte of pkt len   */
	    dlen = *ptr;
	    ++ptr;
	    --len;
	    RState = RS_LEN2;
	    break;
	case RS_LEN2:			/*  second byte of pkt len  */
	    dlen = (dlen << 8) + *ptr + 2;
	    if (dlen < 2)
		fprintf(stderr, "WARNING, DATALEN <2: %02lx %ld\n", Dctl, dlen);
	    if (dlen > MAXPKT+2) {
		fprintf(stderr, "DATALEN FAILURE: %02lx %ld\n", Dctl, dlen);
		RState = RS_SYNC;
		break;
	    }
	    ++ptr;
	    --len;
	    dblen = 0;
	    RState = RS_DATA;
	    break;
	case RS_DATA:
	    len += dblen;	/*  move to beginning of data buffer	*/
	    ptr -= dblen;
	    if (dlen <= len) {
		register uword chk = chkbuf(ptr, dlen - 2);
		if ((chk >> 8) == ptr[dlen-2] && (ubyte)chk == ptr[dlen-1]) {
		    do_cmd(Dctl, ptr, dlen-2);
		    len -= dlen;
		    ptr += dlen;
		} else {
		    if ((Dctl & PKF_MASK) == PKCMD_WRITE)
			replywindow(Dctl >> 5); /*  NAK the packet */
		}
		RState = RS_SYNC;
	    } else {
		goto break2;	/*  incomplete read  */
	    }
	    break;
	}
    }
break2:
    if (Rto_act) {
	AbortIO(&Rto);
	WaitIO(&Rto);
	Rto_act = 0;
    }
    n = NetReady();
    if (n > MAXPKT)
	n = MAXPKT;
    switch(RState) {
    default:
	printf("SoftError: Illegal state %ld\n", RState);
	RState = RS_SYNC;
	/* fall through */
    case RS_SYNC:	/*  Wait for sync & cmd.  No timeout needed	*/
	ptr = &Raux->sync;
	len = MAX(3,n);
	break;
    case RS_CTL:	/*  Have sync, need CTL and CCHK.		*/
	ptr = &Raux->ctl;
	len = MAX(2,n);
	break;
    case RS_CCHK:
	ptr = &Raux->cchk;
	len = MAX(1,n);
	break;
    case RS_LEN1:
	ptr = &Raux->lenh;
	len = MAX(2,n);
	break;
    case RS_LEN2:
	ptr = &Raux->lenl;
	len = MAX(1,n);
	break;
    case RS_DATA:	/*  need dlen, have len.  Start read request and TO   */
	if (ptr != Raux->data && len)
	    CopyMem(ptr, Raux->data, len);
	dblen = len;
	ptr = (ubyte *)Raux->data + dblen;
	len = dlen - len;
	Rto.tr_time.tv_secs = RTimeoutVal / 1000000;	/*  packet to  */
	Rto.tr_time.tv_micro= RTimeoutVal % 1000000;
	SendIO(&Rto);
	Rto_act = 1;
	break;
    }
    do_cmd(-1, NULL, 0);
    NetStartRead(ptr, len);
}

/*
 *  WNET:   The last data packet has been sent to the network...  Start a
 *	    timeout sequence (1 second).  If this times out we will send
 *	    a CHECK packet for all unresolved transmission windows.
 */

do_wnet()
{
    if (Wto_act) {
	AbortIO(&Wto);
	WaitIO(&Wto);
    }
    Wto.tr_time.tv_secs = WTimeoutVal / 1000000;
    Wto.tr_time.tv_micro= WTimeoutVal % 1000000;
    SendIO(&Wto);
    Wto_act = 1;
}

/*
 *  DO_WUPDATE()
 *
 *  (1) Remove EMPTY windows at head of transmit queue.
 *  (2) Fill up transmit queue with pending requests, if any.
 *
 *  First two bits determine CMD as follows:
 *	    0bbbbbbb		DATA	    0-127 bytes of DATA
 *	    10bbbccc		DATA	    0-7 bytes of DATA, ccc=channel
 *								  command.
 *	    11bbbbbb bbbbbbbb	DATA	    0-1023 bytes of DATA
 */

do_wupdate()
{
    static short XPri;
    int maxpktsize;
    register IOR *ior;
    register PKT *pkt;
    register long len;

    while (WPUsed && WPak[0]->state == EMPTY) {
	pkt = WPak[0];
	WPak[0] = WPak[1];
	WPak[1] = WPak[2];
	WPak[2] = WPak[3];
	WPak[3] = pkt;
	--WPUsed;
	++WPStart;
    }
    if (Restart)
	return(0);
    while (WPUsed != 4 && (ior = (IOR *)RemHead(&TxList))) {
	register long offset = 0;
	if (DDebug)
	    printf("Extract Request: %08lx %ld bytes\n", ior, ior->io_Length);
	{
	    short npri;

	    if (ior->io_Command == SCMD_DATA) {
		ubyte chan = (ulong)ior->io_Unit;
		if (Chan[chan].state == CHAN_CLOSE) {   /* channel closed */
		    ior->io_Error = 1;
		    ReplyMsg(ior);
		    continue;
		}
		npri = ior->io_Message.mn_Node.ln_Pri << 2;
	    } else {
		npri = XPri;
	    }
	    if (npri >= XPri)
		XPri = npri;
	    else {
		if (XPri - npri > 100)
		    XPri -= 10;
		else if (XPri - npri > 50)
		    XPri -= 5;
		else
		    --XPri;
	    }
	    maxpktsize = MAXPKT - (XPri - npri);
	    if (maxpktsize < MINPKT)
		maxpktsize = MINPKT;
	}
	pkt = WPak[WPUsed];
	pkt->state = READY;
	pkt->sync  = SYNC;
	pkt->ctl   = PKCMD_WRITE | PKF_DATA | ((WPStart + WPUsed)<<5);
	pkt->cchk = (pkt->sync << 1) ^ pkt->ctl;
	for (;;) {
	    if (offset > (maxpktsize-8))        /*  not enough room */
		break;
	    if (ior->io_Command == SCMD_DATA && (ulong)ior->io_Unit   != WChan) {
		/*  CSWITCH */
		WChan = (ulong)ior->io_Unit;
		pkt->data[offset+0] = 0x80|SCMD_SWITCH|(2<<3);
		pkt->data[offset+1] = WChan >> 8;
		pkt->data[offset+2] = WChan;
		offset += 3;
	    }
	    len = ior->io_Length - ior->io_Actual;
	    if (ior->io_Command == SCMD_DATA) {     /*  DATA OUT   */
		if (offset + len > (maxpktsize-4))
		    len = (maxpktsize-4) - offset;
		if (len < 128) {
		    pkt->data[offset] = len;
		    ++offset;
		} else {
		    pkt->data[offset+0] = (len>>8)|0xC0;
		    pkt->data[offset+1] = len;
		    offset += 2;
		}
		BytesOut += len;
	    } else {				    /*	COMMAND OUT */
		pkt->data[offset] = 0x80|ior->io_Command|(len<<3);
		++offset;
	    }
	    CopyMem((char *)ior->io_Data + ior->io_Actual, pkt->data + offset, len);
	    offset += len;
	    ior->io_Actual += len;
	    if (ior->io_Actual == ior->io_Length) {
		ReplyMsg(ior);
		ior = (IOR *)RemHead(&TxList);        /*  Next packet     */
		if (ior == NULL)
		    break;
	    }
	}
	pkt->iolength = offset + OVERHEAD;
	pkt->lenh = offset >> 8;
	pkt->lenl = offset;
	{
	    register uword chksum = chkbuf(pkt->data, offset);
	    pkt->data[offset+0] = chksum >> 8;
	    pkt->data[offset+1] = chksum;
	}
	NetWrite(&pkt->sync, pkt->iolength, 1);
	if (ior) {
	    ++ior->io_Message.mn_Node.ln_Pri;
	    Enqueue(&TxList, ior);
	    --ior->io_Message.mn_Node.ln_Pri;
	}
	++WPUsed;
	++PacketsOut;
	ResetIdle();
	break;	    /*	One at a time, else would take too long */
    }
}

void
dumpcheck(ptr)
register ubyte *ptr;
{
    register short i;
    for (i = 0; i < 8; ++i) {
	if (ptr[i])
	    replywindow(i);
	ptr[i] = 0;
    }
}

do_cmd(ctl, buf, bytes)
uword ctl;	/*  usually just 8 bits though	*/
ubyte *buf;
{
    ubyte window = ctl >> 5;
    ubyte rwindow;
    static ubyte Chk, Chkwin[8];

    if (ctl == 0xFFFF) {
	if (Chk) {
	    dumpcheck(Chkwin);
	    Chk = 0;
	}
	return;
    }

    if (PDebug)
	printf("RECV %02x %4ld bytes\n", ctl, bytes);

    if ((ctl & PKF_MASK) == PKCMD_CHECK) {
	Chkwin[window] = 1;
	Chk = 1;
	return;
    }
    if (Chk) {
	dumpcheck(Chkwin);
	Chk = 0;
    }
    switch(ctl & PKF_MASK) {
    case PKCMD_WRITE:
	rwindow = (window - RPStart) & 7;
	if (rwindow < 4) {
	    CopyMem(buf, RPak[rwindow]->data, bytes);
	    RPak[rwindow]->iolength = bytes;
	    RPak[rwindow]->state = READY;
	    do_rupdate();
	}
	replywindow(window);
	break;
    case PKCMD_ACK:
	rwindow = (window - WPStart) & 7;
	if (rwindow < WPUsed)       /*  mark as sent    */
	    WPak[rwindow]->state = EMPTY;
	break;
    case PKCMD_NAK:		    /*	resend		*/
	rwindow = (window - WPStart) & 7;
	if (rwindow < WPUsed) {     /*  resend          */
	    ++PacketsResent;
	    NetWrite(&WPak[rwindow]->sync, WPak[rwindow]->iolength, 0);
	} else {
	    printf("Soft Error: Illegal NAK: %ld %ld %ld %ld\n",
		window, WPStart, rwindow, WPUsed
	    );
	}
	break;
    case PKCMD_RESTART:
    case PKCMD_ACKRSTART:
	if ((ctl & PKF_MASK) == PKCMD_ACKRSTART)
	    Restart = 0;
	do_netreset();
	/* RxPtr? */

	if ((ctl & PKF_MASK) == PKCMD_RESTART) {
	    short len;
	    uword chksum;
	    static ubyte buf[32];

	    NetWaitWrite();
	    strcpy(buf+5, HostName);
	    len = strlen(buf+5)+1;
	    buf[0] = SYNC;
	    buf[1] = PKCMD_ACKRSTART | PKF_DATA;
	    buf[2] = (SYNC << 1) ^ buf[1];
	    buf[3] = 0;
	    buf[4] = len;
	    chksum = chkbuf(buf+5, len);
	    buf[5+len] = chksum >> 8;
	    buf[6+len] = chksum;
	    NetWrite(buf, 7+len, 1);
	}
	/*
	if (bytes)
	    setlistenport(buf);
	else
	    setlistenport("");
	*/
	break;
    }
    do_rupdate();
}

do_rupdate()
{
    while (RPak[0]->state == READY) {
	register PKT *pkt = RPak[0];
	register ubyte *ptr = pkt->data;
	register uword len;
	uword iolen = pkt->iolength;
	ubyte cmd;

	while (iolen) {
	    cmd = SCMD_DATA;
	    len = ptr[0];
	    ++ptr;
	    --iolen;
	    if (len >= 128) {
		if (len < 0xC0) {
		    cmd = len & 7;
		    len = (len >> 3) & 7;
		} else {
		    len = ((len << 8) | *ptr) & 0x3FFF;
		    ++ptr;
		    --iolen;
		}
	    }
	    iolen -= len;
	    if (DDebug)
		printf("RECEIVE CMD %2ld ", cmd);
	    do_reccmd(cmd, ptr, len);
	    ptr += len;
	}
	RPak[0] = RPak[1];
	RPak[1] = RPak[2];
	RPak[2] = RPak[3];
	RPak[3] = pkt;
	pkt->state = EMPTY;
	++RPStart;
    }
}

do_reccmd(cmd, ptr, len)
ubyte *ptr;
{
    switch(cmd) {
    case SCMD_DATA:	/*  data for channel	*/
	if (RChan < MAXCHAN && (Chan[RChan].flags & CHANF_ROK)) {
	    register IOR *ior = (IOR *)AllocMem(sizeof(IOR), MEMF_PUBLIC);
	    ior->io_Unit = (struct Unit *)RChan;
	    ior->io_Data = (APTR)AllocMem(len, MEMF_PUBLIC);
	    ior->io_Length = len;
	    ior->io_Actual = 0;
	    CopyMem(ptr, ior->io_Data, len);
	    ior->io_Message.mn_Node.ln_Name = (char *)PKT_REQ;
	    ior->io_Command = DNCMD_WRITE;
	    ior->io_Message.mn_ReplyPort = IOSink;
	    PutMsg(Chan[RChan].port, ior);
	    BytesIn += len;
	    ResetIdle();    /*  not idle, have received data    */
	}
	break;
    case SCMD_SWITCH:
	RChan = (ptr[0]<<8)|ptr[1];
	break;
    case SCMD_OPEN:
	{
	    register COPEN *cop = (COPEN *)ptr;
	    PORT *port;
	    CACKCMD ack;
	    char buf[32];
	    uword chan = (cop->chanh << 8) | cop->chanl;
	    uword portnum = (cop->porth << 8) | cop->portl;

	    ack.chanh = cop->chanh;
	    ack.chanl = cop->chanl;
	    ack.error = 0;

	    if (chan >= MAXCHAN || Chan[chan].state) {
		ack.error = 33;     /* magic */
		WriteStream(SCMD_ACKCMD, &ack, sizeof(CACKCMD), -1);
		break;
	    }
	    sprintf(buf, "DNET.PORT.%ld", portnum);
	    if ((port = (PORT *)FindPort(buf)) == NULL) {
		RunServer(portnum);
		if ((port = (PORT *)FindPort(buf)) == NULL) {
		    ack.error = 2;
		    WriteStream(SCMD_ACKCMD, &ack, sizeof(CACKCMD), -1);
		    break;
		}
	    }
	    /*	ln_Name type of 0 causes reply to go to DNetPort    */
	    WritePort(port, DNCMD_SOPEN, NULL, 0, 0, chan);
	    Chan[chan].state = CHAN_ROPEN;
	    Chan[chan].pri = cop->pri;
	}
	break;
    case SCMD_CLOSE:	/*  receive close   */
	{
	    register CCLOSE *clo = (CCLOSE *)ptr;
	    uword chan = (clo->chanh<<8)|clo->chanl;

	    if (DDebug)
		printf("Remote close, chan %d state %d\n", chan, Chan[chan].state);

	    if (chan >= MAXCHAN || Chan[chan].state == CHAN_FREE) {
		break;
	    }
	    Chan[chan].state = CHAN_CLOSE;
	    Chan[chan].flags |= CHANF_RCLOSE;
	    Chan[chan].flags &= ~(CHANF_ROK|CHANF_WOK);
	    if (Chan[chan].flags & CHANF_LCLOSE) {
		if (DDebug)
		    printf("Local already closed, reply %08lx\n", Chan[chan].ior);
		Chan[chan].state = CHAN_FREE;
		ReplyMsg(Chan[chan].ior);
		Chan[chan].ior = NULL;
	    } else {	/*  send EOF  */
		if (DDebug)
		    printf("Local not already closed\n");
		WritePort(Chan[chan].port, DNCMD_CLOSE, NULL, 0, PKT_REQ, chan);
	    }
	}
	break;
    case SCMD_ACKCMD:	/*  acknowledge of my open	*/
	{
	    register CACKCMD *cack = (CACKCMD *)ptr;
	    uword chan = (cack->chanh<<8)|cack->chanl;
	    if (chan >= MAXCHAN || Chan[chan].state != CHAN_LOPEN) {
		break;
	    }
	    /*
	     *	Channel in use (collision), try again
	     */
	    if (cack->error == 33) {
		uword newchan = alloc_channel();
		COPEN co;
		if (newchan < MAXCHAN) {
		    Chan[newchan] = Chan[chan];
		    Chan[chan].state = CHAN_FREE;
		    Chan[chan].ior = NULL;
		    co.chanh = newchan >> 8;
		    co.chanl = newchan;
		    co.porth = (ulong)Chan[newchan].ior->io_Unit >> 8;
		    co.portl = (ulong)Chan[newchan].ior->io_Unit;
		    co.error = 0;
		    co.pri = Chan[chan].pri;
		    WriteStream(SCMD_OPEN, &co, sizeof(COPEN), chan);
		    break;
		}
	    }
	    if (cack->error) {
		Chan[chan].state = CHAN_FREE;
		Chan[chan].ior->io_Error = cack->error;
		ReplyMsg(Chan[chan].ior);
		Chan[chan].ior = NULL;
	    } else {
		Chan[chan].state = CHAN_OPEN;
		Chan[chan].ior->io_Error = 0;
		Chan[chan].ior->io_Unit = (struct Unit *)chan;
		Chan[chan].flags = CHANF_ROK|CHANF_WOK;
		ReplyMsg(Chan[chan].ior);
		Chan[chan].ior = NULL;
	    }
	}
	break;
    case SCMD_EOFCMD:	/*  EOF on channel		*/
	{
	    register CEOFCMD *eof = (CEOFCMD *)ptr;
	    uword chan = (eof->chanh<<8)|eof->chanl;

	    if (chan < MAXCHAN && Chan[chan].state == CHAN_OPEN) {
		Chan[chan].flags &= ~eof->flags;
		if (eof->flags & CHANF_ROK)
		    WritePort(Chan[chan].port, DNCMD_EOF, NULL, 0, PKT_REQ, chan);
	    } else {
		printf("SCMD_EOFCMD: Error chan %ld  state %ld\n",
		    chan, Chan[chan].state
		);
	    }
	}
	break;
    case SCMD_IOCTL:
	{
	    register CIOCTL *cio = (CIOCTL *)ptr;
	    uword chan = (cio->chanh<<8)|cio->chanl;

	    if (Chan[chan].state == CHAN_OPEN)
		WritePort(Chan[chan].port, DNCMD_IOCTL, cio, sizeof(*cio), PKT_REQ, chan);
	}
	break;
    default:
	if (DDebug)
	    printf("BAD SCMD, %ld\n", cmd);
	break;
    }
}

replywindow(window)
{
    ubyte rwindow = (window - RPStart) & 7;

    if (rwindow >= 4 || RPak[rwindow]->state == READY) {  /* data ready */
	NetWrite(AckPkt[window], 3, 0);
	++PacketsIn;
    } else {
	NetWrite(NakPkt[window], 3, 0);
	++PacketsNakd;
    }
}