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 
40 static uint_least16_t can_crc_bits(
41  uint_least16_t crc, uint_least8_t byte, int off, int bits);
42 
44 static uint_least16_t can_crc_bytes(
45  uint_least16_t crc, const unsigned char *bp, size_t n);
46 
47 int
48 can_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 
185 int
186 snprintf_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
251 int
252 asprintf_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 
277 uint_least16_t
278 can_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 
307 static uint_least16_t
308 can_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 
323 static uint_least16_t
324 can_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