BOROSOFTWAREAll work →
← scantool/diag_l3.c 326 lines
2 findings in this fileB14L138A4L317
B14A4
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 * L3 code, interface to diagnostic protocols such as SAEJ1979 (ODB II), VAG,
24 * etc
25 *
26 */
27
28#include <assert.h>
29#include <stdbool.h>
30#include <stdlib.h>
31
32#include "diag.h"
33#include "diag_os.h"
34#include "diag_err.h"
35#include "diag_l1.h"
36#include "diag_l2.h"
37#include "diag_l3.h"
38
39#include "utlist.h"
40
41int diag_l3_debug;
42
43static diag_mtx connlist_mtx;
44static struct diag_l3_conn *diag_l3_list;
45static bool init_done;
46
47void diag_l3_init(void) {
48 if (init_done) {
49 return;
50 }
51
52 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_INIT, DIAG_DBGLEVEL_V,
53 FLFMT "entered diag_l3_init\n", FL);
54
55 diag_os_initstaticmtx(&connlist_mtx);
56
57 init_done = true;
58 return;
59}
60
61void diag_l3_end(void) {
62 diag_os_delmtx(&connlist_mtx);
63 init_done = false;
64 return;
65}
66
67struct diag_l3_conn *diag_l3_start(const char *protocol, struct diag_l2_conn *d_l2_conn) {
68 struct diag_l3_conn *d_l3_conn = NULL;
69 unsigned int i;
70 int rv;
71 const struct diag_l3_proto *dp;
72
73 assert(d_l2_conn != NULL);
74
75 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
76 FLFMT "start protocol %s l2 %p\n",
77 FL, protocol, (void *)d_l2_conn);
78
79 /* Find the protocol */
80 dp = NULL;
81 for (i=0; diag_l3_protocols[i]; i++) {
82 if (strcasecmp(protocol, diag_l3_protocols[i]->proto_name) == 0) {
83 dp = diag_l3_protocols[i]; /* Found. */
84 break;
85 }
86 }
87
88 if (dp) {
89 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
90 FLFMT "start protocol %s found\n", FL, dp->proto_name);
91 /*
92 * Malloc us a L3
93 */
94 rv = diag_calloc(&d_l3_conn, 1);
95 if (rv != 0) {
96 return diag_pfwderr(rv);
97 }
98
99 d_l3_conn->d_l3l2_conn = d_l2_conn;
100 d_l3_conn->d_l3_proto = dp;
101
102 /* Get L2 flags */
103 (void)diag_l2_ioctl(d_l2_conn,
104 DIAG_IOCTL_GET_L2_FLAGS,
105 &d_l3_conn->d_l3l2_flags);
106
107 /* Get L1 flags */
108 (void)diag_l2_ioctl(d_l2_conn,
109 DIAG_IOCTL_GET_L1_FLAGS,
110 &d_l3_conn->d_l3l1_flags);
111
112 /* Call the proto routine */
113 rv = dp->diag_l3_proto_start(d_l3_conn);
114 if (rv < 0) {
115 free(d_l3_conn);
116 return diag_pfwderr(rv);
117 }
118 /*
119 * Set time to now
120 */
121 d_l3_conn->timer=diag_os_getms();
122
123 /*
124 * And add to list
125 */
126 diag_os_lock(&connlist_mtx);
127 LL_PREPEND(diag_l3_list, d_l3_conn);
128 diag_os_unlock(&connlist_mtx);
129 }
130
131 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_OPEN, DIAG_DBGLEVEL_V,
132 FLFMT "start returns %p\n", FL, (void *)d_l3_conn);
133
134 return d_l3_conn;
135}
136
137
138int diag_l3_stop(struct diag_l3_conn *d_l3_conn) {
139 int rv;
140
141 assert(d_l3_conn != NULL);
142
143 const struct diag_l3_proto *dp = d_l3_conn->d_l3_proto;
144
145 /* Remove from list */
146 diag_os_lock(&connlist_mtx);
147 LL_DELETE(diag_l3_list, d_l3_conn);
148 diag_os_unlock(&connlist_mtx);
149
150 rv = dp->diag_l3_proto_stop(d_l3_conn);
151 if (d_l3_conn->msg != NULL) {
152 diag_freemsg(d_l3_conn->msg);
153 }
154
155 free(d_l3_conn);
156
157 return rv? diag_ifwderr(rv):0;
158}
159
160int diag_l3_send(struct diag_l3_conn *d_l3_conn, struct diag_msg *msg) {
161 int rv;
162 const struct diag_l3_proto *dp = d_l3_conn->d_l3_proto;
163
164 rv = dp->diag_l3_proto_send(d_l3_conn, msg);
165
166 if (!rv) {
167 d_l3_conn->timer = diag_os_getms();
168 }
169
170 return rv? diag_ifwderr(rv):0;
171}
172
173int diag_l3_recv(struct diag_l3_conn *d_l3_conn, unsigned int timeout,
174 void (* rcv_call_back)(void *handle,struct diag_msg *), void *handle) {
175 const struct diag_l3_proto *dp = d_l3_conn->d_l3_proto;
176 int rv;
177
178 rv=dp->diag_l3_proto_recv(d_l3_conn, timeout,
179 rcv_call_back, handle);
180
181 if (rv == 0) {
182 d_l3_conn->timer = diag_os_getms();
183 }
184
185 if (rv == DIAG_ERR_TIMEOUT) {
186 return rv;
187 }
188 return rv? diag_ifwderr(rv):0;
189}
190
191
192void diag_l3_decode(struct diag_l3_conn *d_l3_conn,
193 struct diag_msg *msg, char *buf, const size_t bufsize) {
194 const struct diag_l3_proto *dp = d_l3_conn->d_l3_proto;
195
196 dp->diag_l3_proto_decode(d_l3_conn, msg, buf, bufsize);
197 return;
198}
199
200
201int diag_l3_ioctl(struct diag_l3_conn *d_l3_conn, unsigned int cmd, void *data) {
202 const struct diag_l3_proto *dp = d_l3_conn->d_l3_proto;
203
204 /* Call the L3 ioctl routine if applicable */
205 if (dp->diag_l3_proto_ioctl) {
206 return dp->diag_l3_proto_ioctl(d_l3_conn, cmd, data);
207 }
208
209 /* Otherwise L2 ioctl */
210 return diag_l2_ioctl(d_l3_conn->d_l3l2_conn, cmd, data);
211}
212
213
214struct diag_msg *diag_l3_request(struct diag_l3_conn *dl3c, struct diag_msg *txmsg, int *errval) {
215 struct diag_msg *rxmsg;
216 const struct diag_l3_proto *dl3p = dl3c->d_l3_proto;
217
218 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
219 FLFMT "_request dl3c=%p msg=%p called\n",
220 FL, (void *)dl3c, (void *)txmsg);
221
222 /* Call protocol specific send routine */
223 if (dl3p->diag_l3_proto_request) {
224 rxmsg = dl3p->diag_l3_proto_request(dl3c, txmsg, errval);
225 } else {
226 rxmsg = NULL;
227 }
228
229 DIAG_DBGM(diag_l3_debug, DIAG_DEBUG_WRITE, DIAG_DBGLEVEL_V,
230 FLFMT "_request returns %p, err %d\n",
231 FL, (void *)rxmsg, *errval);
232
233 if (rxmsg==NULL) {
234 return diag_pfwderr(*errval);
235 }
236 //update timers
237 dl3c->timer = diag_os_getms();
238
239 return rxmsg;
240}
241
242/*
243 * Note: This is called regularly from a signal handler.
244 * (see diag_os.c)
245 * XXX This calls non async-signal-safe functions!
246 */
247void diag_l3_timer(void) {
248 /*
249 * Regular timer routine
250 * Call protocol specific timer
251 */
252 struct diag_l3_conn *conn;
253 unsigned long now=diag_os_getms();
254
255 if (periodic_done() || !diag_os_trylock(&connlist_mtx)) {
256 return;
257 }
258
259 LL_FOREACH(diag_l3_list, conn) {
260 /* Call L3 timer routine for this connection */
261 const struct diag_l3_proto *dp = conn->d_l3_proto;
262
263 //skip connection if L1 does the keepalive stuff
264 if (conn->d_l3l1_flags & DIAG_L1_DOESKEEPALIVE) {
265 continue;
266 }
267
268 if (dp->diag_l3_proto_timer) {
269 unsigned long diffms;
270
271 diffms = now - conn->timer;
272
273 (void) dp->diag_l3_proto_timer(conn, diffms);
274 }
275 }
276 diag_os_unlock(&connlist_mtx);
277}
278
279
280/* Base implementations for some functions */
281
282
283int diag_l3_base_start(UNUSED(struct diag_l3_conn *d_l3_conn)) {
284 return 0;
285}
286
287
288int diag_l3_base_stop(UNUSED(struct diag_l3_conn *d_l3_conn)) {
289 return 0;
290}
291
292int diag_l3_base_send(struct diag_l3_conn *d_l3_conn,
293 UNUSED(struct diag_msg *msg)) {
294 d_l3_conn->timer=diag_os_getms();
295 return 0;
296}
297
298
299int diag_l3_base_recv(struct diag_l3_conn *d_l3_conn,
300 UNUSED(unsigned int timeout),
301 UNUSED(void (* rcv_call_back)(void *handle,struct diag_msg *)),
302 UNUSED(void *handle)) {
303 d_l3_conn->timer=diag_os_getms();
304 return 0;
305}
306
307//this implementation is rather naive and untested. It simply forwards the
308//txmsg straight to the L2 request function and returns the response msg
309//as-is.
310struct diag_msg *diag_l3_base_request(struct diag_l3_conn *dl3c,
311 struct diag_msg *txmsg, int *errval) {
312
313 struct diag_msg *rxmsg = NULL;
314
315 *errval=0;
316
317 rxmsg = diag_l2_request(dl3c->d_l3l2_conn, txmsg, errval);
318
319 if (rxmsg == NULL) {
320 return diag_pfwderr(*errval);
321 }
322 dl3c->timer=diag_os_getms();
323
324 return rxmsg;
325}
326