// ParPort.cpp

/* Copyright (C) 2000-2003 Hewlett-Packard Company
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * is provided AS IS, WITHOUT ANY WARRANTY; without even the implied
 * warranty of MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, and
 * NON-INFRINGEMENT.  See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston,
 * MA 02111-1307, USA.
 */

/* Original author: David Paschal */

#define ffs ffs__ParPort	// Workaround link failure on FreeBSD.
#include "ParPort.h"


/*****************************************************************************\
| Constructor, [destructor,] dump():
\*****************************************************************************/

ParPort::ParPort(int baseaddr0,int baseaddr1,int portType) {
	int r;

	setPortType(portType,1);
	initTimevals();
	regInit(baseaddr0,baseaddr1);
	ecpFifoSize=0;
	currentMode=MODE_COMPAT;
	currentChannel=0;
	ecpChannelSetNeeded=0;
	setHtrCount(DEFAULT_HTR_COUNT);
	forwardMode=MODE_COMPAT;
	reverseMode=MODE_NIBBLE;
	desiredChannel=0;

	if (getIoPortAccess()==ERROR) {
		LOG_ERROR_FATAL(cEXBP,0,cCauseFuncFailed,
			"Access denied to parallel port!\n");
	}

	/* Reset control lines in preparation for port type detection. */
	controlWrite(CONTROL_NSTROBE|CONTROL_NAUTOFD|CONTROL_NINIT);

	/* Try ECP. */
	if (portTypeIsEcp()) {
		ecpControlSetMode(ECP_CONTROL_MODE_BIDIRECTIONAL);
		r=ecpControlRead();
		if (r==(ECP_CONTROL_MODE_BIDIRECTIONAL|
		     ECP_CONTROL_ALWAYS_SET|ECP_CONTROL_EMPTY) &&
		    r!=controlRead()) {
			ecpConfigRead();
			if (!portTypeIsEcpWithHwAssist()) goto ecpNoHwAssist;
			if ((ecpConfigA&0x70)!=0x10) {
				/* Set PORTTYPE_ECP if FIFO width>8 bits. */
				LOG_WARN(cEXBP,0,"Using software ECP because "
					"hardware word size exceeds 8 bits.\n");
ecpNoHwAssist:
				setPortType(PORTTYPE_ECP,1);
			} else {
				setPortType(PORTTYPE_ECPHW);
			}
		} else if (portTypeIsValid()) {
			setPortType(PORTTYPE_BPP,1);
		}
	}

	/* Try SPP. */
	if (dataTest(42)==ERROR ||
	    dataTest(24)==ERROR) {
		setPortType(PORTTYPE_NONE,1);

	/* Try BPP. */
	} else if (portTypeIsBidirectional()) {
		controlSetClear(-1,
			CONTROL_REVERSE_DATA,
			0);

		if (dataTest(43)==ERROR &&
		    dataTest(34)==ERROR) {
			setPortType(PORTTYPE_BPP);
		} else {
			setPortType(PORTTYPE_SPP,1);
		}

		controlSetClear(-1,
			0,
			CONTROL_REVERSE_DATA);
		dataWrite(0);
	}

	if (portTypeIsEcpWithHwAssist() &&
	    ecpControlSetMode(ECP_CONTROL_MODE_TEST)!=ERROR) {
		while (!ecpFifoIsEmpty()) ecpDataRead();
		while (!ecpFifoIsFull()) {
			ecpDataWrite(0);
			ecpFifoSize++;
		}
		while (!ecpFifoIsEmpty()) ecpDataRead();
		ecpControlSetMode(ECP_CONTROL_MODE_BIDIRECTIONAL);
		if (!ecpFifoSize) {
			LOG_WARN(cEXBP,0,"Using software ECP because "
				"hardware FIFO size is %d byte(s).\n",
				ecpFifoSize);
			setPortType(PORTTYPE_ECP,1);
		}
	}

	terminate();
}

void ParPort::dump(void) {
	printf("portType=%d\n",portType);
	dumpTimeval(TIMEVAL_COMPAT_SETUP_DELAY,"compatSetupDelay");
	dumpTimeval(TIMEVAL_COMPAT_STROBE_DELAY,"compatStrobeDelay");
	dumpTimeval(TIMEVAL_COMPAT_HOLD_DELAY,"compatHoldDelay");
	dumpTimeval(TIMEVAL_ECP_SETUP_DELAY,"ecpSetupDelay");
	dumpTimeval(TIMEVAL_ECP_POST_HTR_DELAY,"ecpPostHtrDelay");
	dumpTimeval(TIMEVAL_SIGNAL_TIMEOUT,"signalTimeout");
	dumpTimeval(TIMEVAL_BUSY_TIMEOUT,"busyTimeout");
	dumpTimeval(TIMEVAL_EPP_TERM_DELAY,"eppTermDelay");
	printf("baseaddr=0x%3.3X\n",regaddr[REG_SPP_DATA]);
	printf("basehigh=0x%3.3X\n",regaddr[REG_ECP_DATA]);
	// iofd (FreeBSD only)
	// statusWaitTimer
	// ecpConfigA, ecpConfigB (see below)
	printf("ecpFifoSize=%d\n",ecpFifoSize);
	printf("currentMode=0x%2.2X\n",currentMode);
	printf("currentChannel=%d\n",currentChannel);
	printf("ecpChannelSetNeeded=%d\n",ecpChannelSetNeeded);
	printf("htrCount=%d\n",htrCount);
	printf("forwardMode=0x%2.2X\n",forwardMode);
	printf("reverseMode=0x%2.2X\n",reverseMode);
	printf("desiredChannel=%d\n",desiredChannel);

	printf("status       =0x%2.2X\n",statusRead());
	printf("control      =0x%2.2X\n",controlRead());
    if (portTypeIsEcp()) {
	printf("ECP config A =0x%2.2X\n",ecpConfigA);
	printf("ECP config B =0x%2.2X\n",ecpConfigB);
	printf("ECP control  =0x%2.2X\n",ecpControlRead());
    }
}


/*****************************************************************************\
| Low-level 1284 mode/direction negotiation:
\*****************************************************************************/

int ParPort::negotiate(int mode) {
	int selectSet=0,selectClear=0,r;

	if (!modeIsCompat()) {
		LOG_ERROR(cEXBP,0,cCauseBadState,
			"negotiate(mode=0x%2.2X): not compatibility mode!\n",
			mode);
		return ERROR;
	}

	ecpFifoWaitForEmpty(TIMEVAL_BUSY_TIMEOUT);

	/* Disable hardware-assisted compatibility mode. */
	ecpControlSetMode(ECP_CONTROL_MODE_UNIDIRECTIONAL);

	/* EPP requires bidirectional capability. */
	if (modeIsEpp(mode)) {
		r=ecpControlSetMode(ECP_CONTROL_MODE_BIDIRECTIONAL);
		if (r==ERROR) {
			LOG_ERROR(cEXBP,0,cCauseFuncFailed,
				"negotiate(mode=0x%2.2X): "
				"not supported by hardware!\n",
				mode);
			return ERROR;
		}
	}

	/* Event 0: Write extensibility request to data lines. */
	dataWrite(mode);

	/* Event 1: nSelectIn=1, nAutoFd=0, nStrobe=1, nInit=1,
	 * drive data lines. */
	controlSetClear(1,
		CONTROL_NSELECTIN|CONTROL_NSTROBE|CONTROL_NINIT,
		CONTROL_NAUTOFD|CONTROL_REVERSE_DATA);

	/* Event 2: PError=1, Select=1, nFault=1, nAck=0. */
	r=statusWaitSetClear(2,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_PERROR|STATUS_SELECT|STATUS_NFAULT,
		STATUS_NACK);
	if (r==ERROR) goto abort;

	/* Event 3: nStrobe=0. */
	controlSetClear(3,
		0,
		CONTROL_NSTROBE);
	PolledTimer::delay(lookupTimeval(TIMEVAL_COMPAT_STROBE_DELAY));

	/* Event 4: nStrobe=1, nAutoFd=1. */
	controlSetClear(4,
		CONTROL_NSTROBE|CONTROL_NAUTOFD,
		0);

	/* Event 6: nAck=1. */
	r=statusWaitSetClear(6,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_NACK,
		0);
	if (r==ERROR) goto abort;

	/* Event 5: Select=0 for nibble-0, =1 for other modes. */
	if (modeIsNibble0(mode)) {
		selectClear=STATUS_SELECT;
	} else {
		selectSet=STATUS_SELECT;
	}
	if (!statusTestSetClear(selectSet,selectClear)) {
		LOG_WARN(cEXBP,0,
			"negotiate(mode=0x%2.2X): rejected by peripheral!\n",
			mode);
		currentMode=!mode;
		goto abort;
	}
	currentMode=mode&MODE_1284_MASK;

	/* Extra signalling for ECP mode. */
	if (modeIsEcp()) {
		/* Event 30: nAutoFd=0. */
		controlSetClear(30,
			0,
			CONTROL_NAUTOFD);

		/* Event 31: PError=1. */
		r=statusWaitSetClear(31,TIMEVAL_SIGNAL_TIMEOUT,
			STATUS_PERROR,
			0);
		if (r==ERROR) goto abort;

		/* Event 33: Host is idle (nAutoFd=1). */
		controlSetClear(33,
			CONTROL_NAUTOFD,
			0);

		/* Enable hardware-assisted ECP mode if possible. */
		if (modeIsHwAssist(mode)) {
			ecpControlSetMode(ECP_CONTROL_MODE_ECP);
			currentMode|=MODE_HW_ASSIST;
		}

		currentChannel=0;

	/* Extra signalling for EPP mode. */
	} else if (modeIsEpp()) {
		/* Event 14: Tristate data lines. */
		controlSetClear(14,
			CONTROL_REVERSE_DATA,
			0);
	}

	return OK;

abort:
	terminate();
	return ERROR;
}

int ParPort::terminate(int mode) {
	int r,selectSet=0,selectClear=0;
	int priorStatus=statusRead();

    if (modeIsEpp()) {
	/* Event 68: nInit=0. */
	controlSetClear(68,
		CONTROL_NSELECTIN|CONTROL_NAUTOFD|CONTROL_NSTROBE,
		CONTROL_NINIT);
	PolledTimer::delay(lookupTimeval(TIMEVAL_EPP_TERM_DELAY));

	/* Event 69: nInit=1, nSelectIn=0, drive data lines. */
	controlSetClear(69,
		CONTROL_NINIT,
		CONTROL_NSELECTIN|CONTROL_REVERSE_DATA);

	ecpControlSetMode(ECP_CONTROL_MODE_UNIDIRECTIONAL);

    } else {
	if (modeIsEcp()) {
		ecpRevToFwd();
	}

	ecpFifoWaitForEmpty(TIMEVAL_BUSY_TIMEOUT);

	/* Disable hardware-assisted ECP mode if necessary. */
	ecpControlSetMode(ECP_CONTROL_MODE_UNIDIRECTIONAL);

	/* Event 22: nSelectIn=0, nAutoFd=1, nStrobe=1, nInit=1,
	 * drive data lines. */
	controlSetClear(22,
		CONTROL_NAUTOFD|CONTROL_NSTROBE|CONTROL_NINIT,
		CONTROL_NSELECTIN|CONTROL_REVERSE_DATA);

	if (!modeIsCompat()) {
		/* Event 23: Busy=1, nFault=1. */
		/* Event 24: nAck=0, Select=toggled
		 * (1 if terminating nibble-0, 0 otherwise). */
		if (currentMode) {
			selectClear=STATUS_SELECT;
		} else {
			selectSet=STATUS_SELECT;
		}
		r=statusWaitSetClear(23,TIMEVAL_SIGNAL_TIMEOUT,
			STATUS_BUSY|STATUS_NFAULT|selectSet,
			STATUS_NACK|selectClear);

		if (r!=ERROR) {
			/* Event 25: nAutoFd=0. */
			controlSetClear(25,
				0,
				CONTROL_NAUTOFD);
		}

		/* Event 26: PError, nFault, Select to compat. mode values. */
		/* Event 27: nAck=1. */
		r=statusWaitSetClear(27,TIMEVAL_SIGNAL_TIMEOUT,
			STATUS_NACK,
			0);
		if (r==ERROR) goto abort;

		/* Event 28: nAutoFd=1. */
		controlSetClear(28,
			CONTROL_NAUTOFD,
			0);

		/* Event 29: Busy to compatibility mode value. */
	}
    }

	currentMode=MODE_COMPAT;

	/* Enable hardware-assisted compatibility mode if possible. */
	if (modeIsHwAssist(mode)) {
		ecpControlSetMode(ECP_CONTROL_MODE_FAST_CENTRONICS);
		currentMode|=MODE_HW_ASSIST;
	}

	return OK;

abort:
	LOG_WARN(cEXBP,0,
		"terminate: priorStatus=0x%2.2X.\n",priorStatus);
	currentMode=MODE_COMPAT;
	terminate();	/* This is recursive.  OK if MODE_COMPAT. */
	return ERROR;
}

int ParPort::ecpFwdToRev(int mode) {
	int r;

	if (modeIsEcpReverse()) return OK;
	if (!modeIsEcpForward()) return ERROR;

	ecpFifoWaitForEmpty(TIMEVAL_BUSY_TIMEOUT);

	/* Disable hardware-assisted ECP mode if necessary. */
	r=ecpControlSetMode(ECP_CONTROL_MODE_BIDIRECTIONAL);
	if (r==ERROR) {
		LOG_ERROR(cEXBP,0,cCauseFuncFailed,
			"ecpFwdToRev: not supported by hardware!\n");
		return ERROR;
	}

	/* Event 38: nAutoFd=0, tristate data lines. */
	/* There really should be a proper delay between these two events. */
	/* Event 39: nInit=0. */
	controlSetClear(38,
		CONTROL_REVERSE_DATA,
		CONTROL_NAUTOFD|CONTROL_NINIT);

	/* Event 40: PError=0. */
	r=statusWaitSetClear(40,TIMEVAL_SIGNAL_TIMEOUT,
		0,
		STATUS_PERROR);
	if (r==ERROR) return ERROR;

	currentMode=(currentMode&~MODE_HW_ASSIST)|MODE_REVERSE|
		(mode&MODE_HW_ASSIST);

	/* We enable hardware-assisted ECP mode later if desired. */
	return OK;
}

int ParPort::ecpRevToFwd(int mode) {
	int r;

	if (modeIsEcpForward()) return OK;
	if (!modeIsEcpReverse()) return ERROR;

	/* For bounded ECP, wait for nFault=1.  Ignore timeout. */
	if (modeIsBoundedEcp()) {
		statusWaitSetClear(147,TIMEVAL_BUSY_TIMEOUT,
			STATUS_NFAULT,
			0);
	}

	/* Event 47: nInit=1.
	 * Also force nAutoFd=0 in case we're using HW assistance. */
	controlSetClear(47,
		CONTROL_NINIT,
		CONTROL_NAUTOFD);

	/* Print error message if FIFO has data. */
	if (ecpHwAssistIsEnabled()) {
		while (!ecpFifoIsEmpty()) {
			LOG_ERROR(cEXBP,0,cCausePeriphError,
				"ecpRevToFwd: lost reverse data 0x%2.2X!\n",
				ecpDataRead());
		}
	}

	/* Disable hardware-assisted ECP mode if necessary. */
	ecpControlSetMode(ECP_CONTROL_MODE_BIDIRECTIONAL);

	/* Event 48: nAck=1, Busy/nFault=valid. */
	/* Event 49: PError=1. */
	r=statusWaitSetClear(49,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_NACK|STATUS_PERROR,
		0);
	if (r==ERROR) return ERROR;

	/* Event 33: Host is idle (nAutoFd=1), drive data lines. */
	controlSetClear(33,
		CONTROL_NAUTOFD,
		CONTROL_REVERSE_DATA);

	currentMode&=~(MODE_REVERSE|MODE_HW_ASSIST);

	/* Enable hardware-assisted ECP mode if possible. */
	if (modeIsHwAssist(mode)) {
		ecpControlSetMode(ECP_CONTROL_MODE_ECP);
		currentMode|=MODE_HW_ASSIST;
	}

	return OK;
}


/*****************************************************************************\
| Forward data transfers:
\*****************************************************************************/

int ParPort::writeEcp(const unsigned char *buffer,int len,
    int isCommand) {
	int r,htrCountdown=htrCount,countup=0;

	/* Event 34: Write command line (data lines written below). */
	if (!isCommand) {
		controlSetClear(34,
			CONTROL_NAUTOFD,
			0);
	} else {
		controlSetClear(34,
			0,
			CONTROL_NAUTOFD);
	}

    while (42) {
	/* Event 32: Busy=0. */
	r=statusWaitSetClear(32,TIMEVAL_BUSY_TIMEOUT,
		0,
		STATUS_BUSY);
	if (len<=0 || r==ERROR) break;

	/* Event 34: Write data lines (command line written above). */
	dataWrite(*buffer);
	PolledTimer::delay(lookupTimeval(TIMEVAL_ECP_SETUP_DELAY));

	/* Event 35: nStrobe=0. */
	controlSetClear(35,
		0,
		CONTROL_NSTROBE);

	/* Event 36: Busy=1. */
	r=statusWaitSetClear(36,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_BUSY,
		0);
	if (r==ERROR) {
		LOG_WARN(cEXBP,0,"Host transfer recovery!\n");

		/* Event 72: nInit=0. */
		controlSetClear(72,
			0,
			CONTROL_NINIT);

		/* Event 73: PError=0. */
		r=statusWaitSetClear(73,TIMEVAL_SIGNAL_TIMEOUT,
			0,
			STATUS_PERROR);

		/* Event 74: nInit=1, nStrobe=1. */
		controlSetClear(74,
			CONTROL_NINIT|CONTROL_NSTROBE,
			0);

		if (r==ERROR || htrCountdown<=1) break;

		PolledTimer::delay(lookupTimeval(TIMEVAL_ECP_POST_HTR_DELAY));

		htrCountdown--;
		continue;
	}

	/* Event 37: nStrobe=1. */
	controlSetClear(37,
		CONTROL_NSTROBE,
		0);

	buffer++;
	countup++;
	len--;
    }

	return countup;
}

int ParPort::writeEpp(const unsigned char *buffer,int len,
    int isAddress) {
	int countup=0,eppSet,eppClear,r;
	eppGetSetClearLines(isAddress,&eppSet,&eppClear);

	/* Event 56/62 (partial): nStrobe=0(=write), drive data lines. */
	controlSetClear(56,
		0,
		CONTROL_NSTROBE|CONTROL_REVERSE_DATA);

    while (42) {
	/* Event 60: Busy=0. */
	r=statusWaitSetClear(60,TIMEVAL_BUSY_TIMEOUT,
		0,
		STATUS_BUSY);
	if (len<=0 || r==ERROR) break;

	/* Event 56/62 (partial): write data lines,
	 * isAddress?nSelectIn=0:nAutoFd=0. */
	dataWrite(*buffer);
	controlSetClear(62,
		eppSet,
		eppClear);

	/* Event 58: Busy=1. */
	r=statusWaitSetClear(58,TIMEVAL_BUSY_TIMEOUT,
		STATUS_BUSY,
		0);
	/* We check the return code below. */

	/* Event 59/63: nAutoFd=1, nSelectIn=1. */
	controlSetClear(59,
		CONTROL_NAUTOFD|CONTROL_NSELECTIN,
		0);

	if (r==ERROR) break;

	buffer++;
	countup++;
	len--;
    }

	/* Event 61: nStrobe=1, tristate data lines. */
	controlSetClear(61,
		CONTROL_NSTROBE|CONTROL_REVERSE_DATA,
		0);

	return countup;
}


/*****************************************************************************\
| Reverse data transfers:
\*****************************************************************************/

int ParPort::readNibble(unsigned char *buffer,int len) {
	int r,countup=0,index=0;

    while (len>0) {
	/* For the first nibble make sure the peripheral is asserting
	 * nDataAvail (nFault). */
	if (!index && !statusReverseDataIsAvailable()) {
		LOG_WARN(cEXBP,0,"readNibble: no more data!\n");
		break;
	}

	/* Event 7/12: nAutoFd=0. */
	controlSetClear(7,
		0,
		CONTROL_NAUTOFD);

	/* Event 9: nAck=0. */
	r=statusWaitSetClear(9,TIMEVAL_BUSY_TIMEOUT,
		0,
		STATUS_NACK);
	if (r==ERROR) {
		/* Undo event 7/12: nAutoFd=1. */
		controlSetClear(-7,
			CONTROL_NAUTOFD,
			0);
		break;
	}

	/* Event 8: Read nibble. */
	if (!index) {
		*buffer=statusReadLowNibble();
	} else {
		*buffer|=statusReadHighNibble();
	}

	/* Event 10: nAutoFd=1. */
	controlSetClear(10,
		CONTROL_NAUTOFD,
		0);

	/* Event 11: nAck=1. */
	r=statusWaitSetClear(11,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_NACK,
		0);
	/* We check the return code below. */

	index^=1;
	if (!index) {
		buffer++;
		countup++;
		len--;
	}

	if (r==ERROR) break;
    }

	return countup;
}

int ParPort::readEcpSw(unsigned char *buffer,int len) {
	int r,countup=0;

    while (len>0) {
	/* Event 43: nAck=0. */
	r=statusWaitSetClear(43,TIMEVAL_BUSY_TIMEOUT,
		0,
		STATUS_NACK);
	if (r==ERROR) break;

	/* Event 42: Peripheral drives data lines, Busy=nCmd(ignored). */
	*buffer=dataRead();

	/* Event 44: nAutoFd=1. */
	controlSetClear(44,
		CONTROL_NAUTOFD,
		0);

	/* Event 45: nAck=1. */
	r=statusWaitSetClear(45,TIMEVAL_SIGNAL_TIMEOUT,
		STATUS_NACK,
		0);
	/* We check the return code below. */

	/* Event 46: nAutoFd=0. */
	controlSetClear(46,
		0,
		CONTROL_NAUTOFD);

	buffer++;
	countup++;
	len--;

	/* Event 41: Peripheral is idle. */
	if (r==ERROR) break;
    }

	return countup;
}

int ParPort::readEpp(unsigned char *buffer,int len,int isAddress) {
	int countup=0,eppSet,eppClear,r;
	eppGetSetClearLines(isAddress,&eppSet,&eppClear);

	/* Event 64/67 (partial): nStrobe=1(=read), tristate data lines. */
	controlSetClear(64,
		CONTROL_NSTROBE|CONTROL_REVERSE_DATA,
		0);

    while (42) {
	/* Event 60: Busy=0. */
	r=statusWaitSetClear(60,TIMEVAL_BUSY_TIMEOUT,
		0,
		STATUS_BUSY);
	if (len<=0 || r==ERROR) break;

	/* This is non-standard for EPP: */
	if (!statusReverseDataIsAvailable()) break;

	/* Event 64/67 (partial): isAddress?nSelectIn=0:nAutoFd=0. */
	controlSetClear(67,
		eppSet,
		eppClear);

	/* Event 58: Busy=1. */
	int r=statusWaitSetClear(158,TIMEVAL_BUSY_TIMEOUT,
		STATUS_BUSY,
		0);
	/* We check the return code below. */

	/* Event 65: Read data lines. */
	*buffer=dataRead();

	/* Event 59/63: nAutoFd=1, nSelectIn=1. */
	controlSetClear(59,
		CONTROL_NAUTOFD|CONTROL_NSELECTIN,
		0);

	if (r==ERROR) break;

	buffer++;
	countup++;
	len--;
    }

	return countup;
}

char *ParPort::getDeviceID(void) {
	unsigned char slength[2];
	char *buffer=0;
	int r,length;

	r=negotiate(MODE_NIBBLE|MODE_DEVICE_ID);
	if (r==ERROR) {
		LOG_WARN(cEXBP,0,
			"getDeviceID: negotiate() returns %d!\n",r);
		goto abort;
	}

	r=read(slength,2);
	if (r!=2) {
		LOG_ERROR(cEXBP,0,cCausePeriphError,
			"getDeviceID: read %d bytes, expected=2!\n",r);
		goto abort;
	}
	length=((slength[0]<<8)|slength[1])-2;
	if (length<0) {
		LOG_ERROR(cEXBP,0,cCausePeriphError,
			"getDeviceID: invalid length=%d!\n",length);
		goto abort;
	}

	buffer=new char[length+1];
	if (!buffer) {
		LOG_ERROR(cEXBP,0,cCauseNoMem,
			"getDeviceID: error allocating %d-byte buffer!\n",
			length+1);
		goto abort;
	}

	r=read((unsigned char *)buffer,length);
	if (r!=length) {
		LOG_ERROR(cEXBP,0,cCausePeriphError,
			"getDeviceID: read %d bytes, expected=%d!\n",
			r,length);
		if (r<0) goto abort;
	}
	buffer[r]=0;		/* Null-terminate string. */

done:
	r=terminate();
	if (r==ERROR) {
		LOG_ERROR(cEXBP,0,cCausePeriphError,
			"getDeviceID: terminate() returns %d!\n",r);
	}
	return buffer;

abort:
	if (buffer) {
		delete[] buffer;
		buffer=0;
	}
	goto done;
}