BOROSOFTWAREAll work →
← scantool/diag_l2_iso9141.c 736 lines
5 findings in this fileB9L120A6L284A13L410D9L410A7L560
B9A6B9B9A13D9A6A6D9A7
1/*
2 * freediag - Vehicle Diagnostic Utility
3 *
4 * Copyright (C) 2001 Richard Almeida & Ibex Ltd ([email protected])
5 *
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 *
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
19 *
20 *************************************************************************
21 *
22 * Diag
23 *
24 * L2 driver for ISO 9141 protocol.
25 *
26 * NOTE: ISO9141-2 says that if the target address is 0x33, then the SAE-J1979
27 * Scantool Application Protocol is used.
28 *
29 * Other addresses are manufacturer-specific, and MAY EXCEED THIS IMPLEMENTATION.
30 * (But we still let you TRY to use them... :) Just keep in mind that ISO9141 messages
31 * have a maximum payload of 7 bytes.
32 */
33
34#include <string.h>
35#include <stdio.h>
36#include <stdlib.h>
37
38#include "diag.h"
39#include "diag_err.h"
40#include "diag_os.h"
41#include "diag_tty.h"
42#include "diag_l1.h"
43#include "diag_l2.h"
44
45#include "diag_l2_iso9141.h" /* prototypes for this file */
46
47
48
49/*
50 * This implements the handshaking process between Tester and ECU.
51 * It is used to wake up an ECU and get its KeyBytes.
52 *
53 * The process as defined in ISO9141 is:
54 * 1 - Tester sends target address (0x33) at 5 baud;
55 * 2 - ECU wakes up, sends synch pattern 0x55 at approx. 10400 baud;
56 * 3 - Tester clocks synch pattern and defines baud rate (10400 baud);
57 * 4 - ECU sends first KeyByte;
58 * 5 - ECU sends second KeyByte;
59 * 6 - Tester regulates p2 time according to KeyBytes;
60 * 6 - Tester sends second KeyByte inverted;
61 * 7 - ECU sends received address inverted (0xCC);
62 * This concludes a successfull handshaking in ISO9141.
63 */
64int dl2p_iso9141_wakeupECU(struct diag_l2_conn *d_l2_conn) {
65 struct diag_l1_initbus_args in;
66 uint8_t kb1, kb2, inv_address;
67 int rv = 0;
68 struct diag_l2_iso9141 *dp;
69
70 kb1 = kb2 = inv_address = 0;
71 dp = d_l2_conn->diag_l2_proto_data;
72
73 // Flush unread input:
74 (void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
75
76 // Wait for idle bus:
77 diag_os_millisleep(W5min);
78
79 // Do 5Baud init (write Address, read Synch Pattern):
80 in.type = DIAG_L1_INITBUS_5BAUD;
81 in.addr = dp->target;
82 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
83 if (rv < 0) {
84 return diag_ifwderr(rv);
85 }
86
87 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) {
88 d_l2_conn->diag_l2_kb1=0x08;
89 d_l2_conn->diag_l2_kb2=0x08; //possibly not true, but who cares.
90 d_l2_conn->diag_l2_p2min = 25;
91 return 0;
92 }
93
94 // The L1 device has read the 0x55, and reset the previous speed.
95
96 // Receive the first KeyByte:
97 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d, &kb1, 1, W2max+RXTOFFSET);
98 if (rv < 0) {
99 return diag_iseterr(DIAG_ERR_WRONGKB);
100 }
101
102 // Receive the second KeyByte:
103 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d, &kb2, 1, W3max+RXTOFFSET);
104 if (rv < 0) {
105 return diag_iseterr(DIAG_ERR_WRONGKB);
106 }
107
108 // Check keybytes, these can be 0x08 0x08 or 0x94 0x94:
109 if ( (kb1 != kb2) || ( (kb1 != 0x08) && (kb1 != 0x94) ) ) {
110 fprintf(stderr, FLFMT "Wrong Keybytes: got %02X %02X\n", FL, kb1, kb2);
111 return diag_iseterr(DIAG_ERR_WRONGKB);
112 }
113
114 // Copy KeyBytes to protocol session data:
115 d_l2_conn->diag_l2_kb1 = kb1;
116 d_l2_conn->diag_l2_kb2 = kb2;
117
118 // set p2min according to KeyBytes:
119 // P2min is 0 for kb 0x94, 25ms for kb 0x08;
120 if (kb1 == 0x94) {
121 d_l2_conn->diag_l2_p2min = 0;
122 } else {
123 d_l2_conn->diag_l2_p2min = 25;
124 }
125
126 // Now send inverted KeyByte2, and receive inverted address
127 // (unless L1 deals with this):
128 if ( (d_l2_conn->diag_link->l1flags
129 & DIAG_L1_DOESSLOWINIT) == 0) {
130 //Wait W4min:
131 diag_os_millisleep(W4min);
132
133 //Send inverted kb2:
134 uint8_t inv_kb2 = (uint8_t) ~kb2;
135 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
136 &inv_kb2, 1, 0);
137 if (rv < 0) {
138 return diag_ifwderr(rv);
139 }
140
141 // Wait for the address byte inverted:
142 // XXX I added RXTOFFSET as a band-aid fix for systems, like
143 //mine, that don't receive ~addr with only W4max. See #define
144 //NOTE : l2_iso14230 uses a huge 350ms timeout for this!!
145 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
146 &inv_address, 1, W4max + RXTOFFSET);
147 if (rv < 0) {
148 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
149 FLFMT
150 "wakeupECU(dl2conn %p) did not get "
151 "inv. address; rx error %d\n",
152 FL, (void *)d_l2_conn, rv);
153 return diag_ifwderr(rv);
154 }
155
156 // Check the received inverted address:
157 if ( inv_address != ((~dp->target) & 0xff)) {
158 fprintf(stderr,
159 FLFMT "wakeupECU(dl2conn %p) addr mismatch: 0x%02X != 0x%02X\n",
160 FL, (void *)d_l2_conn, inv_address, ~dp->target);
161 return diag_iseterr(DIAG_ERR_BADDATA);
162 }
163 }
164
165 //Success! Handshaking done.
166
167 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
168 FLFMT "_wakeupECU dl2con=%p kb1=0x%02X kb2=0x%02X\n",
169 FL, (void *)d_l2_conn, kb1, kb2);
170
171 return 0;
172}
173
174
175/*
176 * This implements the start of a new protocol session.
177 * It wakes up an ECU if not in monitor mode.
178 */
179static int dl2p_iso9141_startcomms(struct diag_l2_conn *d_l2_conn,
180 flag_type flags, unsigned int bitrate,
181 target_type target, source_type source) {
182 int rv;
183 struct diag_serial_settings set;
184 struct diag_l2_iso9141 *dp;
185
186 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
187 FLFMT "_startcomms conn %p %ubps tgt=0x%X src=0x%X\n",
188 FL, (void *)d_l2_conn, bitrate, target, source);
189
190 rv = diag_calloc(&dp, 1);
191 if (rv != 0) {
192 return diag_ifwderr(rv);
193 }
194
195 dp->srcaddr = source;
196 dp->target = target;
197 dp->state = STATE_CONNECTING;
198 d_l2_conn->diag_l2_kb1 = 0;
199 d_l2_conn->diag_l2_kb2 = 0;
200 d_l2_conn->diag_l2_proto_data = (void *)dp;
201
202 // Prepare the port for this protocol:
203 // Data bytes are in {7bits, OddParity, 1stopbit}, but we
204 // read and write as {8bits, NoParity, 1stopbit}.
205 // That must be taken into account by the application / layer 3 !!!
206 if (bitrate == 0) {
207 bitrate = 10400;
208 }
209 d_l2_conn->diag_l2_speed = bitrate;
210 set.speed = bitrate;
211 set.databits = diag_databits_8;
212 set.stopbits = diag_stopbits_1;
213 set.parflag = diag_par_n;
214
215 if ((rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, &set))) {
216 free(dp);
217 d_l2_conn->diag_l2_proto_data=NULL;
218 return diag_ifwderr(rv);
219 }
220
221 // Initialize ECU (unless if in monitor mode):
222 switch (flags & DIAG_L2_TYPE_INITMASK) {
223 case DIAG_L2_TYPE_MONINIT:
224 rv = 0;
225 break;
226 case DIAG_L2_TYPE_SLOWINIT:
227 rv = dl2p_iso9141_wakeupECU(d_l2_conn);
228 break;
229 default:
230 //CARB and FASTINIT are not in iso9141.
231 rv = DIAG_ERR_INIT_NOTSUPP;
232 break;
233 }
234
235
236 if (rv) {
237 free(dp);
238 d_l2_conn->diag_l2_proto_data=NULL;
239 return diag_iseterr(rv);
240 }
241
242 dp->state = STATE_ESTABLISHED;
243
244 return 0;
245}
246
247
248/*
249 * Free session-specific allocated data.
250 * ISO9141 doesn't have a StopCommunication mechanism like iso14230,
251 * so we just "undo" what iso9141_startcomms did.
252 */
253static int dl2p_iso9141_stopcomms(struct diag_l2_conn *d_l2_conn) {
254 struct diag_l2_iso9141 *dp;
255
256 dp = (struct diag_l2_iso9141 *)d_l2_conn->diag_l2_proto_data;
257 if (dp) {
258 free(dp);
259 }
260 d_l2_conn->diag_l2_proto_data=NULL;
261
262 //Always OK for now.
263 return 0;
264}
265
266
267/*
268 * This implements the interpretation of a response message.
269 * With ISO9141, the data length will depend on the
270 * content of the message, and cannot be guessed at the L1
271 * level; therefore, only L3 / application will be able to
272 * check it correctly. We just assume the data length is always =
273 * (received_len - (header + chksm))
274 * So this only verifies minimal length and valid header bytes.
275 * Should only really be used by the _int_recv function.
276 */
277static int dl2p_iso9141_decode(uint8_t *data, int len,
278 uint8_t *hdrlen, int *datalen, uint8_t *source, uint8_t *dest) {
279
280 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V, data, len,
281 FLFMT "decode len %d: ", FL, len);
282
283 //Check header bytes:
284 if (data[0] != 0x48 || data[1] != 0x6B) {
285 return diag_iseterr(DIAG_ERR_BADDATA);
286 }
287
288 //verify minimal length
289 if (len - OHLEN_ISO9141 > 0) {
290 *datalen = len - OHLEN_ISO9141;
291 } else {
292 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
293 FLFMT "decode len short \n", FL);
294
295 return diag_iseterr(DIAG_ERR_INCDATA);
296 }
297
298 //Set header length (always the same):
299 *hdrlen = OHLEN_ISO9141 - 1;
300
301 // Set Source and Destination addresses:
302 if (dest) {
303 *dest = 0xF1; // Always the Tester.
304 }
305 if (source) {
306 *source = data[2]; // Originating ECU;
307 }
308
309 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_PROTO, DIAG_DBGLEVEL_V,
310 FLFMT "decode total len = %d, datalen = %d\n",
311 FL, len, *datalen);
312
313 return OHLEN_ISO9141 + *datalen;
314}
315
316/*
317 * This implements the reading of all the ECU responses to a Tester request.
318 * One ECU may send multiple responses to one request.
319 * Multiple ECUS may send responses to one request.
320 * The end of all responses is marked by p2max timeout. (p3min > p2max so
321 * that if we (the tester) send a new request after p3min, we are guaranteed
322 * not to clobber an ECU response)
323 * Ret 0 if OK.
324 *
325 * "timeout" has to be long enough to receive at least 1 byte;
326 * in theory it could be P2min + (8 / baudrate) but there is no
327 * harm in using P2max.
328 *
329 * XXX Dilemma. To properly split messages, do we trust our timing VS iso9141 P2min/max requirements?
330 * Do we try to find valid headers + checksum and filter out bad frames ?
331 * Do we let L3_saej1979 try and DJ the framing through L2 ?
332 */
333int dl2p_iso9141_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout) {
334 int rv, l1_doesl2frame, l1flags;
335 unsigned int tout = 0;
336 int state;
337 struct diag_l2_iso9141 *dp;
338 struct diag_msg *tmsg, *lastmsg;
339
340#define ST_STATE1 1 // Start - wait for a frame.
341#define ST_STATE2 2 // In frame - wait for more bytes.
342#define ST_STATE3 3 // End of frame - wait for more frames.
343
344 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
345 FLFMT "_int_recv offset 0x%X\n",
346 FL, d_l2_conn->rxoffset);
347
348 dp = (struct diag_l2_iso9141 *)d_l2_conn->diag_l2_proto_data;
349
350 // Clear out last received message if not done already.
351 if (d_l2_conn->diag_msg) {
352 diag_freemsg(d_l2_conn->diag_msg);
353 d_l2_conn->diag_msg = NULL;
354 }
355
356 // Check if L1 device does L2 framing:
357 l1flags = d_l2_conn->diag_link->l1flags;
358 l1_doesl2frame = (l1flags & DIAG_L1_DOESL2FRAME);
359 if (l1_doesl2frame) {
360 // Extend timeouts for the "smart" interfaces:
361 if (timeout < SMART_TIMEOUT) {
362 timeout += SMART_TIMEOUT;
363 }
364 }
365
366 // Message read cycle: byte-per-byte for passive interfaces,
367 // frame-per-frame for smart interfaces (DOESL2FRAME).
368 // ISO-9141-2 says:
369 // Inter-byte gap in a frame < p1max
370 // Inter-frame gap < p2max
371 // We are a bit more flexible than that, see below.
372 // Frames get acumulated in the protocol structure list.
373 state = ST_STATE1;
374 while (1) {
375 switch (state) {
376 case ST_STATE1:
377 // Ready for first byte, use timeout
378 // specified by user.
379 tout = timeout;
380 break;
381
382 case ST_STATE2:
383 // Inter-byte timeout within a frame.
384 // ISO says p1max is the maximum, but in fact
385 // we give ourselves up to p2min minus a little bit.
386 tout = d_l2_conn->diag_l2_p2min - 2;
387 if (tout < d_l2_conn->diag_l2_p1max) {
388 tout = d_l2_conn->diag_l2_p1max;
389 }
390 break;
391
392 case ST_STATE3:
393 // This is the timeout waiting for any more
394 // responses from the ECU. ISO says min is p2max
395 // but we'll use p3min.
396 // Aditionaly, for "smart" interfaces, we expand
397 // the timeout to let them process the data.
398 //TODO: maybe set tout=p3min + margin ?
399 tout = d_l2_conn->diag_l2_p3min;
400 if (l1_doesl2frame) {
401 tout += SMART_TIMEOUT;
402 }
403 break;
404 }
405
406 // If L0/L1 does L2 framing, we get full frames, so we don't
407 // need to do the read byte-per-byte (skip state2):
408 if ((state == ST_STATE2) && l1_doesl2frame) {
409 rv = DIAG_ERR_TIMEOUT;
410 } else if (dp->rxoffset == MAXLEN_ISO9141) {
411 rv = DIAG_ERR_TIMEOUT; //we got a full frame already !
412 } else {
413 // Receive data into the buffer:
414 rv = diag_l1_recv(d_l2_conn->diag_link->l2_dl0d,
415 &dp->rxbuf[dp->rxoffset],
416 MAXLEN_ISO9141 - dp->rxoffset, tout);
417 }
418
419 // Timeout = end of message or end of responses.
420 if (rv == DIAG_ERR_TIMEOUT) {
421 switch (state) {
422 case ST_STATE1:
423 // If we got 0 bytes on the 1st read,
424 // just return the timeout error.
425 if (dp->rxoffset == 0) {
426 break;
427 }
428
429 // Otherwise try to read more bytes into
430 // this message.
431 state = ST_STATE2;
432 continue;
433 break;
434
435 case ST_STATE2:
436 // End of that message, maybe more to come;
437 // Copy data into a message.
438 tmsg = diag_allocmsg((size_t)dp->rxoffset);
439 if (tmsg == NULL) {
440 return diag_iseterr(DIAG_ERR_NOMEM);
441 }
442 memcpy(tmsg->data, dp->rxbuf,
443 (size_t)dp->rxoffset);
444 tmsg->rxtime = diag_os_getms();
445
446 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
447 dp->rxbuf, (size_t)dp->rxoffset,
448 "l2_iso9141_recv: ");
449
450 dp->rxoffset = 0;
451
452 // Add received message to response list:
453 diag_l2_addmsg(d_l2_conn, tmsg);
454
455 // Finished this one, get more:
456 state = ST_STATE3;
457 continue;
458 break;
459
460 case ST_STATE3:
461 /*
462 * No more messages, but we did get one
463 */
464 rv = d_l2_conn->diag_msg->len;
465 break;
466 default:
467 break;
468 } //switch (state)
469
470 // end of all response messages:
471 if (state == ST_STATE3) {
472 break;
473 }
474 } //if diag_err_timeout
475
476 // Other reception errors.
477 if (rv <= 0 || rv > 255) {
478 break;
479 }
480
481 // Data received OK.
482 // Add length to offset.
483 dp->rxoffset += (uint8_t) rv;
484
485 // This is where some tweaking might be needed if
486 // we are in monitor mode... but not yet.
487
488 // Got some data in state1/3, now we're in a message!
489 if ((state == ST_STATE1) || (state == ST_STATE3)) {
490 state = ST_STATE2;
491 }
492
493 }//end while (read cycle).
494
495 // Now walk through the response message list,
496 // and strip off their headers and checksums
497 // after verifying them.
498 if (rv < 0) {
499 return diag_iseterr(rv);
500 }
501
502 tmsg = d_l2_conn->diag_msg;
503 lastmsg = NULL;
504
505 while (tmsg) {
506 int datalen;
507 uint8_t hdrlen=0, source=0, dest=0;
508
509 if ((l1flags & DIAG_L1_NOHDRS)==0) {
510 // Parse message structure, if headers are present
511 rv = dl2p_iso9141_decode( tmsg->data,
512 tmsg->len, &hdrlen, &datalen, &source, &dest);
513
514 if (rv < 0 || rv > 255) {
515 // decode failure!
516 return diag_iseterr(DIAG_ERR_BADDATA);
517 }
518 } else if (!l1_doesl2frame) {
519 // Absent headers, and no framing -> illegal !
520 fprintf(stderr, "Warning : insane L1flags (l2frame && nohdrs ?)\n");
521 return diag_iseterr(DIAG_ERR_GENERAL);
522 }
523
524 // Process L2 framing, if L1 doesn't do it.
525 if (l1_doesl2frame == 0) {
526
527 //we'll have to assume we have only a single message, since at the L1 level
528 //it's impossible to guess the message length. But, if we got more than MAXLEN_ISO9141 bytes,
529 //(not allowed by standard), we try splitting the message (assuming the first message had the maximum
530 //length). It's the best that can be done at this level.
531
532 if (rv > MAXLEN_ISO9141) {
533 struct diag_msg *amsg;
534 amsg = diag_dupsinglemsg(tmsg);
535 if (amsg == NULL) {
536 return diag_iseterr(DIAG_ERR_NOMEM);
537 }
538 amsg->len = (uint8_t) MAXLEN_ISO9141;
539 tmsg->len -= (uint8_t) MAXLEN_ISO9141;
540 tmsg->data += MAXLEN_ISO9141;
541
542 /* Insert new amsg before old msg */
543 amsg->next = tmsg;
544 if (lastmsg == NULL) {
545 d_l2_conn->diag_msg = amsg;
546 } else {
547 lastmsg->next = amsg;
548 }
549
550 tmsg = amsg; /* Finish processing this one */
551 }
552
553 }
554
555 // If L1 doesn't strip the checksum byte, verify it:
556 if ((l1flags & DIAG_L1_STRIPSL2CKSUM) == 0) {
557 uint8_t rx_cs = tmsg->data[tmsg->len - 1];
558 if (rx_cs != diag_cks1(tmsg->data, tmsg->len - 1)) {
559 fprintf(stderr, FLFMT "Checksum error in received message!\n", FL);
560 tmsg->fmt |= DIAG_FMT_BADCS;
561 } else {
562 tmsg->fmt &= ~DIAG_FMT_BADCS;
563 tmsg->fmt |= DIAG_FMT_FRAMED; //if the checksum fits, it means we framed things properly.
564 }
565 // "Remove" the checksum byte:
566 tmsg->len--;
567 } else {
568 tmsg->fmt |= DIAG_FMT_FRAMED; //if L1 stripped the checksum, it was probably valid ?
569 }
570
571 //if the headers aren't stripped by L1 already:
572 if ((l1flags & DIAG_L1_NOHDRS)==0) {
573 // Set source address:
574 tmsg->src = source;
575 // Set destination address:
576 tmsg->dest = dest;
577
578 // and "remove" headers:
579 tmsg->data += (OHLEN_ISO9141 - 1);
580 tmsg->len -= (OHLEN_ISO9141 - 1);
581 }
582
583
584 // Message done. Flag it up:
585 tmsg->fmt |= DIAG_FMT_CKSUMMED;
586
587 // Prepare to decode next message:
588 lastmsg = tmsg;
589 tmsg = tmsg->next;
590 } //while tmsg
591
592 return 0;
593}
594
595
596
597//_recv : timeout is fed directly to _int_recv and should be long enough
598//to guarantee we receive at least one byte. (P2Max should do the trick)
599//ret 0 if ok
600static int dl2p_iso9141_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
601 void (*callback)(void *handle, struct diag_msg *msg), void *handle) {
602 int rv;
603
604 rv = dl2p_iso9141_int_recv(d_l2_conn, timeout);
605 if ((rv >= 0) && (d_l2_conn->diag_msg !=NULL)) {
606 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
607 FLFMT "_recv : handle=%p\n", FL, handle);
608
609 /*
610 * Call user callback routine
611 */
612 if (callback) {
613 callback(handle, d_l2_conn->diag_msg);
614 }
615
616 /* No longer needed */
617 diag_freemsg(d_l2_conn->diag_msg);
618 d_l2_conn->diag_msg = NULL;
619 }
620
621 return (rv<0)? diag_ifwderr(rv):0;
622}
623
624/*
625 * Package the data into a message frame with header and checksum.
626 * Addresses were supplied by the protocol session initialization.
627 * Checksum is calculated on-the-fly.
628 * Apply inter-frame delay (p2).
629 * ret 0 if ok
630 */
631static int dl2p_iso9141_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
632 int rv;
633 unsigned int sleeptime;
634 uint8_t buf[MAXLEN_ISO9141];
635 int offset;
636 struct diag_l2_iso9141 *dp;
637
638 dp = d_l2_conn->diag_l2_proto_data;
639
640 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
641 FLFMT "_send dl2c=%p msg=%p\n", FL,
642 (void *)d_l2_conn, (void *)msg);
643
644 // Check if the payload plus the overhead (and checksum) exceed protocol packet size:
645 if (msg->len + OHLEN_ISO9141 > MAXLEN_ISO9141) {
646 fprintf(stderr, FLFMT "send: Message payload exceeds maximum allowed by protocol!\n", FL);
647 return diag_iseterr(DIAG_ERR_BADLEN);
648 }
649
650 /*
651 * Make sure enough time between last receive and this send
652 * In fact, because of the timeout on recv(), this is pretty small, but
653 * we take the safe road and wait the whole of p3min plus whatever
654 * delay happened before
655 */
656 sleeptime = d_l2_conn->diag_l2_p3min;
657 if (sleeptime > 0) {
658 diag_os_millisleep(sleeptime);
659 }
660
661 offset = 0;
662
663 //if L1 requires headerless data, send directly :
664 if (d_l2_conn->diag_link->l1flags & DIAG_L1_DATAONLY) {
665 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
666 msg->data, msg->len, d_l2_conn->diag_l2_p4min);
667 return rv? diag_ifwderr(rv):0;
668 }
669
670 /* add ISO9141-2 header */
671 buf[offset++] = 0x68; //defined by spec;
672 buf[offset++] = 0x6A; //defined by spec;
673 buf[offset++] = dp->srcaddr;
674
675 // Now copy in data
676 memcpy(&buf[offset], msg->data, msg->len);
677 offset += msg->len;
678
679 // If the interface doesn't do ISO9141-2 checksum, add it in:
680 if ((d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2CKSUM) == 0) {
681 uint8_t curoff = (uint8_t) offset;
682 buf[offset++] = diag_cks1(buf, curoff);
683 }
684
685 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
686 buf, (size_t)offset,
687 "l2_iso9141_send: ");
688
689 // Send it over the L1 link:
690 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
691 buf, (size_t)offset, d_l2_conn->diag_l2_p4min);
692
693
694 return rv? diag_ifwderr(rv):0;
695}
696
697
698//_request: ret a new message if ok; NULL if failed
699static struct diag_msg *dl2p_iso9141_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
700 int *errval) {
701 int rv;
702 struct diag_msg *rmsg = NULL;
703
704 rv = diag_l2_send(d_l2_conn, msg);
705 if (rv < 0) {
706 *errval = rv;
707 return NULL;
708 }
709
710 /* And wait for response */
711 rv = dl2p_iso9141_int_recv(d_l2_conn, d_l2_conn->diag_l2_p2max + RXTOFFSET);
712 if ((rv >= 0) && d_l2_conn->diag_msg) {
713 /* OK */
714 rmsg = d_l2_conn->diag_msg;
715 d_l2_conn->diag_msg = NULL;
716 } else {
717 /* Error */
718 *errval = DIAG_ERR_TIMEOUT;
719 rmsg = NULL;
720 }
721
722 return rmsg;
723}
724
725const struct diag_l2_proto diag_l2_proto_iso9141 = {
726 DIAG_L2_PROT_ISO9141,
727 "ISO9141",
728 DIAG_L2_FLAG_FRAMED,
729 dl2p_iso9141_startcomms,
730 dl2p_iso9141_stopcomms,
731 dl2p_iso9141_send,
732 dl2p_iso9141_recv,
733 dl2p_iso9141_request,
734 NULL
735};
736