BOROSOFTWAREAll work →
← scantool/diag_l2_iso14230.c 1199 lines
3 findings in this fileD10L163A13L400C1L768
D10D10A13D10C1C1
1/*
2 * freediag - Vehicle Diagnostic Utility
3 *
4 *
5 * Copyright (C) 2001 Richard Almeida & Ibex Ltd ([email protected])
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 *
12 * This program is distributed in the hope that it will be useful,
13 * but WITHOUT ANY WARRANTY; without even the implied warranty of
14 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 * GNU General Public License for more details.
16 *
17 * You should have received a copy of the GNU General Public License
18 * along with this program; if not, write to the Free Software
19 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
20 *
21 *************************************************************************
22 *
23 * Diag
24 *
25 * L2 driver for ISO14230-2 layer 2
26 *
27 */
28
29#include <stdio.h>
30#include <stdlib.h>
31#include <string.h>
32
33#include "diag.h"
34#include "diag_os.h"
35#include "diag_tty.h"
36#include "diag_l0.h"
37#include "diag_l1.h"
38#include "diag_l2.h"
39#include "diag_err.h"
40#include "diag_iso14230.h"
41
42#include "diag_l2_iso14230.h" /* prototypes for this file */
43
44
45
46/*
47 * Useful internal routines
48 */
49
50/*
51 * Decode the message header, returning the expected length
52 * of the message (hdr+data+checksum) if a complete header was received.
53 * Note that this may be called with more than one message
54 * but it only worries about the first message
55 * only proto_14230_intrecv should use this...
56 */
57static int dl2p_14230_decode(uint8_t *data, int len,
58 uint8_t *hdrlen, int *datalen, uint8_t *source, uint8_t *dest,
59 int first_frame) {
60 uint8_t dl;
61
62 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
63 data, (size_t) len,
64 FLFMT "decode len %d, ", FL, len);
65
66 dl = data[0] & 0x3f;
67 if (dl == 0) {
68 /* Additional length field present */
69 switch (data[0] & 0xC0) {
70 case 0x80:
71 case 0xC0:
72 /* Addresses supplied, additional len byte */
73 if (len < 4) {
74 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
75 FLFMT "decode len short \n", FL);
76 return diag_iseterr(DIAG_ERR_INCDATA);
77 }
78 *hdrlen = 4;
79 *datalen = data[3];
80 if (dest) {
81 *dest = data[1];
82 }
83 if (source) {
84 *source = data[2];
85 }
86 break;
87 case 0x00:
88 /* Addresses not supplied, additional len byte */
89 if (first_frame) {
90 return diag_iseterr(DIAG_ERR_BADDATA);
91 }
92 if (len < 2) {
93 return diag_iseterr(DIAG_ERR_INCDATA);
94 }
95 *hdrlen = 2;
96 *datalen = data[1];
97 if (dest) {
98 *dest = 0;
99 }
100 if (source) {
101 *source = 0;
102 }
103 break;
104 case 0X40:
105 /* CARB MODE */
106 // not part of 14230 (4.2.1) so we flag this.
107 default:
108 return diag_iseterr(DIAG_ERR_BADDATA);
109 break;
110 }
111 } else {
112 /* Additional length field not present */
113 switch (data[0] & 0xC0) {
114 case 0x80:
115 case 0xC0:
116 /* Addresses supplied, NO additional len byte */
117 if (len < 3) {
118 return diag_iseterr(DIAG_ERR_INCDATA);
119 }
120 *hdrlen = 3;
121 *datalen = dl;
122 if (dest) {
123 *dest = data[1];
124 }
125 if (source) {
126 *source = data[2];
127 }
128 break;
129 case 0x00:
130 /* Addresses not supplied, No additional len byte */
131 if (first_frame) {
132 return diag_iseterr(DIAG_ERR_BADDATA);
133 }
134 *hdrlen = 1;
135 *datalen = dl;
136 if (dest) {
137 *dest = 0;
138 }
139 if (source) {
140 *source = 0;
141 }
142 break;
143 case 0X40:
144 /* CARB MODE */
145 default:
146 return diag_iseterr(DIAG_ERR_BADDATA);
147 break;
148 }
149 }
150 /*
151 * If len is silly [i.e 0] we've got this mid stream
152 */
153 if (*datalen == 0) {
154 return diag_iseterr(DIAG_ERR_BADDATA);
155 }
156
157 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
158 FLFMT "decode hdrlen=%d, datalen=%d, cksum=1\n",
159 FL, *hdrlen, *datalen);
160
161 // return "theoretical" frame length, including headers + data + checksum byte. We
162 // don't complain if the actual len is shorter because sometimes the cksum will be stripped.
163 return (*hdrlen + *datalen + 1);
164
165 /*
166 * And confirm data is long enough, incl cksum
167 * If not, return saying data is incomplete so far
168 */
169 if (len < (*hdrlen + *datalen + 1)) {
170 return diag_iseterr(DIAG_ERR_INCDATA);
171 }
172
173 return (*hdrlen + *datalen + 1);
174}
175
176/*
177 * Internal receive function: does all the message building, but doesn't
178 * do call back. Strips header and checksum; if address info was present
179 * then msg->dest and msg->src are !=0.
180 *
181 * Data from the first message is put into *data, and len into *datalen if
182 * those pointers are non-null.
183 *
184 * If the L1 interface is clever (DOESL2FRAME), then each read will give
185 * us a complete message, and we will wait a little bit longer than the normal
186 * timeout to detect "end of all responses"
187 *
188 * Similar to 9141_int_recv; timeout has to be long enough to catch at least
189 * 1 byte.
190 * XXX this implementation doesn't accurately detect end-of-responses, because
191 * it's trying to read MAXRBUF (a large number) of bytes, for every state.
192 * if there is a short (say 3 byte) response from the ECU while in state 1,
193 * the timer will expire because it's waiting for MAXBUF
194 * bytes (MAXRBUF is much larger than any message, by design). Then, even
195 * though there are no more responses, we still do another MAXRBUF read
196 * in state 2 for P2min, and a last P2max read in state 3 !
197 * TODO: change state1 to read 1 byte maybe ?
198 * I think a more rigorous way to do this (if L1 doesn't do L2 framing), would
199 * be to loop, reading 1 byte at a time, with timeout=P1max or p2min to split
200 * messages, and timeout=P2max to detect the last byte of a response... this
201 * means calling diag_l1_recv a whole lot more often however.
202
203 */
204static int dl2p_14230_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout) {
205 struct diag_l2_14230 *dp;
206 int rv, l1_doesl2frame, l1flags;
207 unsigned int tout;
208 int state;
209 struct diag_msg *tmsg, *lastmsg;
210
211#define ST_STATE1 1 /* Start */
212#define ST_STATE2 2 /* Interbyte */
213#define ST_STATE3 3 /* Inter message */
214
215 dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
216
217 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
218 FLFMT "_int_recv dl2conn=%p offset=0x%X, tout=%u\n",
219 FL, (void *)d_l2_conn, dp->rxoffset, timeout);
220
221 state = ST_STATE1;
222 tout = timeout;
223
224 /* Clear out last received messages if not done already */
225 if (d_l2_conn->diag_msg) {
226 diag_freemsg(d_l2_conn->diag_msg);
227 d_l2_conn->diag_msg = NULL;
228 }
229
230 l1flags = d_l2_conn->diag_link->l1flags;
231
232 if (l1flags & DIAG_L1_DOESL2FRAME) {
233 l1_doesl2frame = 1;
234 } else {
235 l1_doesl2frame = 0;
236 }
237
238 if (l1_doesl2frame) {
239 if (timeout < SMART_TIMEOUT) { /* Extend timeouts */
240 timeout += 100;
241 }
242 }
243
244
245 while (1) {
246 switch (state) {
247 case ST_STATE1:
248 tout = timeout;
249 break;
250 case ST_STATE2:
251 //State 2 : if we're between bytes of the same message; if we timeout with P2min it's
252 //probably because the message is ended.
253 tout = d_l2_conn->diag_l2_p2min - 2;
254 if (tout < d_l2_conn->diag_l2_p1max) {
255 tout = d_l2_conn->diag_l2_p1max;
256 }
257 break;
258 case ST_STATE3:
259 //State 3: we timed out during state 2
260 if (l1_doesl2frame) {
261 tout = 150; /* Arbitrary, short, value ... */
262 } else {
263 tout = d_l2_conn->diag_l2_p2max;
264 }
265 }
266
267 /* Receive data into the buffer */
268
269 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
270 FLFMT "before recv, state=%d timeout=%u, rxoffset %d\n",
271 FL, state, tout, dp->rxoffset);
272
273 /*
274 * In l1_doesl2frame mode, we get full frames, so we don't
275 * do the read in state2
276 */
277 if ((state == ST_STATE2) && l1_doesl2frame) {
278 rv = DIAG_ERR_TIMEOUT;
279 } else {
280 rv = diag_l1_recv(d_l2_conn->diag_link->l2_dl0d,
281 &dp->rxbuf[dp->rxoffset],
282 sizeof(dp->rxbuf) - dp->rxoffset,
283 tout);
284 }
285
286 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
287 FLFMT "after recv, rv=%d rxoffset=%d\n",
288 FL, rv, dp->rxoffset);
289
290 if (rv == DIAG_ERR_TIMEOUT) {
291 /* Timeout, end of message, or end of responses */
292 switch (state) {
293 case ST_STATE1:
294 /*
295 * 1st read, if we got 0 bytes, just return
296 * the timeout error
297 */
298 if (dp->rxoffset == 0) {
299 break;
300 }
301 /*
302 * Otherwise see if there are more bytes in
303 * this message,
304 */
305 state = ST_STATE2;
306 continue;
307 case ST_STATE2:
308 /*
309 * End of that message, maybe more to come
310 * Copy data into a message
311 */
312 tmsg = diag_allocmsg((size_t)dp->rxoffset);
313 if (tmsg == NULL) {
314 return diag_iseterr(DIAG_ERR_NOMEM);
315 }
316 memcpy(tmsg->data, dp->rxbuf, (size_t)dp->rxoffset);
317 tmsg->rxtime = diag_os_getms();
318 dp->rxoffset = 0;
319 /*
320 * ADD message to list
321 */
322 diag_l2_addmsg(d_l2_conn, tmsg);
323 if (d_l2_conn->diag_msg == tmsg) {
324 DIAG_DBGMDATA(diag_l2_debug, (DIAG_DEBUG_PROTO | DIAG_DEBUG_DATA),
325 DIAG_DBGLEVEL_V, tmsg->data, tmsg->len,
326 FLFMT "Copying %u bytes to data: ", FL, tmsg->len);
327 }
328 state = ST_STATE3;
329 continue;
330 case ST_STATE3:
331 /*
332 * No more messages, but we did get one
333 */
334 rv = d_l2_conn->diag_msg->len;
335 break;
336 }
337 if (state == ST_STATE3) {
338 break;
339 }
340 } //if diag_err_timeout
341
342 if (rv <= 0) {
343 break;
344 }
345
346 /* Data received OK */
347 dp->rxoffset += rv;
348
349 /* This was wrong : some valid 14230 frames can start with 0x00, and
350 * would have their first byte deleted systematically.
351 * Note, this is still wrong (monitor mode will still drop some valid frames),
352 * but arguably less so.
353 */
354 if (dp->monitor_mode &&
355 (dp->rxoffset && (dp->rxbuf[0] == 0))) {
356 /*
357 * We get this when in
358 * monitor mode and there is
359 * a fastinit, pretend it didn't exist
360 */
361 dp->rxoffset--;
362 if (dp->rxoffset) {
363 memcpy(&dp->rxbuf[0], &dp->rxbuf[1],
364 (size_t)dp->rxoffset);
365 }
366 continue;
367 }
368 if ( (state == ST_STATE1) || (state == ST_STATE3) ) {
369 /*
370 * Got some data in state1/3, now we're in a message
371 */
372 state = ST_STATE2;
373 }
374 }
375
376 /*
377 * Now check the messages that we have checksum etc, stripping
378 * off headers etc
379 */
380 if (rv < 0) {
381 return rv;
382 }
383
384 tmsg = d_l2_conn->diag_msg;
385 lastmsg = NULL;
386
387 while (tmsg != NULL) {
388 int datalen=0;
389 uint8_t hdrlen=0, source=0, dest=0;
390
391
392 if ((l1flags & DIAG_L1_NOHDRS)==0) {
393
394 dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
395 rv = dl2p_14230_decode( tmsg->data,
396 tmsg->len,
397 &hdrlen, &datalen, &source, &dest,
398 dp->first_frame);
399
400 if (rv <= 0 || rv > 260) { /* decode failure */
401 return diag_iseterr(DIAG_ERR_BADDATA);
402 }
403
404 // check for sufficient data: (rv = expected len = hdrlen + datalen + ckslen)
405 if (l1_doesl2frame == 0) {
406 unsigned expected_len = rv;
407 if (l1flags & DIAG_L1_STRIPSL2CKSUM) {
408 expected_len -= 1;
409 }
410 if (expected_len > tmsg->len) {
411 return diag_iseterr(DIAG_ERR_INCDATA);
412 }
413 }
414 }
415
416
417
418 /*
419 * If L1 isnt doing L2 framing then it is possible
420 * we have misframed this message and it is infact
421 * more than one message, so see if we can decode it
422 */
423 if ((l1_doesl2frame == 0) && (rv < (int) tmsg->len)) {
424 /*
425 * This message contains more than one
426 * data frame (because it arrived with
427 * odd timing), this means we have to
428 * do horrible copy about the data
429 * things ....
430 */
431 struct diag_msg *amsg;
432 amsg = diag_dupsinglemsg(tmsg);
433 if (amsg == NULL) {
434 return diag_iseterr(DIAG_ERR_NOMEM);
435 }
436 amsg->len = (uint8_t) rv;
437 tmsg->len -= (uint8_t) rv;
438 tmsg->data += rv;
439
440 /* Insert new amsg before old msg */
441 amsg->next = tmsg;
442 if (lastmsg == NULL) {
443 d_l2_conn->diag_msg = amsg;
444 } else {
445 lastmsg->next = amsg;
446 }
447
448 tmsg = amsg; /* Finish processing this one */
449 }
450
451 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
452 FLFMT
453 "msg %p decode/rejig done rv=%d hdrlen=%u "
454 "datalen=%d src=%02X dst=%02X\n",
455 FL, (void *)tmsg, rv, hdrlen, datalen, source, dest);
456
457 tmsg->fmt = DIAG_FMT_FRAMED;
458
459 if ((l1flags & DIAG_L1_NOHDRS)==0) {
460 if ((tmsg->data[0] & 0xC0) == 0xC0) {
461 tmsg->fmt |= DIAG_FMT_ISO_FUNCADDR;
462 }
463 }
464
465
466 //if cs wasn't stripped, we check it:
467 if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0) {
468 uint8_t calc_sum=diag_cks1(tmsg->data, (tmsg->len)-1);
469 if (calc_sum != tmsg->data[tmsg->len -1]) {
470 fprintf(stderr, FLFMT "Bad checksum: needed %02X,got%02X. Data:",
471 FL, calc_sum, tmsg->data[tmsg->len -1]);
472 tmsg->fmt |= DIAG_FMT_BADCS;
473 diag_data_dump(stderr, tmsg->data, tmsg->len -1);
474 fprintf(stderr, "\n");
475 }
476 /* and remove checksum byte */
477 tmsg->len--;
478
479 }
480 tmsg->fmt |= DIAG_FMT_CKSUMMED; //checksum was verified
481
482 tmsg->src = source;
483 tmsg->dest = dest;
484 tmsg->data += hdrlen; /* Skip past header */
485 tmsg->len -= hdrlen; /* remove header */
486
487
488
489 dp->first_frame = 0;
490
491 lastmsg = tmsg;
492 tmsg = tmsg->next;
493 }
494 return rv;
495}
496
497
498/* External interface */
499
500static int dl2p_14230_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg);
501/*
502 * The complex initialisation routine for ISO14230, which supports
503 * 2 types of initialisation (5-BAUD, FAST) and functional
504 * and physical addressing. The ISO14230 spec describes CARB initialisation
505 * which is done in the ISO9141 code
506 *
507 * Remember, we have to wait longer on smart L1 interfaces.
508 */
509static int dl2p_14230_startcomms( struct diag_l2_conn *d_l2_conn, flag_type flags,
510 unsigned int bitrate, target_type target, source_type source) {
511 struct diag_l2_14230 *dp;
512 struct diag_msg msg = {0};
513 uint8_t data[MAXRBUF];
514 int rv;
515 unsigned int wait_time;
516 uint8_t cbuf[MAXRBUF];
517 unsigned int timeout;
518 struct diag_serial_settings set;
519
520 struct diag_l1_initbus_args in;
521
522 rv = diag_calloc(&dp, 1);
523 if (rv != 0) {
524 return diag_ifwderr(rv);
525 }
526
527 d_l2_conn->diag_l2_proto_data = (void *)dp;
528 dp->initype = flags & DIAG_L2_TYPE_INITMASK; //only keep initmode flags
529
530 dp->srcaddr = source;
531 dp->dstaddr = target;
532 //set iso14230-specific flags according to what we were given
533 if (flags & DIAG_L2_IDLE_J1978) {
534 dp->modeflags |= ISO14230_IDLE_J1978;
535 }
536 if (flags & DIAG_L2_TYPE_FUNCADDR) {
537 dp->modeflags |= ISO14230_FUNCADDR;
538 }
539
540 dp->first_frame = 0;
541 dp->monitor_mode = 0;
542
543 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
544 FLFMT "_startcomms flags=0x%X tgt=0x%X src=0x%X\n", FL,
545 flags, target, source);
546
547 memset(data, 0, sizeof(data));
548
549 /*
550 * If 0 has been specified, use the correct speed
551 * for ISO14230 protocol
552 */
553 if (bitrate == 0) {
554 bitrate = 10400;
555 }
556 d_l2_conn->diag_l2_speed = bitrate;
557
558 set.speed = bitrate;
559 set.databits = diag_databits_8;
560 set.stopbits = diag_stopbits_1;
561 set.parflag = diag_par_n;
562
563 /* Set the speed*/
564 if ((rv=diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, (void *) &set))) {
565 free(dp);
566 d_l2_conn->diag_l2_proto_data=NULL; //delete pointer to dp
567 return diag_ifwderr(rv);
568 }
569
570 dp->state = STATE_CONNECTING;
571
572 /* Flush unread input, then wait for idle bus. */
573 (void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
574 diag_os_millisleep(300);
575
576 //inside this switch, we set rv=0 or rv=error before "break;"
577 switch (dp->initype & DIAG_L2_TYPE_INITMASK) {
578 /* Fast initialisation */
579 case DIAG_L2_TYPE_FASTINIT:
580
581 /* Build an ISO14230 StartComms message */
582 if (flags & DIAG_L2_TYPE_FUNCADDR) {
583 msg.fmt = DIAG_FMT_ISO_FUNCADDR;
584 d_l2_conn->diag_l2_physaddr = 0; /* Don't know it yet */
585 in.physaddr = 0;
586 } else {
587 msg.fmt = 0;
588 d_l2_conn->diag_l2_physaddr = target;
589 in.physaddr = 1;
590 }
591 msg.src = source;
592 msg.dest = target;
593 msg.len = 1;
594 data[0]= DIAG_KW2K_SI_SCR; /* startCommunication rqst*/
595 msg.data = data;
596
597 /* Do fast init stuff */
598 in.type = DIAG_L1_INITBUS_FAST;
599 in.addr = target;
600 in.testerid = source;
601
602 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
603 if (rv < 0) {
604 break;
605 }
606
607 // some L0 devices already do the full startcomm transaction:
608 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) {
609 //TODO : somehow extract keybyte data for those cases...
610 //original elm327s have the "atkw" command to get the keybytes, but clones suck.
611 dp->state = STATE_ESTABLISHED;
612 break;
613 }
614
615 /* Send the prepared message */
616 if (dl2p_14230_send(d_l2_conn, &msg)) {
617 rv=DIAG_ERR_GENERAL;
618 break;
619 }
620
621 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2FRAME) {
622 timeout = 200;
623 } else {
624 timeout = d_l2_conn->diag_l2_p2max + RXTOFFSET;
625 }
626
627 /* And wait for a response, ISO14230 says will arrive in P2 */
628 rv = dl2p_14230_int_recv(d_l2_conn, timeout);
629 if (rv < 0) {
630 break;
631 }
632
633 // _int_recv() should have filled d_l2_conn->diag_msg properly.
634 // check if message is OK :
635 if (d_l2_conn->diag_msg->fmt & DIAG_FMT_BADCS) {
636 rv=DIAG_ERR_BADCSUM;
637 break;
638 }
639
640 switch (d_l2_conn->diag_msg->data[0]) {
641 case DIAG_KW2K_RC_SCRPR: /* StartComms positive response */
642
643 d_l2_conn->diag_l2_kb1 = d_l2_conn->diag_msg->data[1];
644 d_l2_conn->diag_l2_kb2 = d_l2_conn->diag_msg->data[2];
645 d_l2_conn->diag_l2_physaddr = d_l2_conn->diag_msg->src;
646
647 if (d_l2_conn->diag_l2_kb2 != 0x8f) {
648 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
649 "Warning : non-standard KB2 received (0x%02X) !\n", d_l2_conn->diag_l2_kb2);
650 }
651
652 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
653 FLFMT "_StartComms Physaddr=0x%X KB1=%02X, KB2=%02X\n",
654 FL, d_l2_conn->diag_l2_physaddr, d_l2_conn->diag_l2_kb1, d_l2_conn->diag_l2_kb2);
655 rv=0;
656 dp->state = STATE_ESTABLISHED;
657 break;
658 case DIAG_KW2K_RC_NR:
659 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
660 FLFMT "_StartComms got neg response\n", FL);
661 rv=DIAG_ERR_ECUSAIDNO;
662 break;
663 default:
664 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
665 FLFMT "_StartComms got unexpected response 0x%X\n",
666 FL, d_l2_conn->diag_msg->data[0]);
667
668 rv=DIAG_ERR_ECUSAIDNO;
669 break;
670 } //switch data[0]
671 // finished with the response message:
672 diag_freemsg(d_l2_conn->diag_msg);
673 d_l2_conn->diag_msg = NULL;
674 break; //case _FASTINIT
675
676 /* 5 Baud init */
677 case DIAG_L2_TYPE_SLOWINIT:
678 in.type = DIAG_L1_INITBUS_5BAUD;
679 in.addr = target;
680 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
681
682 //some L0 devices handle the full init transaction:
683 if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) && (rv==0)) {
684 dp->state = STATE_ESTABLISHED;
685 break;
686 }
687
688 if (rv < 0) {
689 break;
690 }
691
692 /* Mode bytes are in 7-Odd-1, read as 8N1 and ignore parity */
693 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
694 cbuf, 2, 100);
695 if (rv < 0) {
696 break;
697 }
698
699 /* ISO14230 uses KB2 of 0x8F */
700 if (cbuf[1] != 0x8f) {
701 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
702 "Warning : non-standard KB1 received (0x%02X) !\n", d_l2_conn->diag_l2_kb1);
703 }
704
705 /* Note down the mode bytes */
706 // KB1 : we eliminate the Parity bit (MSB !)
707 d_l2_conn->diag_l2_kb1 = cbuf[0] & 0x7f;
708 d_l2_conn->diag_l2_kb2 = cbuf[1];
709
710 if ( (d_l2_conn->diag_link->l1flags
711 & DIAG_L1_DOESSLOWINIT) == 0) {
712
713 /*
714 * Now transmit KB2 inverted
715 */
716 cbuf[0] = ~d_l2_conn->diag_l2_kb2;
717 rv = diag_l1_send(d_l2_conn->diag_link->l2_dl0d,
718 cbuf, 1, d_l2_conn->diag_l2_p4min);
719 if (rv) {
720 break;
721 }
722
723 /*
724 * And wait for the address byte inverted
725 */
726 //first init cbuf[0] to the wrong value in case l1_recv gets nothing
727 cbuf[0]= (uint8_t) target;
728 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
729 cbuf, 1, 350);
730 if (rv < 0) {
731 break;
732 }
733
734 if (cbuf[0] != ((~target) & 0xFF) ) {
735 fprintf(stderr, FLFMT "_startcomms : addr mismatch %02X!=%02X\n",
736 FL, cbuf[0], (uint8_t) ~target);
737 rv=DIAG_ERR_WRONGKB;
738 break;
739 }
740
741 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
742 FLFMT "ISO14230 KB1=%02X KB2=%02X\n",
743 FL, d_l2_conn->diag_l2_kb1, d_l2_conn->diag_l2_kb2);
744 }
745 rv=0;
746 dp->state = STATE_ESTABLISHED;
747 break; //case _SLOWINIT
748 case DIAG_L2_TYPE_MONINIT:
749 /* Monitor mode, don't send anything */
750 dp->first_frame = 1;
751 dp->monitor_mode = 1;
752 dp->state = STATE_ESTABLISHED;
753 rv = 0;
754 break;
755 default:
756 rv = DIAG_ERR_INIT_NOTSUPP;
757 break;
758 } //end of switch dp->initype
759 //At this point we just finished the handshake and got KB1 and KB2
760
761 if (rv < 0) {
762 free(dp);
763 d_l2_conn->diag_l2_proto_data=NULL; //delete pointer to dp
764 return diag_iseterr(rv);
765 }
766 // Analyze keybytes and set modeflags properly. See
767 // iso14230 5.2.4.1 & Table 8
768 dp->modeflags |= ((d_l2_conn->diag_l2_kb1 & 1)? ISO14230_FMTLEN:0) |
769 ((d_l2_conn->diag_l2_kb1 & 2)? ISO14230_LENBYTE:0) |
770 ((d_l2_conn->diag_l2_kb1 & 4)? ISO14230_SHORTHDR:0) |
771 ((d_l2_conn->diag_l2_kb1 & 8)? ISO14230_LONGHDR:0);
772
773 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
774 FLFMT "new modeflags=0x%04X\n", FL, dp->modeflags);
775
776 //For now, we won't bother with Normal / Extended timings. We don't
777 //need to unless we use the AccessTimingParameters service (scary)
778
779 /*
780 * Now, we want to remove any rubbish left
781 * in inbound buffers, and wait for the bus to be
782 * quiet for a while before we will communicate
783 * (so that the next byte received is the first byte
784 * of an iso14230 frame, not a middle byte)
785 * We use 1/2 of P2max (inter response gap) or
786 * 5 * p4max (inter byte delay), whichever is larger
787 * a correct value to use
788 */
789 wait_time = d_l2_conn->diag_l2_p2max / 2;
790 if ((d_l2_conn->diag_l2_p4max * 5) > wait_time) {
791 wait_time = d_l2_conn->diag_l2_p4max * 5;
792 }
793
794 while (diag_l1_recv(d_l2_conn->diag_link->l2_dl0d, data,
795 sizeof(data), wait_time) != DIAG_ERR_TIMEOUT) {
796 ;
797 }
798
799 /* And we're done */
800 dp->state = STATE_ESTABLISHED;
801
802 return 0;
803}
804
805//We need this in _stopcomms
806static struct diag_msg *dl2p_14230_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
807 int *errval);
808
809/* _stopcomms:
810 * Send a stopcomms message, and wait for the +ve response, for upto
811 * p3max
812 * we'll therefore
813 * use dl2p_14230_request() as it was meant to be used.
814 * However, if we get no response or an unidentified negative response,
815 * we do warn the user that he should to wait P3max for the connection
816 * to time out.
817 - the layer 2 code that called this marked the connection as
818 * STATE_CLOSING so the keepalive should be disabled
819 */
820static int dl2p_14230_stopcomms(struct diag_l2_conn *pX) {
821 struct diag_msg stopmsg = {0};
822 struct diag_msg *rxmsg;
823 uint8_t stopreq=DIAG_KW2K_SI_SPR;
824 int errval=0;
825 char *debugstr;
826
827 stopmsg.len=1;
828 stopmsg.data=&stopreq;
829 stopmsg.dest=0;
830 stopmsg.src=0;
831
832 rxmsg=dl2p_14230_request(pX, &stopmsg, &errval);
833
834 if (rxmsg != NULL) {
835 //we got a message;
836 if (!errval) {
837 //no error : positive response from ECU.
838 debugstr="ECU acknowledged request (RC=";
839 } else {
840 debugstr="ECU refused request; connection will time-out in 5s.(RC=";
841 }
842 errval=rxmsg->data[0];
843 diag_freemsg(rxmsg);
844 } else {
845 //no message received...
846 debugstr="ECU did not respond to request, connection will timeout in 5s. (err=";
847 }
848
849 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_CLOSE, DIAG_DBGLEVEL_V,
850 FLFMT "_stopcomms: %s0x%02X).\n", FL, debugstr, errval);
851
852 //and free() what startcomms alloc'ed.
853 if (pX->diag_l2_proto_data) {
854 free(pX->diag_l2_proto_data);
855 pX->diag_l2_proto_data=NULL;
856 }
857
858 return 0;
859}
860
861/*
862 * Just send the data
863 *
864 * We add the header and checksums here as appropriate, based on the keybytes
865 *
866 * We take the source and dest from the internal data NOT from the msg fields
867 *
868 * We also wait p3 ms
869 * return 0 if ok, <0 if err
870 * argument msg must have .len, .data, .dest and .src assigned.
871 */
872static int dl2p_14230_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
873 int rv;
874 size_t len;
875 uint8_t buf[MAXRBUF];
876 int offset=0; //data payload starts at buf[offset}
877 struct diag_l2_14230 *dp;
878
879 if (msg->len < 1) {
880 return diag_iseterr(DIAG_ERR_BADLEN);
881 }
882
883 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
884 FLFMT "_send: dl2conn=%p msg=%p len=%u\n",
885 FL, (void *)d_l2_conn, (void *)msg, msg->len);
886
887 dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
888
889
890 //if L1 requires headerless data, send directly :
891 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DATAONLY) {
892 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
893 msg->data, msg->len, d_l2_conn->diag_l2_p4min);
894 return rv? diag_ifwderr(rv):0;
895 }
896
897 /* Build the new message */
898
899 if ((dp->modeflags & ISO14230_LONGHDR) || !(dp->modeflags & ISO14230_SHORTHDR)) {
900 //we use full headers if they're supported or if we don't have a choice.
901 //set the "address present" bit
902 //and functional addressing if applicable
903 if (dp->modeflags & ISO14230_FUNCADDR) {
904 buf[0] = 0xC0;
905 } else {
906 buf[0] = 0x80;
907 }
908
909 /* If user supplied addresses, use them, else use the originals */
910 if (msg->dest) {
911 buf[1] = msg->dest;
912 } else {
913 buf[1] = dp->dstaddr;
914 }
915
916 if (msg->src) {
917 buf[2] = msg->src;
918 } else {
919 buf[2] = dp->srcaddr;
920 }
921 offset = 3;
922 } else if (dp->modeflags & ISO14230_SHORTHDR) {
923 //here, short addressless headers are required.
924 //basic format byte : 0
925 buf[0]=0;
926 offset = 1;
927 }
928
929
930
931 if ((dp->modeflags & ISO14230_FMTLEN) || !(dp->modeflags & ISO14230_LENBYTE)) {
932 //if ECU supports length in format byte, or doesn't support extra len byte
933 if (msg->len < 64) {
934 buf[0] |= msg->len;
935 } else {
936 //len >=64, can we use a length byte ?
937 if (dp->modeflags & ISO14230_LENBYTE) {
938 buf[offset] = msg->len;
939 offset += 1;
940 } else {
941 fprintf(stderr, FLFMT "can't send >64 byte msgs to this ECU !\n", FL);
942 return diag_iseterr(DIAG_ERR_BADLEN);
943 }
944 }
945 } else if (dp->modeflags & ISO14230_LENBYTE) {
946 // if the ecu needs a length byte
947 buf[offset] = msg->len;
948 offset += 1;
949 }
950
951 memcpy(&buf[offset], msg->data, msg->len);
952
953 len = msg->len + offset; /* data + hdr */
954
955 if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2CKSUM) == 0) {
956 /* We must add checksum, which is sum of bytes */
957 buf[len] = diag_cks1(buf, len);
958 len++; /* + checksum */
959 }
960
961 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V, buf, len,
962 FLFMT "_send: ", FL);
963
964 /* Wait p3min milliseconds, but not if doing fast/slow init */
965 if (dp->state == STATE_ESTABLISHED) {
966 diag_os_millisleep(d_l2_conn->diag_l2_p3min);
967 }
968
969 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
970 buf, len, d_l2_conn->diag_l2_p4min);
971
972 return rv? diag_ifwderr(rv):0;
973}
974
975/*
976 * Protocol receive routine
977 *
978 * Will sleep until a complete set of responses has been received, or fail
979 * with a timeout error
980 *
981 * The interbyte type in data from an ECU is between P1Min and P1Max
982 * The intermessage time for part of one response is P2Min and P2Max
983 *
984 * If we are running with an intelligent L1 interface, then we will be
985 * getting one message per frame, and we will wait a bit longer
986 * for extra messages
987 */
988static int dl2p_14230_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
989 void (*callback)(void *handle, struct diag_msg *msg),
990 void *handle) {
991 int rv;
992
993 /* Call internal routine */
994 rv = dl2p_14230_int_recv(d_l2_conn, timeout);
995
996 if (rv < 0) { /* Failure */
997 return rv;
998 }
999
1000 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
1001 FLFMT "_int_recv : handle=%p timeout=%u\n",
1002 FL, handle, timeout);
1003
1004 /*
1005 * Call user callback routine
1006 */
1007 if (callback) {
1008 callback(handle, d_l2_conn->diag_msg);
1009 }
1010
1011 /* No longer needed */
1012 diag_freemsg(d_l2_conn->diag_msg);
1013 d_l2_conn->diag_msg = NULL;
1014
1015 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
1016 FLFMT "_recv callback completed\n", FL);
1017
1018 return 0;
1019}
1020
1021//iso14230 diag_l2_proto_request. See diag_l2.h
1022//This handles busyRepeatRequest and RspPending negative responses.
1023//return NULL if failed, or a newly alloced diag_msg if succesful.
1024//Caller must free that diag_msg.
1025//Sends using diag_l2_send() in order to have the timestamps updated
1026static struct diag_msg *dl2p_14230_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
1027 int *errval) {
1028 int rv;
1029 struct diag_msg *rmsg = NULL;
1030 int retries=3; //if we get a BusyRepeatRequest response.
1031
1032 *errval=0;
1033
1034 rv = diag_l2_send(d_l2_conn, msg);
1035 if (rv < 0) {
1036 *errval = rv;
1037 return diag_pfwderr(rv);
1038 }
1039
1040 while (1) {
1041 /* do read only if no messages pending */
1042 if (!d_l2_conn->diag_msg) {
1043 rv = dl2p_14230_int_recv(d_l2_conn,
1044 d_l2_conn->diag_l2_p2max + 10);
1045
1046 if (rv < 0) {
1047 *errval = DIAG_ERR_TIMEOUT;
1048 if (rv == DIAG_ERR_TIMEOUT) {
1049 return NULL;
1050 }
1051 return diag_pfwderr(rv);
1052 }
1053 }
1054
1055 /*
1056 * The connection now has the received message(s)
1057 * stored. Temporarily remove from dl2c
1058 */
1059 rmsg = d_l2_conn->diag_msg;
1060 d_l2_conn->diag_msg = NULL;
1061
1062 /* Check for negative response */
1063 if (rmsg && rmsg->data[0] != DIAG_KW2K_RC_NR) {
1064 /* Success */
1065 break;
1066 }
1067
1068 if (rmsg && rmsg->data[2] == DIAG_KW2K_RC_B_RR) {
1069 /*
1070 * Msg is busyRepeatRequest
1071 * So send again (if retries>0).
1072 *
1073 * Is there any possibility that we would have received 2 messages,
1074 * {busyrepeatrequest + a valid (unrelated) response} ?
1075 * Not sure, let's simply discard everything.
1076 */
1077 diag_freemsg(rmsg);
1078
1079 if (retries > 0) {
1080 rv = diag_l2_send(d_l2_conn, msg);
1081 } else {
1082 rv=DIAG_ERR_GENERAL;
1083 fprintf(stderr, FLFMT "got too many BusyRepeatRequest responses!\n", FL);
1084 }
1085
1086 retries--;
1087
1088 if (rv < 0) {
1089 *errval = rv;
1090 return diag_pseterr(rv);
1091 }
1092 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
1093 FLFMT "got BusyRepeatRequest: retrying...\n", FL);
1094
1095 continue;
1096 }
1097
1098 if (rmsg && rmsg->data[2] == DIAG_KW2K_RC_RCR_RP) {
1099 /*
1100 * Msg is a requestCorrectlyRcvd-RspPending
1101 * so do read again
1102 */
1103 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
1104 FLFMT "got RspPending: retrying...\n", FL);
1105
1106 /* reattach the rest of the chain, in case the good response
1107 * was already received
1108 */
1109 d_l2_conn->diag_msg = rmsg->next;
1110 rmsg->next = NULL;
1111 diag_freemsg(rmsg);
1112 continue;
1113 }
1114 /* Some other type of error */
1115 *errval= DIAG_ERR_ECUSAIDNO;
1116 break;
1117 } //while 1
1118 /* Return the message to user, who is responsible for freeing it */
1119 return rmsg;
1120}
1121
1122/*
1123 * Timeout, - if we don't send something to the ECU it will timeout
1124 * soon, so send it a keepalive message now.
1125 */
1126static void dl2p_14230_timeout(struct diag_l2_conn *d_l2_conn) {
1127 struct diag_l2_14230 *dp;
1128 struct diag_msg msg = {0};
1129 uint8_t data[256];
1130 unsigned int timeout;
1131 int debug_l2_orig=diag_l2_debug; //save debug flags; disable them for this procedure
1132 int debug_l1_orig=diag_l1_debug;
1133 int debug_l0_orig=diag_l0_debug;
1134
1135 dp = (struct diag_l2_14230 *)d_l2_conn->diag_l2_proto_data;
1136
1137 /* XXX fprintf not async-signal-safe */
1138 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_TIMER, DIAG_DBGLEVEL_V,
1139 FLFMT "\ntimeout impending for dl2c=%pd\n",
1140 FL, (void *)d_l2_conn);
1141
1142 diag_l2_debug=0; //disable
1143 diag_l1_debug=0;
1144 diag_l0_debug=0;
1145
1146 /* Prepare the "keepalive" message */
1147 if (dp->modeflags & ISO14230_IDLE_J1978) {
1148 /* Idle using J1978 / J1979 keep alive message : SID 1 PID 0 */
1149 msg.len = 2;
1150 data[0] = 1;
1151 data[1] = 0;
1152 } else {
1153 /* Idle using ISO "Tester Present" message */
1154 msg.len = 1;
1155 msg.dest = 0; /* Use default */
1156 msg.src = 0; /* Use default */
1157 data[0] = DIAG_KW2K_SI_TP;
1158 }
1159
1160 msg.data = data;
1161
1162 /*
1163 * There is no point in checking for errors, or checking
1164 * the received response as we can't pass an error back
1165 * from here
1166 * TODO: we could at least if NEGRESP was received and warn the user...
1167 */
1168
1169 /* Send it, important to use l2_send as it updates the timers */
1170 (void)diag_l2_send(d_l2_conn, &msg);
1171
1172 /*
1173 * Get the response in p2max, we allow longer, and even
1174 * longer on "smart" L2 interfaces
1175 */
1176 timeout = d_l2_conn->diag_l2_p2max;
1177 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2FRAME) {
1178 if (timeout < SMART_TIMEOUT) {
1179 timeout += SMART_TIMEOUT;
1180 }
1181 }
1182 (void)diag_l2_recv(d_l2_conn, timeout, NULL, NULL);
1183 diag_l2_debug=debug_l2_orig; //restore debug flags
1184 diag_l1_debug=debug_l1_orig;
1185 diag_l0_debug=debug_l0_orig;
1186
1187}
1188const struct diag_l2_proto diag_l2_proto_iso14230 = {
1189 DIAG_L2_PROT_ISO14230,
1190 "ISO14230",
1191 DIAG_L2_FLAG_FRAMED | DIAG_L2_FLAG_KEEPALIVE,
1192 dl2p_14230_startcomms,
1193 dl2p_14230_stopcomms,
1194 dl2p_14230_send,
1195 dl2p_14230_recv,
1196 dl2p_14230_request,
1197 dl2p_14230_timeout
1198};
1199