From cb2577b29fe7b93e9b168ded7f35da748fdeaf1d Mon Sep 17 00:00:00 2001 From: Fritz Elfert Date: Tue, 5 Mar 2002 17:58:11 +0000 Subject: - Re-Implemented lower levels of ncpd (packet and link). ncpd is now multithreaded. Results in much better performance and less CPU usage. --- ncpd/packet.cc | 610 +++++++++++++++++++++++++++++++++------------------------ 1 file changed, 357 insertions(+), 253 deletions(-) (limited to 'ncpd/packet.cc') 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 #include #include +#include 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; ioutBuffer[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; iinBuffer[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; } /* -- cgit v1.2.3