Lely core libraries 2.3.4
msg.c
Go to the documentation of this file.
1
24#include "can.h"
25#include <lely/can/msg.h>
26#include <lely/util/bits.h>
27#include <lely/util/errnum.h>
28#include <lely/util/util.h>
29
30#include <assert.h>
31#if !LELY_NO_STDIO
32#include <stdio.h>
33// Include inttypes.h after stdio.h to enforce declarations of format specifiers
34// in Newlib.
35#include <inttypes.h>
36#include <stdlib.h>
37#endif
38
40static uint_least16_t can_crc_bits(
41 uint_least16_t crc, uint_least8_t byte, int off, int bits);
42
44static uint_least16_t can_crc_bytes(
45 uint_least16_t crc, const unsigned char *bp, size_t n);
46
47int
48can_msg_bits(const struct can_msg *msg, enum can_msg_bits_mode mode)
49{
50 assert(msg);
51
52#if !LELY_NO_CANFD
53 if (msg->flags & CAN_FLAG_FDF) {
55 return -1;
56 }
57#endif
58
59 if (msg->len > CAN_MAX_LEN) {
61 return -1;
62 }
63
64 switch (mode) {
66 int bits = (msg->flags & CAN_FLAG_IDE) ? 67 : 47;
67 if (!(msg->flags & CAN_FLAG_RTR))
68 bits += msg->len * 8;
69 return bits;
70 }
72 int bits = (msg->flags & CAN_FLAG_IDE) ? 80 : 55;
73 if (!(msg->flags & CAN_FLAG_RTR))
74 bits += msg->len * 10;
75 return bits;
76 }
77 case CAN_MSG_BITS_MODE_EXACT: break;
78 default: set_errnum(ERRNUM_INVAL); return -1;
79 }
80
81 uint_least8_t data[16] = { 0 };
82 uint_least8_t *bp = data;
83 int off = 0;
84 int bits = 0;
85
86 if (msg->flags & CAN_FLAG_IDE) {
87 // s = SOF, B = (base) Identifier, S = SRR, I = IDE,
88 // E = Identifier (extension), R = RTR = 1 = R1, 0 = R0,
89 // DLC4 = DLC, 0-7 = Data, C = CRC
90 // data[0-3] |.sBBBBBB BBBBBSIE EEEEEEEE EEEEEEEE|
91 // data[4-7] |ER10DLC4 00000000 11111111 22222222|
92 // data[8-11] |33333333 44444444 55555555 66666666|
93 // data[12-14] |77777777 CCCCCCCC CCCCCCC. ........|
94 uint_least32_t id = msg->id & CAN_MASK_EID;
95 off = 1;
96 *bp++ = (id >> 23) & 0x3f; // SOF = 0, base (Indentifier)
97 bits += 8 - off;
98
99 *bp++ = ((id >> 15) & 0xf8) // base (Indentifier)
100 | (0x03 << 1) // SRR, IDE
101 | ((id >> 17) & 0x01); // Identifier (extension)
102 bits += 8;
103
104 *bp++ = (id >> 9) & 0xff; // Identifier (extension)
105 bits += 8;
106
107 *bp++ = (id >> 1) & 0xff; // Identifier (extension)
108 bits += 8;
109
110 *bp++ = ((id << 7) & 0x80) // Identifier (extension)
111 | (!!(msg->flags & CAN_FLAG_RTR) << 6) // RTR
112 | (msg->len & 0x0f); // R1 = 0, R0 = 0, DLC
113 bits += 8;
114 } else {
115 // s = SOF, B = (base) Identifier, R = RTR, I = IDE, 0 = R0,
116 // DLC4 = DLC, 0-7 = Data, C = CRC
117 // data[0-3] |.....sBB BBBBBBBB BRI0DLC4 00000000|
118 // data[4-7] |11111111 22222222 33333333 44444444|
119 // data[8-11] |55555555 66666666 77777777 CCCCCCCC|
120 // data[12-14] |CCCCCCC. ........ ........ ........|
121 uint_least32_t id = msg->id & CAN_MASK_BID;
122 off = 5;
123 *bp++ = (id >> 9) & 0x03; // SOF = 0, base (Indentifier)
124 bits += 8 - off;
125
126 *bp++ = (id >> 1) & 0xff; // base (Indentifier)
127 bits += 8;
128
129 *bp++ = ((id << 7) & 0x80) // base (Indentifier)
130 | (!!(msg->flags & CAN_FLAG_RTR) << 6) // RTR
131 | (msg->len & 0x0f); // IDE = 0, R0 = 0, DLC
132 bits += 8;
133 }
134
135 if (!(msg->flags & CAN_FLAG_RTR) && msg->len) {
136 for (uint_least8_t i = 0; i < msg->len; i++, bits += 8)
137 *bp++ = msg->data[i] & 0xff;
138 }
139
140 uint_least16_t crc = can_crc(0, data, off, bits);
141 assert(!((off + bits) % 8));
142 *bp++ = (crc >> 7) & 0xff;
143 *bp++ = (crc << 1) & 0xff;
144 bits += 15;
145
146 // Count the stuffed bits.
147 int stuff = 0;
148 uint_least8_t mask = 0x1f;
149 uint_least8_t same = mask;
150 for (int i = off; i < off + bits;) {
151 // Alternate between looking for a series of zeros and ones.
152 same = same ? 0 : mask;
153 // Extract 5 bits at i at look for a bit flip.
154 // clang-format off
155 uint_least8_t five = (((uint_least16_t)data[i / 8] << 8)
156 | data[i / 8 + 1]) >> (16 - 5 - i % 8);
157 // clang-format on
158 int n = clz8((five & mask) ^ same) - 3;
159 i += n;
160 if (n < 5) {
161 // No bit stuffing needed. Check the next 5 bits.
162 mask = 0x1f;
163 } else {
164 // Insert a stuffed bit and look for the next 4 bits
165 // (the 5th bit is the stuffed one).
166 if (mask == 0x1e)
167 i--;
168 else
169 mask = 0x1e;
170 if (i <= off + bits)
171 stuff++;
172 }
173 }
174 bits += stuff;
175
176 bits += 3; // CRC delimiter, ACK slot, ACK delimiter
177 bits += 7; // EOF sequence
178 bits += 3; // intermission
179
180 return bits;
181}
182
183#if !LELY_NO_STDIO
184
185int
186snprintf_can_msg(char *s, size_t n, const struct can_msg *msg)
187{
188 if (!s)
189 n = 0;
190
191 if (!msg)
192 return 0;
193
194 uint_least8_t len = msg->len;
195#if !LELY_NO_CANFD
196 if (msg->flags & CAN_FLAG_FDF)
197 len = MIN(len, CANFD_MAX_LEN);
198 else
199#endif
200 len = MIN(len, CAN_MAX_LEN);
201
202 int r, t = 0;
203
204 if (msg->flags & CAN_FLAG_IDE)
205 // cppcheck-suppress nullPointer symbolName=s
206 r = snprintf(s, n, "%08" PRIX32, msg->id & CAN_MASK_EID);
207 else
208 // cppcheck-suppress nullPointer symbolName=s
209 r = snprintf(s, n, "%03" PRIX32, msg->id & CAN_MASK_BID);
210 if (r < 0)
211 return r;
212 t += r;
213 r = MIN((size_t)r, n);
214 s += r;
215 n -= r;
216
217#if !LELY_NO_CANFD
218 if (msg->flags & CAN_FLAG_FDF)
219 r = snprintf(s, n, " [%02d] ", len);
220 else
221#endif
222 r = snprintf(s, n, " [%d] ", len);
223 if (r < 0)
224 return r;
225 t += r;
226 r = MIN((size_t)r, n);
227 s += r;
228 n -= r;
229
230 if (msg->flags & CAN_FLAG_RTR) {
231 r = snprintf(s, n, " remote request");
232 if (r < 0)
233 return r;
234 t += r;
235 } else {
236 for (uint_least8_t i = 0; i < len; i++) {
237 int r = snprintf(s, n, " %02X", msg->data[i]);
238 if (r < 0)
239 return r;
240 t += r;
241 r = MIN((size_t)r, n);
242 s += r;
243 n -= r;
244 }
245 }
246
247 return t;
248}
249
250#if !LELY_NO_MALLOC
251int
252asprintf_can_msg(char **ps, const struct can_msg *msg)
253{
254 int n = snprintf_can_msg(NULL, 0, msg);
255 if (n < 0)
256 return n;
257
258 char *s = malloc(n + 1);
259 if (!s)
260 return -1;
261
262 n = snprintf_can_msg(s, n + 1, msg);
263 if (n < 0) {
264 int errsv = errno;
265 free(s);
266 errno = errsv;
267 return n;
268 }
269
270 *ps = s;
271 return n;
272}
273#endif // !LELY_NO_MALLOC
274
275#endif // !LELY_NO_STDIO
276
277uint_least16_t
278can_crc(uint_least16_t crc, const void *ptr, int off, size_t bits)
279{
280 assert(ptr || !bits);
281
282 const uint_least8_t *bp = ptr;
283 bp += off / 8;
284 off %= 8;
285 if (off < 0) {
286 bp--;
287 off += 8;
288 }
289
290 if (off && bits) {
291 int n = MIN((size_t)(8 - off), bits);
292 crc = can_crc_bits(crc, *bp++, off, n);
293 bits -= n;
294 }
295
296 size_t n = bits / 8;
297 crc = can_crc_bytes(crc, bp, n);
298 bp += n;
299 bits -= n * 8;
300
301 if (bits)
302 crc = can_crc_bits(crc, *bp, 0, bits);
303
304 return crc;
305}
306
307static uint_least16_t
308can_crc_bits(uint_least16_t crc, uint_least8_t byte, int off, int bits)
309{
310 assert(off >= 0);
311 assert(bits >= 0);
312 assert(off + bits <= 8);
313
314 for (byte <<= off; bits--; byte <<= 1) {
315 if ((byte ^ (crc >> 7)) & 0x80)
316 crc = (crc << 1) ^ 0x4599;
317 else
318 crc <<= 1;
319 }
320 return crc & 0x7fff;
321}
322
323static uint_least16_t
324can_crc_bytes(uint_least16_t crc, const unsigned char *bp, size_t n)
325{
326 assert(bp || !n);
327
328 // This table contains precomputed CRC-15-CAN checksums for each of the
329 // 256 bytes. The table was computed with the following code:
330 /*
331 uint_least16_t tab[256];
332 for (int n = 0; n < 256; n++) {
333 uint_least16_t crc = n << 7;
334 for (int k = 0; k < 8; k++) {
335 if (crc & 0x4000)
336 crc = (crc << 1) ^ 0x4599;
337 else
338 crc <<= 1;
339 }
340 tab[n] = crc & 0x7fff;
341 }
342 */
343 // clang-format off
344 static const uint_least16_t tab[] = {
345 0x0000, 0x4599, 0x4eab, 0x0b32, 0x58cf, 0x1d56, 0x1664, 0x53fd,
346 0x7407, 0x319e, 0x3aac, 0x7f35, 0x2cc8, 0x6951, 0x6263, 0x27fa,
347 0x2d97, 0x680e, 0x633c, 0x26a5, 0x7558, 0x30c1, 0x3bf3, 0x7e6a,
348 0x5990, 0x1c09, 0x173b, 0x52a2, 0x015f, 0x44c6, 0x4ff4, 0x0a6d,
349 0x5b2e, 0x1eb7, 0x1585, 0x501c, 0x03e1, 0x4678, 0x4d4a, 0x08d3,
350 0x2f29, 0x6ab0, 0x6182, 0x241b, 0x77e6, 0x327f, 0x394d, 0x7cd4,
351 0x76b9, 0x3320, 0x3812, 0x7d8b, 0x2e76, 0x6bef, 0x60dd, 0x2544,
352 0x02be, 0x4727, 0x4c15, 0x098c, 0x5a71, 0x1fe8, 0x14da, 0x5143,
353 0x73c5, 0x365c, 0x3d6e, 0x78f7, 0x2b0a, 0x6e93, 0x65a1, 0x2038,
354 0x07c2, 0x425b, 0x4969, 0x0cf0, 0x5f0d, 0x1a94, 0x11a6, 0x543f,
355 0x5e52, 0x1bcb, 0x10f9, 0x5560, 0x069d, 0x4304, 0x4836, 0x0daf,
356 0x2a55, 0x6fcc, 0x64fe, 0x2167, 0x729a, 0x3703, 0x3c31, 0x79a8,
357 0x28eb, 0x6d72, 0x6640, 0x23d9, 0x7024, 0x35bd, 0x3e8f, 0x7b16,
358 0x5cec, 0x1975, 0x1247, 0x57de, 0x0423, 0x41ba, 0x4a88, 0x0f11,
359 0x057c, 0x40e5, 0x4bd7, 0x0e4e, 0x5db3, 0x182a, 0x1318, 0x5681,
360 0x717b, 0x34e2, 0x3fd0, 0x7a49, 0x29b4, 0x6c2d, 0x671f, 0x2286,
361 0x2213, 0x678a, 0x6cb8, 0x2921, 0x7adc, 0x3f45, 0x3477, 0x71ee,
362 0x5614, 0x138d, 0x18bf, 0x5d26, 0x0edb, 0x4b42, 0x4070, 0x05e9,
363 0x0f84, 0x4a1d, 0x412f, 0x04b6, 0x574b, 0x12d2, 0x19e0, 0x5c79,
364 0x7b83, 0x3e1a, 0x3528, 0x70b1, 0x234c, 0x66d5, 0x6de7, 0x287e,
365 0x793d, 0x3ca4, 0x3796, 0x720f, 0x21f2, 0x646b, 0x6f59, 0x2ac0,
366 0x0d3a, 0x48a3, 0x4391, 0x0608, 0x55f5, 0x106c, 0x1b5e, 0x5ec7,
367 0x54aa, 0x1133, 0x1a01, 0x5f98, 0x0c65, 0x49fc, 0x42ce, 0x0757,
368 0x20ad, 0x6534, 0x6e06, 0x2b9f, 0x7862, 0x3dfb, 0x36c9, 0x7350,
369 0x51d6, 0x144f, 0x1f7d, 0x5ae4, 0x0919, 0x4c80, 0x47b2, 0x022b,
370 0x25d1, 0x6048, 0x6b7a, 0x2ee3, 0x7d1e, 0x3887, 0x33b5, 0x762c,
371 0x7c41, 0x39d8, 0x32ea, 0x7773, 0x248e, 0x6117, 0x6a25, 0x2fbc,
372 0x0846, 0x4ddf, 0x46ed, 0x0374, 0x5089, 0x1510, 0x1e22, 0x5bbb,
373 0x0af8, 0x4f61, 0x4453, 0x01ca, 0x5237, 0x17ae, 0x1c9c, 0x5905,
374 0x7eff, 0x3b66, 0x3054, 0x75cd, 0x2630, 0x63a9, 0x689b, 0x2d02,
375 0x276f, 0x62f6, 0x69c4, 0x2c5d, 0x7fa0, 0x3a39, 0x310b, 0x7492,
376 0x5368, 0x16f1, 0x1dc3, 0x585a, 0x0ba7, 0x4e3e, 0x450c, 0x0095
377 };
378 // clang-format on
379
380 while (n--)
381 crc = (tab[(*bp++ ^ (crc >> 7)) & 0xff] ^ (crc << 8)) & 0x7fff;
382 return crc;
383}
This header file is part of the utilities library; it contains the bit function definitions.
int clz8(uint_least8_t x)
Counts the number of leading zero bits in the unsigned 8-bit integer x.
Definition bits.h:330
int snprintf_can_msg(char *s, size_t n, const struct can_msg *msg)
Prints the contents of a CAN or CAN FD format frame to a string buffer.
Definition msg.c:186
static uint_least16_t can_crc_bytes(uint_least16_t crc, const unsigned char *bp, size_t n)
Computes a CRC-15-CAN checksum.
Definition msg.c:324
int asprintf_can_msg(char **ps, const struct can_msg *msg)
Equivalent to snprintf_can_msg(), except that it allocates a string large enough to hold the output,...
Definition msg.c:252
int can_msg_bits(const struct can_msg *msg, enum can_msg_bits_mode mode)
Computes the size (in bits) of the specified CAN format frame on the CAN bus.
Definition msg.c:48
uint_least16_t can_crc(uint_least16_t crc, const void *ptr, int off, size_t bits)
Computes a bitwise CRC-15-CAN checksum, based on the 0x4599 generator polynomial.
Definition msg.c:278
static uint_least16_t can_crc_bits(uint_least16_t crc, uint_least8_t byte, int off, int bits)
Computes a bitwise CRC-15-CAN checksum of a single byte.
Definition msg.c:308
This header file is part of the CAN library; it contains the CAN frame declarations.
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition msg.h:43
@ CAN_FLAG_RTR
The Remote Transmission Request (RTR) flag (unavailable in CAN FD format frames).
Definition msg.h:48
@ CAN_FLAG_FDF
The FD Format (FDF) flag, formerly known as Extended Data Length (EDL).
Definition msg.h:54
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition msg.h:34
#define CAN_MAX_LEN
The maximum number of bytes in the payload of a CAN format frame.
Definition msg.h:72
#define CANFD_MAX_LEN
The maximum number of bytes in the payload of a CAN FD format frame.
Definition msg.h:76
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition msg.h:31
can_msg_bits_mode
The method used to compute te size (in bits) of a CAN frame.
Definition msg.h:127
@ CAN_MSG_BITS_MODE_WORST
Simple worst case estimate.
Definition msg.h:131
@ CAN_MSG_BITS_MODE_EXACT
Exact calculation based of frame content and CRC.
Definition msg.h:133
@ CAN_MSG_BITS_MODE_NO_STUFF
Simple calculation assuming no bit stuffing.
Definition msg.h:129
This header file is part of the utilities library; it contains the native and platform-independent er...
@ ERRNUM_INVAL
Invalid argument.
Definition errnum.h:132
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition errnum.h:424
This is the public header file of the utilities library.
#define MIN(a, b)
Returns the minimum of a and b.
Definition util.h:57
This is the internal header file of the CAN bus operation queue functions.
This header file is part of the C11 and POSIX compatibility library; it includes <stdio....
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib....
A CAN or CAN FD format frame.
Definition msg.h:87
uint_least8_t data[CAN_MSG_MAX_LEN]
The frame payload (in case of a data frame).
Definition msg.h:102
uint_least32_t id
The identifier (11 or 29 bits, depending on the CAN_FLAG_IDE flag).
Definition msg.h:89
uint_least8_t flags
The flags (any combination of CAN_FLAG_IDE, CAN_FLAG_RTR, CAN_FLAG_FDF, CAN_FLAG_BRS and CAN_FLAG_ESI...
Definition msg.h:94
uint_least8_t len
The number of bytes in data (or the requested number of bytes in case of a remote frame).
Definition msg.h:100