/*
* Send the data
*
* - with VAG protocol this will sleep as the message is sent as each byte
* is ack'ed by the far end
*
* - after each byte reset the send timer or the timeout routine may try
* and send something
*
* 1st byte of message is command, followed by data
*/
static int
diag_l2_proto_vag_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg)
{
int rv = 0;
/* int i;*/
/* int csum;*/
/* int len;*/
/* uint8_t buf[MAXRBUF];*/
/* int offset;*/
//struct diag_l2_vag *dp;
if (diag_l2_debug & DIAG_DEBUG_WRITE)
fprintf(stderr,
FLFMT "diag_l2_vag_send %p msg %p len %d called\n",
FL, (void *)d_l2_conn, (void *)msg, msg->len);
//dp = (struct diag_l2_vag *)d_l2_conn->diag_l2_proto_data; //not used ?
#if xx
rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, 0,
buf, len, d_l2_conn->diag_l2_p4min);
#endif
return rv? diag_iseterr(rv):0 ;
}
/*
* Send a byte, and ensure we get the inverted ack back
*/
static int
diag_l2_proto_vag_send_byte(struct diag_l2_conn *d_l2_conn, databyte_type databyte)
{
uint8_t rx_data = 0;
int rv;
/* Send the data byte */
uint8_t db = (uint8_t)databyte;
rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, 0,
&db, 1, d_l2_conn->diag_l2_p4min);
if (rv < 0)
return rv;
//XXX diag_l2_sendstamp(d_l2_conn); /* update the last sent timer */
//since this function (_send_byte) should only be used internally;
//and nobody is supposed to use diag_l2_vag by itself (always go through
//the diag_l2 public interface; this shouldn't be required.
/* Receive the ack */
if (rx_data != ((~databyte) & 0xFF) )
return 0; /* Wrong data */
return 0;
}
/*
* Just send the data, with no processing etc
* ret 0 if ok
*/
int
dl2p_raw_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
int rv;
DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
FLFMT "diag_l2_send %p, msg %p len %d called\n",
FL, (void *)d_l2_conn, (void *)msg, msg->len);
rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d, 0,
msg->data, msg->len, d_l2_conn->diag_l2_p4min);
return rv? diag_ifwderr(rv):0 ;
}
/*
* Just send the data, with no processing etc
* ret 0 if ok
*/
int
diag_l2_proto_raw_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg)
{
int rv;
if (diag_l2_debug & DIAG_DEBUG_WRITE)
fprintf(stderr,
FLFMT "diag_l2_send %p, msg %p len %d called\n",
FL, (void *)d_l2_conn, (void *)msg, msg->len);
rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, 0,
msg->data, msg->len, d_l2_conn->diag_l2_p4min);
return rv? diag_iseterr(rv):0 ;
}
/*
* Just send the data
*
* We add the header and checksums here as appropriate, based on the keybytes
*
* We take the source and dest from the internal data NOT from the msg fields
*
* We also wait p3 ms
* return 0 if ok, <0 if err
* argument msg must have .len, .data, .dest and .src assigned.
*/
static int
diag_l2_proto_14230_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg)
{
int rv;
uint8_t csum;
unsigned int i;
size_t len;
uint8_t buf[MAXRBUF];
int offset=0; //data payload starts at buf[offset}
struct diag_l2_14230 *dp;
if (msg->len < 1)
return diag_iseterr(DIAG_ERR_BADLEN);
if (diag_l2_debug & DIAG_DEBUG_WRITE)
fprintf(stderr,
FLFMT "_send: dl2conn=%p msg=%p len=%d\n",
FL, (void *)d_l2_conn, (void *)msg, msg->len);
dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
//if L1 requires headerless data, send directly :
if (d_l2_conn->diag_link->l1flags & DIAG_L1_DATAONLY) {
rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, NULL,
msg->data, msg->len, d_l2_conn->diag_l2_p4min);
return rv? diag_iseterr(rv):0;
}
/* Build the new message */
if ((dp->modeflags & ISO14230_LONGHDR) || !(dp->modeflags & ISO14230_SHORTHDR)) {
//we use full headers if they're supported or if we don't have a choice.
//set the "address present" bit
//and functional addressing if applicable
if (dp->modeflags & ISO14230_FUNCADDR)
buf[0] = 0xC0;
else
buf[0] = 0x80;
/* If user supplied addresses, use them, else use the originals */
if (msg->dest)
buf[1] = msg->dest;
else
buf[1] = dp->dstaddr;
if (msg->src)
buf[2] = msg->src;
else
buf[2] = dp->srcaddr;
offset = 3;
} else if (dp->modeflags & ISO14230_SHORTHDR) {
//here, short addressless headers are required.
//basic format byte : 0
buf[0]=0;
offset = 1 ;
}
if ((dp->modeflags & ISO14230_FMTLEN)|| !(dp->modeflags & ISO14230_LENBYTE)) {
//if ECU supports length in format byte, or doesn't support extra len byte
if (msg->len < 64) {
buf[0] |= msg->len;
} else {
//len >=64, can we use a length byte ?
if (dp->modeflags & ISO14230_LENBYTE) {
buf[offset] = msg->len;
offset += 1;
} else {
fprintf(stderr, FLFMT "can't send >64 byte msgs to this ECU !\n", FL);
return diag_iseterr(DIAG_ERR_BADLEN);
}
}
} else if (dp->modeflags & ISO14230_LENBYTE) {
// if the ecu needs a length byte
buf[offset] = msg->len;
offset += 1;
}
memcpy(&buf[offset], msg->data, msg->len);
len = msg->len + offset; /* data + hdr */
if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2CKSUM) == 0) {
/* We must add checksum, which is sum of bytes */
for (i = 0, csum = 0; i < len; i++)
csum += buf[i];
buf[len] = csum;
len++; /* + checksum */
}
if ((diag_l2_debug & DIAG_DEBUG_WRITE) && (diag_l2_debug & DIAG_DEBUG_DATA)) {
fprintf(stderr, FLFMT "_send: ", FL);
diag_data_dump(stderr, buf, len);
fprintf(stderr, "\n");
}
/* Wait p3min milliseconds, but not if doing fast/slow init */
if (dp->state == STATE_ESTABLISHED)
diag_os_millisleep(d_l2_conn->diag_l2_p3min);
rv = diag_l1_send (d_l2_conn->diag_link->diag_l2_dl0d, NULL,
buf, len, d_l2_conn->diag_l2_p4min);
return rv? diag_iseterr(rv):0;
}
/*
* 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;
}
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;
}
