BOROSOFTWAREAll work →
← scantool/diag_l2_saej1850.c 435 lines
1 finding in this fileA13L28
A13A13A13
1/*
2 *
3 * freediag - Vehicle Diagnostic Utility
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 SAEJ1850
26 *
27 *
28 * INCOMPLETE, will not work, but doesnt coredump. This has been checked in
29 * because scantool.c was checked in for other reasons, and needs this so it
30 * doesnt coredump ...
31 */
32
33#include <stdint.h>
34#include <stdio.h>
35#include <stdlib.h>
36#include <string.h>
37
38#include "diag.h"
39#include "diag_err.h"
40#include "diag_os.h"
41#include "diag_l1.h"
42#include "diag_l2.h"
43
44#include "diag_l2_saej1850.h" /* prototypes for this file */
45
46
47/*
48 * SAEJ1850 specific data
49 */
50struct diag_l2_j1850 {
51 uint8_t type; /* FAST/SLOW/CARB */
52
53 uint8_t srcaddr; /* Src address used */
54 uint8_t dstaddr; /* Dest address used */
55 //uint16_t modeflags; /* Flags XXXunused ! */
56
57 uint8_t state;
58 uint8_t rxbuf[MAXRBUF]; /* Receive buffer, for building message in */
59 int rxoffset; /* Offset to write into buffer */
60};
61
62#define STATE_CLOSED 0 /* Established comms */
63#define STATE_CONNECTING 1 /* Connecting */
64#define STATE_ESTABLISHED 2 /* Established */
65
66/* Prototypes */
67uint8_t dl2p_j1850_crc(uint8_t *msg_buf, int nbytes);
68
69/* External interface */
70
71/*
72 * The complex initialisation routine for SAEJ1850
73 */
74
75static int dl2p_j1850_startcomms(struct diag_l2_conn *d_l2_conn,
76 UNUSED(flag_type flags),
77 UNUSED(unsigned int bitrate),
78 target_type target, source_type source) {
79 struct diag_l2_j1850 *dp;
80 int rv;
81
82 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
83 FLFMT "diag_l2_j1850_startcomms dl2conn %p\n",
84 FL, (void *)d_l2_conn);
85
86 rv = diag_calloc(&dp, 1);
87 if (rv != 0) {
88 return diag_ifwderr(rv);
89 }
90
91 d_l2_conn->diag_l2_proto_data = (void *)dp;
92
93 dp->srcaddr = source;
94 dp->dstaddr = target;
95
96 dp->state = STATE_CONNECTING;
97
98 /* Empty our Receive buffer and wait for idle bus */
99 /* XXX is the timeout value right ? It is 300 in other places. */
100
101 (void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
102 diag_os_millisleep(50);
103
104 /* Always OK */
105 return 0;
106}
107
108/*
109 */
110static int dl2p_j1850_stopcomms(struct diag_l2_conn *d_l2_conn) {
111 struct diag_l2_j1850 *dp;
112
113 dp = (struct diag_l2_j1850 *)d_l2_conn->diag_l2_proto_data;
114
115 if (dp) {
116 free(dp);
117 }
118 d_l2_conn->diag_l2_proto_data=NULL;
119
120 /* Always OK for now */
121 return 0;
122}
123
124
125/* Thanks to B. Roadman's web site for this CRC code */
126uint8_t dl2p_j1850_crc(uint8_t *msg_buf, int nbytes) {
127 uint8_t crc_reg=0xff,poly,i,j;
128 uint8_t *byte_point;
129 uint8_t bit_point;
130
131 for (i=0, byte_point=msg_buf; i<nbytes; ++i, ++byte_point) {
132 for (j=0, bit_point=0x80 ; j<8; ++j, bit_point>>=1) {
133 if (bit_point & *byte_point) { // case for new bit = 1
134 if (crc_reg & 0x80) {
135 poly=1; // define the polynomial
136 } else {
137 poly = 0x1c;
138 }
139 crc_reg= ( (crc_reg << 1) | 1) ^ poly;
140 } else { // case for new bit = 0
141 poly=0;
142 if (crc_reg & 0x80) {
143 poly = 0x1d;
144 }
145 crc_reg= (crc_reg << 1) ^ poly;
146 }
147 }
148 }
149 return ~crc_reg; // Return CRC
150}
151
152/*
153 * Just send the data
154 *
155 * We add the header and checksums here as appropriate
156 * ret 0 if ok
157 */
158static int dl2p_j1850_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
159 int l1flags, rv, l1protocol;
160 struct diag_l2_j1850 *dp;
161
162 uint8_t buf[MAXRBUF];
163 int offset = 0;
164
165 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
166 FLFMT "diag_l2_j1850_send %p msg %p len %u called\n",
167 FL, (void *)d_l2_conn, (void *)msg, msg->len);
168
169 if ((msg->len + 4) >= MAXRBUF) {
170 return diag_iseterr(DIAG_ERR_BADLEN);
171 }
172
173 dp = (struct diag_l2_j1850 *)d_l2_conn->diag_l2_proto_data;
174 l1flags = d_l2_conn->diag_link->l1flags;
175 l1protocol = d_l2_conn->diag_link->l1proto;
176
177 if (l1flags & DIAG_L1_DATAONLY) {
178 //no headers to add, just the message
179 //nop
180 } else {
181 // Add the J1850 header to the data
182
183 if (l1protocol == DIAG_L1_J1850_PWM) {
184 buf[0] = 0x61;
185 } else {
186 buf[0] = 0x68;
187 }
188 buf[1] = dp->dstaddr;
189 buf[2] = dp->srcaddr;
190 offset += 3;
191 }
192
193 // Now copy in data
194 memcpy(&buf[offset], msg->data, msg->len);
195 offset += msg->len;
196
197 if (((l1flags & DIAG_L1_DOESL2CKSUM) == 0) &&
198 ((l1flags & DIAG_L1_DATAONLY) == 0)) {
199 // Add in J1850 CRC
200 int curoff = offset;
201 buf[offset++] = dl2p_j1850_crc(buf, curoff);
202 }
203
204 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
205 FLFMT "diag_l2_j1850_send sending %d bytes to L1\n",
206 FL, offset);
207
208 // And send data to Layer 1
209 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
210 buf, (size_t)offset, 0);
211
212 return rv? diag_ifwderr(rv):0;
213}
214
215/*
216 * Protocol receive routine
217 *
218 * Receive all messages until timeout has elapsed, split + save on d_l2_conn->diag_msg
219 * This is implemented differently from the ISO L2s (9141 and 14230), in that
220 * timeout is measured starting at this function's entry.
221 *
222 * Ret 0 if ok, whether or not there were any messages.
223 */
224static int dl2p_j1850_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout) {
225 int rv;
226 struct diag_l2_j1850 *dp;
227 unsigned long long t_done; //time elapsed
228 unsigned long long t_us; //total timeout, in us
229 unsigned long long t0; //start time
230
231 int l1flags = d_l2_conn->diag_link->l1flags;
232
233 t0 = diag_os_gethrt();
234
235 dp = (struct diag_l2_j1850 *)d_l2_conn->diag_l2_proto_data;
236 diag_freemsg(d_l2_conn->diag_msg);
237
238 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
239 FLFMT "diag_l2_j1850_int_recv offset 0x%X, "
240 "timeout=%u\n",
241 FL, dp->rxoffset, timeout);
242
243 // No support for non framing L2 interfaces yet ...
244 if (!(l1flags & DIAG_L1_DOESL2FRAME)) {
245 return diag_iseterr(DIAG_ERR_PROTO_NOTSUPP);
246 }
247
248 /* Extend timeouts since L0/L1 does framing */
249 timeout += SMART_TIMEOUT;
250 t_us = timeout * 1000ULL;
251 t_done = 0;
252
253 dp->rxoffset = 0;
254
255 /* note : some of this is not necessary since we assume every L0/L1 does J1850 framing properly. */
256 while (t_done < t_us) {
257 //loop while there's time left
258 unsigned long tout;
259 struct diag_msg *tmsg;
260 unsigned datalen;
261
262 tout = timeout - (t_done / 1000);
263
264 //Unofficially, smart L0s (like ME,SIM) return max 1 response per call to l1_recv()
265 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
266 &dp->rxbuf[dp->rxoffset],
267 sizeof(dp->rxbuf) - dp->rxoffset,
268 tout);
269
270 if (rv == DIAG_ERR_TIMEOUT) {
271 break;
272 }
273
274 if (rv < 0) {
275 // Other errors are more serious.
276 diag_freemsg(d_l2_conn->diag_msg);
277 return rv;
278 }
279 dp->rxoffset += rv;
280
281 //update elapsed time
282 t_done = diag_os_hrtus(diag_os_gethrt() - t0);
283 if (rv == 0) {
284 continue; // no data ?
285 }
286
287 datalen = dp->rxoffset;
288
289 // get data payload length
290 if (!(l1flags & DIAG_L1_NOHDRS)) {
291 //headers present
292 if (datalen <= 3) {
293 continue;
294 }
295 datalen -= 3;
296 }
297 if (!(l1flags & DIAG_L1_STRIPSL2CKSUM)) {
298 //CRC present
299 if (datalen <= 1) {
300 continue;
301 }
302 datalen -= 1;
303 }
304
305 //alloc msg and analyze
306 tmsg = diag_allocmsg(datalen);
307 if (tmsg == NULL) {
308 diag_freemsg(d_l2_conn->diag_msg);
309 return diag_iseterr(DIAG_ERR_NOMEM);
310 }
311
312 if (!(l1flags & DIAG_L1_NOHDRS)) {
313 //get header content & trim
314
315 tmsg->dest = dp->rxbuf[1];
316 tmsg->src = dp->rxbuf[2];
317 //and copy, skipping header bytes.
318 memcpy(tmsg->data, &dp->rxbuf[3], datalen);
319 } else {
320 memcpy(tmsg->data, dp->rxbuf, datalen);
321 }
322
323 if (!(l1flags & DIAG_L1_STRIPSL2CKSUM)) {
324 //test & trim checksum
325 uint8_t tcrc=dl2p_j1850_crc(dp->rxbuf, dp->rxoffset - 1);
326 if (dp->rxbuf[dp->rxoffset - 1] != tcrc) {
327 fprintf(stderr, "Bad checksum detected: needed %02X got %02X\n",
328 tcrc, dp->rxbuf[dp->rxoffset - 1]);
329 tmsg->fmt |= DIAG_FMT_BADCS;
330 }
331 }
332
333 tmsg->fmt |= DIAG_FMT_CKSUMMED; //either L1 did it or we just did
334 tmsg->fmt |= DIAG_FMT_FRAMED;
335
336 tmsg->rxtime = diag_os_getms();
337 dp->rxoffset = 0;
338
339 diag_l2_addmsg(d_l2_conn, tmsg);
340
341 } //while !timed out
342
343 dp->state = STATE_ESTABLISHED;
344 return 0;
345}
346
347
348static int dl2p_j1850_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
349 void (*callback)(void *handle, struct diag_msg *msg),
350 void *handle) {
351 int rv;
352 struct diag_msg *tmsg;
353
354 rv = dl2p_j1850_int_recv(d_l2_conn, timeout);
355
356 if (rv < 0) {
357 /* Failed, or timed out */
358 return rv;
359 }
360
361 if (d_l2_conn->diag_msg == NULL) {
362 return DIAG_ERR_TIMEOUT;
363 }
364
365 /*
366 * We now have data stored on the L2 descriptor
367 */
368 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
369 FLFMT "calling rcv msg=%p callback, handle=%p\n",
370 FL, (void *)d_l2_conn->diag_msg, handle);
371
372 tmsg = d_l2_conn->diag_msg;
373 d_l2_conn->diag_msg = NULL;
374
375 /* Call used callback */
376 if (callback) {
377 callback(handle, tmsg);
378 }
379
380 /* message no longer needed */
381 diag_freemsg(tmsg);
382
383 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
384 FLFMT "rcv callback completed\n", FL);
385
386 return 0;
387}
388
389/*
390 * Send a request and wait for a response
391 */
392static struct diag_msg *dl2p_j1850_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
393 int *errval) {
394 int rv;
395 struct diag_msg *rmsg = NULL;
396
397 /* First send the message */
398 rv = diag_l2_send(d_l2_conn, msg);
399 if (rv < 0) {
400 *errval = rv;
401 return diag_pseterr(DIAG_ERR_GENERAL);
402 }
403
404 /* And now wait for a response */
405 /* XXX, whats the correct timeout for this ??? */
406 rv = dl2p_j1850_int_recv(d_l2_conn, 250);
407 if (rv < 0) {
408 *errval = rv;
409 return diag_pseterr(DIAG_ERR_GENERAL);
410 }
411
412 /* any responses ? */
413 if (!d_l2_conn->diag_msg) {
414 *errval = DIAG_ERR_TIMEOUT;
415 return NULL;
416 }
417
418 /* Return the message to user, who is responsible for freeing it */
419 rmsg = d_l2_conn->diag_msg;
420 d_l2_conn->diag_msg = NULL;
421 return rmsg;
422}
423
424const struct diag_l2_proto diag_l2_proto_saej1850 = {
425 DIAG_L2_PROT_SAEJ1850,
426 "SAEJ1850",
427 DIAG_L2_FLAG_FRAMED | DIAG_L2_FLAG_CONNECTS_ALWAYS,
428 dl2p_j1850_startcomms,
429 dl2p_j1850_stopcomms,
430 dl2p_j1850_send,
431 dl2p_j1850_recv,
432 dl2p_j1850_request,
433 NULL
434};
435