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