- Re-Implemented lower levels of ncpd (packet and link).

  ncpd is now multithreaded. Results in much better performance and less
  CPU usage.

1 files changed, 357 insertions, 253 deletions

diff --git a/ncpd/packet.cc b/ncpd/packet.cc
index fc0ce11..84f0f66 100644
--- a/ncpd/packet.cc
+++ b/ncpd/packet.cc
@@ -36,329 +36,433 @@
 #include <errno.h>
 #include <sys/ioctl.h>
 #include <termios.h>
+#include <signal.h>
 
 extern "C" {
 #include "mp_serial.h"
 }
-#include "bufferstore.h"
 #include "packet.h"
-#include "iowatch.h"
+#include "link.h"
 
-#define BUFFERLEN 2000
+#define BUFLEN 4096 // Must be a power of 2
+#define BUFMASK (BUFLEN-1)
+#define hasSpace(dir) (((dir##Write + 1) & BUFMASK) != dir##Read)
+#define hasData(dir) (dir##Write != dir##Read)
+#define inca(idx,amount) do { \
+    idx = (idx + amount) & BUFMASK; \
+} while (0)
+#define inc1(idx) inca(idx, 1)
+#define normalize(idx) do { idx &= BUFMASK; } while (0)
 
-packet::packet(const char *fname, int _baud, IOWatch *_iow, short int _verbose = 0):
-iow(_iow)
+static unsigned short pumpverbose = 0;
+
+extern "C" {
+/**
+ * Signal handler does nothing. It just exists
+ * for having the select() below return an
+ * interrupted system call.
+ */
+static void usr1handler(int sig)
 {
-	verbose = _verbose;
-	devname = strdup(fname);
-	baud = _baud;
-	inPtr = inBuffer = new unsigned char[BUFFERLEN + 1];
-	outPtr = outBuffer = new unsigned char[BUFFERLEN + 1];
-	inLen = outLen = termLen = 0;
-	foundSync = 0;
-	esc = false;
-	lastFatal = false;
-	iowLocal = false;
-	serialStatus = -1;
-	crcIn = crcOut = 0;
+    signal(SIGUSR1, usr1handler);
+}
 
-	fd = init_serial(devname, baud, 0);
-	if (!_iow) {
-		iow = new IOWatch();
-		iowLocal = true;
+
+static void *pump_run(void *arg)
+{
+    packet *p = (packet *)arg;
+    pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
+    while (1) {
+	if (p->fd != -1) {
+	    fd_set r_set;
+	    fd_set w_set;
+	    int res;
+	    int count;
+
+	    FD_ZERO(&r_set);
+	    w_set = r_set;
+	    if (hasSpace(p->in))
+		FD_SET(p->fd, &r_set);
+	    if (hasData(p->out))
+		FD_SET(p->fd, &w_set);
+	    res = select(p->fd+1, &r_set, &w_set, NULL, NULL);
+	    switch (res) {
+		case 0:
+		    break;
+		case -1:
+		    break;
+		default:
+		    if (FD_ISSET(p->fd, &w_set)) {
+			count = p->outWrite - p->outRead;
+			if (count < 0)
+			    count = (BUFLEN - p->outRead);
+			res = write(p->fd, &p->outBuffer[p->outRead], count);
+			if (res > 0) {
+			    if (pumpverbose & PKT_DEBUG_DUMP) {
+				int i;
+				printf("pump: wrote %d bytes: (", res);
+				for (i = 0; i<res; i++)
+				    printf("%02x ",
+					   p->outBuffer[p->outRead + i]);
+				printf(")\n");
+			    }
+			    int hadSpace = hasSpace(p->out);
+			    inca(p->outRead, res);
+			    if (!hadSpace)
+				    pthread_kill(p->thisThread, SIGUSR1);
+			}
+		    }
+		    if (FD_ISSET(p->fd, &r_set)) {
+			count = p->inRead - p->inWrite;
+			if (count <= 0)
+			    count = (BUFLEN - p->inWrite);
+			res = read(p->fd, &p->inBuffer[p->inWrite], count);
+			if (res > 0) {
+			    if (pumpverbose & PKT_DEBUG_DUMP) {
+				int i;
+				printf("pump: read %d bytes: (", res);
+				for (i = 0; i<res; i++)
+				    printf("%02x ", p->inBuffer[p->inWrite + i]);
+				printf(")\n");
+			    }
+			    inca(p->inWrite, res);
+			    p->findSync();
+			}
+		    } else {
+			if (hasData(p->in))
+			    p->findSync();
+		    }
+		    break;
+	    }
 	}
-	if (fd == -1)
-		lastFatal = true;
-	else 
-		iow->addIO(fd);
+    }
 }
 
-void packet::reset()
+//static pthread_mutex_t outMutex;
+//static pthread_mutex_t inMutex;
+};
+
+packet::
+packet(const char *fname, int _baud, Link *_link, unsigned short _verbose)
 {
-	if (verbose & PKT_DEBUG_LOG)
-		cout << "resetting serial connection" << endl;
-	if (fd != -1) {
-		iow->remIO(fd);
-		ser_exit(fd);
-	}
-	usleep(100000);
-	inLen = outLen = termLen = 0;
-	inPtr = inBuffer;
-	outPtr = outBuffer;
-	foundSync = 0;
-	esc = false;
+    verbose = pumpverbose = _verbose;
+    devname = strdup(fname);
+    assert(devname);
+    baud = _baud;
+    theLINK = _link;
+    isEPOC = false;
+
+    // Initialize CRC table
+    crc_table[0] = 0;
+    for (int i = 0; i < 128; i++) {
+	unsigned int carry = crc_table[i] & 0x8000;
+	unsigned int tmp = (crc_table[i] << 1) & 0xffff;
+	crc_table[i * 2 + (carry ? 0 : 1)] = tmp ^ 0x1021;
+	crc_table[i * 2 + (carry ? 1 : 0)] = tmp;
+    }
+
+    inRead = inWrite = outRead = outWrite = 0;
+    inBuffer = new unsigned char[BUFLEN + 1];
+    outBuffer = new unsigned char[BUFLEN + 1];
+    assert(inBuffer);
+    assert(outBuffer);
+
+    esc = false;
+    lastFatal = false;
+    serialStatus = -1;
+    lastSYN = startPkt = -1;
+    crcIn = crcOut = 0;
+
+    thisThread = pthread_self();
+    fd = init_serial(devname, baud, 0);
+    if (fd == -1)
+	lastFatal = true;
+    else {
+//	pthread_mutex_init(&inMutex, NULL);
+//	pthread_mutex_init(&outMutex, NULL);
+	signal(SIGUSR1, usr1handler);
+	pthread_create(&datapump, NULL, pump_run, this);
+    }
+}
+
+packet::
+~packet()
+{
+    if (fd != -1) {
+	pthread_cancel(datapump);
+	ser_exit(fd);
+    }
+    fd = -1;
+    delete []inBuffer;
+    delete []outBuffer;
+    free(devname);
+}
+
+void packet::
+reset()
+{
+    if (verbose & PKT_DEBUG_LOG)
+	cout << "resetting serial connection" << endl;
+    if (fd != -1) {
+	pthread_cancel(datapump);
+	ser_exit(fd);
+	fd = -1;
+    }
+    usleep(100000);
+    inRead = inWrite = outRead = outWrite = 0;
+    esc = false;
+    lastFatal = false;
+    serialStatus = -1;
+    lastSYN = startPkt = -1;
+    crcIn = crcOut = 0;
+    fd = init_serial(devname, baud, 0);
+    if (fd != -1)
 	lastFatal = false;
-	serialStatus = -1;
-	crcIn = crcOut = 0;
-	fd = init_serial(devname, baud, 0);
-	if (fd != -1) {
-		iow->addIO(fd);
-		lastFatal = false;
-	}
-	if (verbose & PKT_DEBUG_LOG)
-		cout << "serial connection reset, fd=" << fd << endl;
-	sleep(1);
+    else {
+//	pthread_mutex_init(&inMutex, NULL);
+//	pthread_mutex_init(&outMutex, NULL);
+	pthread_create(&datapump, NULL, pump_run, this);
+    }
+    if (verbose & PKT_DEBUG_LOG)
+	cout << "serial connection reset, fd=" << fd << endl;
 }
 
 short int packet::
 getVerbose()
 {
-	return verbose;
+    return verbose;
 }
 
 void packet::
 setVerbose(short int _verbose)
 {
-	verbose = _verbose;
+    verbose = pumpverbose = _verbose;
 }
 
-packet::~packet()
+void packet::
+setEpoc(bool _epoc)
 {
-	if (fd != -1) {
-		iow->remIO(fd);
-		ser_exit(fd);
-	}
-	usleep(100000);
-	delete[]inBuffer;
-	delete[]outBuffer;
-	free(devname);
-	if (iowLocal)
-		delete iow;
+    isEPOC = _epoc;
 }
 
 void packet::
-send(unsigned char type, const bufferStore & b)
+send(bufferStore &b)
 {
-	if (verbose & PKT_DEBUG_LOG) {
-		cout << "packet: type " << hex << (int) type << " >> ";
-		if (verbose & PKT_DEBUG_DUMP)
-			cout << b << endl;
-		else
-			cout << "len=" << b.getLen() << endl;
-	}
-	opByte(0x16);
-	opByte(0x10);
-	opByte(0x02);
+    opByte(0x16);
+    opByte(0x10);
+    opByte(0x02);
 
-	crcOut = 0;
-	opByte(type);
-	addToCrc(type, &crcOut);
+    crcOut = 0;
+    long len = b.getLen();
 
-	long len = b.getLen();
-	for (int i = 0; i < len; i++) {
-		unsigned char c = b.getByte(i);
-		if (c == 0x10)
-			opByte(c);
-		opByte(c);
-		addToCrc(c, &crcOut);
-	}
-
-	opByte(0x10);
-	opByte(0x03);
-
-	opByte(crcOut >> 8);
-	opByte(crcOut & 0xff);
-	realWrite();
-}
+    if (verbose & PKT_DEBUG_LOG) {
+	cout << "packet: >> ";
+	if (verbose & PKT_DEBUG_DUMP)
+	    cout << b;
+	else
+	    cout << " len=" << dec << len;
+	cout << endl;
+    }
 
-void packet::
-addToCrc(unsigned short c, unsigned short *crc)
-{
-	c <<= 8;
-	for (int i = 0; i < 8; i++) {
-		if ((*crc ^ c) & 0x8000)
-			*crc = (*crc << 1) ^ 0x1021;
-		else
-			*crc <<= 1;
-		c <<= 1;
+    for (int i = 0; i < len; i++) {
+	unsigned char c = b.getByte(i);
+	switch (c) {
+	    case 0x03:
+		if (isEPOC) {
+		    opByte(0x10);
+		    opByte(0x04);
+		    addToCrc(0x03, &crcOut);
+		} else
+		    opCByte(c, &crcOut);
+		break;
+	    case 0x10:
+		opByte(0x10);
+		// fall thru
+	    default:
+		opCByte(c, &crcOut);
 	}
+    }
+    opByte(0x10);
+    opByte(0x03);
+    opByte(crcOut >> 8);
+    opByte(crcOut & 0xff);
+    realWrite();
 }
 
 void packet::
 opByte(unsigned char a)
 {
-	*outPtr++ = a;
-	outLen++;
-	if (outLen >= BUFFERLEN)
-		realWrite();
+    if (!hasSpace(out))
+	realWrite();
+    outBuffer[outWrite] = a;
+    inc1(outWrite);
 }
 
 void packet::
-realWrite()
+opCByte(unsigned char a, unsigned short *crc)
 {
-	outPtr = outBuffer;
-	while (outLen > 0) {
-		int r = write(fd, outPtr, outLen);
-		if (verbose & PKT_DEBUG_LOG)
-			cout << "packet: WR=" << dec << r << endl;
-		if (r > 0) {
-			outLen -= r;
-			outPtr += r;
-		}
-	}
-	outPtr = outBuffer;
+    addToCrc(a, crc);
+    if (!hasSpace(out))
+	realWrite();
+    outBuffer[outWrite] = a;
+    inc1(outWrite);
 }
 
-bool packet::
-get(unsigned char &type, bufferStore & ret)
+void packet::
+realWrite()
 {
-    while (!terminated()) {
-	if (linkFailed())
-	    return false;
-	int res = read(fd, inPtr, BUFFERLEN - inLen);
-	if (res > 0) {
-	    if (verbose & PKT_DEBUG_LOG)
-		cout << "packet: rcv " << dec << res << endl;
-	    inPtr += res;
-	    inLen += res;
-	}
-	if (res < 0)
-	    return false;
-	// XXX Solaris returns 0 on non blocking TTY lines
-	// even when VMIN > 0
-	if( res == 0 && inLen == 0 )
-	    return false;
-	if (inLen >= BUFFERLEN) {
-	    cerr << "packet: input buffer overflow!!!!" << endl;
-	    inLen = 0;
-	    inPtr = inBuffer;
-	    return false;
-	}
-    }
-    if (verbose & PKT_DEBUG_LOG) {
-	cout << "packet: get ";
-	if (verbose & PKT_DEBUG_DUMP) {
-	    for (int i = foundSync - 3; i < termLen; i++)
-		cout << hex << setw(2) << setfill('0') << (int) inBuffer[i] << " ";
-	} else
-	    cout << "len=" << dec << termLen;
-	cout << endl;
-    }
-    inLen -= termLen;
-    termLen = 0;
-    foundSync = 0;
-    bool crcOk = (endPtr[0] == ((crcIn >> 8) & 0xff) && endPtr[1] == (crcIn & 0xff));
-    if (inLen > 0)
-	memmove(inBuffer, &endPtr[2], inLen);
-    inPtr = inBuffer + inLen;
-    if (crcOk) {
-	type = rcv.getByte(0);
-	ret = rcv;
-	ret.discardFirstBytes(1);
-	return true;
-    } else {
-	if (verbose & PKT_DEBUG_LOG)
-	    cout << "packet: BAD CRC" << endl;
+    pthread_kill(datapump, SIGUSR1);
+    while (!hasSpace(out)) {
+	sigset_t sigs;
+	int dummy;
+	sigemptyset(&sigs);
+	sigaddset(&sigs, SIGUSR1);
+	sigwait(&sigs, &dummy);
     }
-    return false;
 }
 
-bool packet::
-terminated()
+void packet::
+findSync()
 {
-    unsigned char *p;
-    int l;
+    int inw = inWrite;
+    int p;
 
-    if (inLen < 6)
-	return false;
-    p = inBuffer + termLen;
-    if (!foundSync) {
-	while (!foundSync && (inLen - termLen >= 6))
-	{
-	    termLen++;
-	    if (*p++ != 0x16)
+ outerLoop:
+    p = (lastSYN >= 0) ? lastSYN : inRead;
+    if (startPkt < 0) {
+	while (p != inw) {
+	    normalize(p);
+	    if (inBuffer[p++] != 0x16)
 		continue;
-	    termLen++;
-	    if (*p++ != 0x10)
+	    lastSYN = p - 1;
+	    normalize(p);
+	    if (inBuffer[p++] != 0x10)
 		continue;
-	    termLen++;
-	    if (*p++ != 0x02)
+	    normalize(p);
+	    if (inBuffer[p++] != 0x02)
 		continue;
-	    foundSync = termLen;
-	}
-	if (!foundSync)
-	    return false;
-
-	if (verbose & PKT_DEBUG_LOG) {
-	    if (foundSync != 3)
-		cout << "packet: terminated found sync at " << foundSync << endl;
+	    normalize(p);
+	    lastSYN = startPkt = p;
+	    crcIn = inCRCstate = 0;
+	    rcv.init();
+	    esc = false;
+	    break;
 	}
-	esc = false;
-	// termLen = 3;
-	crcIn = 0;
-	rcv.init();
     }
-    for (l = termLen; l < inLen - 2; p++, l++) {
-	if (esc) {
-	    esc = false;
-	    if (*p == 0x03) {
-		endPtr = p + 1;
-		termLen = l + 3;
-		return true;
-	    }
-	    addToCrc(*p, &crcIn);
-	    rcv.addByte(*p);
-	} else {
-	    if (*p == 0x10)
-		esc = true;
-	    else {
-		addToCrc(*p, &crcIn);
-		rcv.addByte(*p);
+    if (startPkt >= 0) {
+	while (p != inw) {
+	    unsigned char c = inBuffer[p];
+	    switch (inCRCstate) {
+		case 0:
+		    if (esc) {
+			esc = false;
+			switch (c) {
+			    case 0x03:
+				inCRCstate = 1;
+				break;
+			    case 0x04:
+				addToCrc(0x03, &crcIn);
+				rcv.addByte(0x03);
+				break;
+			    default:
+				addToCrc(c, &crcIn);
+				rcv.addByte(c);
+				break;
+			}
+		    } else {
+			if (c == 0x10)
+			    esc = true;
+			else {
+			    addToCrc(c, &crcIn);
+			    rcv.addByte(c);
+			}
+		    }
+		    break;
+		case 1:
+		    receivedCRC = c;
+		    receivedCRC <<= 8;
+		    inCRCstate = 2;
+		    break;
+		case 2:
+		    receivedCRC |= c;
+		    inc1(p);
+		    inRead = p;
+		    startPkt = lastSYN = -1;
+		    inCRCstate = 0;
+		    if (receivedCRC != crcIn) {
+			if (verbose & PKT_DEBUG_LOG)
+			    cout << "packet: BAD CRC" << endl;
+		    } else {
+			// inQueue += rcv;
+			if (verbose & PKT_DEBUG_LOG) {
+			    cout << "packet: << ";
+			    if (verbose & PKT_DEBUG_DUMP)
+				cout << rcv;
+			    else
+				cout << "len=" << dec << rcv.getLen();
+			    cout << endl;
+			}
+			theLINK->receive(rcv);
+		    }
+		    rcv.init();
+		    if (hasData(out))
+			return;
+		    goto outerLoop;
 	    }
+	    inc1(p);
 	}
+	lastSYN = p;
     }
-    termLen = l;
-    return false;
 }
 
 bool packet::
 linkFailed()
 {
-	int arg;
-	int res;
-	bool failed = false;
+    int arg;
+    int res;
+    bool failed = false;
 
-	if (lastFatal)
-		reset();
-	res = ioctl(fd, TIOCMGET, &arg);
-	if (res < 0)
+    if (lastFatal)
+	reset();
+    res = ioctl(fd, TIOCMGET, &arg);
+    if (res < 0)
+	lastFatal = true;
+    if ((serialStatus == -1) || (arg != serialStatus)) {
+	if (verbose & PKT_DEBUG_HANDSHAKE)
+	    cout << "packet: < DTR:" << ((arg & TIOCM_DTR)?1:0)
+		 << " RTS:" << ((arg & TIOCM_RTS)?1:0)
+		 << " DCD:" << ((arg & TIOCM_CAR)?1:0)
+		 << " DSR:" << ((arg & TIOCM_DSR)?1:0)
+		 << " CTS:" << ((arg & TIOCM_CTS)?1:0) << endl;
+	if (!((arg & TIOCM_RTS) && (arg & TIOCM_DTR))) {
+	    arg |= (TIOCM_DTR | TIOCM_RTS);
+	    res = ioctl(fd, TIOCMSET, &arg);
+	    if (res < 0)
 		lastFatal = true;
-	if ((serialStatus == -1) || (arg != serialStatus)) {
-		if (verbose & PKT_DEBUG_HANDSHAKE)
-			cout << "packet: < DTR:" << ((arg & TIOCM_DTR)?1:0)
-			     << " RTS:" << ((arg & TIOCM_RTS)?1:0)
-			     << " DCD:" << ((arg & TIOCM_CAR)?1:0)
-			     << " DSR:" << ((arg & TIOCM_DSR)?1:0)
-			     << " CTS:" << ((arg & TIOCM_CTS)?1:0) << endl;
-		if (!((arg & TIOCM_RTS) && (arg & TIOCM_DTR))) {
-			arg |= (TIOCM_DTR | TIOCM_RTS);
-			res = ioctl(fd, TIOCMSET, &arg);
-			if (res < 0)
-				lastFatal = true;
-			if (verbose & PKT_DEBUG_HANDSHAKE)
-				cout << "packet: > DTR:" << ((arg & TIOCM_DTR)?1:0)
-				     << " RTS:" << ((arg & TIOCM_RTS)?1:0)
-				     << " DCD:" << ((arg & TIOCM_CAR)?1:0)
-				     << " DSR:" << ((arg & TIOCM_DSR)?1:0)
-				     << " CTS:" << ((arg & TIOCM_CTS)?1:0) << endl;
-		}
-		serialStatus = arg;
+	    if (verbose & PKT_DEBUG_HANDSHAKE)
+		cout << "packet: > DTR:" << ((arg & TIOCM_DTR)?1:0)
+		     << " RTS:" << ((arg & TIOCM_RTS)?1:0)
+		     << " DCD:" << ((arg & TIOCM_CAR)?1:0)
+		     << " DSR:" << ((arg & TIOCM_DSR)?1:0)
+		     << " CTS:" << ((arg & TIOCM_CTS)?1:0) << endl;
 	}
-	if (((arg & TIOCM_CTS) == 0)
+	serialStatus = arg;
+    }
+    if (((arg & TIOCM_CTS) == 0)
 #ifndef sun
-	    || ((arg & TIOCM_DSR) == 0)
+	|| ((arg & TIOCM_DSR) == 0)
 #endif
-	    ) {
-	    // eat possible junk on line
-	    while (read(fd, &res, sizeof(res)) > 0)
-		;
-	    failed = true;
-	}
-	if ((verbose & PKT_DEBUG_LOG) && lastFatal)
-		cout << "packet: linkFATAL\n";
-	if ((verbose & PKT_DEBUG_LOG) && failed)
-		cout << "packet: linkFAILED\n";
-	return lastFatal || failed;
+	) {
+	// eat possible junk on line
+	//while (read(fd, &res, sizeof(res)) > 0)
+	//	;
+	failed = true;
+    }
+    if ((verbose & PKT_DEBUG_LOG) && lastFatal)
+	cout << "packet: linkFATAL\n";
+    if ((verbose & PKT_DEBUG_LOG) && failed)
+	cout << "packet: linkFAILED\n";
+    return lastFatal || failed;
 }
 
 /*