// 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;
}
int
diag_l1_add_l0dev(const struct diag_l0 *l0dev) {
int rv;
struct diag_l0_node *last_node, *new_node;
if (l0dev_list == NULL) {
/*
* No devices yet, create the root.
*/
if ( (rv = diag_calloc(&l0dev_list, 1)) )
return rv;
l0dev_list->l0dev = l0dev;
return 0;
}
for (last_node = l0dev_list; last_node != NULL; last_node = last_node->next)
if (last_node->l0dev == l0dev)
return diag_iseterr(DIAG_ERR_GENERAL); /* Already there. */
if ( (rv = diag_calloc(&new_node, 1)) )
return rv;
for (last_node = l0dev_list; last_node->next != NULL; last_node = last_node->next)
/* Search for the next-to-last node */;
new_node->l0dev = l0dev;
last_node->next = new_node;
return 0;
}
// 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;
}
// 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;
}
int main(void)
{
struct diag_vcdiff_pcode *pcodes;
struct diag_vcdiff_script *script;
uint8_t *result;
size_t size, i;
pcodes = diag_calloc(2, sizeof(struct diag_vcdiff_pcode));
pcodes[0].inst = DIAG_VCD_RUN;
pcodes[0].size = 1;
pcodes[0].attr.byte = 'a';
pcodes[1].inst = DIAG_VCD_COPY;
pcodes[1].size = 20;
pcodes[1].attr.addr = 0;
script = diag_malloc(sizeof(struct diag_vcdiff_script));
script->source = NULL;
script->s_pcodes = 2;
script->pcodes = pcodes;
result = diag_vcdiff_expand(script, &size);
ASSERT_NOT_NULL(result);
ASSERT_EQ_UINT32(21, size);
for (i = 0; i < size; i++) {
ASSERT_EQ_UINT8('a', result[i]);
}
diag_free(result);
diag_free(script);
diag_free(pcodes);
return EXIT_SUCCESS;
}
/*
* XXX All commands should probably have optional "init" hooks.
*/
int set_init(void)
{
/* Reset parameters to defaults. */
set_speed = 10400; /* Comms speed; ECUs will probably send at 10416 bps (96us per bit) */
set_testerid = 0xf1; /* Our tester ID */
set_addrtype = 1; /* Use virtual addressing */
set_destaddr = 0x33; /* Dest ECU address */
store_set_destaddr = set_destaddr;
set_L1protocol = DIAG_L1_ISO9141; /* L1 protocol type */
set_L2protocol = DIAG_L2_PROT_ISO9141; /* Protocol type */
set_initmode = DIAG_L2_TYPE_FASTINIT ;
set_display = 0; /* English (1), or Metric (0) */
set_vehicle = "ODBII"; /* Vehicle */
set_ecu = "ODBII"; /* ECU name */
set_interface_idx= DEFAULT_INTERFACE;
set_interface = l0_names[DEFAULT_INTERFACE].code; /* Default H/w interface to use */
strncpy(set_subinterface,"/dev/null",sizeof(set_subinterface));
printf( "%s: Interface set to default: %s on %s\n", progname, l0_names[set_interface_idx].longname, set_subinterface);
if (diag_calloc(&set_simfile, strlen(DB_FILE)+1))
return diag_iseterr(DIAG_ERR_GENERAL);
strcpy(set_simfile, DB_FILE); //default simfile for use with CARSIM
diag_l0_sim_setfile(set_simfile);
return 0;
}
/*
* 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 ;
}
// Parses a response's text to data.
// Replaces special tokens with function results.
void sim_parse_response(struct sim_ecu_response* resp_p)
{
// the magic number here is "5": all elements in a file line
// are exactly 5 characters long ("0x00 ", "cks1 ", etc).
// don't screw this! :)
#define TOKEN_SINE1 "sin1"
#define TOKEN_SAWTOOTH1 "swt1"
#define TOKEN_ISO9141CS "cks1"
#define SRESP_SIZE 255
uint8_t synth_resp[SRESP_SIZE]; // 255 response bytes.
char token[5]; // to get tokens from the text.
char* parse_offset = NULL;
int ret = 0;
uint8_t pos = 0;
// extract byte values from response line, allowing for tokens.
memset(synth_resp, 0, SRESP_SIZE);
do {
if ((size_t) pos*5 >= strlen(resp_p->text))
break;
// scan an element.
parse_offset = resp_p->text + pos*5;
ret = sscanf(parse_offset, "%s", token);
// try replacing a token with a calculated value.
if (strcmp(token, TOKEN_SINE1) == 0)
synth_resp[pos] = sine1(synth_resp, pos);
else if (strcmp(token, TOKEN_SAWTOOTH1) == 0)
synth_resp[pos] = sawtooth1(synth_resp, pos);
else if (strcmp(token, TOKEN_ISO9141CS) == 0)
synth_resp[pos] = diag_cks1(synth_resp, pos);
else {
// failed. try scanning element as an Hex byte.
ret = sscanf(parse_offset, "%X", ((unsigned int *)(&synth_resp[pos])));
if (ret != 1) {
// failed. something's wrong.
fprintf(stderr, FLFMT "Error parsing line: %s at position %d.\n", FL, resp_p->text, pos*5);
break;
}
}
// next byte.
pos++;
} while (ret != 0);
// copy to user.
//resp_p->data = calloc(pos, sizeof(uint8_t));
if (diag_calloc(&(resp_p->data),pos)) {
fprintf(stderr, FLFMT "Error parsing response\n", FL);
return;
}
memcpy(resp_p->data, synth_resp, pos);
resp_p->len = pos;
}
struct diag_command *diag_command_new(char **argv,
const char *dir,
const char *in,
const char *out,
const char *err)
{
struct diag_command *command = diag_malloc(sizeof(*command));
size_t argc;
for (argc = 0; argv[argc]; argc++) ;
command->argv = diag_calloc(argc + 1, sizeof(char *));
size_t i;
for (i = 0; i < argc; i++) {
command->argv[i] = diag_strdup(argv[i]);
}
command->argv[argc] = NULL; /* NULL-terminated */
command->file = argv[0];
if (dir) {
command->dir = diag_strdup(dir);
} else {
command->dir = diag_malloc(PATH_LENGTH);
char *p;
#if defined(_WIN32) && defined(__MINGW32__)
p = _getcwd(command->dir, PATH_LENGTH);
#elif defined(HAVE_UNISTD_H)
p = getcwd(command->dir, PATH_LENGTH);
#endif
if (!p) {
perror(NULL);
diag_fatal("failed to get current working directory");
}
}
if (in) {
command->in = diag_strdup(in);
} else {
command->in = NULL;
}
if (out) {
command->out = diag_strdup(out);
} else {
command->out = NULL;
}
if (err) {
command->err = diag_strdup(err);
} else {
command->err = NULL;
}
return command;
}
/*
* 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;
}
static int
br_new(struct diag_l0_device *dl0d) {
struct br_device *dev;
int rv;
assert(dl0d);
rv = diag_calloc(&dev, 1);
if (rv != 0) {
return diag_ifwderr(rv);
}
dl0d->l0_int = dev;
rv = diag_cfgn_tty(&dev->port);
if (rv != 0) {
free(dev);
return diag_ifwderr(rv);
}
dev->port.next = NULL;
return 0;
}
void *diag_qselect(void *base, size_t nmemb, size_t size,
int (*cmp)(const void *, const void *),
size_t k)
{
assert(base);
assert(nmemb > 0);
assert(size > 0);
if (nmemb <= k) return NULL;
if (nmemb == 1) return base;
int *rv = diag_calloc(nmemb, sizeof(int));
size_t i;
size_t n_lt;
size_t n_gt;
size_t n_eq;
char *p;
partition:
assert(k < nmemb);
if (nmemb == 1) goto done;
n_lt = 0;
n_gt = 0;
p = (char *)base;
for (i = 1; i < nmemb; i++) {
int r = cmp(p, p + size * i);
if (r < 0) n_lt++;
if (0 < r) n_gt++;
rv[i] = r;
}
n_eq = nmemb - n_lt - n_gt;
if (k < n_lt) {
size_t j = n_lt + 1;
for (i = 1; i <= n_lt; i++) {
if (rv[i] >= 0) {
while (j < nmemb && rv[j] >= 0) {
j++;
}
assert(j < nmemb);
memcpy(p + size * i, p + size * j++, size);
}
}
base = p + size;
nmemb = n_lt;
/* k is unchanged */
goto partition;
}
if (k >= n_lt + n_eq) {
size_t j = n_gt + 1;
for (i = 1; i <= n_gt; i++) {
if (rv[i] <= 0) {
while (j < nmemb && rv[j] <= 0) {
j++;
}
assert(j < nmemb);
memcpy(p + size * i, p + size * j++, size);
}
}
base = p + size;
nmemb = n_gt;
k -= n_lt + n_eq;
goto partition;
}
done:
diag_free(rv);
return base;
}
/*
* 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;
}
/*
* 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;
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]
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->diag_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->diag_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->diag_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->diag_l2_dl0d, 0,
data, sizeof(data), wait_time) != DIAG_ERR_TIMEOUT) ;
/* And we're done */
dp->state = STATE_ESTABLISHED ;
return 0;
}
int diag_tty_open(struct diag_l0_device *dl0d,
const char *portname)
{
int rv;
struct unix_tty_int *uti;
#if defined(_POSIX_TIMERS) && (SEL_TIMEOUT==S_POSIX || SEL_TIMEOUT==S_AUTO)
struct sigevent to_sigev;
struct sigaction sa;
clockid_t timeout_clkid;
#endif
assert(dl0d != NULL);
if (dl0d->tty_int) return diag_iseterr(DIAG_ERR_GENERAL);
if ((rv=diag_calloc(&uti,1))) {
return diag_iseterr(rv);
}
#if defined(_POSIX_TIMERS) && (SEL_TIMEOUT==S_POSIX || SEL_TIMEOUT==S_AUTO)
//set-up the r/w timeouts clock - here we just create it; it will be armed when needed
#ifdef _POSIX_MONOTONIC_CLOCK
timeout_clkid = CLOCK_MONOTONIC;
#else
timeout_clkid = CLOCK_REALTIME;
#endif // _POSIX_MONOTONIC_CLOCK
sa.sa_flags = SA_SIGINFO;
sa.sa_sigaction = diag_tty_rw_timeout_handler;
sigemptyset(&sa.sa_mask);
if(sigaction(SIGUSR1, &sa, NULL) != 0) {
fprintf(stderr, FLFMT "Could not set-up action for timeout timer... report this\n", FL);
free(uti);
return diag_iseterr(DIAG_ERR_GENERAL);
}
to_sigev.sigev_notify = SIGEV_SIGNAL;
to_sigev.sigev_signo = SIGUSR1;
to_sigev.sigev_value.sival_ptr = uti;
if(timer_create(timeout_clkid, &to_sigev, &uti->timerid) != 0) {
fprintf(stderr, FLFMT "Could not create timeout timer... report this\n", FL);
free(uti);
return diag_iseterr(DIAG_ERR_GENERAL);
}
#endif
uti->fd = DL0D_INVALIDHANDLE;
size_t n = strlen(portname) + 1;
if ((rv=diag_malloc(&uti->name, n))) {
free(uti);
return diag_iseterr(rv);
}
strncpy(uti->name, portname, n);
dl0d->tty_int = uti;
//past this point, we can call diag_tty_close(dl0d) to abort in case of errors
errno = 0;
#if defined(O_NONBLOCK) && (SEL_TTYOPEN==S_ALT1 || SEL_TTYOPEN==S_AUTO)
/*
* For POSIX behavior: Open serial device non-blocking to avoid
* modem control issues, then set to blocking.
*/
{
int fl;
uti->fd = open(uti->name, O_RDWR | O_NONBLOCK);
if (uti->fd > 0) {
errno = 0;
if ((fl = fcntl(uti->fd, F_GETFL, 0)) < 0) {
fprintf(stderr,
FLFMT "Can't get flags with fcntl on fd %d: %s.\n",
FL, uti->fd, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
fl &= ~O_NONBLOCK;
errno = 0;
if (fcntl(uti->fd, F_SETFL, fl) < 0) {
fprintf(stderr,
FLFMT "Can't set flags with fcntl on fd %d: %s.\n",
FL, uti->fd, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
}
}
#else
#ifndef O_NONBLOCK
#warning No O_NONBLOCK on your system ?! Please report this
#endif
uti->fd = open(uti->name, O_RDWR);
#endif // O_NONBLOCK
if (uti->fd >= 0) {
if (diag_l0_debug & DIAG_DEBUG_OPEN)
fprintf(stderr, FLFMT "Device %s opened, fd %d\n",
FL, uti->name, uti->fd);
} else {
fprintf(stderr,
FLFMT "Could not open \"%s\" : %s. "
"Make sure the device specified corresponds to the "
"serial device your interface is connected to.\n",
FL, uti->name, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
/*
* Save original settings so can reset
* device on close - we also set "current" settings to
* those we just read aswell
*/
#if defined(__linux__)
if (ioctl(uti->fd, TIOCGSERIAL, &uti->ss_orig) < 0) {
fprintf(stderr,
FLFMT "open: TIOCGSERIAL failed: %s\n", FL, strerror(errno));
uti->tioc_works = 0;
} else {
uti->ss_cur = uti->ss_orig;
uti->tioc_works = 1;
}
#endif
if (ioctl(uti->fd, TIOCMGET, &uti->modemflags) < 0) {
fprintf(stderr,
FLFMT "open: TIOCMGET failed: %s\n", FL, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
#ifdef USE_TERMIOS2
rv = ioctl(uti->fd, TCGETS, &uti->st_orig);
#else
rv = tcgetattr(uti->fd, &uti->st_orig);
#endif
if (rv != 0) {
fprintf(stderr, FLFMT "open: could not get orig settings: %s\n",
FL, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//and set common flags
uti->st_cur = uti->st_orig;
/* "stty raw"-like iflag settings: */
/* Clear a bunch of un-needed flags */
uti->st_cur.c_iflag &= ~ (IGNBRK | BRKINT | IGNPAR | PARMRK
| INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF
| IXANY | IMAXBEL);
#ifdef __linux__
uti->st_cur.c_iflag &= ~(IUCLC); /* non-posix; disable ucase/lcase conversion */
#endif
uti->st_cur.c_oflag &= ~(OPOST); //disable impl-defined output processing
/* Disable canonical input and keyboard signals.
+* There is no need to also clear the many ECHOXX flags, both because
+* many systems have non-POSIX flags and also because the ECHO
+* flags don't don't matter when ICANON is clear.
*/
/* CJH: However, taking 'man termios' at its word, the ECHO flag is
*not* affected by ICANON, and it seems we do need to clear it */
uti->st_cur.c_lflag &= ~( ICANON | ISIG | ECHO | IEXTEN);
uti->st_cur.c_cflag &= ~( CRTSCTS ); //non-posix; disables hardware flow ctl
uti->st_cur.c_cflag |= (CLOCAL | CREAD); //ignore modem control lines; enable read
uti->st_cur.c_cc[VMIN] = 1; //Minimum # of bytes before read() returns (default: 0!!!)
//and update termios with new flags.
#ifdef USE_TERMIOS2
rv = ioctl(uti->fd, TCSETS, &uti->st_cur);
rv |= ioctl(uti->fd, TCGETS2, &uti->st2_cur);
#else
rv=tcsetattr(uti->fd, TCSAFLUSH, &uti->st_cur);
#endif
if (rv != 0) {
fprintf(stderr, FLFMT "open: can't set input flags: %s.\n",
FL, strerror(errno));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//arbitrarily set the single byte write timeout to 1ms
uti->byte_write_timeout_us = 1000ul;
return 0;
}
//diag_tty_open : open specified port for L0
int diag_tty_open(struct diag_l0_device *dl0d,
const char *portname)
{
int rv;
struct tty_int *wti;
size_t n = strlen(portname) + 1;
COMMTIMEOUTS devtimeouts;
if (!dl0d) return diag_iseterr(DIAG_ERR_GENERAL);
if ((rv=diag_calloc(&wti,1))) {
return diag_iseterr(rv);
}
dl0d->tty_int = wti;
wti->fd = INVALID_HANDLE_VALUE;
//allocate space for portname name
if ((rv=diag_malloc(&wti->name, n))) {
free(dl0d->tty_int);
return diag_iseterr(rv);
}
//Now, in case of errors we can call diag_tty_close() on the dl0d since its members are alloc'ed
strncpy(wti->name, portname, n);
wti->fd=CreateFile(portname, GENERIC_READ | GENERIC_WRITE, 0, NULL,
OPEN_EXISTING,
FILE_ATTRIBUTE_NORMAL | FILE_FLAG_NO_BUFFERING | FILE_FLAG_WRITE_THROUGH,
NULL);
//File hande is created as non-overlapped. This may change eventually.
if (wti->fd != INVALID_HANDLE_VALUE) {
if (diag_l0_debug & DIAG_DEBUG_OPEN)
fprintf(stderr, FLFMT "Device %s opened, fd %p\n",
FL, wti->name, wti->fd);
} else {
fprintf(stderr,
FLFMT "Open of device interface \"%s\" failed: %s\n",
FL, wti->name, diag_os_geterr(0));
fprintf(stderr, FLFMT
"(Make sure the device specified corresponds to the\n", FL );
fprintf(stderr,
FLFMT "serial device your interface is connected to.\n", FL);
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//purge & abort everything.
PurgeComm(wti->fd,PURGE_TXABORT | PURGE_RXABORT | PURGE_TXCLEAR | PURGE_RXCLEAR);
//as opposed to the unix diag_tty.c ; this one doesn't save previous commstate. The next program to use the COM port
//will need to deal with it...
//We will load the DCB with the current comm state. This way we only need to call GetCommState once during a session
//and the DCB should contain coherent initial values
if (! GetCommState(wti->fd, &wti->dcb)) {
fprintf(stderr, FLFMT "Could not get comm state: %s\n",FL, diag_os_geterr(0));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
//Finally set COMMTIMEOUTS to reasonable values (all in ms) ?
devtimeouts.ReadIntervalTimeout=30; //i.e. more than 30ms between received bytes
devtimeouts.ReadTotalTimeoutMultiplier=5; //timeout per requested byte
devtimeouts.ReadTotalTimeoutConstant=20; // (constant + multiplier*numbytes) = total timeout on read(buf, numbytes)
devtimeouts.WriteTotalTimeoutMultiplier=0; //probably useless as all flow control will be disabled ??
devtimeouts.WriteTotalTimeoutConstant=0;
if (! SetCommTimeouts(wti->fd,&devtimeouts)) {
fprintf(stderr, FLFMT "Could not set comm timeouts: %s\n",FL, diag_os_geterr(0));
diag_tty_close(dl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
return 0;
} //diag_tty_open
int diag_tty_open(struct diag_l0_device **ppdl0d,
const char *subinterface,
const struct diag_l0 *dl0,
void *dl0_handle)
{
int rv;
struct diag_ttystate *dt;
struct diag_l0_device *dl0d;
char *endptr;
int iInterface;
const char *tty_template ="/dev/obdII%d";
if (rv=diag_calloc(&dl0d, 1)) //free'd in diag_tty_close
return diag_iseterr(rv);
dl0d->fd = -1;
dl0d->dl0_handle = dl0_handle;
dl0d->dl0 = dl0;
if ((rv=diag_calloc(&dl0d->ttystate, 1))) {
free(dl0d);
return diag_iseterr(rv);
}
*ppdl0d = dl0d;
/*
* XXX this should probably be removed... "historical compatibility" with what ?? Who ?
* For historical compatibility, if the subinterface decodes cleanly
* as an integer we will write it into a string to get the name.
* You can create a symlink to "/dev/obdII<NUMBER>" if you want to,
* or just set the subinterface to a valid device name.
*/
iInterface = strtol(subinterface, &endptr, 10);
if (*endptr == 0) {
/* Entire string is a valid number: Provide compatibility. */
size_t n = strlen(tty_template) + 32;
printf("Warning : using deprecated /dev/obdII<x> subinterface definition.\n");
printf("Support for this will be discontinued shortly...\n");
if ((rv=diag_malloc(&dl0d->name, n))) {
(void)diag_tty_close(ppdl0d);;
return diag_iseterr(rv);
}
(void)snprintf(dl0d->name, n, tty_template, iInterface);
} else {
size_t n = strlen(subinterface) + 1;
if ((rv=diag_malloc(&dl0d->name, n))) {
(void)diag_tty_close(ppdl0d);;
return diag_iseterr(rv);
}
strncpy(dl0d->name, subinterface, n);
}
errno = 0;
#if defined(__linux__) && (TRY_POSIX == 0)
dl0d->fd = open(dl0d->name, O_RDWR);
#else
/*
+* For POSIX behavior: Open serial device non-blocking to avoid
+* modem control issues, then set to blocking.
*/
/* CODE BLOCK */
{
int fl;
dl0d->fd = open(dl0d->name, O_RDWR | O_NONBLOCK);
if (dl0d->fd > 0) {
errno = 0;
if ((fl = fcntl(dl0d->fd, F_GETFL, 0)) < 0) {
fprintf(stderr,
FLFMT "Can't get flags with fcntl on fd %d: %s.\n",
FL, dl0d->fd, strerror(errno));
(void)diag_tty_close(ppdl0d);;
return diag_iseterr(DIAG_ERR_GENERAL);
}
fl &= ~O_NONBLOCK;
errno = 0;
if (fcntl(dl0d->fd, F_SETFL, fl) < 0) {
fprintf(stderr,
FLFMT "Can't set flags with fcntl on fd %d: %s.\n",
FL, dl0d->fd, strerror(errno));
(void)diag_tty_close(ppdl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
}
}
#endif
if (dl0d->fd >= 0) {
if (diag_l0_debug & DIAG_DEBUG_OPEN)
fprintf(stderr, FLFMT "Device %s opened, fd %d\n",
FL, dl0d->name, dl0d->fd);
} else {
fprintf(stderr,
FLFMT "Open of device interface \"%s\" failed: %s\n",
FL, dl0d->name, strerror(errno));
fprintf(stderr, FLFMT
"(Make sure the device specified corresponds to the\n", FL );
fprintf(stderr,
FLFMT "serial device your interface is connected to.\n", FL);
(void)diag_tty_close(ppdl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
dt = dl0d->ttystate;
/*
* Save original settings so can reset
* device on close - we also set "current" settings to
* those we just read aswell
*/
#if defined(__linux__) && (TRY_POSIX == 0)
if (ioctl(dl0d->fd, TIOCGSERIAL, &dt->dt_osinfo) < 0)
{
fprintf(stderr,
FLFMT "open: Ioctl TIOCGSERIAL failed %d\n", FL, errno);
(void)diag_tty_close(ppdl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
dt->dt_sinfo = dt->dt_osinfo;
#endif
if (ioctl(dl0d->fd, TIOCMGET, &dt->dt_modemflags) < 0)
{
fprintf(stderr,
FLFMT "open: Ioctl TIOCMGET failed: %s\n", FL, strerror(errno));
(void)diag_tty_close(ppdl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (tcgetattr(dl0d->fd, &dt->dt_otinfo) < 0)
{
fprintf(stderr, FLFMT "open: tcgetattr failed %s\n",
FL, strerror(errno));
(void)diag_tty_close(ppdl0d);
return diag_iseterr(DIAG_ERR_GENERAL);
}
dt->dt_tinfo = dt->dt_otinfo;
return 0;
}
static int
diag_l2_proto_vag_startcomms( struct diag_l2_conn *d_l2_conn,
UNUSED(flag_type flags),
unsigned int bitrate, target_type target, UNUSED(source_type source))
{
struct diag_serial_settings set;
struct diag_l2_vag *dp;
uint8_t data[MAXRBUF];
int rv;
/* int wait_time;*/
/* int hdrlen;*/
/* int datalen;*/
/* int datasrc;*/
uint8_t cbuf[MAXRBUF];
/* int len;*/
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;
memset(data, 0, sizeof(data));
/*
* If 0 has been specified, use a useful default of 9600
*/
if (bitrate == 0)
bitrate = 9600;
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 as shown */
rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, &set);
if (rv < 0)
{
free(dp);
d_l2_conn->diag_l2_proto_data=NULL;
return diag_iseterr(rv);
}
/* Flush unread input, then wait for idle bus. */
(void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
diag_os_millisleep(300);
/* Now do 5 baud init of supplied address */
in.type = DIAG_L1_INITBUS_5BAUD;
in.addr = target;
rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
if (rv < 0) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL;
return rv;
}
/* Mode bytes are in 7-Odd-1, read as 8N1 and ignore parity */
rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_dl0d, 0,
cbuf, 1, 100);
if (rv < 0) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL;
return diag_iseterr(rv);
}
rv = diag_l1_recv (d_l2_conn->diag_link->diag_l2_dl0d, 0,
&cbuf[1], 1, 100);
if (rv < 0) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL;
return diag_iseterr(rv);
}
/* Keybytes are 0x1 0x8a for VAG protocol */
if ((cbuf[0] != 0x01) || (cbuf[1] != 0x8a)) {
free(dp);
d_l2_conn->diag_l2_proto_data=NULL;
return diag_iseterr(DIAG_ERR_WRONGKB);
}
/* Note down the mode bytes */
d_l2_conn->diag_l2_kb1 = cbuf[0] & 0x7f;
d_l2_conn->diag_l2_kb2 = cbuf[1] & 0x7f;
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->diag_l2_dl0d, 0,
cbuf, 1, d_l2_conn->diag_l2_p4min);
}
/*
* Now receive the first 3 messages
* which show ECU versions etc
*/
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;
}
/*
* 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;
}
