BOROSOFTWAREAll work →
← scantool/diag_l2_mb1.c 430 lines
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 * L2 driver for Mercedes Benz protocol used on things like the
24 * EGS (auto gearbox controller) on 1999/2000/2001 cars
25 * I have called this Mercedes Benz protocol 1, since all other control
26 * units I have played with use ISO14230
27 * TODO :
28 */
29
30
31#include <stdint.h>
32#include <string.h>
33
34#include "diag.h"
35#include "diag_err.h"
36#include "diag_os.h"
37#include "diag_tty.h"
38#include "diag_l1.h"
39#include "diag_l2.h"
40
41#include "diag_l2_mb1.h" /* prototypes for this file */
42
43
44static int dl2p_mb1_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
45 uint8_t *data, int len);
46
47
48static int dl2p_mb1_startcomms( struct diag_l2_conn *d_l2_conn,
49 UNUSED(flag_type flags),
50 unsigned int bitrate,
51 target_type target,
52 UNUSED(source_type source)) {
53 struct diag_l1_initbus_args in;
54 uint8_t cbuf[2];
55 int rv;
56 unsigned int baud;
57 uint8_t rxbuf[MAXRBUF];
58 struct diag_serial_settings set;
59
60 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
61 FLFMT "startcomms conn %p\n", FL, (void *)d_l2_conn);
62
63 /*
64 * If 0 has been specified, use a suitable default
65 */
66 if (bitrate == 0) {
67 baud = 9600;
68 } else {
69 baud = bitrate;
70 }
71 d_l2_conn->diag_l2_speed = baud;
72
73 set.speed = bitrate;
74 set.databits = diag_databits_8;
75 set.stopbits = diag_stopbits_1;
76 set.parflag = diag_par_n;
77
78 /* Set the speed as shown */
79 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETSPEED, (void *) &set);
80 if (rv < 0) {
81 return diag_ifwderr(rv);
82 }
83
84 /* Flush unread input, then wait for idle bus. */
85 (void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
86 diag_os_millisleep(300);
87
88 /*
89 * Do 5Baud init
90 */
91 in.type = DIAG_L1_INITBUS_5BAUD;
92 in.addr = target;
93 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
94 if (rv < 0) {
95 return diag_ifwderr(rv);
96 }
97
98 /*
99 * L0 code should have set correct baud rate now etc
100 * Now read keybytes, ignoring parity
101 */
102 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
103 cbuf, 1, 100);
104 if (rv < 0) {
105 return diag_ifwderr(rv);
106 }
107 rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
108 &cbuf[1], 1, 100);
109 if (rv < 0) {
110 return diag_ifwderr(rv);
111 }
112
113 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
114 FLFMT "startcomms conn %p got kb 0x%X 0x%X\n",
115 FL, (void *)d_l2_conn, cbuf[0], cbuf[1]);
116
117 /*
118 * Check the received keybytes
119 */
120 d_l2_conn->diag_l2_kb1 = cbuf[0];
121 d_l2_conn->diag_l2_kb2 = cbuf[1];
122
123 if (cbuf[0] != 0xC2) {
124 return diag_iseterr(DIAG_ERR_WRONGKB);
125 }
126 if (cbuf[1] != 0xCD) {
127 return diag_iseterr(DIAG_ERR_WRONGKB);
128 }
129
130 /*
131 * Set the P3max (idle timer) to 1 second
132 */
133 d_l2_conn->diag_l2_p3max = 1000;
134
135 /*
136 * Now we probably get a message back that we don't want
137 * particularly that tells us the ecu part num, h/w and s/w versions
138 */
139 (void) dl2p_mb1_int_recv(d_l2_conn, 1000, rxbuf, sizeof(rxbuf));
140
141 return diag_iseterr(rv);
142}
143
144static int dl2p_mb1_stopcomms(UNUSED(struct diag_l2_conn *dl2c)) {
145 return 0;
146}
147
148/*
149 * Decode the message header, and check the message is complete
150 * and that the checksum is correct.
151 *
152 * Once we know the actual msglen, *msglen is filled in, else it is set to 0
153 *
154 * Data/len is received data/len
155 */
156static int dl2p_mb1_decode(uint8_t *data, int len, int *msglen) {
157 uint16_t cksum;
158 int i;
159
160 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V, data, len,
161 FLFMT "decode len %d; ", FL, len);
162
163 *msglen = 0;
164
165 if (len < 3) {
166 return diag_iseterr(DIAG_ERR_INCDATA);
167 }
168
169 if (data[2] > len) {
170 return diag_iseterr(DIAG_ERR_INCDATA);
171 }
172
173 *msglen = data[3];
174
175 for (i = 0, cksum = 0; i < len - 2; i++) {
176 cksum += data[i];
177 }
178 if (data[len-2] != (cksum &0xff)) {
179 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
180 FLFMT "recv cksum 0x%02X 0x%02X, wanted 0x%X\n",
181 FL, data[len - 1] & 0xff, data[len - 2] & 0xff,
182 cksum & 0xffff);
183 return diag_iseterr(DIAG_ERR_BADCSUM);
184 }
185 if (data[len-1] != ((cksum>>8) & 0xff)) {
186 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
187 FLFMT "recv cksum 0x%02X 0x%02X, wanted 0x%X\n",
188 FL, data[len - 1] & 0xff, data[len - 2] & 0xff,
189 cksum & 0xffff);
190 return diag_iseterr(DIAG_ERR_BADCSUM);
191 }
192 return 0;
193}
194
195/*
196 * Internal receive, reads a whole message from the ECU -
197 * returns the data length of the packet received
198 */
199static int dl2p_mb1_int_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
200 uint8_t *data, int len) {
201 int rxoffset, rv;
202 unsigned int tout;
203 int msglen;
204 size_t readlen;
205
206 rxoffset = 0;
207 tout = timeout;
208 msglen = 0;
209 readlen = 3;
210 while ( (rv = diag_l1_recv (d_l2_conn->diag_link->l2_dl0d,
211 &data[rxoffset], readlen, tout)) > 0) {
212 rxoffset += rv;
213 tout = 100;
214
215 /* Got some data */
216
217 rv = dl2p_mb1_decode(data, rxoffset, &msglen);
218
219 if (rv >= 0) {
220 /* Full packet ! */
221 break;
222 }
223 if (rv != DIAG_ERR_INCDATA) {
224 /* Bad things happened */
225 rxoffset = rv;
226 break;
227 }
228
229 /* Not full, read some more */
230
231 if (msglen) {
232 readlen = msglen - rxoffset;
233 } else if (rxoffset < 3) {
234 readlen = 3;
235 } else {
236 readlen = len - rxoffset;
237 }
238 }
239 return rxoffset;
240}
241
242
243/*
244 * Read data, attempts to get complete set of responses
245 *
246 */
247static int dl2p_mb1_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
248 void (*callback)(void *handle, struct diag_msg *msg), void *handle) {
249 uint8_t rxbuf[MAXRBUF];
250 int rv;
251 struct diag_msg *msg;
252
253 rv = dl2p_mb1_int_recv(d_l2_conn, timeout, rxbuf,
254 sizeof(rxbuf));
255
256 if (rv < 0 || rv > (255 + 4)) {
257 return diag_iseterr(DIAG_ERR_GENERAL);
258 }
259
260 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
261 FLFMT "recv conn %p got %d byte message\n",
262 FL, (void *)d_l2_conn, rv);
263
264 if (rv < 5) {
265 /* Bad, minimum message is 5 bytes */
266 return diag_iseterr(DIAG_ERR_BADDATA);
267 }
268
269 /*
270 * Ok, alloc a message
271 */
272 msg = diag_allocmsg((size_t)(rv - 4));
273 if (msg == NULL) {
274 return diag_iseterr(DIAG_ERR_NOMEM);
275 }
276 msg->data[0] = rxbuf[1]; /* Command */
277 memcpy(&msg->data[1], &rxbuf[3], (size_t)(rv - 3)); /* Data */
278 msg->rxtime = diag_os_getms();
279 msg->fmt = DIAG_FMT_FRAMED;
280
281 /*
282 * Call user callback routine
283 */
284 if (callback) {
285 callback(handle, msg);
286 }
287
288 /* No longer needed */
289 diag_freemsg(msg);
290
291 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
292 FLFMT "recv() callback completed\n", FL);
293
294 return 0;
295}
296
297/*
298 * Send the data, using MB1 protocol
299 * ret 0 if ok
300 */
301static int dl2p_mb1_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
302 int rv;
303 unsigned int sleeptime;
304 uint8_t txbuf[MAXRBUF];
305 uint16_t cksum;
306 unsigned i;
307
308 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
309 FLFMT "diag_l2_send %p, msg %p called\n",
310 FL, (void *)d_l2_conn, (void *)msg);
311
312 /*
313 * Make sure enough time between last receive and this send
314 * In fact, because of the timeout on recv(), this is pretty small, but
315 * we take the safe road and wait the whole of p3min plus whatever
316 * delay happened before
317 */
318 sleeptime = d_l2_conn->diag_l2_p3min;
319 if (sleeptime > 0) {
320 diag_os_millisleep(sleeptime);
321 }
322
323 txbuf[0] = d_l2_conn->diag_l2_destaddr;
324 txbuf[1] = msg->data[0]; /* Command */
325 txbuf[2] = msg->len + 4; /* Data + Hdr + 2 byte checksum */
326 memcpy(&txbuf[3], &msg->data[1], (size_t)(msg->len-1));
327
328 /* Checksum is 16 bit addition, in LSB order on packet */
329 for (i = 0, cksum = 0; i < (msg->len + 2); i++) {
330 cksum += txbuf[i];
331 }
332
333 txbuf[msg->len+2] = (uint8_t) (cksum & 0xff);
334 txbuf[msg->len+3] = (uint8_t) ((cksum>>8) & 0xff);
335
336 DIAG_DBGMDATA(diag_l2_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
337 txbuf, txbuf[2],
338 FLFMT "send %d bytes; ", FL, txbuf[2]);
339
340 rv = diag_l1_send (d_l2_conn->diag_link->l2_dl0d,
341 txbuf, txbuf[2], d_l2_conn->diag_l2_p4min);
342
343
344 return rv? diag_ifwderr(rv):0;
345}
346
347static struct diag_msg *dl2p_mb1_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
348 int *errval) {
349 int rv;
350 struct diag_msg *rmsg = NULL;
351 uint8_t rxbuf[MAXRBUF];
352
353 rv = diag_l2_send(d_l2_conn, msg);
354 if (rv < 0) {
355 *errval = rv;
356 return diag_pseterr(DIAG_ERR_GENERAL);
357 }
358 /* And wait for response for 1 second */
359
360 rv = dl2p_mb1_int_recv(d_l2_conn, 1000, rxbuf,
361 sizeof(rxbuf));
362
363 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_READ, DIAG_DBGLEVEL_V,
364 FLFMT "msg receive conn %p got %d byte message\n", FL,
365 (void *)d_l2_conn, rv);
366
367 if (rv < 5 || rv > (255+4)) {
368 /* Bad, minimum message is 5 bytes, or error happened */
369 if (rv < 0) {
370 *errval = rv;
371 } else {
372 *errval = DIAG_ERR_BADDATA;
373 }
374 } else {
375 /*
376 * Ok, alloc a message
377 */
378 rmsg = diag_allocmsg((size_t)(rv - 4));
379 if (rmsg == NULL) {
380 return diag_pseterr(DIAG_ERR_NOMEM);
381 }
382 rmsg->data[0] = rxbuf[1]; /* Command */
383 memcpy(&rmsg->data[1], &rxbuf[3], (size_t)(rv - 3)); /* Data */
384 rmsg->rxtime = diag_os_getms();
385 rmsg->fmt = DIAG_FMT_FRAMED;
386 }
387 return rmsg;
388}
389
390
391/*
392 * Timeout, called to send idle packet to keep link to ECU alive
393 */
394static void dl2p_mb1_timeout(struct diag_l2_conn *d_l2_conn) {
395 struct diag_msg msg = {0};
396 uint8_t txbuf[8];
397 uint8_t rxbuf[1000];
398 int rv;
399
400 /* XXX Not async-signal-safe */
401 DIAG_DBGM(diag_l2_debug, DIAG_DEBUG_TIMER, DIAG_DBGLEVEL_V,
402 FLFMT "timeout conn %p\n", FL, (void *)d_l2_conn);
403
404 txbuf[0] = 0x50; /* Idle command */
405 txbuf[1] = 0x01;
406 msg.data = txbuf;
407 msg.len = 2;
408
409 /* Use l2_send as it updates timeout timers */
410 rv = diag_l2_send(d_l2_conn, &msg);
411
412 /* And receive/ignore the response */
413 if (rv >= 0) {
414 (void)dl2p_mb1_int_recv(d_l2_conn, 1000, rxbuf, sizeof(rxbuf));
415 }
416 return;
417}
418
419const struct diag_l2_proto diag_l2_proto_mb1 = {
420 DIAG_L2_PROT_MB1,
421 "MB1",
422 DIAG_L2_FLAG_FRAMED | DIAG_L2_FLAG_KEEPALIVE,
423 dl2p_mb1_startcomms,
424 dl2p_mb1_stopcomms,
425 dl2p_mb1_send,
426 dl2p_mb1_recv,
427 dl2p_mb1_request,
428 dl2p_mb1_timeout
429};
430