/*
* Fastinit: ISO14230-2 sec 5.2.4.2.3
* Caller should have waited W5 (>300ms) before calling this (from _initbus only!)
* we assume the L line was at the correct state (1) during that time.
* returns 0 (success), 50ms after starting the wake-up pattern.
* Exceptionally we dont diag_iseterr on return since _initbus() takes care of that.
*/
static int
diag_l0_dumb_fastinit(struct diag_l0_device *dl0d)
{
int rv=0;
uint8_t cbuf[MAXRBUF];
if (diag_l0_debug & DIAG_DEBUG_INIT)
fprintf(stderr, FLFMT "dl0d=%p fastinit\n",
FL, (void *)dl0d);
//Tidle before break : W5 (>300ms) on poweron; P3 (>55ms) after a StopCommunication; or 0ms after a P3 timeout.
// We assume the caller took care of this.
/* Send 25/25 ms break as initialisation pattern (TiniL) */
//ISO14230-2 says we should send the same sync pattern on both L and K together.
// we do it almost perfectly; the L \_/ pulse starts before and ends after the K \_/ pulse.
if (dumb_flags & USE_LLINE) {
// do K+L only if the user wants to do both
if (dumb_flags & FAST_BREAK) {
//but we can't use diag_tty_fastbreak while doing the L-line.
fprintf(stderr, FLFMT "Warning : not using L line for FAST_BREAK.\n", FL);
rv=diag_tty_fastbreak(dl0d, 50-WUPFLUSH);
} else {
//normal fast break on K and L.
//note : if LLINE_INV is 1, then we need to clear RTS to pull L down !
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)) < 0) {
fprintf(stderr, FLFMT "fastinit: Failed to set L\\_\n", FL);
return DIAG_ERR_GENERAL;
}
rv=diag_tty_break(dl0d, 25); //K line low for 25ms
/* Now restore DTR/RTS */
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & SET_RTS)) < 0) {
fprintf(stderr, FLFMT "fastinit: Failed to restore DTR & RTS!\n",
FL);
}
diag_os_millisleep(25-WUPFLUSH);
} //if FAST_BREAK
} else {
// do K line only
if (dumb_flags & FAST_BREAK) {
rv=diag_tty_fastbreak(dl0d, 50-WUPFLUSH);
} else {
//normal break
rv=diag_tty_break(dl0d, 25); //K line low for 25ms
diag_os_millisleep(25-WUPFLUSH);
}
} //if USE_LLINE
// here we have WUPFLUSH ms before tWUP is done; we use this
// short time to flush RX buffers. (L2 needs to send a StartComm
// request very soon.)
diag_tty_read(dl0d, cbuf, sizeof(cbuf), WUPFLUSH);
//there may have been a problem in diag_tty_break, if so :
if (rv) {
fprintf(stderr, FLFMT " L0 fastinit : problem !\n", FL);
return DIAG_ERR_GENERAL;
}
return 0;
}
static void
diag_l0_dumb_Lline(struct diag_l0_device *dl0d, uint8_t ecuaddr)
{
/*
* The bus has been high for w0 ms already, now send the
* 8 bit ecuaddr at 5 baud LSB first
*
*/
int i, rv=0;
// uint8_t cbuf[10];
// We also toggle DTR to disable RXD (blocking it at logical 1).
// However, at least one system I tested doesn't react well to
// DTR-toggling.
/* Set RTS low, for 200ms (Start bit) */
if (diag_tty_control(dl0d, (dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)) < 0) {
fprintf(stderr, FLFMT "_LLine: Failed to set DTR & RTS\n", FL);
return;
}
diag_os_millisleep(BPS_PERIOD); /* 200ms -5% */
for (i=0; i<8; i++) {
if (ecuaddr & (1<<i)) {
/* High */
rv |= diag_tty_control(dl0d, (dumb_flags & CLEAR_DTR), (dumb_flags & LLINE_INV));
} else {
/* Low */
rv |= diag_tty_control(dl0d, (dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV));
}
if (rv < 0) {
fprintf(stderr, FLFMT "_LLine: Failed to set DTR & RTS\n",
FL);
return;
}
diag_os_millisleep(BPS_PERIOD); /* 200ms -5% */
}
/* And set high for the stop bit */
if (diag_tty_control(dl0d, (dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)) < 0) {
fprintf(stderr, FLFMT "_LLine: Failed to set DTR & RTS\n",
FL);
return;
}
diag_os_millisleep(BPS_PERIOD); /* 200ms -5% */
/* Now put DTR/RTS back correctly so RX side is enabled */
if (diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & SET_RTS)) < 0) {
fprintf(stderr, FLFMT "_LLine: Failed to restore DTR & RTS\n",
FL);
}
/* And clear out the break XXX no, _slowinit will do this for us after calling dumb_Lline*/
// diag_tty_read(dl0d, cbuf, sizeof(cbuf), 20);
return;
}
/*
* Slowinit:
* We need to send a byte (the address) at 5 baud, then
* switch back to 10400 baud
* and then wait W1 (60-300ms) until we get Sync byte 0x55.
* Caller (in L2 typically) must have waited with bus=idle beforehand.
* This optionally does the L_line stuff according to the flags in dumb_flags.
* Ideally returns 0 (success) immediately after receiving Sync byte.
* This one, like fastinit, doesnt diag_iseterr when returning errors
* since _initbus() takes care of that.
*
*/
static int
diag_l0_dumb_slowinit(struct diag_l0_device *dl0d, struct diag_l1_initbus_args *in,
struct diag_l0_dumb_device *dev)
{
uint8_t cbuf[10];
int rv;
unsigned int tout;
struct diag_serial_settings set;
if (diag_l0_debug & DIAG_DEBUG_PROTO) {
fprintf(stderr, FLFMT "slowinit dl0d=%p address 0x%X\n",
FL, (void *)dl0d, in->addr);
}
//two methods of sending at 5bps. Most USB-serial converts don't support such a slow bitrate !
if (dumb_flags & MAN_BREAK) {
//MAN_BREAK means we bitbang K and optionally L as well.
if (dumb_flags & USE_LLINE) {
//do manual 5bps init on K and L.
//we need to send the byte at in->addr, bit by bit.
int bitcounter;
uint8_t tempbyte=in->addr;
diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)); //L is the logical opposite of RTS...
diag_tty_break(dl0d, BPS_PERIOD); //start startbit
for (bitcounter=0; bitcounter<=7; bitcounter++) {
//LSB first.
if (tempbyte & 1) {
diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & LLINE_INV)); //release L
diag_os_millisleep(BPS_PERIOD);
} else {
unsigned int lowtime=BPS_PERIOD;
//to prevent spurious breaks if we have a sequence of 0's :
//this is an RLE of sorts...
for (; bitcounter <=6; bitcounter++) {
if (tempbyte & 2) //test bit 1; we just tested bit 0 before getting here.
break;
lowtime += BPS_PERIOD;
tempbyte = tempbyte >>1;
}
//this way, we know for sure the next bit is 1 (either bit 7==1 or stopbit==1 !)
diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), !(dumb_flags & LLINE_INV)); //pull L down
diag_tty_break(dl0d, lowtime);
}
tempbyte = tempbyte >>1;
}
//at this point we just finished the last bit, we'll wait the duration of the stop bit.
diag_tty_control(dl0d, !(dumb_flags & CLEAR_DTR), (dumb_flags & SET_RTS)); //release L
diag_os_millisleep(BPS_PERIOD); //stop bit
} else {
int diag_tty_break(struct diag_l0_device *dl0d, const int ms)
{
HANDLE hd;
long h;
/*
* I'm going through this convoluted two-step conversion
* to avoid compiler warnings:
*/
h = get_osfhandle(dl0d->fd);
hd = (HANDLE)h;
if (tcdrain(dl0d->fd)) {
fprintf(stderr, FLFMT "tcdrain returned %s.\n",
FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
SetCommBreak(hd);
diag_os_millisleep(ms);
ClearCommBreak(hd);
return 0;
}
// diag_tty_break #1 : use Set / ClearCommBreak
// and return as soon as break is cleared.
// ret 0 if ok
int diag_tty_break(struct diag_l0_device *dl0d, const unsigned int ms) {
LARGE_INTEGER qpc1, qpc2; //for timing verification
long real_t; //"real" duration
struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
int errval=0;
if (wti->fd == INVALID_HANDLE_VALUE) {
fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (ms <= 1)
return diag_iseterr(DIAG_ERR_GENERAL);
QueryPerformanceCounter(&qpc1);
errval = !SetCommBreak(wti->fd);
QueryPerformanceCounter(&qpc2);
//that call can take quite a while (6ms !!) on some setups (win7 + CH340 USB-Serial).
//It's still impossible to know (from here) when exactly TXD goes low (beginning or end of the call)
real_t=(long) (pf_conv * (qpc2.QuadPart-qpc1.QuadPart)) / 1000L;
real_t = (long) ms - real_t; //time remaining
if (real_t <= 0) real_t = 0;
diag_os_millisleep((unsigned int ) real_t);
errval |= !ClearCommBreak(wti->fd);
if (errval) {
//if either of the calls failed
fprintf(stderr, FLFMT "tty_break could not set/clear break!\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
return 0;
}
// diag_tty_break #1 : use Set / ClearCommBreak
// and return as soon as break is cleared.
// ret 0 if ok
int diag_tty_break(struct diag_l0_device *dl0d, const unsigned int ms) {
LARGE_INTEGER qpc1, qpc2; //for timing verification
static long correction=0; //running average offset (us) to add to the timeout
long real_t; //"real" duration measured in us
struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
int errval=0;
if (wti->fd == INVALID_HANDLE_VALUE) {
fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
if ( (ms + correction/1000)<1)
return diag_iseterr(DIAG_ERR_GENERAL);
QueryPerformanceCounter(&qpc1);
errval=!SetCommBreak(wti->fd);
diag_os_millisleep(ms + correction/1000);
QueryPerformanceCounter(&qpc2);
errval += !ClearCommBreak(wti->fd);
if (errval) {
//if either of the calls failed
fprintf(stderr, FLFMT "tty_break could not set/clear break!\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
real_t=(long) (pf_conv * (qpc2.QuadPart-qpc1.QuadPart));
//now verify if it's within 1ms of the requested delay.
real_t = real_t - (ms*1000);
if (real_t < -3000) {
diag_os_millisleep((unsigned int)(real_t / -1000));
} else if ((real_t > -1000) && (real_t < 1000)) {
//good enough:
return 0;
}
//we're here if we were off by more than -3ms or +1ms.
//correct by a fraction of the error.
//diag_os_millisleep also does some self-correcting; we don't want to overdo it.
correction = correction - (real_t / 3);
//fprintf(stderr, FLFMT "tty_break off by %ldus, new correction=%ldus.\n",
// FL, real_t, correction);
return 0;
}
/* fake run measures */
int fake_run_measure_data(int data_pid)
{
int rpm;
diag_os_millisleep(250);
rpm = 1000 + counter2 * 200;
if (counter2 < 5500/200)
counter2++;
else
counter2--;
if (data_pid == RPM_PID)
return rpm;
else if (data_pid == SPEED_PID)
return rpm * (9000*100/6000) / 36;
}
int diag_tty_break(struct diag_l0_device *dl0d, const int ms)
{
if (tcdrain(dl0d->fd)) {
fprintf(stderr, FLFMT "tcdrain returned %s.\n",
FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (ioctl(dl0d->fd, TIOCSBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCSBRK failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
diag_os_millisleep(ms);
if (ioctl(dl0d->fd, TIOCCBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCCBRK failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
return 0;
}
/*
* Send a load of data
*
* P4 is the inter byte gap
*
* This does very un-clever half duplex removal, there better not be
* any outstanding data on the bus (or in the l0 buffers) or this
* will think it has a half-duplex failure, i.e a bus error
*
* Returns 0 on success -1 on failure
*/
int
diag_l1_send(struct diag_l0_device *dl0d, const char *subinterface, const void *data, size_t len, int p4)
{
int rv = -1;
int l0flags;
const struct diag_l0 *dl0 = diag_l0_device_dl0(dl0d);
/*
* If p4 is zero and not in half duplex mode, or if
* L1 is a "DOESL2" interface send the whole message to L0
* as one write
*/
l0flags = diag_l1_getflags(dl0d);
if ( ((p4 == 0) && ((l0flags & DIAG_L1_HALFDUPLEX) == 0)) ||
(l0flags & DIAG_L1_DOESL2FRAME) || (l0flags & DIAG_L1_DOESP4WAIT) ) {
/*
* Send the lot if we don't need to delay, or collect
* the echos
*/
rv = (dl0->diag_l0_send)(dl0d, subinterface, data, len);
} else {
const uint8_t *dp = (const uint8_t *)data;
/* Send each byte */
while (len--) {
rv = (dl0->diag_l0_send)(dl0d, subinterface, dp, 1);
if (rv != 0)
break;
/*
* If half duplex, read back the echo, if
* the echo is wrong then this is an error
* i.e something wrote on the diag bus whilst
* we were writing
*/
if (l0flags & DIAG_L1_HALFDUPLEX) {
uint8_t c;
c = *dp - 1; /* set it with wrong val */
if (diag_l1_saferead(dl0d, &c, 1, 1000) < 0) {
rv=DIAG_ERR_GENERAL;
break;
}
if (c != *dp) {
if (c == *dp - 1)
fprintf(stderr,"Half duplex interface not echoing!\n");
else
fprintf(stderr,"Bus Error: got 0x%x expected 0x%x\n",
c&0xff, *dp & 0xff);
rv = DIAG_ERR_BUSERROR;
break;
}
}
dp++;
if (p4) /* Inter byte gap */
diag_os_millisleep(p4);
}
}
return rv;
}
/* fake loss measures */
int fake_loss_measure_data() {
unsigned long tv;
unsigned long elapsed; /* elapsed time */
int speed;
if (counter == 0) {
tv0d=diag_os_getms();
}
diag_os_millisleep(250);
/* get elapsed time */
tv=diag_os_getms();
elapsed = tv - tv0d;
//I think we're supposed to simulate an exponential decreasing
//function ; speed = b * e^(elapsed / a).
float a= -62810;
float b= 25027;
return (int) (b * exp(elapsed / a));
elapsed += 11000;
//XXX This was a most interesting lookup table!! Can be deleted soon.
if (elapsed < 11983) speed = 89*10000/36;
else if (elapsed < 12723) speed = 88*10000/36;
else if (elapsed < 13473) speed = 87*10000/36;
else if (elapsed < 14713) speed = 85*10000/36;
else if (elapsed < 15713) speed = 84*10000/36;
else if (elapsed < 16213) speed = 83*10000/36;
else if (elapsed < 16963) speed = 82*10000/36;
else if (elapsed < 17463) speed = 81*10000/36;
else if (elapsed < 18463) speed = 80*10000/36;
else if (elapsed < 19463) speed = 79*10000/36;
else if (elapsed < 20213) speed = 78*10000/36;
else if (elapsed < 20963) speed = 77*10000/36;
else if (elapsed < 22463) speed = 76*10000/36;
else if (elapsed < 22963) speed = 75*10000/36;
else if (elapsed < 23713) speed = 74*10000/36;
else if (elapsed < 25213) speed = 73*10000/36;
else if (elapsed < 26453) speed = 72*10000/36;
else if (elapsed < 27203) speed = 71*10000/36;
else if (elapsed < 27953) speed = 70*10000/36;
else if (elapsed < 29193) speed = 69*10000/36;
else if (elapsed < 29943) speed = 68*10000/36;
else if (elapsed < 30693) speed = 67*10000/36;
else if (elapsed < 32183) speed = 66*10000/36;
else if (elapsed < 32933) speed = 65*10000/36;
else if (elapsed < 33683) speed = 64*10000/36;
else if (elapsed < 34683) speed = 63*10000/36;
else if (elapsed < 35433) speed = 62*10000/36;
else if (elapsed < 36433) speed = 61*10000/36;
else if (elapsed < 37183) speed = 60*10000/36;
else if (elapsed < 38433) speed = 59*10000/36;
else if (elapsed < 39433) speed = 58*10000/36;
else if (elapsed < 40183) speed = 57*10000/36;
else if (elapsed < 40683) speed = 56*10000/36;
else if (elapsed < 42183) speed = 55*10000/36;
else if (elapsed < 43183) speed = 54*10000/36;
else if (elapsed < 44683) speed = 53*10000/36;
else if (elapsed < 45933) speed = 52*10000/36;
else if (elapsed < 47183) speed = 51*10000/36;
else if (elapsed < 48183) speed = 50*10000/36;
else if (elapsed < 49433) speed = 49*10000/36;
else speed = 48*10000/36;
counter++;
return speed;
}
/*
* diag_tty_fastbreak: send 0x00 at 360bps => fixed 25ms break; return [ms] after starting break.
* This is for ISO14230 fast init : typically diag_tty_fastbreak(dl0d, 50)
* It assumes the interface is half-duplex.
* Ret 0 if ok
*/
int diag_tty_fastbreak(struct diag_l0_device *dl0d, const unsigned int ms) {
HANDLE dh; //just to clarify code
struct tty_int *wti = (struct tty_int *)dl0d->tty_int;
DCB tempDCB; //for sabotaging the settings just to do the break
DCB origDCB;
LARGE_INTEGER qpc1, qpc2, qpc3; //to time the break period
LONGLONG timediff; //64bit delta
long int tremain,counts, break_error;
uint8_t cbuf;
int xferd;
DWORD byteswritten;
dh = wti->fd;
if (ms<25) //very funny
return diag_iseterr(DIAG_ERR_TIMEOUT);
if (dh == INVALID_HANDLE_VALUE) {
fprintf(stderr, FLFMT "Error. Is the port open ?\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
GetCommState(dh, &origDCB);
GetCommState(dh, &tempDCB); //ugly, but a memcpy would be worse
tempDCB.BaudRate=360;
tempDCB.ByteSize=8;
tempDCB.fParity=0;
tempDCB.Parity=NOPARITY;
tempDCB.StopBits=ONESTOPBIT;
if (! SetCommState(dh, &tempDCB)) {
fprintf(stderr, FLFMT "SetCommState error\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
/* Send a 0x00 byte message */
if (! WriteFile(dh, "\0", 1, &byteswritten, NULL)) {
fprintf(stderr, FLFMT "WriteFile error:%s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
//get approx starting time. I think this is the closest we can
//get to the actual time the byte gets sent since we call FFB
//right after.
QueryPerformanceCounter(&qpc1);
if (!FlushFileBuffers(dh)) {
fprintf(stderr, FLFMT "FFB error, %s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
/*
* And read back the single byte echo, which shows TX completes
*/
xferd = diag_tty_read(dl0d, &cbuf, 1, ms + 20);
//we'll usually have a few ms left to wait; we'll use this
//to restore the port settings
if (! SetCommState(dh, &origDCB)) {
fprintf(stderr, FLFMT "tty_fastbreak: could not restore setting: %s\n", FL, diag_os_geterr(0));
return diag_iseterr(DIAG_ERR_GENERAL);
}
//Not getting the echo byte doesn't mean fastbreak has necessarily
// failed. But we really should be getting an echo back...
if (xferd < 0)
return diag_iseterr(xferd);
if ((xferd == 0) || (cbuf != 0)) {
/* Error, EOF or bad echo */
fprintf(stderr, FLFMT "Did not get fastbreak echo!\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
QueryPerformanceCounter(&qpc2); //get current time,
timediff=qpc2.QuadPart-qpc1.QuadPart; //elapsed counts since diag_tty_write
counts=(ms*perfo_freq.QuadPart)/1000; //total # of counts for requested tWUP
tremain=counts-timediff; //counts remaining
if (tremain<=0)
return 0;
tremain = ((LONGLONG) tremain*1000)/perfo_freq.QuadPart; //convert to ms; imprecise but that should be OK.
diag_os_millisleep((unsigned int) tremain);
QueryPerformanceCounter(&qpc3);
timediff=qpc3.QuadPart-qpc1.QuadPart; //total cycle time.
break_error= (long) timediff - counts; //real - requested
break_error= (long) (break_error * pf_conv); //convert to us !
if (break_error > 1000 || break_error < -1000)
fprintf(stderr, FLFMT "tty_fastbreak: tWUP out of spec by %ldus!\n", FL, break_error);
return 0;
} //diag_tty_fastbreak
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;
}
ssize_t
diag_tty_slow_write(struct diag_l0_device *dl0d,
const void *buf, const size_t count)
{
// int ms = 950 * dl0d->ttystate->dt_sinfo.custom_divisor/24000000;
#if 1
/* for rtc interrupt: */
int ms = 204;
#else
/* for nanosleep interrupt (try up to 191 / 192): */
int ms = 191;
#endif
short bits, parBit, stpBit, i, high=1, parity;
size_t num;
char message_buff[3];
if((dl0d->ttystate->dt_tinfo.c_cflag & CS8) == CS8) {
bits = diag_databits_8;
sprintf(&message_buff[0], "%c", '8');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & CS8) == CS7) {
bits = diag_databits_7;
sprintf(&message_buff[0], "%c", '7');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & CS8) == CS6) {
bits = diag_databits_6;
sprintf(&message_buff[0], "%c", '6');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & CS8) == CS5) {
bits = diag_databits_5;
sprintf(&message_buff[0], "%c", '5');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & PARENB) == PARENB) {
parBit = diag_par_e;
sprintf(&message_buff[1], "%c", 'E');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & (PARENB | PARODD)) == (PARENB | PARODD)) {
parBit = diag_par_o;
sprintf(&message_buff[1], "%c", 'O');
}
if(!(dl0d->ttystate->dt_tinfo.c_cflag & PARENB)) {
parBit = diag_par_n;
sprintf(&message_buff[1], "%c", 'N');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & CSTOPB) == CSTOPB) {
stpBit = diag_stopbits_2;
sprintf(&message_buff[2], "%c", '2');
}
if((dl0d->ttystate->dt_tinfo.c_cflag & ~CSTOPB) != CSTOPB) {
stpBit = diag_stopbits_1;
sprintf(&message_buff[2], "%c", '1');
}
// start bit
if (ioctl(dl0d->fd, TIOCSBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCSBRK failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high = 0;
printf("Init bits: 0");
diag_os_millisleep(ms);
for(num=0; num < count; num++) {
parity = 1;
for(i=0; i< bits; i++) {
if(*((char *) (buf+num)) & (1<<i)) {
parity *= -1;
if(!high) {
if (ioctl(dl0d->fd, TIOCCBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCCBRK failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=1;
}
} else {
if(high) {
if (ioctl(dl0d->fd, TIOCSBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCSBRK failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=0;
}
}
if(high)
printf("1");
else
printf("0");
diag_os_millisleep(ms);
}
// parity bit
if(parBit == diag_par_e) {
if (parity < 0) {
if(!high) {
if (ioctl(dl0d->fd, TIOCCBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCCBRK2 failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=1;
}
} else {
if(high) {
if (ioctl(dl0d->fd, TIOCSBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCSBRK2 failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=0;
}
}
if(high)
printf("1");
else
printf("0");
diag_os_millisleep(ms);
}
if(parBit == diag_par_o) {
if (parity > 0) {
if(!high) {
if (ioctl(dl0d->fd, TIOCCBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCCBRK3 failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=1;
}
} else {
if(high) {
if (ioctl(dl0d->fd, TIOCSBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCSBRK3 failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=0;
}
}
if(high)
printf("1");
else
printf("0");
diag_os_millisleep(ms);
}
// stop bit(s)
if(!high) {
if (ioctl(dl0d->fd, TIOCCBRK, 0) < 0) {
fprintf(stderr,
FLFMT "open: Ioctl TIOCCBRK2 failed %s\n", FL, strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
high=1;
}
if (stpBit == diag_stopbits_1) {
printf("1");
diag_os_millisleep(ms);
} else {
diag_os_millisleep(2*ms);
printf("11");
}
}
printf(", which is ");
for(num=0; num < count; num++)
printf("<%d>", *(char *) (buf+num));
printf(", %.*s at 5 bps\n", 3, (char *) &message_buff);
return count;
}
/*
* diag_tty_break
*/
int diag_tty_break(struct diag_l0_device *dl0d, const int ms)
{
char cbuf;
struct timeval tv;
int xferd;
struct diag_serial_settings set;
/*
* We need to send an 25ms (+/- 1ms) break
* and then wait 25ms. We must have waited Tidle (300ms) first
*
* The trouble here is Linux doesn't let us be that accurate sending
* a break - so we do it by sending a '0x00' at a baud rate that means
* the output is low for 25ms, then we wait for 25ms, using busy
* waits (which we get from being in real-time mode and doing nanosleeps
* of less than 2ms each)
*
*/
/* Set baud rate etc to 360 baud, 8, N, 1 */
set.speed = 360;
set.databits = diag_databits_8;
set.stopbits = diag_stopbits_1;
set.parflag = diag_par_n;
diag_tty_setup(dl0d, &set);
gettimeofday(&tv,NULL);
/* Send a 0x00 byte message */
diag_tty_write(dl0d, "", 1);
if (diag_l0_debug & DIAG_DEBUG_TIMER) {
fprintf(stderr, FLFMT "%04ld.%03ld : break start\n", FL, tv.tv_sec, tv.tv_usec);
}
/*
* And read back the single byte echo, which shows TX completes
*/
while ( (xferd = diag_tty_read(dl0d, &cbuf, 1, 1000)) <= 0)
{
if (xferd == DIAG_ERR_TIMEOUT)
return diag_iseterr(DIAG_ERR_TIMEOUT);
if (xferd == 0)
{
/* Error, EOF */
fprintf(stderr, FLFMT "read returned EOF.\n", FL);
return diag_iseterr(DIAG_ERR_GENERAL);
}
if (errno != EINTR)
{
/* Error, EOF */
fprintf(stderr, FLFMT "read returned error %s.\n", FL,
strerror(errno));
return diag_iseterr(DIAG_ERR_GENERAL);
}
}
/* Now wait the requested number of ms */
gettimeofday(&tv,NULL);
diag_os_millisleep(ms);
if (diag_l0_debug & DIAG_DEBUG_TIMER) {
fprintf(stderr, FLFMT "%04ld.%03ld : end of break_L\n", FL, tv.tv_sec, tv.tv_usec);
}
return 0;
}
/*
* Send a load of data
*
* P4 is the inter byte gap
*
* This does very un-clever half duplex removal, there better not be
* any outstanding data on the bus (or in the l0 buffers) or this
* will think it has a half-duplex failure, i.e a bus error
*
* Returns 0 on success
*/
int
diag_l1_send(struct diag_l0_device *dl0d, const char *subinterface, const void *data, size_t len, unsigned int p4) {
int rv = DIAG_ERR_GENERAL;
uint32_t l0flags;
uint8_t duplexbuf[MAXRBUF];
if (len > MAXRBUF) {
return diag_iseterr(DIAG_ERR_BADLEN);
}
l0flags = diag_l1_getflags(dl0d);
DIAG_DBGMDATA(diag_l1_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V, data, len,
FLFMT "_send: len=%d P4=%u l0flags=0x%X; ",
FL, (int) len, p4, l0flags);
/*
* If p4 is zero and not in half duplex mode, or if
* L1 is a "DOESL2" interface, or if L0 takes care of P4 waits,
* or if P4==0 and we do per-message duplex removal:
* send the whole message to L0 as one write
*/
if ( ((p4 == 0) && ((l0flags & DIAG_L1_HALFDUPLEX) == 0)) ||
(l0flags & DIAG_L1_DOESL2FRAME) || (l0flags & DIAG_L1_DOESP4WAIT) ||
((p4==0) && (l0flags & DIAG_L1_BLOCKDUPLEX)) ) {
/*
* Send the lot
*/
rv = diag_l0_send(dl0d, subinterface, data, len);
//optionally remove echos
if ((l0flags & DIAG_L1_BLOCKDUPLEX) && (rv==0)) {
//try to read the same number of sent bytes; timeout=300ms + 1ms/byte
//This is plenty OK for typical 10.4kbps but should be changed
//if ever slow speeds are used.
if (diag_l0_recv(dl0d, NULL, duplexbuf, len, 300+len) != (int) len) {
rv=DIAG_ERR_GENERAL;
}
//compare to sent bytes
if ( memcmp(duplexbuf, data, len) !=0) {
fprintf(stderr,FLFMT "Bus Error: bad half duplex echo!\n", FL);
rv=DIAG_ERR_BUSERROR;
}
}
} else {
/* else: send each byte */
const uint8_t *dp = (const uint8_t *)data;
while (len--) {
rv = diag_l0_send(dl0d, subinterface, dp, 1);
if (rv != 0) {
break;
}
/*
* If half duplex, read back the echo, if
* the echo is wrong then this is an error
* i.e something wrote on the diag bus whilst
* we were writing
*/
if (l0flags & DIAG_L1_HALFDUPLEX) {
uint8_t c;
c = *dp - 1; /* set it with wrong val. */
if (diag_l0_recv(dl0d, NULL, &c, 1, 200) != 1) {
rv=DIAG_ERR_GENERAL;
break;
}
if (c != *dp) {
if (c == *dp - 1) {
fprintf(stderr,"Half duplex interface not echoing!\n");
} else {
fprintf(stderr,
"Bus Error: got 0x%X "
"expected 0x%X\n",
c, *dp);
}
rv = DIAG_ERR_BUSERROR;
break;
}
}
dp++;
if (p4) { /* Inter byte gap */
diag_os_millisleep(p4);
}
}
}
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;
}
/*
* 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;
}
/* fake loss measures */
int fake_loss_measure_data()
{
struct timeval tv; /* measuring time */
int elapsed; /* elapsed time */
int speed;
if (counter == 0)
{
gettimeofday(&tv0, 0);
}
diag_os_millisleep(250);
/* get elapsed time */
gettimeofday(&tv, 0);
elapsed = MILLIS(tv) - MILLIS(tv0);
elapsed += 11000;
if (elapsed < 11983) speed = 89*10000/36;
else if (elapsed < 12723) speed = 88*10000/36;
else if (elapsed < 13473) speed = 87*10000/36;
else if (elapsed < 14713) speed = 85*10000/36;
else if (elapsed < 15713) speed = 84*10000/36;
else if (elapsed < 16213) speed = 83*10000/36;
else if (elapsed < 16963) speed = 82*10000/36;
else if (elapsed < 17463) speed = 81*10000/36;
else if (elapsed < 18463) speed = 80*10000/36;
else if (elapsed < 19463) speed = 79*10000/36;
else if (elapsed < 20213) speed = 78*10000/36;
else if (elapsed < 20963) speed = 77*10000/36;
else if (elapsed < 22463) speed = 76*10000/36;
else if (elapsed < 22963) speed = 75*10000/36;
else if (elapsed < 23713) speed = 74*10000/36;
else if (elapsed < 25213) speed = 73*10000/36;
else if (elapsed < 26453) speed = 72*10000/36;
else if (elapsed < 27203) speed = 71*10000/36;
else if (elapsed < 27953) speed = 70*10000/36;
else if (elapsed < 29193) speed = 69*10000/36;
else if (elapsed < 29943) speed = 68*10000/36;
else if (elapsed < 30693) speed = 67*10000/36;
else if (elapsed < 32183) speed = 66*10000/36;
else if (elapsed < 32933) speed = 65*10000/36;
else if (elapsed < 33683) speed = 64*10000/36;
else if (elapsed < 34683) speed = 63*10000/36;
else if (elapsed < 35433) speed = 62*10000/36;
else if (elapsed < 36433) speed = 61*10000/36;
else if (elapsed < 37183) speed = 60*10000/36;
else if (elapsed < 38433) speed = 59*10000/36;
else if (elapsed < 39433) speed = 58*10000/36;
else if (elapsed < 40183) speed = 57*10000/36;
else if (elapsed < 40683) speed = 56*10000/36;
else if (elapsed < 42183) speed = 55*10000/36;
else if (elapsed < 43183) speed = 54*10000/36;
else if (elapsed < 44683) speed = 53*10000/36;
else if (elapsed < 45933) speed = 52*10000/36;
else if (elapsed < 47183) speed = 51*10000/36;
else if (elapsed < 48183) speed = 50*10000/36;
else if (elapsed < 49433) speed = 49*10000/36;
else speed = 48*10000/36;
counter++;
return (speed);
}
