static int
cmd_log(int argc, char **argv)
{
char autofilename[20]="";
char *file;
struct stat buf;
time_t now;
int i;
file=autofilename;
if (global_logfp != NULL) {
printf("Already logging\n");
return CMD_FAILED;
}
/* Turn on logging */
if (argc > 1) {
file = argv[1]; //if a file name was specified, use that
} else {
//else, generate an auto log file
for (i = 0; i <= 100; i++) {
sprintf(autofilename,"log.%02d",i);
if (stat(file, &buf) == -1 && errno == ENOENT)
break;
}
if (i == 100) {
printf("Can't create log.%d; remember to clean old auto log files\n",i);
return CMD_FAILED;
}
}
global_logfp = fopen(file, "a"); //add to end of log or create file
if (global_logfp == NULL) {
printf("Failed to create log file %s\n", file);
return CMD_FAILED;
}
now = time(NULL);
//reset stopwatch:
unsigned long t1;
t1=diag_os_chronoms(0);
(void) diag_os_chronoms(t1);
fprintf(global_logfp, "%s\n", LOG_FORMAT);
log_timestamp("#");
fprintf(global_logfp, "logging started at %s",
asctime(localtime(&now)));
printf("Logging to file %s\n", file);
return CMD_OK;
}
static void
log_timestamp(const char *prefix)
{
unsigned long tv;
tv=diag_os_chronoms(0);
fprintf(global_logfp, "%s %04lu.%03lu ", prefix, tv / 1000, tv % 1000);
}
/*
* Set/Clear DTR and RTS lines, as specified
*/
int
diag_tty_control(struct diag_l0_device *dl0d, unsigned int dtr, unsigned int rts)
{
int flags; /* Current flag values. */
struct unix_tty_int *uti = (struct unix_tty_int *)dl0d->tty_int;
int setflags = 0, clearflags = 0;
if (dtr)
setflags = TIOCM_DTR;
else
clearflags = TIOCM_DTR;
if (rts)
setflags = TIOCM_RTS;
else
clearflags = TIOCM_RTS;
errno = 0;
if (ioctl(uti->fd, TIOCMGET, &flags) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCMGET failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
flags |= setflags;
flags &= ~clearflags;
if (ioctl(uti->fd, TIOCMSET, &flags) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCMSET failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (diag_l0_debug & DIAG_DEBUG_TIMER) {
unsigned long tc=diag_os_chronoms(0);
fprintf(stderr, FLFMT "%lu : DTR/RTS changed\n", FL, tc);
}
return 0;
}
int
diag_l2_proto_raw_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
void (*callback)(void *handle, struct diag_msg *msg), void *handle)
{
uint8_t rxbuf[MAXRBUF];
struct diag_msg msg={0}; //local message structure that will disappear when we return
int rv;
/*
* Read data from fd
*/
rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_dl0d, 0,
rxbuf, sizeof(rxbuf), timeout);
if (rv <= 0 || rv > 255) /* Failure, or 0 bytes (which cant happen) */
return rv;
msg.len = (uint8_t) rv;
msg.data = rxbuf;
/* This is raw, unframed data; we don't set .fmt */
msg.next = NULL;
msg.idata=NULL;
msg.rxtime = diag_os_chronoms(0);
if (diag_l2_debug & DIAG_DEBUG_READ)
{
fprintf(stderr, FLFMT "l2_proto_raw_recv: handle=%p\n", FL,
(void *)handle); //%pcallback! we won't try to printf the callback pointer.
}
/*
* Call user callback routine
*/
if (callback)
callback(handle, &msg);
return 0;
}
struct diag_msg *
diag_l2_proto_raw_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
int *errval)
{
int rv;
struct diag_msg *rmsg = NULL;
uint8_t rxbuf[MAXRBUF];
rv = diag_l2_send(d_l2_conn, msg);
if (rv < 0)
{
*errval = rv;
return diag_pseterr(DIAG_ERR_GENERAL);
}
/* And wait for response */
rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_dl0d,
0, rxbuf, sizeof(rxbuf), 1000);
if (rv <= 0 || rv>255)
{
*errval = rv;
}
else
{
/*
* Ok, alloc a message
*/
rmsg = diag_allocmsg((size_t)rv);
if (rmsg == NULL)
return diag_pseterr(DIAG_ERR_NOMEM);
memcpy(&rmsg->data, rxbuf, (size_t)rv); /* Data */
rmsg->fmt = 0;
rmsg->rxtime = diag_os_chronoms(0);
}
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;
}
