/*
* Protocol receive routine
*
* Will sleep until a complete set of responses has been received, or fail
* with a timeout error
*
* The interbyte type in data from an ECU is between P1Min and P1Max
* The intermessage time for part of one response is P2Min and P2Max
*
* If we are running with an intelligent L1 interface, then we will be
* getting one message per frame, and we will wait a bit longer
* for extra messages
*/
static int
diag_l2_proto_14230_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
void (*callback)(void *handle, struct diag_msg *msg),
void *handle)
{
int rv;
/* Call internal routine */
rv = diag_l2_proto_14230_int_recv(d_l2_conn, timeout);
if (rv < 0) /* Failure */
return rv;
if (diag_l2_debug & DIAG_DEBUG_READ)
fprintf(stderr, FLFMT "_int_recv : handle=%p timeout=%u\n", FL,
(void *)handle, timeout); //%pcallback! we won't try to printf the callback pointer.
/*
* Call user callback routine
*/
if (callback)
callback(handle, d_l2_conn->diag_msg);
/* No longer needed */
diag_freemsg(d_l2_conn->diag_msg);
d_l2_conn->diag_msg = NULL;
if (diag_l2_debug & DIAG_DEBUG_READ)
fprintf(stderr, FLFMT "_recv callback completed\n", FL);
return 0;
}
static int
diag_l2_proto_14230_stopcomms(struct diag_l2_conn* pX)
{
struct diag_msg stopmsg={0};
struct diag_msg *rxmsg;
uint8_t stopreq=DIAG_KW2K_SI_SPR;
int errval=0;
char * debugstr;
stopmsg.len=1;
stopmsg.data=&stopreq;
stopmsg.dest=0;
stopmsg.src=0;
rxmsg=diag_l2_proto_14230_request(pX, &stopmsg, &errval);
if (rxmsg != NULL) {
//we got a message;
if (!errval) {
//no error : positive response from ECU.
debugstr="ECU acknowledged request (RC=";
} else {
debugstr="ECU refused request; connection will time-out in 5s.(RC=";
}
errval=rxmsg->data[0];
diag_freemsg(rxmsg);
} else {
//no message received...
debugstr="ECU did not respond to request, connection will timeout in 5s. (err=";
}
if (diag_l2_debug & DIAG_DEBUG_CLOSE) {
fprintf(stderr, FLFMT "_stopcomms: %s0x%02X).\n", FL, debugstr, errval);
}
//and free() what startcomms alloc'ed.
if (pX->diag_l2_proto_data) {
free(pX->diag_l2_proto_data);
pX->diag_l2_proto_data=NULL;
}
return 0;
}
/*
* Protocol receive routine
*
* Will sleep until a complete set of responses has been received, or fail
* with a timeout error
*
* The interbyte type in data from an ecu is between P1Min and P1Max
* The intermessage time for part of one response is P2Min and P2Max
*
* If we are running with an intelligent L1 interface, then we will be
* getting one message per frame, and we will wait a bit longer
* for extra messages
*/
static int
diag_l2_proto_vag_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
void (*callback)(void *handle, struct diag_msg *msg),
void *handle)
{
uint8_t data[256];
int rv;
int datalen;
/* Call internal routine */
rv = diag_l2_proto_vag_int_recv(d_l2_conn, timeout, data, &datalen);
if (rv <= 0) /* Failure */
return rv;
if (diag_l2_debug & DIAG_DEBUG_READ)
{
fprintf(stderr, FLFMT "calling rcv callback, handle=%p\n", FL,
(void *)handle); //%pcallback! we won't try to printf the callback pointer.
}
/*
* Call user callback routine
*/
if (callback)
callback(handle, d_l2_conn->diag_msg);
/* No longer needed */
diag_freemsg(d_l2_conn->diag_msg);
d_l2_conn->diag_msg = NULL;
if (diag_l2_debug & DIAG_DEBUG_READ)
{
fprintf(stderr, FLFMT "rcv callback completed\n", FL);
}
return 0;
}
//iso14230 diag_l2_proto_request. See diag_l2.h
//This handles busyRepeatRequest and RspPending negative responses.
//return NULL if failed, or a newly alloced diag_msg if succesful.
//Caller must free that diag_msg.
//Sends using diag_l2_send() in order to have the timestamps updated
static struct diag_msg *
diag_l2_proto_14230_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
int *errval)
{
int rv;
struct diag_msg *rmsg = NULL;
int retries=3; //if we get a BusyRepeatRequest response.
*errval=0;
rv = diag_l2_send(d_l2_conn, msg);
if (rv < 0) {
*errval = rv;
return diag_pseterr(rv);
}
while (1) {
rv = diag_l2_proto_14230_int_recv(d_l2_conn,
d_l2_conn->diag_l2_p2max + 10);
if (rv < 0) {
*errval = DIAG_ERR_TIMEOUT;
return diag_pseterr(rv);
}
/*
* The connection now has the received message data
* stored, remove it and deal with it
*/
rmsg = d_l2_conn->diag_msg;
d_l2_conn->diag_msg = NULL;
/* Got a Error message */
if (rmsg->data[0] == DIAG_KW2K_RC_NR) {
if (rmsg->data[2] == DIAG_KW2K_RC_B_RR) {
/*
* Msg is busyRepeatRequest
* So do a send again (if retries>0)
*/
if (retries > 0) {
rv = diag_l2_send(d_l2_conn, msg);
} else {
rv=DIAG_ERR_GENERAL;
fprintf(stderr, FLFMT "got too many BusyRepeatRequest responses!\n", FL);
}
retries--;
if (rv < 0) {
*errval = rv;
return diag_pseterr(rv);
}
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "got BusyRepeatRequest: retrying...\n", FL);
diag_freemsg(rmsg);
continue;
}
if (rmsg->data[2] == DIAG_KW2K_RC_RCR_RP) {
/*
* Msg is a requestCorrectlyRcvd-RspPending
* so do read again
*/
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "got RspPending: retrying...\n", FL);
diag_freemsg(rmsg);
continue;
}
/* Some other type of error */
*errval= DIAG_ERR_ECUSAIDNO;
break;
} else {
/* Success */
break;
}
}
/* Return the message to user, who is responsible for freeing it */
return rmsg;
}
/*
* Internal receive function: does all the message building, but doesn't
* do call back. Strips header and checksum; if address info was present
* then msg->dest and msg->src are !=0.
*
* Data from the first message is put into *data, and len into *datalen if
* those pointers are non-null.
*
* If the L1 interface is clever (DOESL2FRAME), then each read will give
* us a complete message, and we will wait a little bit longer than the normal
* timeout to detect "end of all responses"
*
*Similar to 9141_int_recv; timeout has to be long enough to catch at least
*1 byte.
* XXX this implementation doesn't accurately detect end-of-responses, because
* it's trying to read MAXRBUF (a large number) of bytes, for every state.
*if there is a short (say 3 byte) response from the ECU while in state 1,
*the timer will expire because it's waiting for MAXBUF
*bytes (MAXRBUF is much larger than any message, by design). Then, even
*though there are no more responses, we still do another MAXRBUF read
*in state 2 for P2min, and a last P2max read in state 3 !
* TODO: change state1 to read 1 byte maybe ?
* I think a more rigorous way to do this (if L1 doesn't do L2 framing), would
* be to loop, reading 1 byte at a time, with timeout=P1max or p2min to split
* messages, and timeout=P2max to detect the last byte of a response... this
* means calling diag_l1_recv a whole lot more often however.
*/
static int
diag_l2_proto_14230_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout)
{
struct diag_l2_14230 *dp;
int rv, l1_doesl2frame, l1flags;
unsigned int tout;
int state;
struct diag_msg *tmsg, *lastmsg;
#define ST_STATE1 1 /* Start */
#define ST_STATE2 2 /* Interbyte */
#define ST_STATE3 3 /* Inter message */
dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
if (diag_l2_debug & DIAG_DEBUG_READ)
fprintf(stderr,
FLFMT "_int_recv dl2conn=%p offset=0x%X, tout=%u\n",
FL, (void *) d_l2_conn, dp->rxoffset, timeout);
state = ST_STATE1;
tout = timeout;
/* Clear out last received messages if not done already */
if (d_l2_conn->diag_msg) {
diag_freemsg(d_l2_conn->diag_msg);
d_l2_conn->diag_msg = NULL;
}
l1flags = d_l2_conn->diag_link->l1flags;
if (l1flags & DIAG_L1_DOESL2FRAME)
l1_doesl2frame = 1;
else
l1_doesl2frame = 0;
if (l1_doesl2frame) {
if (timeout < SMART_TIMEOUT) /* Extend timeouts */
timeout += 100;
}
while (1) {
switch (state) {
case ST_STATE1:
tout = timeout;
break;
case ST_STATE2:
//State 2 : if we're between bytes of the same message; if we timeout with P2min it's
//probably because the message is ended.
tout = d_l2_conn->diag_l2_p2min - 2;
if (tout < d_l2_conn->diag_l2_p1max)
tout = d_l2_conn->diag_l2_p1max;
break;
case ST_STATE3:
//State 3: we timed out during state 2
if (l1_doesl2frame)
tout = 150; /* Arbitrary, short, value ... */
else
tout = d_l2_conn->diag_l2_p2max;
}
/* Receive data into the buffer */
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "before recv, state=%d timeout=%u, rxoffset %d\n",
FL, state, tout, dp->rxoffset);
/*
* In l1_doesl2frame mode, we get full frames, so we don't
* do the read in state2
*/
if ( (state == ST_STATE2) && l1_doesl2frame )
rv = DIAG_ERR_TIMEOUT;
else
rv = diag_l1_recv(d_l2_conn->diag_link->diag_l2_dl0d, 0,
&dp->rxbuf[dp->rxoffset],
sizeof(dp->rxbuf) - dp->rxoffset,
tout);
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr,
FLFMT "after recv, rv=%d rxoffset=%d\n", FL, rv, dp->rxoffset);
if (rv == DIAG_ERR_TIMEOUT) {
/* Timeout, end of message, or end of responses */
switch (state) {
case ST_STATE1:
/*
* 1st read, if we got 0 bytes, just return
* the timeout error
*/
if (dp->rxoffset == 0)
break;
/*
* Otherwise see if there are more bytes in
* this message,
*/
state = ST_STATE2;
continue;
case ST_STATE2:
/*
* End of that message, maybe more to come
* Copy data into a message
*/
tmsg = diag_allocmsg((size_t)dp->rxoffset);
if (tmsg == NULL)
return diag_iseterr(DIAG_ERR_NOMEM);
memcpy(tmsg->data, dp->rxbuf, (size_t)dp->rxoffset);
tmsg->rxtime = diag_os_chronoms(0);
dp->rxoffset = 0;
/*
* ADD message to list
*/
diag_l2_addmsg(d_l2_conn, tmsg);
if (d_l2_conn->diag_msg == tmsg) {
if ((diag_l2_debug & DIAG_DEBUG_DATA) && (diag_l2_debug & DIAG_DEBUG_PROTO)) {
fprintf(stderr, FLFMT "Copying %u bytes to data: ",
FL, tmsg->len);
diag_data_dump(stderr, tmsg->data, tmsg->len);
fprintf(stderr, "\n");
}
}
state = ST_STATE3;
continue;
case ST_STATE3:
/*
* No more messages, but we did get one
*/
rv = d_l2_conn->diag_msg->len;
break;
}
if (state == ST_STATE3)
break;
} //if diag_err_timeout
if (rv<=0)
break;
/* Data received OK */
dp->rxoffset += rv;
if (dp->rxoffset && (dp->rxbuf[0] == '\0')) {
/*
* We get this when in
* monitor mode and there is
* a fastinit, pretend it didn't exist
*/
dp->rxoffset--;
if (dp->rxoffset)
memcpy(&dp->rxbuf[0], &dp->rxbuf[1],
(size_t)dp->rxoffset);
continue;
}
if ( (state == ST_STATE1) || (state == ST_STATE3) ) {
/*
* Got some data in state1/3, now we're in a message
*/
state = ST_STATE2;
}
}
/*
* Now check the messages that we have checksum etc, stripping
* off headers etc
*/
if (rv < 0)
return rv;
tmsg = d_l2_conn->diag_msg;
lastmsg = NULL;
while (tmsg != NULL) {
int datalen=0;
uint8_t hdrlen=0, source=0, dest=0;
if ((l1flags & DIAG_L1_NOHDRS)==0) {
dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
rv = diag_l2_proto_14230_decode( tmsg->data,
tmsg->len,
&hdrlen, &datalen, &source, &dest,
dp->first_frame);
if (rv <= 0 || rv>255) /* decode failure */
return diag_iseterr(rv);
// check for sufficient data: (rv = expected len = hdrlen + datalen + ckslen)
if (l1_doesl2frame == 0) {
if ((!(l1flags & DIAG_L1_STRIPSL2CKSUM) && (tmsg->len < rv)) ||
((l1flags & DIAG_L1_STRIPSL2CKSUM) && (tmsg->len < (rv-1))))
return diag_iseterr(DIAG_ERR_INCDATA);
}
}
/*
* If L1 isnt doing L2 framing then it is possible
* we have misframed this message and it is infact
* more than one message, so see if we can decode it
*/
if ((l1_doesl2frame == 0) && (rv < tmsg->len)) {
/*
* This message contains more than one
* data frame (because it arrived with
* odd timing), this means we have to
* do horrible copy about the data
* things ....
*/
struct diag_msg *amsg;
amsg = diag_dupsinglemsg(tmsg);
if (amsg == NULL) {
return diag_iseterr(DIAG_ERR_NOMEM);
}
amsg->len = (uint8_t) rv;
tmsg->len -= (uint8_t) rv;
tmsg->data += rv;
/* Insert new amsg before old msg */
amsg->next = tmsg;
if (lastmsg == NULL)
d_l2_conn->diag_msg = amsg;
else
lastmsg->next = amsg;
tmsg = amsg; /* Finish processing this one */
}
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr,
FLFMT "msg %p decode/rejig done rv=%d hdrlen=%u datalen=%d src=%02X dst=%02X\n",
FL, (void *)tmsg, rv, hdrlen, datalen, source, dest);
tmsg->fmt = DIAG_FMT_FRAMED;
if ((l1flags & DIAG_L1_NOHDRS)==0) {
if ((tmsg->data[0] & 0xC0) == 0xC0) {
tmsg->fmt |= DIAG_FMT_ISO_FUNCADDR;
}
}
//if cs wasn't stripped, we check it:
if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0) {
uint8_t calc_sum=diag_cks1(tmsg->data, (tmsg->len)-1);
if (calc_sum != tmsg->data[tmsg->len -1]) {
fprintf(stderr, FLFMT "Bad checksum: needed %02X,got%02X. Data:",
FL, calc_sum, tmsg->data[tmsg->len -1]);
tmsg->fmt |= DIAG_FMT_BADCS;
diag_data_dump(stderr, tmsg->data, tmsg->len -1);
fprintf(stderr, "\n");
}
/* and remove checksum byte */
tmsg->len--;
}
tmsg->fmt |= DIAG_FMT_CKSUMMED; //checksum was verified
tmsg->src = source;
tmsg->dest = dest;
tmsg->data += hdrlen; /* Skip past header */
tmsg->len -= hdrlen; /* remove header */
dp->first_frame = 0;
lastmsg = tmsg;
tmsg = tmsg->next;
}
return rv;
}
static int
diag_l2_proto_vag_int_recv(struct diag_l2_conn *d_l2_conn,
UNUSED(unsigned int timeout),
UNUSED(uint8_t *data),
UNUSED(int *datalen))
{
struct diag_l2_vag *dp;
int rv = 0;
/* struct diag_msg *tmsg;*/
dp = (struct diag_l2_vag *)d_l2_conn->diag_l2_proto_data;
if (diag_l2_debug & DIAG_DEBUG_READ)
fprintf(stderr,
FLFMT "diag_l2_vag_intrecv offset %X\n",
FL, dp->rxoffset);
/* Clear out last received message if not done already */
if (d_l2_conn->diag_msg)
{
diag_freemsg(d_l2_conn->diag_msg);
d_l2_conn->diag_msg = NULL;
}
/*
* And receive the new message
*/
#if notdef
/*
* Now check the messages that we have checksum etc, stripping
* off headers etc
*/
if (rv >= 0)
{
tmsg = d_l2_conn->diag_msg;
while (tmsg)
{
struct diag_l2_vag *dp;
int datalen;
uint8_t hdrlen, source, dest;
/*
* We have the message with the header etc, we
* need to strip the header and checksum
*/
dp = (struct diag_l2_vag *)d_l2_conn->diag_l2_proto_data;
rv = diag_l2_proto_vag_decode( tmsg->data,
tmsg->len,
&hdrlen, &datalen, &source, &dest,
dp->first_frame);
if (rv < 0) /* decode failure */
{
return rv;
}
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, "msg %X decode done rv %d hdrlen %d datalen %d source %02X dest %02X\n",
tmsg, rv, hdrlen, datalen, source, dest);
if (tmsg->data[0] & 0xC0 == 0xC0)
{
tmsg->fmt = DIAG_FMT_ISO_FUNCADDR;
} else {
tmsg->fmt = 0;
}
tmsg->fmt |= DIAG_FMT_FRAMED;
tmsg->fmt |= DIAG_FMT_CKSUMMED;
tmsg->src = source;
tmsg->dest = dest;
tmsg->data += hdrlen; /* Skip past header */
tmsg->len -= (hdrlen + 1); /* And remove hdr/cksum */
dp->first_frame = 0;
tmsg = tmsg->next;
}
}
#endif
return rv;
}
//iso14230 diag_l2_proto_request. See diag_l2.h
//This handles busyRepeatRequest and RspPending negative responses.
//return NULL if failed, or a newly alloced diag_msg if succesful.
//Caller must free that diag_msg.
//Sends using diag_l2_send() in order to have the timestamps updated
static struct diag_msg *
diag_l2_proto_14230_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
int *errval)
{
int rv;
struct diag_msg *rmsg = NULL;
int retries=3; //if we get a BusyRepeatRequest response.
*errval=0;
rv = diag_l2_send(d_l2_conn, msg);
if (rv < 0) {
*errval = rv;
return diag_pseterr(rv);
}
while (1) {
/* do read only if no messages pending */
if (!d_l2_conn->diag_msg) {
rv = diag_l2_proto_14230_int_recv(d_l2_conn,
d_l2_conn->diag_l2_p2max + 10);
if (rv < 0) {
*errval = DIAG_ERR_TIMEOUT;
if (rv == DIAG_ERR_TIMEOUT) {
return NULL;
} else {
return diag_pseterr(rv);
}
}
}
/*
* The connection now has the received message(s)
* stored. Temporarily remove from dl2c
*/
rmsg = d_l2_conn->diag_msg;
d_l2_conn->diag_msg = NULL;
/* Check for negative response */
if (rmsg->data[0] != DIAG_KW2K_RC_NR) {
/* Success */
break;
}
if (rmsg->data[2] == DIAG_KW2K_RC_B_RR) {
/*
* Msg is busyRepeatRequest
* So send again (if retries>0).
*
* Is there any possibility that we would have received 2 messages,
* {busyrepeatrequest + a valid (unrelated) response} ?
* Not sure, let's simply discard everything.
*/
diag_freemsg(rmsg);
if (retries > 0) {
rv = diag_l2_send(d_l2_conn, msg);
} else {
rv=DIAG_ERR_GENERAL;
fprintf(stderr, FLFMT "got too many BusyRepeatRequest responses!\n", FL);
}
retries--;
if (rv < 0) {
*errval = rv;
return diag_pseterr(rv);
}
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "got BusyRepeatRequest: retrying...\n", FL);
continue;
}
if (rmsg->data[2] == DIAG_KW2K_RC_RCR_RP) {
/*
* Msg is a requestCorrectlyRcvd-RspPending
* so do read again
*/
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "got RspPending: retrying...\n", FL);
/* reattach the rest of the chain, in case the good response
* was already received
*/
d_l2_conn->diag_msg = rmsg->next;
rmsg->next = NULL;
diag_freemsg(rmsg);
continue;
}
/* Some other type of error */
*errval= DIAG_ERR_ECUSAIDNO;
break;
} //while 1
/* Return the message to user, who is responsible for freeing it */
return rmsg;
}
/*
* The complex initialisation routine for ISO14230, which supports
* 2 types of initialisation (5-BAUD, FAST) and functional
* and physical addressing. The ISO14230 spec describes CARB initialisation
* which is done in the ISO9141 code
*
* Remember, we have to wait longer on smart L1 interfaces.
*/
static int
diag_l2_proto_14230_startcomms( struct diag_l2_conn *d_l2_conn, flag_type flags,
unsigned int bitrate, target_type target, source_type source)
{
struct diag_l2_14230 *dp;
struct diag_msg msg={0};
uint8_t data[MAXRBUF];
int rv;
unsigned int wait_time;
uint8_t cbuf[MAXRBUF];
unsigned int timeout;
struct diag_serial_settings set;
struct diag_l1_initbus_args in;
if (diag_calloc(&dp, 1))
return diag_iseterr(DIAG_ERR_NOMEM);
d_l2_conn->diag_l2_proto_data = (void *)dp;
dp->initype = flags & DIAG_L2_TYPE_INITMASK; //only keep initmode flags
dp->srcaddr = source;
dp->dstaddr = target;
//set iso14230-specific flags according to what we were given
if (flags & DIAG_L2_IDLE_J1978)
dp->modeflags |= ISO14230_IDLE_J1978;
if (flags & DIAG_L2_TYPE_FUNCADDR)
dp->modeflags |= ISO14230_FUNCADDR;
if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2FRAME) {
dp->first_frame = 0;
} else {
dp->first_frame = 1;
}
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "_startcomms flags=0x%X tgt=0x%X src=0x%X\n",
FL, flags, target, source);
memset(data, 0, sizeof(data));
/*
* If 0 has been specified, use the correct speed
* for ISO14230 protocol
*/
if (bitrate == 0)
bitrate = 10400;
d_l2_conn->diag_l2_speed = bitrate;
set.speed = bitrate;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
/* Set the speed*/
if ((rv=diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, (void *) &set))) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL; //delete pointer to dp
return diag_iseterr(rv);
}
dp->state = STATE_CONNECTING ;
/* Flush unread input, then wait for idle bus. */
(void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
diag_os_millisleep(300);
//inside this switch, we set rv=0 or rv=error before "break;"
switch (dp->initype & DIAG_L2_TYPE_INITMASK) {
/* Fast initialisation */
case DIAG_L2_TYPE_FASTINIT:
/* Build an ISO14230 StartComms message */
if (flags & DIAG_L2_TYPE_FUNCADDR) {
msg.fmt = DIAG_FMT_ISO_FUNCADDR;
d_l2_conn->diag_l2_physaddr = 0; /* Don't know it yet */
in.physaddr = 0;
} else {
msg.fmt = 0;
d_l2_conn->diag_l2_physaddr = target;
in.physaddr = 1;
}
msg.src = source;
msg.dest = target;
msg.len = 1 ;
data[0]= DIAG_KW2K_SI_SCR ; /* startCommunication rqst*/
msg.data = data;
/* Do fast init stuff */
in.type = DIAG_L1_INITBUS_FAST;
in.addr = target;
in.testerid = source;
rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
// some L0 devices already do the full startcomm transaction:
if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) && (rv==0)) {
//TODO : somehow extract keybyte data for those cases...
//original elm327s have the "atkw" command to get the keybytes, but clones suck.
dp->state = STATE_ESTABLISHED;
break;
}
if (rv < 0)
break;
/* Send the prepared message */
if (diag_l2_proto_14230_send(d_l2_conn, &msg)) {
rv=DIAG_ERR_GENERAL;
break;
}
if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2FRAME)
timeout = 200;
else
timeout = d_l2_conn->diag_l2_p2max + RXTOFFSET;
/* And wait for a response, ISO14230 says will arrive in P2 */
rv = diag_l2_proto_14230_int_recv(d_l2_conn, timeout);
if (rv < 0)
break;
// _int_recv() should have filled d_l2_conn->diag_msg properly.
// check if message is OK :
if (d_l2_conn->diag_msg->fmt & DIAG_FMT_BADCS) {
rv=DIAG_ERR_BADCSUM;
break;
}
switch (d_l2_conn->diag_msg->data[0]) {
case DIAG_KW2K_RC_SCRPR: /* StartComms positive response */
d_l2_conn->diag_l2_kb1 = d_l2_conn->diag_msg->data[1];
d_l2_conn->diag_l2_kb2 = d_l2_conn->diag_msg->data[2];
d_l2_conn->diag_l2_physaddr = d_l2_conn->diag_msg->src;
if (diag_l2_debug & DIAG_DEBUG_PROTO) {
fprintf(stderr,
FLFMT "_StartComms Physaddr=0x%X KB1=%02X, KB2=%02X\n",
FL, d_l2_conn->diag_l2_physaddr, d_l2_conn->diag_l2_kb1, d_l2_conn->diag_l2_kb2);
}
rv=0;
dp->state = STATE_ESTABLISHED ;
break;
case DIAG_KW2K_RC_NR:
if (diag_l2_debug & DIAG_DEBUG_PROTO) {
fprintf(stderr,
FLFMT "_StartComms got neg response\n", FL);
}
rv=DIAG_ERR_ECUSAIDNO;
break;
default:
if (diag_l2_debug & DIAG_DEBUG_PROTO) {
fprintf(stderr,
FLFMT "_StartComms got unexpected response 0x%X\n",
FL, d_l2_conn->diag_msg->data[0]);
}
rv=DIAG_ERR_ECUSAIDNO;
break;
} //switch data[0]
// finished with the response message:
diag_freemsg(d_l2_conn->diag_msg);
d_l2_conn->diag_msg = NULL;
break; //case _FASTINIT
/* 5 Baud init */
case DIAG_L2_TYPE_SLOWINIT:
in.type = DIAG_L1_INITBUS_5BAUD;
in.addr = target;
rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
//some L0 devices handle the full init transaction:
if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) && (rv==0)) {
dp->state = STATE_ESTABLISHED ;
break;
}
if (rv < 0)
break;
/* Mode bytes are in 7-Odd-1, read as 8N1 and ignore parity */
rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d, 0,
cbuf, 2, 100);
if (rv < 0)
break;
/* ISO14230 uses KB2 of 0x8F */
if (cbuf[1] != 0x8f) {
rv=DIAG_ERR_WRONGKB;
break;
}
/* Note down the mode bytes */
// KB1 : we eliminate the Parity bit (MSB !)
d_l2_conn->diag_l2_kb1 = cbuf[0] & 0x7f;
d_l2_conn->diag_l2_kb2 = cbuf[1];
if ( (d_l2_conn->diag_link->l1flags
& DIAG_L1_DOESSLOWINIT) == 0) {
/*
* Now transmit KB2 inverted
*/
cbuf[0] = ~ d_l2_conn->diag_l2_kb2;
rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d, 0,
cbuf, 1, d_l2_conn->diag_l2_p4min);
/*
* And wait for the address byte inverted
*/
//first init cbuf[0] to the wrong value in case l1_recv gets nothing
cbuf[0]= (uint8_t) target;
rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d, 0,
cbuf, 1, 350);
if (cbuf[0] != ((~target) & 0xFF) ) {
fprintf(stderr, FLFMT "_startcomms : addr mismatch %02X!=%02X\n",
FL, cbuf[0], (uint8_t) ~target);
rv=DIAG_ERR_WRONGKB;
break;
}
if (diag_l2_debug & DIAG_DEBUG_INIT)
fprintf(stderr, FLFMT "ISO14230 KB1=%02X KB2=%02X\n", FL,
d_l2_conn->diag_l2_kb1, d_l2_conn->diag_l2_kb2);
}
rv=0;
dp->state = STATE_ESTABLISHED ;
break; //case _SLOWINIT
case DIAG_L2_TYPE_MONINIT:
/* Monitor mode, don't send anything */
dp->state = STATE_ESTABLISHED ;
rv = 0;
break;
default:
rv = DIAG_ERR_INIT_NOTSUPP;
break;
} //end of switch dp->initype
//At this point we just finished the handshake and got KB1 and KB2
if (rv < 0) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL; //delete pointer to dp
return diag_iseterr(rv);
}
// Analyze keybytes and set modeflags properly. See
// iso14230 5.2.4.1 & Table 8
dp->modeflags |= ((d_l2_conn->diag_l2_kb1 & 1)? ISO14230_FMTLEN:0) |
((d_l2_conn->diag_l2_kb1 & 2)? ISO14230_LENBYTE:0) |
((d_l2_conn->diag_l2_kb1 & 4)? ISO14230_SHORTHDR:0) |
((d_l2_conn->diag_l2_kb1 & 8)? ISO14230_LONGHDR:0);
if (diag_l2_debug & DIAG_DEBUG_PROTO)
fprintf(stderr, FLFMT "new modeflags=0x%04X\n", FL, dp->modeflags);
//For now, we won't bother with Normal / Extended timings. We don't
//need to unless we use the AccessTimingParameters service (scary)
/*
* Now, we want to remove any rubbish left
* in inbound buffers, and wait for the bus to be
* quiet for a while before we will communicate
* (so that the next byte received is the first byte
* of an iso14230 frame, not a middle byte)
* We use 1/2 of P2max (inter response gap) or
* 5 * p4max (inter byte delay), whichever is larger
* a correct value to use
*/
wait_time = d_l2_conn->diag_l2_p2max / 2 ;
if ((d_l2_conn->diag_l2_p4max * 5) > wait_time)
wait_time = d_l2_conn->diag_l2_p4max * 5;
while ( diag_l1_recv (d_l2_conn->diag_link->l2_dl0d, 0,
data, sizeof(data), wait_time) != DIAG_ERR_TIMEOUT) ;
/* And we're done */
dp->state = STATE_ESTABLISHED ;
return 0;
}