BOROSOFTWAREAll work →
← scantool/diag_general.c 528 lines
2 findings in this fileA6L158A5L297
A6A5A6
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 *
24 * General routines
25 *
26 */
27#include <errno.h>
28#include <stdlib.h>
29#include <string.h>
30#include <assert.h>
31
32#include <stdbool.h>
33#include <stdint.h>
34#include <stdio.h>
35
36#include "diag.h"
37#include "diag_os.h"
38#include "diag_err.h"
39#include "diag_dtc.h"
40#include "diag_l1.h"
41#include "diag_l2.h"
42#include "diag_l3.h"
43
44#include "utlist.h"
45
46
47const char *dbg_prefixes[] = {
48 [DIAG_DEBUGPF_NONE] = "GLOB:",
49 [DIAG_DEBUGPF_OPEN] = "OPEN:",
50 [DIAG_DEBUGPF_CLOSE] = "CLOSE:",
51 [DIAG_DEBUGPF_READ] = "READ:",
52 [DIAG_DEBUGPF_WRITE] = "WRITE:",
53 [DIAG_DEBUGPF_IOCTL] = "IOCTL:",
54 [DIAG_DEBUGPF_PROTO] = "PROTO:",
55 [DIAG_DEBUGPF_INIT] = "INIT:",
56 [DIAG_DEBUGPF_DATA] = "DATA:",
57 [DIAG_DEBUGPF_TIMER] = "TIMER:",
58};
59
60
61
62static diag_atomic_bool periodic_done_wrapper;
63
64static void set_periodic_done(void) {
65 diag_atomic_store_bool(&periodic_done_wrapper, true);
66}
67
68bool periodic_done(void) {
69 return diag_atomic_load_bool(&periodic_done_wrapper);
70}
71
72static int diag_initialized = 0;
73
74// diag_init : should be called once before doing anything.
75// and call diag_end before terminating.
76int diag_init(void) { // returns 0 if normal exit
77 int rv;
78
79 if (diag_initialized) {
80 return 0;
81 }
82
83 // XXX This is interesting: the following functions only ever return 0...
84
85 if ((rv = diag_l1_init())) {
86 return diag_ifwderr(rv);
87 }
88 if ((rv = diag_l2_init())) {
89 return diag_ifwderr(rv);
90 }
91 diag_l3_init();
92
93 diag_atomic_init(&periodic_done_wrapper);
94 if ((rv = diag_os_init())) {
95 diag_atomic_del(&periodic_done_wrapper);
96 return diag_ifwderr(rv);
97 }
98
99 diag_dtc_init();
100 diag_initialized = 1;
101
102 return 0;
103}
104
105// must be called before exiting. Ret 0 if ok
106// this is the "opposite" of diag_init
107int diag_end(void) {
108 int rv = 0;
109 if (!diag_initialized) {
110 return 0;
111 }
112
113 set_periodic_done();
114 if (diag_os_close()) {
115 fprintf(stderr, FLFMT "Could not close OS functions!\n", FL);
116 rv = -1;
117 }
118 diag_l3_end();
119 if (diag_l2_end()) {
120 fprintf(stderr, FLFMT "Could not close L2 level\n", FL);
121 rv = -1;
122 }
123 if (diag_l1_end()) {
124 fprintf(stderr, FLFMT "Could not close L1 level\n", FL);
125 rv = -1;
126 }
127
128 // There would be a race with the periodic timer, trying to take the lock, if we
129 // deleted the mutex.
130 // XXX why ? diag_os_close should've taken care of killing the periodic timer
131 diag_atomic_del(&periodic_done_wrapper);
132
133 // nothing to do for diag_dtc_init
134
135 diag_initialized = 0;
136 return rv;
137}
138
139
140/** Message handling **/
141
142struct diag_msg *diag_allocmsg(size_t datalen) {
143 struct diag_msg *newmsg;
144 int rv;
145
146 if (datalen > DIAG_MAX_MSGLEN) {
147 fprintf(stderr, FLFMT "_allocmsg with >%d bytes !? report this !\n", FL, DIAG_MAX_MSGLEN);
148 return diag_pseterr(DIAG_ERR_BADLEN);
149 }
150
151 rv = diag_calloc(&newmsg, 1);
152 if (rv != 0) {
153 return diag_pfwderr(rv);
154 }
155
156 newmsg->iflags |= DIAG_MSG_IFLAG_MALLOC;
157
158 if (datalen) {
159 rv = diag_calloc(&newmsg->idata, datalen);
160 if (rv != 0) {
161 free(newmsg);
162 return diag_pfwderr(rv);
163 }
164 } else {
165 newmsg->idata = NULL;
166 }
167
168 newmsg->len=datalen;
169 newmsg->next=NULL;
170 newmsg->data = newmsg->idata; /* Keep tab as users change newmsg->data */
171 // i.e. some functions do (diagmsg->data += skiplen) which would prevent us
172 // from doing free(diagmsg->data) (the pointer was changed).
173 // so ->idata is the original alloc'ed pointer, that should never be modified
174 // except by diag_freemsg()
175
176 return newmsg;
177}
178
179/* Duplicate a message, and its contents including all chained messages. XXX nobody uses this !? */
180struct diag_msg *diag_dupmsg(struct diag_msg *msg) {
181 struct diag_msg *newchain, *chain_last, *tmsg;
182
183 assert(msg != NULL);
184 /*
185 * Dup first msg
186 * -- we don't copy "iflags" as that is about how this message
187 * was created, and not about the message we are duplicating
188 */
189
190 /* newchain : point to new chain */
191 newchain = diag_dupsinglemsg(msg);
192 if (newchain == NULL) {
193 return diag_pseterr(DIAG_ERR_NOMEM);
194 }
195
196 chain_last = newchain;
197
198 LL_FOREACH(msg->next, msg) {
199 tmsg = diag_dupsinglemsg(msg); //copy
200 if (tmsg == NULL) {
201 diag_freemsg(newchain); //undo what we have so far
202 return diag_pseterr(DIAG_ERR_NOMEM);
203 }
204
205 //append to end of chain.
206 //Not using LL_APPEND out of principle, to avoid walking the whole list every time !
207 chain_last->next = tmsg;
208 chain_last = tmsg;
209 }
210
211 return newchain;
212}
213
214/* Duplicate a single message, don't follow the chain */
215// (leave ->next undefined)
216struct diag_msg *diag_dupsinglemsg(struct diag_msg *msg) {
217 struct diag_msg *newmsg;
218
219 assert(msg != NULL);
220 /* Dup first msg */
221
222 newmsg = diag_allocmsg(msg->len);
223 if (newmsg == NULL) {
224 return diag_pseterr(DIAG_ERR_NOMEM);
225 }
226
227 newmsg->fmt = msg->fmt;
228 newmsg->type = msg->type;
229 newmsg->dest = msg->dest;
230 newmsg->src = msg->src;
231 newmsg->rxtime = msg->rxtime;
232 /* Dup data if len>0 */
233 if ((msg->len > 0) && (msg->data != NULL)) {
234 memcpy(newmsg->data, msg->data, msg->len);
235 }
236
237 return newmsg;
238}
239
240/* Free a msg that we dup'd, recursively following the whole chain */
241// it doesn't absolutely need to be recursive but in case of trouble
242// it's easier to see the whole call stack leading to the failure.
243// Of course, not async safe.
244void diag_freemsg(struct diag_msg *msg) {
245 if (msg == NULL) {
246 return;
247 }
248
249 if (msg->next != NULL) {
250 diag_freemsg(msg->next); //recurse
251 }
252
253 if ( (msg->iflags & DIAG_MSG_IFLAG_MALLOC) == 0 ) {
254 fprintf(stderr,
255 FLFMT "diag_freemsg free-ing a non diag_allocmsg()'d message %p!\n",
256 FL, (void *)msg);
257 free(msg);
258 return;
259 }
260 if (msg->idata != NULL) {
261 free(msg->idata);
262 }
263
264 free(msg);
265
266 return;
267}
268
269
270// diag_cks1: return simple 8-bit checksum of
271// [len] bytes at *data. Everybody needs this !
272uint8_t diag_cks1(const uint8_t *data, unsigned int len) {
273 uint8_t rv=0;
274
275 while (len > 0) {
276 len--;
277 rv += data[len];
278 }
279 return rv;
280}
281
282//diag_data_dump : print (len) bytes of uint8_t *data
283//to the specified FILE (stderr, etc.)
284void diag_data_dump(FILE *out, const void *data, size_t len) {
285 const uint8_t *p = (const uint8_t *)data;
286 size_t i;
287 for (i = 0; i < len; i++) {
288 fprintf(out, "0x%02X ", p[i]); // the formatter %#02X gives
289 // silly "0X6A" formatting. %#02x
290 // gives "0x6a", not perfect
291 // either...
292 }
293}
294
295//smartcat() : make sure s1 is not too large, then strncat
296//it does NOT verify if *p1 is large enough !
297void smartcat(char *p1, const size_t s1, const char *p2 ) {
298 assert ( s1 > strlen(p1) + strlen (p2) + 1 );
299 strncat(p1, p2, s1);
300}
301
302/*
303 * Error code latching.
304 * "diag_seterr" returns NULL so you can call it from a function
305 * that returns a NULL pointer on error.
306 */
307static int latchedCode;
308
309static const struct {
310 const int code;
311 const char *desc;
312} edesc[] = {
313 { DIAG_ERR_GENERAL, "Unspecified Error" },
314 { DIAG_ERR_BADFD, "Invalid FileDescriptor passed to routine" },
315 { DIAG_ERR_NOMEM, "Malloc/Calloc/Strdup/etc failed - ran out of memory" },
316
317 { DIAG_ERR_INIT_NOTSUPP, "Initbus type not supported by H/W" },
318 { DIAG_ERR_PROTO_NOTSUPP, "Protocol not supported by H/W" },
319 { DIAG_ERR_IOCTL_NOTSUPP, "Ioctl type not supported" },
320 { DIAG_ERR_BADIFADAPTER, "L0 adapter comms failed" },
321
322 { DIAG_ERR_TIMEOUT, "Read/Write timeout" },
323
324 { DIAG_ERR_BUSERROR, "We detected write error on diag bus" },
325 { DIAG_ERR_BADLEN, "Bad length for this i/f" },
326 { DIAG_ERR_BADDATA, "Cant decode msg (ever)" },
327 { DIAG_ERR_BADCSUM, "Bad checksum in recvd message" },
328 { DIAG_ERR_INCDATA, "Incomplete data, need to receive more" },
329 { DIAG_ERR_WRONGKB, "Wrong KeyBytes received" },
330 { DIAG_ERR_BADRATE, "Bit rate specified doesn't match ECU" },
331
332 { DIAG_ERR_ECUSAIDNO, "Ecu returned negative" },
333 { DIAG_ERR_RCFILE, "Trouble loading .rc or .ini file" },
334 { DIAG_ERR_CMDFILE, "Trouble with sourcing commands" },
335 { DIAG_ERR_BADVAL, "Invalid value passed to routine" },
336 { DIAG_ERR_BADCFG, "Bad config/param" },
337};
338
339#define DIAG_ILLEGAL_ERR "Illegal error code: 0x%.2X\n"
340
341const char *diag_errlookup(const int code) {
342 unsigned i;
343 static char ill_str[sizeof(DIAG_ILLEGAL_ERR) + 2 * sizeof(int)];
344
345 for (i = 0; i < ARRAY_SIZE(edesc); i++) {
346 if (edesc[i].code == code) {
347 return edesc[i].desc;
348 }
349 }
350
351 snprintf(ill_str, sizeof(ill_str), DIAG_ILLEGAL_ERR,code);
352 return ill_str;
353}
354
355//do not call diag_pflseterr etc; refer to diag.h for related macros
356void *diag_p_pseterr(const char *name, const int line, const int code) {
357 fprintf(stderr, "%s:%d: %s.\n", name, line, diag_errlookup(code));
358 if (latchedCode == 0) {
359 latchedCode = code;
360 }
361
362 return NULL;
363}
364
365int diag_p_iseterr(const char *name, const int line, const int code) {
366 fprintf(stderr, "%s:%d: %s.\n", name, line, diag_errlookup(code));
367 if (latchedCode == 0) {
368 latchedCode = code;
369 }
370
371 return code;
372}
373
374void *diag_p_pfwderr(const char *name, const int line, const int code) {
375 DIAG_DBGGEN(DIAG_DBGLEVEL_V, "%s:%d: %s.\n", name, line, diag_errlookup(code));
376 return NULL;
377}
378
379int diag_p_ifwderr(const char *name, const int line, const int code) {
380 DIAG_DBGGEN(DIAG_DBGLEVEL_V, "%s:%d: %s.\n", name, line, diag_errlookup(code));
381 return code;
382}
383
384/*
385 * Return and clear the error.
386 */
387int diag_geterr(void) {
388 int oldCode = latchedCode;
389 latchedCode = 0;
390 return oldCode;
391}
392
393/* Memory allocation */
394
395// Stores pointer to a newly allocated buffer of n*s bytes to pp.
396// Also takes filename and line to report for debugging purposes.
397// Returns 0 in the absence of errors.
398int diag_fl_alloc(const char *fName, const int line,
399 void **pp, size_t n, size_t s, bool allocIsCalloc) {
400 char allocator;
401 const char *errMsg = "%s:%d: %calloc(%zu, %zu) failed: %s\n";
402
403 if (allocIsCalloc) {
404 allocator = 'c';
405 } else {
406 allocator = 'm';
407 }
408
409 // Check for null or overflow-causing parameters.
410 if (pp == NULL || n == 0 || s == 0 || (SIZE_MAX / s) < n) {
411 if (pp != NULL) {
412 *pp = NULL;
413 }
414 fprintf(stderr, errMsg, fName, line, allocator,
415 n, s, "Invalid arguments");
416 return diag_iseterr(DIAG_ERR_BADVAL);
417 }
418
419 if (allocIsCalloc) {
420 *pp = calloc(1, n * s);
421 } else {
422 *pp = malloc(n * s);
423 }
424 if (*pp == NULL) {
425 fprintf(stderr, errMsg, fName, line, allocator,
426 n, s, strerror(errno));
427 return diag_iseterr(DIAG_ERR_NOMEM);
428 }
429 return 0;
430}
431
432/* Add a string to array-of-strings (argv style)
433 */
434char **strlist_add(char **list, const char *news, int elems) {
435 char **templist;
436 char *temp;
437
438 assert( news != NULL );
439
440 temp = malloc((strlen(news) * sizeof(char)) + 1);
441 if (temp == NULL) {
442 return diag_pseterr(DIAG_ERR_NOMEM);
443 }
444
445 templist = realloc(list, (elems + 1)* sizeof(char *));
446 if (!templist) {
447 free(temp);
448 return diag_pseterr(DIAG_ERR_NOMEM);
449 }
450
451 strcpy(temp, news);
452 templist[elems] = temp;
453 return templist;
454}
455
456/* Free argv-style list */
457void strlist_free(char **list, int elems) {
458 if (!list) {
459 return;
460 }
461
462 while (elems > 0) {
463 if (list[elems - 1]) {
464 free(list[elems - 1]);
465 }
466 elems--;
467 }
468 free(list);
469}
470
471/*
472 * Message print out / debug routines
473 */
474void diag_printmsg_header(FILE *fp, struct diag_msg *msg, bool timestamp, int msgnum) {
475 if (timestamp) {
476 fprintf(fp, "%lu.%03lu: ", msg->rxtime / 1000,
477 msg->rxtime % 1000);
478 }
479 fprintf(fp, "msg %02d src=0x%02X dest=0x%02X\n", msgnum, msg->src, msg->dest);
480 fprintf(fp, "msg %02d data: ", msgnum);
481}
482
483void diag_printmsg(FILE *fp, struct diag_msg *msg, bool timestamp) {
484 struct diag_msg *tmsg;
485 int i=0;
486
487 LL_FOREACH(msg, tmsg) {
488 diag_printmsg_header(fp, tmsg, timestamp, i);
489 diag_data_dump(fp, tmsg->data, tmsg->len);
490 if (tmsg->fmt & DIAG_FMT_BADCS) {
491 fprintf(fp, " [BAD CKS]\n");
492 } else {
493 fprintf(fp, "\n");
494 }
495 i++;
496 }
497}
498
499// Atomic access functions.
500
501void diag_atomic_store_bool(diag_atomic_bool *a, bool d) {
502 diag_os_lock(&a->mtx);
503 a->v = d;
504 diag_os_unlock(&a->mtx);
505}
506
507void diag_atomic_store_int(diag_atomic_int *a, int d) {
508 diag_os_lock(&a->mtx);
509 a->v = d;
510 diag_os_unlock(&a->mtx);
511}
512
513bool diag_atomic_load_bool(diag_atomic_bool *a) {
514 bool r;
515 diag_os_lock(&a->mtx);
516 r = a->v;
517 diag_os_unlock(&a->mtx);
518 return r;
519}
520
521int diag_atomic_load_int(diag_atomic_int *a) {
522 int r;
523 diag_os_lock(&a->mtx);
524 r = a->v;
525 diag_os_unlock(&a->mtx);
526 return r;
527}
528