// Allocates one new ecu response and fills it with given data.
// (not used yet, here for "just in case")
struct sim_ecu_response*
sim_new_ecu_response_bin(const uint8_t* data, const uint8_t len)
{
struct sim_ecu_response *resp;
if (diag_calloc(&resp, 1))
return diag_pseterr(DIAG_ERR_NOMEM);
//resp = calloc(1, sizeof(struct sim_ecu_response));
resp->data = NULL;
resp->len = 0;
resp->text = NULL;
resp->next = NULL;
if ((len > 0) && (data != NULL)) {
if (diag_calloc(&resp->data, len)) {
free(resp);
return diag_pseterr(DIAG_ERR_NOMEM);
}
//resp->data = calloc(len, sizeof(uint8_t));
memcpy(resp->data, data, len);
resp->len = len;
}
return resp;
}
struct diag_msg *
dl2p_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->l2_dl0d,
0, rxbuf, sizeof(rxbuf), 1000);
if (rv <= 0) {
*errval = rv;
return NULL;
}
/*
* 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_getms();
return rmsg;
}
// Allocates one new ecu response and fills it with given text.
struct sim_ecu_response*
sim_new_ecu_response_txt(const char* text)
{
struct sim_ecu_response *resp;
int rv;
if ((rv=diag_calloc(&resp, 1)))
return diag_pseterr(rv);
resp->data = NULL;
resp->len = 0;
resp->text = NULL;
resp->next = NULL;
if ((text != NULL) && strlen(text)) {
if ((rv=diag_calloc(&(resp->text), strlen(text)+1))) {
free(resp);
return diag_pseterr(rv);
}
strncpy(resp->text, text, strlen(text));
}
return resp;
}
/*
* Open the diagnostic device, returns a file descriptor
* records original state of term interface so we can restore later
*/
static struct diag_l0_device *
diag_l0_muleng_open(const char *subinterface, int iProtocol)
{
int rv;
struct diag_l0_device *dl0d;
struct diag_l0_muleng_device *dev;
struct diag_serial_settings set;
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "open subinterface %s protocol %d\n",
FL, subinterface, iProtocol);
}
diag_l0_muleng_init();
if ((rv=diag_calloc(&dev, 1)))
return diag_pseterr(rv);
dev->protocol = iProtocol;
dl0d = diag_l0_new(&diag_l0_me, (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);
}
/* And set to 19200 baud , 8N1 */
set.speed = 19200;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
if ((rv=diag_tty_setup(dl0d, &set))) {
diag_l0_muleng_close(dl0d);
return diag_pseterr(rv);
}
/* And set DTR high and RTS low to power the device */
if ((rv=diag_tty_control(dl0d, 1, 0))) {
diag_l0_muleng_close(dl0d);
return diag_pseterr(rv);
}
diag_tty_iflush(dl0d); /* Flush unread input */
return dl0d ;
}
// Opens the simulator DB file.
static struct diag_l0_device *
diag_l0_sim_open(UNUSED(const char *subinterface), int iProtocol)
{
int rv;
struct diag_l0_device *dl0d;
struct diag_l0_sim_device *dev;
// If we're doing debugging, print to stderr
if (diag_l0_debug & DIAG_DEBUG_OPEN)
fprintf(stderr, FLFMT "open simfile %s proto=%d\n", FL, simfile, iProtocol);
diag_l0_sim_init();
// Create diag_l0_sim_device:
if ((rv=diag_calloc(&dev, 1)))
return diag_pseterr(rv);
dev->protocol = iProtocol;
// Create diag_l0_device:
if ((rv=diag_calloc(&dl0d, 1))) {
free(dev);
return diag_pseterr(rv);
}
dl0d->dl0_handle = dev;
dl0d->dl0 = &diag_l0_sim;
if ((rv=diag_calloc(&dl0d->name, strlen(simfile)+1))) {
free(dev);
free(dl0d);
return diag_pseterr(rv);
}
strcpy(dl0d->name, simfile);
// Open the DB file:
if ((dev->fp = fopen(dl0d->name, "r")) == NULL) {
fprintf(stderr, FLFMT "Unable to open file \"%s\": ", FL, dl0d->name);
perror(NULL);
free(dl0d->name);
free(dev);
free(dl0d);
return diag_pseterr(DIAG_ERR_GENERAL);
}
rewind(dev->fp);
// Read the configuration flags from the db file:
sim_read_cfg(dev->fp);
return dl0d;
}
/*
* Open the diagnostic device, returns a file descriptor
* records original state of term interface so we can restore later
*/
static struct diag_l0_device *
diag_l0_dumb_open(const char *subinterface, int iProtocol)
{
int rv;
struct diag_l0_device *dl0d;
struct diag_l0_dumb_device *dev;
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "open subinterface %s protocol %d\n",
FL, subinterface, iProtocol);
}
diag_l0_dumb_init(); //make sure it is initted
if ((rv=diag_calloc(&dev, 1)))
return diag_pseterr(DIAG_ERR_NOMEM);
dev->protocol = iProtocol;
dl0d = diag_l0_new(&diag_l0_dumb, (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);
}
/*
* We set RTS to low, and DTR high, because this allows some
* interfaces to work than need power from the DTR/RTS lines;
* this is altered according to dumb_flags.
*/
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & SET_RTS)) < 0) {
diag_l0_dumb_close(&dl0d);
return diag_pseterr(DIAG_ERR_GENERAL);
}
if (dumb_flags & DUMBDEFAULTS)
dumb_flags = DUMBDEFAULTS & MAN_BREAK;
(void)diag_tty_iflush(dl0d); /* Flush unread input */
return dl0d;
}
/*
* Send a message and return a new message with the reply.
*/
struct diag_msg *
diag_l3_request(struct diag_l3_conn *dl3c, struct diag_msg *txmsg, int *errval)
{
struct diag_msg *rxmsg;
const struct diag_l3_proto * dl3p = dl3c->d_l3_proto;
if (diag_l3_debug & DIAG_DEBUG_WRITE)
fprintf(stderr,
FLFMT "_request dl3c=%p msg=%p called\n",
FL, (void *)dl3c, (void *)txmsg);
/* Call protocol specific send routine */
if (dl3p->diag_l3_proto_request)
rxmsg = dl3p->diag_l3_proto_request(dl3c, txmsg, errval);
else
rxmsg = NULL;
if (diag_l2_debug & DIAG_DEBUG_WRITE) {
fprintf(stderr, FLFMT "_request returns %p, err %d\n",
FL, (void *)rxmsg, *errval);
}
if (rxmsg==NULL) {
return diag_pseterr(*errval);
}
//update timers
dl3c->timer = diag_os_getms();
return rxmsg;
}
static struct diag_msg *
diag_l2_proto_vag_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
int *errval)
{
int rv;
struct diag_msg *rmsg = NULL;
rv = diag_l2_send(d_l2_conn, msg);
if (rv < 0)
{
*errval = rv;
return diag_pseterr(DIAG_ERR_GENERAL);
}
/* And wait for response */
//XXX this l2_iso9141 function should be re-written for l2_vag; they may be incompatible...
rv = diag_l2_proto_iso9141_int_recv(d_l2_conn, 1000);
if ((rv >= 0) && d_l2_conn->diag_msg)
{
/* OK */
rmsg = d_l2_conn->diag_msg;
d_l2_conn->diag_msg = NULL;
}
else
{
/* Error */
*errval = DIAG_ERR_TIMEOUT;
rmsg = NULL;
}
return rmsg;
}
static struct cfgi *br_getcfg(struct diag_l0_device *dl0d) {
struct br_device *dev;
if (dl0d == NULL) {
return diag_pseterr(DIAG_ERR_BADCFG);
}
dev = dl0d->l0_int;
return &dev->port;
}
/*
* Open the diagnostic device, return a dl0 device.
*
* Finds the unique name in the l0 device table,
* calls the init routine, with the device parameter from the table
*
* This is passed a L1 subinterface (ie, what type of physical interface
* to run on)
*/
struct diag_l0_device *
diag_l1_open(const char *name, const char *subinterface, int l1protocol)
{
struct diag_l0_node *node;
const struct diag_l0 *l0dev;
for (node = l0dev_list; node; node = node->next) {
l0dev = node->l0dev;
if (strcmp(name, l0dev->diag_l0_name) == 0)
{
/* Found it */
/* Check h/w supports this l1 protocol */
if ((l0dev->diag_l0_type & l1protocol) == 0)
return (struct diag_l0_device *)diag_pseterr(DIAG_ERR_PROTO_NOTSUPP);
/* Call the open routine */
return (l0dev->diag_l0_open)(subinterface, l1protocol);
}
}
/* Not found */
return (struct diag_l0_device *)diag_pseterr(DIAG_ERR_GENERAL);
}
//this implementation is rather naive and untested. It simply forwards the
//txmsg straight to the L2 request function and returns the response msg
//as-is.
struct diag_msg * diag_l3_base_request(struct diag_l3_conn *dl3c,
struct diag_msg* txmsg, int* errval) {
struct diag_msg *rxmsg = NULL;
*errval=0;
rxmsg = diag_l2_request(dl3c->d_l3l2_conn, txmsg, errval);
if (rxmsg == NULL) {
return diag_pseterr(*errval);
}
dl3c->timer=diag_os_getms();
return rxmsg;
}
/*
* Protocol start (connect a protocol on top of a L2 connection)
* make sure to diag_l3_stop afterwards to free() the diag_l3_conn !
* This adds the new l3 connection to the diag_l3_list linked-list
*/
struct diag_l3_conn *
diag_l3_start(const char *protocol, struct diag_l2_conn *d_l2_conn)
{
struct diag_l3_conn *d_l3_conn = NULL;
unsigned int i;
int rv;
const struct diag_l3_proto *dp;
assert(d_l2_conn != NULL);
if (diag_l3_debug & DIAG_DEBUG_OPEN)
fprintf(stderr,FLFMT "start protocol %s l2 %p\n",
FL, protocol, (void *)d_l2_conn);
/* Find the protocol */
dp = NULL;
for (i=0; diag_l3_protocols[i]; i++) {
if (strcasecmp(protocol, diag_l3_protocols[i]->proto_name) == 0)
{
dp = diag_l3_protocols[i]; /* Found. */
break;
}
}
if (dp)
{
if (diag_l3_debug & DIAG_DEBUG_OPEN)
fprintf(stderr,FLFMT "start protocol %s found\n",
FL, dp->proto_name);
/*
* Malloc us a L3
*/
if (diag_calloc(&d_l3_conn, 1))
return diag_pseterr(DIAG_ERR_NOMEM);
d_l3_conn->d_l3l2_conn = d_l2_conn;
d_l3_conn->d_l3_proto = dp;
/* Get L2 flags */
(void)diag_l2_ioctl(d_l2_conn,
DIAG_IOCTL_GET_L2_FLAGS,
&d_l3_conn->d_l3l2_flags);
/* Get L1 flags */
(void)diag_l2_ioctl(d_l2_conn,
DIAG_IOCTL_GET_L1_FLAGS,
&d_l3_conn->d_l3l1_flags);
/* Call the proto routine */
rv = dp->diag_l3_proto_start(d_l3_conn);
if (rv < 0) {
free(d_l3_conn);
return diag_pseterr(rv);
} else {
/*
* Set time to now
*/
d_l3_conn->timer=diag_os_getms();
/*
* And add to list
*/
LL_PREPEND(diag_l3_list, d_l3_conn);
}
}
if (diag_l3_debug & DIAG_DEBUG_OPEN)
fprintf(stderr,FLFMT "start returns %p\n",
FL, (void *)d_l3_conn);
return d_l3_conn;
}
//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;
}
/*
* 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;
}
/*
* Open the diagnostic device
* ELM settings used : no echo (E0), headers on (H1), linefeeds off (L0), mem off (M0)
*/
static struct diag_l0_device *
diag_l0_elm_open(const char *subinterface, int iProtocol)
{
int i,rv;
struct diag_l0_device *dl0d;
struct diag_l0_elm_device *dev;
struct diag_serial_settings sset;
const uint8_t *buf;
uint8_t rxbuf[ELM_BUFSIZE];
const char ** elm_official;
const char ** elm_clones; //point to elm323_ or elm327_ clone and official version string lists
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "open subinterface %s protocol %d\n",
FL, subinterface, iProtocol);
}
diag_l0_elm_init();
if ((rv=diag_calloc(&dev, 1)))
return diag_pseterr(rv);
dev->protocol = iProtocol;
dl0d = diag_l0_new(&diag_l0_elm, (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);
}
//set speed to 38400;8n1.
sset.speed=38400;
sset.databits = diag_databits_8;
sset.stopbits = diag_stopbits_1;
sset.parflag = diag_par_n;
dev->serial = sset;
if ((rv=diag_tty_setup(dl0d, &sset))) {
fprintf(stderr, FLFMT "Error setting 38400;8N1 on %s\n",
FL, subinterface);
diag_l0_elm_close(&dl0d);
return diag_pseterr(rv);
}
diag_tty_iflush(dl0d); /* Flush unread input */
//At this stage, the ELM has possibly been powered up for a while;
//it may have an unfinished command / garbage in its input buffer. We
//need to clear that before sending the real ATZ ==> ATI is quick and safe; elm_purge does this.
dev->elmflags=0; //we know nothing yet
rv=elm_purge(dl0d);
//if rv=0, we got a prompt so we know speed is set properly.
if (rv==0) {
dev->elmflags |= ELM_38400;
} else {
fprintf(stderr, FLFMT "sending ATI @ 38400 failed; trying @ 9600...\n", FL);
sset.speed=9600;
dev->serial = sset;
if ((rv=diag_tty_setup(dl0d, &sset))) {
fprintf(stderr, FLFMT "Error setting 9600;8N1 on %s\n",
FL, subinterface);
diag_l0_elm_close(&dl0d);
return diag_pseterr(rv);
}
diag_tty_iflush(dl0d); /* Flush unread input */
rv = elm_purge(dl0d); //try ATI\r again
if (rv !=0) {
fprintf(stderr, FLFMT "sending ATI @ 9600 failed. Verify connection to ELM\n", FL);
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
}
if (diag_l0_debug&DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "elm_open : sending ATZ...\n", FL);
}
//the command "ATZ" causes a full reset and the ELM replies with
//a string like "ELM32x vX.Xx\n>"
buf=(uint8_t *)"ATZ\x0D";
rv=elm_sendcmd(dl0d, buf, 4, 2000, rxbuf);
if (rv) {
fprintf(stderr, FLFMT "elm_open : ATZ failed !\n", FL);
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
//Correct prompt received; try to identify device.
// 1) guess 323 vs 327
if (strstr((char *)rxbuf, "ELM323")!=NULL) {
dev->elmflags |= ELM_323_BASIC;
elm_official=elm323_official;
elm_clones=elm323_clones;
} else if (strstr((char *)rxbuf, "ELM327")!=NULL) {
dev->elmflags |= ELM_327_BASIC;
elm_official=elm327_official;
elm_clones=elm327_clones;
} else {
fprintf(stderr, FLFMT "no valid version string !! Report this !. Got:\n%s\n", FL, rxbuf);
//no point in continuing...
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
// 2) identify valid VS clone devices.
rv=0; // temp "device identified" flag
for (i=0; elm_clones[i]; i++) {
if (strstr((char *)rxbuf, elm_clones[i])) {
printf("Clone ELM found, v%s\n", elm_clones[i]);
dev->elmflags |= ELM_32x_CLONE;
rv=1;
break;
}
}
if (rv==0) {
for (i=0; elm_official[i]; i++) {
if (strstr((char *)rxbuf, elm_official[i])) {
printf("Official ELM found, v%s\n", elm_official[i]);
rv=1;
break;
}
}
}
if (rv==0) {
//still not identified : assume clone.
dev->elmflags |= ELM_32x_CLONE;
printf("ELM version not recognized! Please report this ! Resp=%s\n", rxbuf);
}
if ((dev->elmflags & ELM_323_BASIC) && (dev->elmflags & ELM_32x_CLONE)) {
printf("A 323 clone ? Report this !\n");
}
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "ELM reset success, elmflags=%#x\n", FL, dev->elmflags);
}
//now send "ATE0\n" command to disable echo.
buf=(uint8_t *)"ATE0\x0D";
if (elm_sendcmd(dl0d, buf, 5, 500, NULL)) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "sending \"ATE0\" failed\n", FL);
}
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
//ATL0 : disable linefeeds
buf=(uint8_t *)"ATL0\x0D";
if (elm_sendcmd(dl0d, buf, 5, 500, NULL)) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "sending \"ATL0\" failed\n", FL);
}
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
//ATH1 : always show header bytes
buf=(uint8_t *)"ATH1\x0D";
if (elm_sendcmd(dl0d, buf, 5, 500, NULL)) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "sending \"ATH1\" failed\n", FL);
}
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
//for elm327 only: disable memory
if (dev->elmflags & ELM_327_BASIC) {
buf=(uint8_t *)"ATM0\x0D";
if (elm_sendcmd(dl0d, buf, 5, 500, NULL)) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "sending \"ATM0\" failed\n", FL);
}
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
}
//check if proto is really supported (323 supports only 9141 and 14230)
if ((dev->elmflags & ELM_323_BASIC) &&
((iProtocol != DIAG_L1_ISO9141) && (iProtocol != DIAG_L1_ISO14230)))
return diag_pseterr(DIAG_ERR_PROTO_NOTSUPP);
//if 327, set proto when possible; we won't if 14230, because init type (fast vs slow) isn't decided yet
// CAN is also not implemented here
buf=NULL;
if (dev->elmflags & ELM_327_BASIC) {
switch (iProtocol) {
case DIAG_L1_J1850_PWM:
buf=(uint8_t *)"ATTP1\x0D";
break;
case DIAG_L1_J1850_VPW:
buf=(uint8_t *)"ATTP2\x0D";
break;
case DIAG_L1_ISO9141:
buf=(uint8_t *)"ATTP3\x0D";
break;
default:
buf=NULL;
}
}
if (buf != NULL) {
if (elm_sendcmd(dl0d, buf, 6, 500, NULL)) {
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "sending \"ATTPx\" failed\n", FL);
}
diag_l0_elm_close(&dl0d);
return diag_pseterr(DIAG_ERR_BADIFADAPTER);
}
}
//at this point : ELM is ready for further ops
if (diag_l0_debug & DIAG_DEBUG_OPEN) {
fprintf(stderr, FLFMT "ELM ready.\n", FL);
}
return dl0d;
}
//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;
}
