/*
* Routine to read a whole BR1 message
* length of which depends on the first value received.
* This also handles "error" messages (top bit of first value set)
*
* Returns length of received message, or TIMEOUT error, or BUSERROR
* if the BR interface tells us theres a congested bus
*/
static int
br_getmsg(struct diag_l0_device *dl0d, uint8_t *dp, unsigned int timeout) {
uint8_t firstbyte;
size_t readlen;
int rv;
struct br_device *dev = dl0d->l0_int;
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
FLFMT "link %p getmsg timeout %u\n", FL, (void *)dl0d, timeout);
/*
* First read the 1st byte, using the supplied timeout
*/
rv = diag_tty_read(dev->tty_int, &firstbyte, 1, timeout);
if (rv != 1) {
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
FLFMT "link %p getmsg 1st byte timed out\n", FL, (void *)dl0d);
return diag_ifwderr(rv);
}
/*
* Now read data. Maximum is 15 bytes.
*/
readlen = firstbyte & 0x0f;
/*
* Reasonable timeout here as the interface told us how
* much data to expect, so it should arrive
*/
rv = diag_tty_read(dev->tty_int, dp, readlen, 100);
if (rv != (int)readlen) {
fprintf(stderr, FLFMT "br_getmsg error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
DIAG_DBGMDATA(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V, dp, readlen,
FLFMT "link %p getmsg read ctl 0x%X data:",
FL, (void *)dl0d, firstbyte & 0xff);
/*
* Message read complete, check error flag
* Top bit set means error, Bit 6 = VPW/PWM bus
* congestion (i.e retry).
*/
if (firstbyte & 0x80) { /* Error indicator */
return diag_iseterr(DIAG_ERR_TIMEOUT);
}
if (firstbyte & 0x40) { /* VPW/PWM bus conflict, need to retry */
return diag_iseterr(DIAG_ERR_BUSERROR);
}
if (readlen == 0) { /* Should never happen */
return diag_iseterr(DIAG_ERR_TIMEOUT);
}
return (int) readlen;
}
/*
* Fastinit: ISO14230-2 sec 5.2.4.2.3
* Caller should have waited W5 (>300ms) before calling this (from _initbus only!)
* we assume the L line was at the correct state (1) during that time.
* returns 0 (success), 50ms after starting the wake-up pattern.
* Exceptionally we dont diag_iseterr on return since _initbus() takes care of that.
*/
static int
diag_l0_dumb_fastinit(struct diag_l0_device *dl0d)
{
int rv=0;
uint8_t cbuf[MAXRBUF];
if (diag_l0_debug & DIAG_DEBUG_INIT)
fprintf(stderr, FLFMT "dl0d=%p fastinit\n",
FL, (void *)dl0d);
//Tidle before break : W5 (>300ms) on poweron; P3 (>55ms) after a StopCommunication; or 0ms after a P3 timeout.
// We assume the caller took care of this.
/* Send 25/25 ms break as initialisation pattern (TiniL) */
//ISO14230-2 says we should send the same sync pattern on both L and K together.
// we do it almost perfectly; the L \_/ pulse starts before and ends after the K \_/ pulse.
if (dumb_flags & USE_LLINE) {
// do K+L only if the user wants to do both
if (dumb_flags & FAST_BREAK) {
//but we can't use diag_tty_fastbreak while doing the L-line.
fprintf(stderr, FLFMT "Warning : not using L line for FAST_BREAK.\n", FL);
rv=diag_tty_fastbreak(dl0d, 50-WUPFLUSH);
} else {
//normal fast break on K and L.
//note : if LLINE_INV is 1, then we need to clear RTS to pull L down !
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)) < 0) {
fprintf(stderr, FLFMT "fastinit: Failed to set L\\_\n", FL);
return DIAG_ERR_GENERAL;
}
rv=diag_tty_break(dl0d, 25); //K line low for 25ms
/* Now restore DTR/RTS */
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & SET_RTS)) < 0) {
fprintf(stderr, FLFMT "fastinit: Failed to restore DTR & RTS!\n",
FL);
}
diag_os_millisleep(25-WUPFLUSH);
} //if FAST_BREAK
} else {
// do K line only
if (dumb_flags & FAST_BREAK) {
rv=diag_tty_fastbreak(dl0d, 50-WUPFLUSH);
} else {
//normal break
rv=diag_tty_break(dl0d, 25); //K line low for 25ms
diag_os_millisleep(25-WUPFLUSH);
}
} //if USE_LLINE
// here we have WUPFLUSH ms before tWUP is done; we use this
// short time to flush RX buffers. (L2 needs to send a StartComm
// request very soon.)
diag_tty_read(dl0d, cbuf, sizeof(cbuf), WUPFLUSH);
//there may have been a problem in diag_tty_break, if so :
if (rv) {
fprintf(stderr, FLFMT " L0 fastinit : problem !\n", FL);
return DIAG_ERR_GENERAL;
}
return 0;
}
//return <0 on error, number of bytes on success
static int
diag_l1_saferead(struct diag_l0_device *dl0d, char *buf, size_t bufsiz, int timeout)
{
int xferd;
/* And read back the single byte echo, which shows TX completes */
while ( (xferd = diag_tty_read(dl0d, buf, bufsiz, timeout)) < 0) {
if (errno != EINTR)
return diag_iseterr(DIAG_ERR_BUSERROR);
xferd = 0; /* Interrupted read, nothing transferred. */
}
return xferd;
}
/*
* flush input buffer and display some of the discarded data
* ret 0 if ok
*
*/
int diag_tty_iflush(struct diag_l0_device *dl0d)
{
uint8_t buf[MAXRBUF];
int rv;
struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
/* Read any old data hanging about on the port */
rv = diag_tty_read(dl0d, buf, sizeof(buf), IFLUSH_TIMEOUT);
if ((rv > 0) && (diag_l0_debug & DIAG_DEBUG_DATA)) {
fprintf(stderr, FLFMT "tty_iflush: >=%d junk bytes discarded: 0x%X...\n", FL, rv, buf[0]);
// diag_data_dump(stderr, (void *) buf, (size_t) rv); //this could take a long time.
// fprintf(stderr,"\n");
}
PurgeComm(wti->fd, PURGE_RXABORT | PURGE_RXCLEAR);
return 0;
}
//elm_bogusinit : send a 01 00 request to force ELM to init bus.
//Only used to force clones to establish a connection... hack-grade because it assumes all ECUs support this.
// non-OBD ECUs may not work with this. ELM clones suck...
// TODO : add argument to allow either a 01 00 request (SID1 PID0, J1979) or 3E (iso14230 TesterPresent) request to force init.
static int
elm_bogusinit(struct diag_l0_device *dl0d, unsigned int timeout)
{
int rv;
uint8_t buf[MAXRBUF];
uint8_t data[]={0x01,0x00};
const char *err_str;
rv = diag_l0_elm_send(dl0d, NULL, data, 2);
if (rv)
return diag_iseterr(rv);
// receive everything; we're hoping for a prompt at the end and no error message.
rv=diag_tty_read(dl0d, buf, MAXRBUF-5, timeout); //rv=# bytes read
if (diag_l0_debug & (DIAG_DEBUG_WRITE | DIAG_DEBUG_READ)) {
fprintf(stderr, FLFMT "received %d bytes\n", FL, rv);
if (diag_l0_debug & DIAG_DEBUG_DATA) {
elm_parse_cr(buf, rv);
fprintf(stderr, FLFMT "(got %.*s)\n", FL, rv, buf);
}
}
if (rv<1) {
//no data or error
if (diag_l0_debug & DIAG_DEBUG_WRITE) {
fprintf(stderr, FLFMT "ELM did not respond\n", FL);
}
return diag_iseterr(DIAG_ERR_GENERAL);
}
buf[rv]=0; //terminate string
if (buf[rv-1] != '>') {
//if last character isn't the input prompt, there is a problem
fprintf(stderr, FLFMT "ELM not ready (no prompt received): %s\n", FL, buf);
return diag_iseterr(DIAG_ERR_GENERAL);
}
err_str = elm_parse_errors(dl0d, buf);
if (err_str != NULL) {
fprintf(stderr, FLFMT "got error while forcing init: %s\n", FL, err_str);
return diag_iseterr(DIAG_ERR_GENERAL);
}
return 0;
}
/*
* Original Linux input-flush implementation that uses the select timeout:
*/
int diag_tty_iflush(struct diag_l0_device *dl0d)
{
char buf[MAXRBUF];
int i, rv;
/* Read any old data hanging about on the port */
rv = diag_tty_read(dl0d, buf, sizeof(buf), 150);
if ((rv > 0) && (diag_l0_debug & DIAG_DEBUG_OPEN))
{
fprintf(stderr, FLFMT "%d junk bytes discarded: ", FL,
rv);
for (i=0; i<rv; i++)
fprintf(stderr, "0x%x ", buf[i] & 0xff);
fprintf(stderr,"\n");
}
return 0;
}
//elm_purge : sends ATI command and checks for a valid prompt. This is faster than ATZ.
//use : if the ELM received garbage before ATI, ex.: "\xFF\xFFATI\r" it will just reject
//the invalid command but still give a valid prompt.
//Return 0 only if a valid prompt was received.
static int elm_purge(struct diag_l0_device *dl0d) {
uint8_t buf[ELM_BUFSIZE] = "ATI\x0D";
int rv;
if (diag_tty_write(dl0d, buf, 4) != 4) {
fprintf(stderr, FLFMT "elm_purge : write error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
rv = diag_tty_read(dl0d, buf, sizeof(buf), ELM_PURGETIME);
if (rv < 1) {
return DIAG_ERR_GENERAL;
}
if (buf[rv-1] != '>') {
if (diag_l0_debug & DIAG_DEBUG_DATA) {
fprintf(stderr, FLFMT "elm_purge: got ", FL);
diag_data_dump(stderr, buf, rv);
fprintf(stderr, "\n");
}
return DIAG_ERR_GENERAL;
}
return 0;
}
/*
* diag_tty_fastbreak: send 0x00 at 360bps => fixed 25ms break; return [ms] after starting break.
* This is for ISO14230 fast init : typically diag_tty_fastbreak(dl0d, 50)
* It assumes the interface is half-duplex.
* Ret 0 if ok
*/
int diag_tty_fastbreak(struct diag_l0_device *dl0d, const unsigned int ms) {
HANDLE dh; //just to clarify code
struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
DCB tempDCB; //for sabotaging the settings just to do the break
DCB origDCB;
LARGE_INTEGER qpc1, qpc2, qpc3; //to time the break period
LONGLONG timediff; //64bit delta
long int tremain,counts, break_error;
uint8_t cbuf;
int xferd;
DWORD byteswritten;
dh = wti->fd;
if (ms<25) //very funny
return diag_iseterr(DIAG_ERR_TIMEOUT);
if (dh == INVALID_HANDLE_VALUE) {
fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
GetCommState(dh, &origDCB);
GetCommState(dh, &tempDCB); //ugly, but a memcpy would be worse
tempDCB.BaudRate=360;
tempDCB.ByteSize=8;
tempDCB.fParity=0;
tempDCB.Parity=NOPARITY;
tempDCB.StopBits=ONESTOPBIT;
if (! SetCommState(dh, &tempDCB)) {
fprintf(stderr, FLFMT "SetCommState error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
/* Send a 0x00 byte message */
if (! WriteFile(dh, "\0", 1, &byteswritten, NULL)) {
fprintf(stderr, FLFMT "WriteFile error:%s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
//get approx starting time. I think this is the closest we can
//get to the actual time the byte gets sent since we call FFB
//right after.
QueryPerformanceCounter(&qpc1);
if (!FlushFileBuffers(dh)) {
fprintf(stderr, FLFMT "FFB error, %s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
/*
* And read back the single byte echo, which shows TX completes
*/
xferd = diag_tty_read(dl0d, &cbuf, 1, ms + 20);
//we'll usually have a few ms left to wait; we'll use this
//to restore the port settings
if (! SetCommState(dh, &origDCB)) {
fprintf(stderr, FLFMT "tty_fastbreak: could not restore setting: %s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
//Not getting the echo byte doesn't mean fastbreak has necessarily
// failed. But we really should be getting an echo back...
if (xferd < 0)
return diag_iseterr(xferd);
if ((xferd == 0) || (cbuf != 0)) {
/* Error, EOF or bad echo */
fprintf(stderr, FLFMT "Did not get fastbreak echo!\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
QueryPerformanceCounter(&qpc2); //get current time,
timediff=qpc2.QuadPart-qpc1.QuadPart; //elapsed counts since diag_tty_write
counts=(ms*perfo_freq.QuadPart)/1000; //total # of counts for requested tWUP
tremain=counts-timediff; //counts remaining
if (tremain<=0)
return 0;
tremain = ((LONGLONG) tremain*1000)/perfo_freq.QuadPart; //convert to ms; imprecise but that should be OK.
diag_os_millisleep((unsigned int) tremain);
QueryPerformanceCounter(&qpc3);
timediff=qpc3.QuadPart-qpc1.QuadPart; //total cycle time.
break_error= (long) timediff - counts; //real - requested
break_error= (long) (break_error * pf_conv); //convert to us !
if (break_error > 1000 || break_error < -1000)
fprintf(stderr, FLFMT "tty_fastbreak: tWUP out of spec by %ldus!\n", FL, break_error);
return 0;
} //diag_tty_fastbreak
/*
* Get data (blocking), returns number of bytes read, between 1 and len
* If timeout is set to 0, this becomes non-blocking
*
* This attempts to read whole message, so if we receive any data, timeout
* is restarted
*
* Messages received from the BR1 are of format
* <control_byte><data ..>
* If control byte is < 16, it's a length byte, else it's a error descriptor
*/
static int
br_recv(struct diag_l0_device *dl0d,
UNUSED(const char *subinterface),
void *data, size_t len, unsigned int timeout) {
int xferd, rv, retrycnt;
uint8_t *pdata = (uint8_t *)data;
struct br_device *dev;
dev = (struct br_device *)dl0d->l0_int;
if (!len) {
return diag_iseterr(DIAG_ERR_BADLEN);
}
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
FLFMT
"link %p recv upto %ld bytes timeout %u, rxlen %d "
"offset %d framenr %d protocol %d state %d\n",
FL, (void *)dl0d, (long)len, timeout, dev->dev_rxlen,
dev->dev_rdoffset, dev->dev_framenr, dev->protocol,
dev->dev_state);
switch (dev->dev_state) {
case BR_STATE_KWP_FASTINIT:
/* Extend timeouts */
timeout = 300;
dev->dev_state = BR_STATE_OPEN;
break;
case BR_STATE_KWP_SENDKB1:
if (len >= 2) {
pdata[0] = dev->dev_kb1;
pdata[1] = dev->dev_kb2;
dev->dev_state = BR_STATE_OPEN;
return 2;
} else if (len == 1) {
*pdata = dev->dev_kb1;
dev->dev_state = BR_STATE_KWP_SENDKB2;
return 1;
}
return 0; /* Strange, user asked for 0 bytes */
break;
case BR_STATE_KWP_SENDKB2:
if (len >= 1) {
*pdata = dev->dev_kb2;
dev->dev_state = BR_STATE_OPEN;
return 1;
}
return 0; /* Strange, user asked for 0 bytes */
break;
default:
//So BR_STATE_CLOSED and BR_STATE_OPEN.
// I don't know what's supposed to happen here, so
fprintf(stderr, FLFMT "Warning : landed in a strange place. Report this please !\n", FL);
return 0;
break;
}
switch (dev->protocol) {
case DIAG_L1_ISO9141:
case DIAG_L1_ISO14230:
/* Raw mode */
xferd = diag_tty_read(dev->tty_int, data, len, timeout);
break;
default:
/*
* PWM/VPW Modes
*
* If theres stuff on the dev-descriptor, give it back
* to the user.
* We extend timeouts here because in PWM/VPW
* modes the interface tells us if there is a timeout, and
* we get out of sync if we dont wait for it.
*/
if (timeout < 500) {
timeout = 500;
}
if (dev->dev_rxlen == 0) {
/*
* No message available, try getting one
*
* If this is the 2nd read after a send, then
* we need to resend the request with the next
* frame number to see if any more data is ready
*/
if (dev->dev_framenr > 1) {
rv = br_writemsg(dl0d,
BR_WRTYPE_DATA,
dev->dev_txbuf, (size_t)dev->dev_txlen);
}
dev->dev_framenr++;
retrycnt = 0;
while (1) {
dev->dev_rdoffset = 0;
rv = br_getmsg(dl0d, dev->dev_rxbuf, timeout);
if (rv >= 0) {
dev->dev_rxlen = rv;
break;
}
if ((rv != DIAG_ERR_BUSERROR) ||
(retrycnt >= 30)) {
dev->dev_rxlen = 0;
return rv;
}
/* Need to resend and try again */
rv = br_writemsg(dl0d,
BR_WRTYPE_DATA, dev->dev_txbuf,
(size_t)dev->dev_txlen);
if (rv < 0) {
return rv;
}
retrycnt++;
}
}
if (dev->dev_rxlen) {
size_t bufbytes = dev->dev_rxlen - dev->dev_rdoffset;
if (bufbytes <= len) {
memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], bufbytes);
dev->dev_rxlen = dev->dev_rdoffset = 0;
return (int) bufbytes;
}
memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], len);
dev->dev_rdoffset += len;
return (int) len;
}
xferd = 0;
break;
}
/* OK, got whole message */
DIAG_DBGMDATA(diag_l0_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
data, (size_t) xferd,
FLFMT "link %p received from BR1: ", FL, (void *)dl0d);
return xferd;
}
/*
* Do 5 Baud initialisation
*
* In the case of ISO9141 we operate in the interface's "raw" mode
* (VAG compatibility mode), in 14230 we do a slow init and send
* a tester present message
*/
static int
diag_l0_muleng_slowinit( struct diag_l0_device *dl0d, struct diag_l1_initbus_args *in,
struct diag_l0_muleng_device *dev)
{
/*
* Slow init
* Build message into send buffer, and calculate checksum
*/
uint8_t txbuf[15];
uint8_t rxbuf[15];
int rv;
unsigned int baud;
memset(txbuf, 0, sizeof(txbuf));
txbuf[0] = INTERFACE_ADDRESS;
switch (dev->protocol) {
case DIAG_L1_ISO9141:
txbuf[1] = 0x20; /* Raw mode 5 baud init */
txbuf[2] = in->addr;
break;
case DIAG_L1_ISO14230:
txbuf[1] = 0x85;
txbuf[2] = 0x01; /* One byte message */
txbuf[3] = DIAG_KW2K_SI_TP; /* tester present */
break;
}
/*
* Calculate the checksum, and send the request
*/
(void)diag_l0_muleng_txcksum(txbuf);
if ((rv = diag_l0_muleng_write(dl0d, txbuf, 15)))
return diag_iseterr(rv);
/*
* Get answer
*/
switch (dev->protocol) {
case DIAG_L1_ISO9141:
/*
* This is raw mode, we should get a single byte back
* with the timing interval, then we need to change speed
* to match that speed. Remember it takes 2 seconds to send
* the 10 bit (1+8+1) address at 5 baud
*/
rv = diag_tty_read(dl0d, rxbuf, 1, 2350);
if (rv != 1)
return diag_iseterr(DIAG_ERR_GENERAL);
if (rxbuf[0] == 0x40) {
/* Problem ..., got an error message */
diag_tty_iflush(dl0d); /* Empty the receive buffer */
return diag_iseterr(DIAG_ERR_GENERAL);
}
baud = me_baud_table[rxbuf[0]];
if (diag_l0_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "device link %p setting baud to %u\n",
FL, (void *)dl0d, baud);
if (baud) {
struct diag_serial_settings set;
set.speed = baud;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
/* And set the baud rate */
diag_tty_setup(dl0d, &set);
}
dev->dev_state = MULENG_STATE_RAW;
break;
case DIAG_L1_ISO14230:
/* XXX
* Should get an ack back, rather than an error response
*/
if ((rv = diag_tty_read(dl0d, rxbuf, 14, 200)) < 0)
return diag_iseterr(rv);
if (rxbuf[1] == 0x80)
return diag_iseterr(DIAG_ERR_GENERAL);
/*
* Now send the "get keybyte" request, and wait for
* response
*/
memset(txbuf, 0, sizeof(txbuf));
txbuf[0] = INTERFACE_ADDRESS;
txbuf[1] = 0x86;
(void)diag_l0_muleng_txcksum(txbuf);
rv = diag_l0_muleng_write(dl0d, txbuf, 15);
if (rv < 0)
return diag_iseterr(rv);
if ((rv = diag_tty_read(dl0d, rxbuf, 14, 200)) < 0)
return diag_iseterr(rv);
if (rxbuf[1] == 0x80) /* Error */
return diag_iseterr(rv);
/*
* Store the keybytes
*/
dev->dev_kb1 = rxbuf[2];
dev->dev_kb2 = rxbuf[3];
/*
* And tell read code to report the keybytes on first read
*/
dev->dev_state = MULENG_STATE_KWP_SENDKB1;
break;
}
return rv;
}
/*
* Open the diagnostic device, return a file descriptor,
* record the original state of term interface so we can restore later
*/
static int br_open(struct diag_l0_device *dl0d, int iProtocol) {
struct br_device *dev = dl0d->l0_int;
int rv;
uint8_t buf[4]; /* Was MAXRBUF. We only use 1! */
struct diag_serial_settings set;
br_init();
dev->protocol = iProtocol;
dev->dev_rdoffset = 0;
dev->dev_txlen = 0;
dev->dev_framenr = 0;
dev->dev_state = BR_STATE_CLOSED;
dev->dev_features = BR_FEATURE_SETADDR;
/* try to open TTY */
dev->tty_int = diag_tty_open(dev->port.val.str);
if (dev->tty_int == NULL) {
return diag_iseterr(DIAG_ERR_GENERAL);
}
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
FLFMT "features 0x%X\n", FL, dev->dev_features);
/* Set serial line to 19200 baud , 8N1 */
set.speed = 19200;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
if (diag_tty_setup(dev->tty_int, &set)) {
fprintf(stderr, FLFMT "open: TTY setup failed\n", FL);
br_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
diag_tty_iflush(dev->tty_int); /* Flush unread input data */
/*
* Initialise the BR1 interface by sending the CHIP CONNECT
* (0x20h) command, we should get a 0xFF back
*/
buf[0] = 0x20;
if (br_write(dl0d, buf, 1)) {
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
FLFMT "CHIP CONNECT write failed link %p\n",
FL, (void *)dl0d);
br_close(dl0d);
return diag_iseterr(DIAG_ERR_BADIFADAPTER);
}
/* And expect 0xff as a response */
if (diag_tty_read(dev->tty_int, buf, 1, 100) != 1) {
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
FLFMT "CHIP CONNECT read failed link %p\n",
FL, (void *)dl0d);
br_close(dl0d);
return diag_iseterr(DIAG_ERR_BADIFADAPTER);
}
if (buf[0] != 0xff) {
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
FLFMT "CHIP CONNECT rcvd 0x%X != 0xff, link %p\n",
FL, buf[0], (void *)dl0d);
br_close(dl0d);
return diag_iseterr(DIAG_ERR_BADIFADAPTER);
}
/* If it's J1850, send initialisation string now */
rv = 0;
switch (iProtocol) {
case DIAG_L1_J1850_VPW:
rv = br_initialise(dl0d, 0, 0);
break;
case DIAG_L1_J1850_PWM:
rv = br_initialise(dl0d, 1, 0);
break;
case DIAG_L1_ISO9141:
case DIAG_L1_ISO14230:
/* This initialisation is done in the SLOWINIT code */
break;
}
if (rv) {
br_close(dl0d);
return diag_ifwderr(rv);
}
DIAG_DBGM(diag_l0_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
FLFMT "open succeeded link %p features 0x%X\n",
FL, (void *)dl0d, dev->dev_features);
dl0d->opened = 1;
return 0;
}
/*
* Open the diagnostic device, return a file descriptor,
* record the original state of term interface so we can restore later
*/
static struct diag_l0_device *
diag_l0_br_open(const char *subinterface, int iProtocol)
{
struct diag_l0_device *dl0d;
struct diag_l0_br_device *dev;
int rv;
uint8_t buf[4]; /* Was MAXRBUF. We only use 1! */
struct diag_serial_settings set;
diag_l0_br_init();
if ((rv=diag_calloc(&dev, 1)))
return diag_pseterr(rv);
dev->protocol = iProtocol;
dev->dev_rdoffset = 0;
dev->dev_txlen = 0;
dev->dev_framenr = 0;
dev->dev_state = BR_STATE_CLOSED;
dev->dev_features = BR_FEATURE_SETADDR;
/* Get an L0 link */
dl0d = diag_l0_new(&diag_l0_br, (void *)dev);
if (!dl0d) {
free(dev);
return diag_pseterr(rv);
}
/* try to open TTY */
if ((rv=diag_tty_open(dl0d, subinterface))) {
diag_l0_del(dl0d);
free(dev);
return diag_pseterr(rv);
}
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "features 0x%X\n", FL, dev->dev_features);
}
/* Set serial line to 19200 baud , 8N1 */
set.speed = 19200;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
if (diag_tty_setup(dl0d, &set)) {
fprintf(stderr, FLFMT "open: TTY setup failed\n", FL);
diag_l0_br_close(&dl0d);
return diag_pseterr(rv);
}
diag_tty_iflush(dl0d); /* Flush unread input data */
/*
* Initialise the BR1 interface by sending the CHIP CONNECT
* (0x20h) command, we should get a 0xFF back
*/
buf[0] = 0x20;
if (diag_l0_br_write(dl0d, buf, 1)) {
if ((diag_l0_debug&DIAG_DEBUG_OPEN)) {
fprintf(stderr, FLFMT "CHIP CONNECT write failed link %p\n",
FL, (void *)dl0d);
}
diag_l0_br_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
/* And expect 0xff as a response */
if (diag_tty_read(dl0d, buf, 1, 100) != 1) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "CHIP CONNECT read failed link %p\n",
FL, (void *)dl0d);
}
diag_l0_br_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
if (buf[0] != 0xff) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "CHIP CONNECT rcvd 0x%X != 0xff, link %p\n",
FL, buf[0], (void *)dl0d);
}
diag_l0_br_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
/* If it's J1850, send initialisation string now */
rv = 0;
switch (iProtocol)
{
case DIAG_L1_J1850_VPW:
rv = diag_l0_br_initialise(dl0d, 0, 0);
break;
case DIAG_L1_J1850_PWM:
rv = diag_l0_br_initialise(dl0d, 1, 0);
break;
case DIAG_L1_ISO9141:
case DIAG_L1_ISO14230:
/* This initialisation is done in the SLOWINIT code */
break;
}
if (rv) {
diag_l0_br_close(&dl0d);
return diag_pseterr(rv);
}
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "open succeeded link %p features 0x%X\n",
FL, (void *)dl0d, dev->dev_features);
}
return dl0d;
}
/*
* Get data (blocking), returns number of bytes read, between 1 and len
* ELM returns a string with format "%02X %02X %02X[...]\n" . But it's slow so we add ELM_SLOWNESS ms to the specified timeout.
* We convert this received ascii string to hex before returning.
* note : "len" is the number of bytes read on the OBD bus, *NOT* the number of ASCII chars received on the serial link !
* TODO MAYBE : decode possible error strings ? not essential...
* TODO: improve "len" semantics for L0 interfaces that do framing, such as this. Currently this returns max 1 message, to
* let L2 do another call to get further messages (typical case of multiple responses)
*/
static int
diag_l0_elm_recv(struct diag_l0_device *dl0d,
UNUSED(const char *subinterface), void *data, size_t len, unsigned int timeout)
{
int rv, xferd;
uint8_t rxbuf[3*MAXRBUF +1]; //I think some hotdog code in L2/L3 calls _recv with MAXRBUF so this needs to be huge.
//the +1 is to \0-terminate the buffer for elm_parse_errors() to work
unsigned long t0,tf; //manual timeout control
int steplen; /* bytes per read */
int wp, rp; /* write & read indexes in rxbuf; a type of FIFO */
if ((!len) || (len > MAXRBUF))
return diag_iseterr(DIAG_ERR_BADLEN);
t0=diag_os_getms();
tf=t0+timeout + ELM_SLOWNESS; //timeout when tf is reached
steplen=2;
wp=0;
rp=0;
xferd=0;
if (diag_l0_debug & DIAG_DEBUG_READ)
fprintf(stderr, FLFMT "Expecting 3*%d bytes from ELM, %u ms timeout(+400)...", FL, (int) len, timeout);
while (1) {
unsigned long tcur;
/* technique : try to read hexpairs (2 bytes); split messages according to
* spacing chars (">\r\n")
*/
tcur = diag_os_getms();
if (tcur >= tf) {
/* timed out : */
return (xferd>0)? xferd:DIAG_ERR_TIMEOUT;
}
timeout = tf - tcur;
rv = diag_tty_read(dl0d, rxbuf+wp, steplen, timeout);
if (rv == DIAG_ERR_TIMEOUT) {
return (xferd>0)? xferd:rv;
}
if (rv <= 0) {
fprintf(stderr, FLFMT "elm_recv error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
wp += rv; /* position for next tty_read */
rxbuf[wp]=0; // '\0'-terminate "string"
int skipc; /* chars to skip */
skipc = strspn((char *)(&rxbuf[rp]), " "); /* skip contig spaces */
rp += skipc;
/* line end ? */
skipc=strspn((char *)(&rxbuf[rp]), "\r\n>");
rp += skipc;
if (skipc > 0) {
/* definitely a line-end / prompt ! return data so far, if any */
if (xferd > 0) {
return xferd;
}
}
if (strlen((char *)(&rxbuf[rp])) < 2) {
/* probably incomplete hexpair. */
steplen=1;
continue;
}
unsigned int rbyte;
if (sscanf((char *)(&rxbuf[rp]), "%02X", &rbyte) == 1) {
/* good hexpair */
((uint8_t *)data)[xferd]=(uint8_t) rbyte;
xferd++;
if ( (size_t)xferd==len)
return xferd;
}
/* here, we just sscanf'd 2 bytes (succesfully or not), so we read 1 more. */
/* we can't read 2 more, in case we're in a multi-message, for instance if we just decoded 0x00 in
* "48 6A 01 00\n48 6A..." , reading two bytes "\n4" would corrupt the next message !
*/
rp += 2;
steplen=1;
} // while (1)
}
/*
* Routine to read a whole BR1 message
* length of which depends on the first value received.
* This also handles "error" messages (top bit of first value set)
*
* Returns length of received message, or TIMEOUT error, or BUSERROR
* if the BR interface tells us theres a congested bus
*/
static int
diag_l0_br_getmsg(struct diag_l0_device *dl0d, uint8_t *dp, int timeout)
{
ssize_t xferd;
size_t offset;
int ret;
uint8_t firstbyte;
size_t readlen;
if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) ) {
fprintf(stderr, FLFMT "link %p getmsg timeout %d\n",
FL, (void *)dl0d, timeout);
}
/*
* First read the 1st byte, using the supplied timeout
*/
ret = diag_tty_read(dl0d, &firstbyte, 1, timeout);
if (ret < 0) {
if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) ) {
fprintf(stderr, FLFMT "link %p getmsg 1st byte timed out\n",
FL, (void *)dl0d);
}
return diag_iseterr(ret);
}
/*
* Now read data. Maximum is 15 bytes.
*/
offset = 0;
readlen = firstbyte & 0x0f;
while (offset != readlen) {
/*
* Reasonable timeout here as the interface told us how
* much data to expect, so it should arrive
*/
xferd = diag_tty_read(dl0d, &dp[offset], (size_t)(readlen - offset), 100);
if (xferd < 0) {
if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA))
== (DIAG_DEBUG_READ|DIAG_DEBUG_DATA) ) {
fprintf(stderr,
FLFMT "link %p getmsg byte %ld of %ld timed out\n",
FL, (void *)dl0d, (long)offset, (long)readlen );
}
return diag_iseterr(DIAG_ERR_TIMEOUT);
}
offset += (size_t) xferd;
}
if ( (diag_l0_debug & (DIAG_DEBUG_READ|DIAG_DEBUG_DATA)) ==
(DIAG_DEBUG_READ|DIAG_DEBUG_DATA) ) {
fprintf(stderr, FLFMT "link %p getmsg read ctl 0x%X data:",
FL, (void *)dl0d, firstbyte & 0xff);
diag_data_dump(stderr, dp, readlen);
printf("\n");
}
/*
* Message read complete, check error flag
* Top bit set means error, Bit 6 = VPW/PWM bus
* congestion (i.e retry).
*/
if (firstbyte & 0x80) /* Error indicator */
return diag_iseterr(DIAG_ERR_TIMEOUT);
if (firstbyte & 0x40) /* VPW/PWM bus conflict, need to retry */
return diag_iseterr(DIAG_ERR_BUSERROR);
if (readlen == 0) /* Should never happen */
return diag_iseterr(DIAG_ERR_TIMEOUT);
return (int) readlen;
}
//Send a command to ELM device and make sure no error occured. Data is passed on directly as a string;
//caller must make sure the string is \r-terminated , i.e. 0x0D-terminated.
//Sending 0A after 0D will cause ELM to interrupt what it's doing to "process" 0A, resulting in a
//failure.
//This func should not be used for commands that elicit a data response (i.e. all data destined to the OBD bus,
//hence not prefixed by "AT"). Response is dumped in *resp (0-terminated) for optional analysis by caller; *resp must be ELM_BUFSIZE long.
//returns 0 if (prompt_good) && ("OK" found anywhere in the response) && (no known error message was present) ||
// (prompt_good && ATZ command was sent) , since response doesn't contain "OK" for ATZ.
//elm_sendcmd should not be called from outside diag_l0_elm.c.
static int
elm_sendcmd(struct diag_l0_device *dl0d, const uint8_t *data, size_t len, unsigned int timeout, uint8_t *resp)
{
//note : we better not request (len == (size_t) -1) bytes ! The casts between ssize_t and size_t are
// "muddy" in here
//ssize_t xferd;
int rv;
uint8_t ebuf[ELM_BUFSIZE]; //local buffer if caller provides none.
uint8_t *buf;
struct diag_l0_elm_device *dev;
const char *err_str; //hold a possible error message
dev = (struct diag_l0_elm_device *)dl0d->l0_int;
//we need access to diag_l0_elm_device to access .elmflags
if (resp==NULL)
buf=ebuf; //use local buffer
else
buf=resp; //or caller-provided buffer
if (!len)
return diag_iseterr(DIAG_ERR_BADLEN);
if (!dev)
return diag_iseterr(DIAG_ERR_BADFD);
if (data[len-1] != 0x0D) {
//Last byte is not a carriage return, this would die.
fprintf(stderr, FLFMT "elm_sendcmd: non-terminated command : %.*s\n", FL, (int) len, (char *) data);
//the %.*s is pure magic : limits the string length to len, even if the string is not null-terminated.
return diag_iseterr(DIAG_ERR_GENERAL);
}
diag_tty_iflush(dl0d); //currently the code often "forgets" data in the input buffer, especially if the previous
//transaction failed. Flushing the input increases the odds of not crashing soon
if (diag_l0_debug & DIAG_DEBUG_WRITE) {
fprintf(stderr, FLFMT "elm_sendcmd: %.*s\n", FL, (int) len-1, (char *)data);
}
rv = diag_tty_write(dl0d, data, len);
if (rv != (int) len) { //XXX danger ! evil cast
fprintf(stderr, FLFMT "elm_sendcmd: write error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//next, receive ELM response, within {ms} delay.
rv=diag_tty_read(dl0d, buf, ELM_BUFSIZE-1, timeout); //rv=# bytes read
if (rv<1) {
//no data or error
if (diag_l0_debug & DIAG_DEBUG_WRITE) {
fprintf(stderr, FLFMT "ELM did not respond\n", FL);
}
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (diag_l0_debug & DIAG_DEBUG_READ) {
elm_parse_cr(buf, rv); //debug output is prettier with this
fprintf(stderr, FLFMT "received %d bytes (%.*s\n); hex: ", FL, rv, rv, (char *)buf);
if (diag_l0_debug & DIAG_DEBUG_DATA) {
diag_data_dump(stderr, buf, rv);
fprintf(stderr, "\n");
}
}
buf[rv]=0; //terminate string
if (buf[rv-1] != '>') {
//if last character isn't the input prompt, there is a problem
fprintf(stderr, FLFMT "ELM not ready (no prompt received): %s\nhex: ", FL, buf);
diag_data_dump(stderr, buf, rv);
fprintf(stderr, "\n");
return diag_iseterr(DIAG_ERR_GENERAL);
}
//At this point we got a prompt but there may have been an error message.
//There is some ambiguity in the ELM datasheets on the exact format of the replies.
//1) the prompt character '>' is always alone on its line;
//2) depending on some parameters, it may be preceded by 0D or 0D 0A
//3) some errors ( "<DATA ERROR" and "<RX ERROR" ) can be appended after
//a reply; others should be at the beginning of the response (alone on a line)
//ex. of good reply : "41 00 00 \r>", bad reply :"NO DATA\r>"
//let's just use strstr() to find occurences for each known error.
//it'll take a while but speed isn't normally critical when sending commands
err_str = elm_parse_errors(dl0d, buf);
if (err_str != NULL) {
fprintf(stderr, FLFMT "ELM returned error : %s\n", FL, err_str);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//check if we either 1)got a positive response "OK"
//2)were sending ATZ (special case hack, it doesn't answer "OK")
if ((strstr((char *)buf, "OK") != NULL) ||
(strstr((char *)data, "ATZ") != NULL)) {
return 0;
}
fprintf(stderr, FLFMT "Response not recognized ! Report this ! Got: \n", FL);
diag_data_dump(stderr, buf, rv);
fprintf(stderr, "\n");
return diag_iseterr(DIAG_ERR_GENERAL);
}
/*
* diag_tty_break
*/
int diag_tty_break(struct diag_l0_device *dl0d, const int ms)
{
char cbuf;
struct timeval tv;
int xferd;
struct diag_serial_settings set;
/*
* We need to send an 25ms (+/- 1ms) break
* and then wait 25ms. We must have waited Tidle (300ms) first
*
* The trouble here is Linux doesn't let us be that accurate sending
* a break - so we do it by sending a '0x00' at a baud rate that means
* the output is low for 25ms, then we wait for 25ms, using busy
* waits (which we get from being in real-time mode and doing nanosleeps
* of less than 2ms each)
*
*/
/* Set baud rate etc to 360 baud, 8, N, 1 */
set.speed = 360;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
diag_tty_setup(dl0d, &set);
gettimeofday(&tv,NULL);
/* Send a 0x00 byte message */
diag_tty_write(dl0d, "", 1);
if (diag_l0_debug & DIAG_DEBUG_TIMER) {
fprintf(stderr, FLFMT "%04ld.%03ld : break start\n", FL, tv.tv_sec, tv.tv_usec);
}
/*
* And read back the single byte echo, which shows TX completes
*/
while ( (xferd = diag_tty_read(dl0d, &cbuf, 1, 1000)) <= 0)
{
if (xferd == DIAG_ERR_TIMEOUT)
return diag_iseterr(DIAG_ERR_TIMEOUT);
if (xferd == 0)
{
/* Error, EOF */
fprintf(stderr, FLFMT "read returned EOF.\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (errno != EINTR)
{
/* Error, EOF */
fprintf(stderr, FLFMT "read returned error %s.\n", FL,
strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
}
/* Now wait the requested number of ms */
gettimeofday(&tv,NULL);
diag_os_millisleep(ms);
if (diag_l0_debug & DIAG_DEBUG_TIMER) {
fprintf(stderr, FLFMT "%04ld.%03ld : end of break_L\n", FL, tv.tv_sec, tv.tv_usec);
}
return 0;
}
static int
diag_l0_muleng_recv(struct diag_l0_device *dl0d,
UNUSED(const char *subinterface),
void *data, size_t len, unsigned int timeout)
{
ssize_t xferd;
int i, rv;
uint8_t *pdata = (uint8_t *)data;
struct diag_l0_muleng_device *dev;
dev = (struct diag_l0_muleng_device *)dl0d->l0_int;
if (!len)
return diag_iseterr(DIAG_ERR_BADLEN);
if (diag_l0_debug & DIAG_DEBUG_READ)
fprintf(stderr,
FLFMT "link %p recv upto %ld bytes timeout %u, rxlen %d offset %d\n",
FL, (void *)dl0d, (long)len, timeout, dev->dev_rxlen, dev->dev_rdoffset);
/*
* Deal with 5 Baud init states where first two bytes read by
* user are the keybytes received from the interface, and where
* we are using the interface in pass thru mode on ISO09141 protocols
*/
switch (dev->dev_state)
{
case MULENG_STATE_KWP_SENDKB1:
if (len >= 2)
{
pdata[0] = dev->dev_kb1;
pdata[1] = dev->dev_kb2;
dev->dev_state = MULENG_STATE_OPEN;
return 2;
}
else if (len == 1)
{
*pdata = dev->dev_kb1;
dev->dev_state = MULENG_STATE_KWP_SENDKB2;
return 1;
}
return 0; /* Strange, user asked for 0 bytes */
case MULENG_STATE_KWP_SENDKB2:
if (len >= 1)
{
*pdata = dev->dev_kb2;
dev->dev_state = MULENG_STATE_OPEN;
return 1;
}
return 0; /* Strange, user asked for 0 bytes */
case MULENG_STATE_RAW:
xferd = diag_tty_read(dl0d, data, len, timeout);
if (diag_l0_debug & DIAG_DEBUG_READ)
fprintf(stderr, FLFMT "link %p read %ld bytes\n", FL,
(void *)dl0d, (long)xferd);
return xferd;
case MULENG_STATE_FASTSTART:
/* Extend timeout for 1st recv */
timeout = 200;
dev->dev_state = MULENG_STATE_OPEN;
/* Drop thru */
default: /* Some other mode */
break;
}
if (dev->dev_rxlen >= 14)
{
/*
* There's a full packet been received, but the user
* has only asked for a few bytes from it previously
* Of the packet, bytes x[2]->x[11] are the network data
* others are header from the ME device
*
* The amount of data remaining to be sent to user is
* as below, -1 because the checksum is at the end
*/
size_t bufbytes = dev->dev_rxlen - dev->dev_rdoffset - 1;
if (bufbytes <= len)
{
memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], bufbytes);
dev->dev_rxlen = dev->dev_rdoffset = 0;
return (int) bufbytes;
}
else
{
memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], len);
dev->dev_rdoffset += len;
return (int) len;
}
}
/*
* There's either no data waiting, or only a partial read in the
* buffer, read some more
*/
if (dev->dev_rxlen < 14) {
rv = diag_tty_read(dl0d, &dev->dev_rxbuf[dev->dev_rxlen],
(size_t)(14 - dev->dev_rxlen), timeout);
if (rv == DIAG_ERR_TIMEOUT) {
return DIAG_ERR_TIMEOUT;
}
if (rv <= 0) {
fprintf(stderr, FLFMT "read returned EOF !!\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
dev->dev_rxlen += rv;
}
/* OK, got whole message */
if (diag_l0_debug & DIAG_DEBUG_READ)
{
fprintf(stderr,
FLFMT "link %p received from ME: ", FL, (void *)dl0d);
for (i=0; i < dev->dev_rxlen; i++)
fprintf(stderr, "0x%X ",
dev->dev_rxbuf[i] & 0xff);
fprintf(stderr, "\n");
}
/*
* Check the checksum, 2nd byte onward
*/
for (i=1, xferd = 0; i < 13; i++)
xferd += dev->dev_rxbuf[i];
if ((xferd & 0xff) != dev->dev_rxbuf[13])
{
/* XXX, we should deal with this properly rather than just printing a message */
fprintf(stderr,"Got bad checksum from ME device 0x%X != 0x%X\n",
(int) xferd & 0xff, dev->dev_rxbuf[13]);
fprintf(stderr,"PC Serial port probably out of spec.\n");
fprintf(stderr,"RX Data: ");
for (i=0; i < dev->dev_rxlen; i++)
fprintf(stderr, "0x%X ",
dev->dev_rxbuf[i] & 0xff);
fprintf(stderr, "\n");
}
/*
* Check the type
*/
if (dev->dev_rxbuf[1] == 0x80)
{
/* It's an error message not a data frame */
dev->dev_rxlen = 0;
if (diag_l0_debug & DIAG_DEBUG_READ)
fprintf(stderr,
FLFMT "link %p ME returns err 0x%X : s/w v 0x%X i/f cap. 0x%X\n",
FL, (void *)dl0d, dev->dev_rxbuf[3],
dev->dev_rxbuf[2], dev->dev_rxbuf[4]);
switch (dev->dev_rxbuf[3])
{
case 0x05: /* No ISO response to request */
case 0x07: /* No J1850 response to request */
case 0x0c: /* No KWP response to request */
return DIAG_ERR_TIMEOUT;
break;
default:
return diag_iseterr(DIAG_ERR_GENERAL);
}
/* NOTREACHED */
}
dev->dev_rdoffset = 2; /* Skip the ME header */
/* Copy data to user */
xferd = (len>11) ? 11 : xferd;
xferd = (xferd>(13-dev->dev_rdoffset)) ? 13-dev->dev_rdoffset : xferd;
memcpy(data, &dev->dev_rxbuf[dev->dev_rdoffset], (size_t)xferd);
dev->dev_rdoffset += xferd;
if (dev->dev_rdoffset == 13)
{
/* End of message, reset pointers */
dev->dev_rxlen = 0;
dev->dev_rdoffset = 0;
}
return xferd;
}
