BOROSOFTWAREAll work →
← scantool/diag_l2_d2.c 322 lines
1 finding in this fileD19L227
D19
1/*
2 * freediag - Vehicle Diagnostic Utility
3 *
4 *
5 * This program is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 *
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
14 *
15 * You should have received a copy of the GNU General Public License
16 * along with this program; if not, write to the Free Software
17 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18 *
19 *************************************************************************
20 *
21 * Diag
22 *
23 * L2 driver for Volvo D2 protocol over K-line (keyword D3 B0)
24 *
25 * This protocol is used by the engine and chassis ECUs for extended
26 * diagnostics on the 1996-1998 Volvo 850, S40, C70, S70, V70, XC70, V90 and
27 * possibly other models.
28 *
29 * The message headers are similar, but not identical, to KWP2000.
30 * In KWP2000, the length value in the header represents the number of
31 * data bytes only in the message; here, it also includes the trailing
32 * checksum byte -- that is, the length value is 1 greater than it would be
33 * in KWP2000.
34 *
35 * See diag_l7_d2 for the corresponding application protocol.
36 *
37 * This driver currently works only with ELM327 interfaces.
38 *
39 */
40
41#include <stdlib.h>
42#include <stdint.h>
43#include <stdio.h>
44#include <string.h>
45
46#include "diag.h"
47#include "diag_tty.h"
48#include "diag_err.h"
49#include "diag_os.h"
50#include "diag_l1.h"
51#include "diag_l2.h"
52#include "diag_l2_d2.h"
53
54/* replace a byte's msb with a parity bit */
55static uint8_t with_parity(uint8_t c, enum diag_parity eo) {
56 uint8_t p;
57 int i;
58
59 p = 0;
60 if (eo == diag_par_o) {
61 p = 1;
62 }
63
64 for (i = 0; i < 7; i++) {
65 p ^= c; p <<= 1;
66 }
67
68 return((c&0x7f)|(p&0x80));
69}
70
71static int dl2p_d2_send(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg) {
72 int rv;
73 uint8_t buf[3 + 62 + 1];
74 struct diag_l2_d2 *dp;
75
76 dp = (struct diag_l2_d2 *)d_l2_conn->diag_l2_proto_data;
77
78 if (msg->len < 1 || msg->len > 62) {
79 return diag_iseterr(DIAG_ERR_BADLEN);
80 }
81
82 buf[0] = 0x80 + msg->len + 1;
83 buf[1] = msg->dest ? msg->dest : dp->dstaddr;
84 buf[2] = msg->src ? msg->src : dp->srcaddr;
85
86 memcpy(&buf[3], msg->data, msg->len);
87
88 diag_os_millisleep(d_l2_conn->diag_l2_p3min);
89
90 rv = diag_l1_send(d_l2_conn->diag_link->l2_dl0d, buf,
91 msg->len + 3, d_l2_conn->diag_l2_p4min);
92
93 return rv?diag_ifwderr(rv):0;
94}
95
96static int dl2p_d2_recv(struct diag_l2_conn *d_l2_conn, unsigned int timeout,
97 void (*callback)(void *handle, struct diag_msg *msg),
98 void *handle) {
99 int rv;
100 uint8_t buf[3 + 62 + 1];
101 struct diag_msg *msg;
102
103 rv = diag_l1_recv(d_l2_conn->diag_link->l2_dl0d, buf,
104 sizeof(buf), timeout + 100);
105 if (rv < 0) {
106 return rv;
107 }
108
109 if (rv < 5) {
110 return diag_iseterr(DIAG_ERR_INCDATA);
111 }
112
113 msg = diag_allocmsg((size_t)(rv - 4));
114 if (msg == NULL) {
115 return diag_iseterr(DIAG_ERR_NOMEM);
116 }
117 memcpy(msg->data, &buf[3], (size_t)(rv - 4));
118 msg->rxtime = diag_os_getms();
119 msg->src = buf[2];
120 msg->dest = buf[1];
121 msg->fmt = DIAG_FMT_FRAMED;
122
123 if (callback) {
124 callback(handle, msg);
125 }
126
127 diag_freemsg(msg);
128
129 return 0;
130}
131
132static void dl2p_d2_request_callback(void *handle, struct diag_msg *in) {
133 struct diag_msg **out = (struct diag_msg **)handle;
134 *out = diag_dupsinglemsg(in);
135}
136
137static struct diag_msg *dl2p_d2_request(struct diag_l2_conn *d_l2_conn, struct diag_msg *msg,
138 int *errval) {
139 int rv;
140 struct diag_msg *rmsg = NULL;
141
142 *errval = 0;
143
144 rv = diag_l2_send(d_l2_conn, msg);
145 if (rv < 0) {
146 *errval = rv;
147 return NULL;
148 }
149
150 do {
151 if (rmsg != NULL) {
152 diag_freemsg(rmsg);
153 }
154 rv = dl2p_d2_recv(d_l2_conn, 1000, dl2p_d2_request_callback, &rmsg);
155 if (rv < 0) {
156 *errval = rv;
157 return NULL;
158 }
159 if (rmsg == NULL) {
160 *errval = DIAG_ERR_NOMEM;
161 return NULL;
162 }
163 /* If we got routineNotCompleteOrServiceInProgress, loop until
164 the final response. */
165 } while (rmsg->len==3 && rmsg->data[0]==0x7e && rmsg->data[1]==msg->data[0] && rmsg->data[2]==0x23);
166
167 return rmsg;
168}
169
170static int dl2p_d2_startcomms(struct diag_l2_conn *d_l2_conn, flag_type flags,
171 unsigned int bitrate, target_type target, source_type source) {
172 struct diag_serial_settings set;
173 struct diag_l2_d2 *dp;
174 int rv;
175 struct diag_msg wm = {0};
176 uint8_t wm_data[] = {0x82, 0, 0, 0xa1};
177 struct diag_l1_initbus_args in;
178
179 if (!(d_l2_conn->diag_link->l1flags & DIAG_L1_DOESFULLINIT) || !(d_l2_conn->diag_link->l1flags & DIAG_L1_DOESL2CKSUM)) {
180 fprintf(stderr, "Can't do D2 over K-line on this L0 interface yet, sorry.\n");
181 return diag_iseterr(DIAG_ERR_PROTO_NOTSUPP);
182 }
183
184 if ((flags & DIAG_L2_TYPE_INITMASK) != DIAG_L2_TYPE_SLOWINIT) {
185 return diag_iseterr(DIAG_ERR_INIT_NOTSUPP);
186 }
187
188 rv = diag_calloc(&dp, 1);
189 if (rv != 0) {
190 return diag_ifwderr(rv);
191 }
192
193 d_l2_conn->diag_l2_proto_data = (void *)dp;
194
195 if (source != 0x13) {
196 fprintf(stderr,
197 "Warning : Using tester address %02X. Some ECUs "
198 "require tester address to be 13.\n",
199 source);
200 }
201
202 dp->srcaddr = source;
203 dp->dstaddr = target;
204
205 if (bitrate == 0) {
206 bitrate = 10400;
207 }
208 d_l2_conn->diag_l2_speed = bitrate;
209
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,
216 (void *)&set))) {
217 goto err;
218 }
219
220 (void)diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_IFLUSH, NULL);
221 diag_os_millisleep(300);
222
223 wm_data[1] = dp->dstaddr;
224 wm_data[2] = dp->srcaddr;
225 wm.data = wm_data;
226 wm.len = sizeof(wm_data);
227 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_SETWM, &wm);
228 if (rv < 0) {
229 goto err;
230 }
231
232 in.type = DIAG_L1_INITBUS_5BAUD;
233 in.addr = with_parity(target, diag_par_o);
234 in.testerid = dp->srcaddr;
235 in.kb1 = 0; in.kb2 = 0;
236 rv = diag_l2_ioctl(d_l2_conn, DIAG_IOCTL_INITBUS, &in);
237 if (rv < 0) {
238 goto err;
239 }
240
241 if (in.kb1 == 0 && in.kb2 == 0) {
242 d_l2_conn->diag_l2_kb1 = 0xd3;
243 d_l2_conn->diag_l2_kb2 = 0xb0;
244 fprintf(stderr, FLFMT "_startcomms : L0 didn't return keybytes, continuing anyway\n", FL);
245 } else {
246 d_l2_conn->diag_l2_kb1 = in.kb1;
247 d_l2_conn->diag_l2_kb2 = in.kb2;
248 }
249
250 if ((d_l2_conn->diag_l2_kb1 != 0xd3) || (d_l2_conn->diag_l2_kb2 != 0xb0)) {
251 fprintf(stderr, FLFMT "_startcomms : wrong keybytes %02X%02X, expecting D3B0\n",
252 FL, d_l2_conn->diag_l2_kb1, d_l2_conn->diag_l2_kb2);
253 rv = DIAG_ERR_WRONGKB;
254 goto err;
255 }
256
257 return 0;
258
259err:
260 free(dp);
261 d_l2_conn->diag_l2_proto_data = NULL;
262 return diag_iseterr(rv);
263}
264
265static int dl2p_d2_stopcomms(struct diag_l2_conn *pX) {
266 struct diag_msg msg = {0};
267 uint8_t data[] = { 0xa0 };
268 int errval = 0;
269 static struct diag_msg *rxmsg;
270
271 msg.len = 1;
272 msg.dest = 0; msg.src = 0; /* use default addresses */
273 msg.data = data;
274
275 rxmsg = dl2p_d2_request(pX, &msg, &errval);
276
277 if (rxmsg == NULL || errval) {
278 fprintf(stderr, "StopDiagnosticSession request failed, waiting for session to time out.\n");
279 diag_os_millisleep(5000);
280 }
281
282 if (rxmsg != NULL) {
283 diag_freemsg(rxmsg);
284 }
285
286 if (pX->diag_l2_proto_data) {
287 free(pX->diag_l2_proto_data);
288 pX->diag_l2_proto_data=NULL;
289 }
290
291 return 0;
292}
293
294static void dl2p_d2_timeout(struct diag_l2_conn *d_l2_conn) {
295 struct diag_msg msg = {0};
296 uint8_t data[] = { 0xa1 };
297 int errval = 0;
298 static struct diag_msg *rxmsg;
299
300 msg.len = 1;
301 msg.dest = 0; msg.src = 0; /* use default addresses */
302 msg.data = data;
303
304 rxmsg = dl2p_d2_request(d_l2_conn, &msg, &errval);
305
306 if (rxmsg != NULL) {
307 diag_freemsg(rxmsg);
308 }
309}
310
311const struct diag_l2_proto diag_l2_proto_d2 = {
312 DIAG_L2_PROT_D2,
313 "D2",
314 DIAG_L2_FLAG_FRAMED | DIAG_L2_FLAG_KEEPALIVE,
315 dl2p_d2_startcomms,
316 dl2p_d2_stopcomms,
317 dl2p_d2_send,
318 dl2p_d2_recv,
319 dl2p_d2_request,
320 dl2p_d2_timeout
321};
322