Lely core libraries  2.2.5
net.c
Go to the documentation of this file.
1 
24 #include "can.h"
25 #include <lely/can/net.h>
26 #include <lely/util/cmp.h>
27 #include <lely/util/dllist.h>
28 #include <lely/util/errnum.h>
29 #include <lely/util/pheap.h>
30 #include <lely/util/rbtree.h>
31 #include <lely/util/time.h>
32 
33 #include <assert.h>
34 #include <stdlib.h>
35 
37 struct __can_net {
39  struct pheap timer_heap;
41  struct timespec time;
43  struct timespec next;
47  void *next_data;
49  struct rbtree recv_tree;
53  void *send_data;
54 };
55 
60 static void can_net_set_next(can_net_t *net);
61 
63 struct __can_timer {
65  struct pnode node;
72  struct timespec start;
74  struct timespec interval;
78  void *data;
79 };
80 
85 #ifdef LELY_NO_CANFD
86 typedef uint_least32_t can_recv_key_t;
87 #else
88 typedef uint_least64_t can_recv_key_t;
89 #endif
90 
92 static inline can_recv_key_t can_recv_key(
93  uint_least32_t id, uint_least8_t flags);
94 
96 static int can_recv_key_cmp(const void *p1, const void *p2);
97 
99 struct __can_recv {
101  struct rbnode node;
103  struct dlnode list;
114  void *data;
115 };
116 
117 void *
118 __can_net_alloc(void)
119 {
120  void *ptr = malloc(sizeof(struct __can_net));
121  if (!ptr)
122  set_errc(errno2c(errno));
123  return ptr;
124 }
125 
126 void
127 __can_net_free(void *ptr)
128 {
129  free(ptr);
130 }
131 
132 struct __can_net *
133 __can_net_init(struct __can_net *net)
134 {
135  assert(net);
136 
138 
139  net->time = (struct timespec){ 0, 0 };
140  net->next = (struct timespec){ 0, 0 };
141 
142  net->next_func = NULL;
143  net->next_data = NULL;
144 
146 
147  net->send_func = NULL;
148  net->send_data = NULL;
149 
150  return net;
151 }
152 
153 void
154 __can_net_fini(struct __can_net *net)
155 {
156  assert(net);
157 
158  rbtree_foreach (&net->recv_tree, node) {
159  can_recv_t *recv = structof(node, can_recv_t, node);
160  dlnode_foreach (&recv->list, node)
161  can_recv_stop(structof(node, can_recv_t, list));
162  }
163 
164  struct pnode *node;
165  while ((node = pheap_first(&net->timer_heap)) != NULL)
166  can_timer_stop(structof(node, can_timer_t, node));
167 }
168 
169 can_net_t *
171 {
172  int errc = 0;
173 
174  can_net_t *net = __can_net_alloc();
175  if (!net) {
176  errc = get_errc();
177  goto error_alloc_net;
178  }
179 
180  if (!__can_net_init(net)) {
181  errc = get_errc();
182  goto error_init_net;
183  }
184 
185  return net;
186 
187 error_init_net:
188  __can_net_free(net);
189 error_alloc_net:
190  set_errc(errc);
191  return NULL;
192 }
193 
194 void
196 {
197  if (net) {
198  __can_net_fini(net);
199  __can_net_free(net);
200  }
201 }
202 
203 void
204 can_net_get_time(const can_net_t *net, struct timespec *tp)
205 {
206  assert(net);
207 
208  if (tp)
209  *tp = net->time;
210 }
211 
212 int
213 can_net_set_time(can_net_t *net, const struct timespec *tp)
214 {
215  assert(net);
216  assert(tp);
217 
218  net->time = *tp;
219 
220  int errc = get_errc();
221  int result = 0;
222 
223  // Keep processing the first timer until we're done.
224  struct pnode *node;
225  while ((node = pheap_first(&net->timer_heap)) != NULL) {
226  can_timer_t *timer = structof(node, can_timer_t, node);
227  // If the timeout of the first timer is after the current time,
228  // we're done.
229  if (timespec_cmp(&timer->start, &net->time) > 0)
230  break;
231 
232  // Requeue the timer before invoking the callback function.
233  pheap_remove(&net->timer_heap, &timer->node);
234  timer->net = NULL;
235  if (timer->interval.tv_sec || timer->interval.tv_nsec) {
236  timespec_add(&timer->start, &timer->interval);
237  timer->net = net;
238  pheap_insert(&net->timer_heap, &timer->node);
239  }
240 
241  // Invoke the callback function and check the result.
242  if (timer->func && timer->func(&net->time, timer->data)
243  && !result) {
244  // Store the first error that occurs.
245  errc = get_errc();
246  result = -1;
247  }
248  }
249 
250  can_net_set_next(net);
251 
252  set_errc(errc);
253  return result;
254 }
255 
256 void
258  const can_net_t *net, can_timer_func_t **pfunc, void **pdata)
259 {
260  assert(net);
261 
262  if (pfunc)
263  *pfunc = net->next_func;
264  if (pdata)
265  *pdata = net->next_data;
266 }
267 
268 void
270 {
271  assert(net);
272 
273  net->next_func = func;
274  net->next_data = data;
275 }
276 
277 int
278 can_net_recv(can_net_t *net, const struct can_msg *msg)
279 {
280  assert(net);
281  assert(msg);
282 
283  int errc = get_errc();
284  int result = 0;
285 
286  can_recv_key_t key = can_recv_key(msg->id, msg->flags);
287  struct rbnode *node = rbtree_find(&net->recv_tree, &key);
288  if (node) {
289  // Loop over all matching receivers.
290  can_recv_t *recv = structof(node, can_recv_t, node);
291  dlnode_foreach (&recv->list, node) {
292  recv = structof(node, can_recv_t, list);
293  // Invoke the callback function and check the result.
294  if (recv->func && recv->func(msg, recv->data)
295  && !result) {
296  // Store the first error that occurs.
297  errc = get_errc();
298  result = -1;
299  }
300  }
301  }
302 
303  set_errc(errc);
304  return result;
305 }
306 
307 int
308 can_net_send(can_net_t *net, const struct can_msg *msg)
309 {
310  assert(net);
311  assert(msg);
312 
313  if (!net->send_func) {
315  return -1;
316  }
317 
318  return net->send_func(msg, net->send_data);
319 }
320 
321 void
323  const can_net_t *net, can_send_func_t **pfunc, void **pdata)
324 {
325  assert(net);
326 
327  if (pfunc)
328  *pfunc = net->send_func;
329  if (pdata)
330  *pdata = net->send_data;
331 }
332 
333 void
335 {
336  assert(net);
337 
338  net->send_func = func;
339  net->send_data = data;
340 }
341 
342 void *
343 __can_timer_alloc(void)
344 {
345  void *ptr = malloc(sizeof(struct __can_timer));
346  if (!ptr)
347  set_errc(errno2c(errno));
348  return ptr;
349 }
350 
351 void
352 __can_timer_free(void *ptr)
353 {
354  free(ptr);
355 }
356 
357 struct __can_timer *
358 __can_timer_init(struct __can_timer *timer)
359 {
360  assert(timer);
361 
362  timer->node.key = &timer->start;
363 
364  timer->net = NULL;
365 
366  timer->start = (struct timespec){ 0, 0 };
367  timer->interval = (struct timespec){ 0, 0 };
368 
369  timer->func = NULL;
370  timer->data = NULL;
371 
372  return timer;
373 }
374 
375 void
376 __can_timer_fini(struct __can_timer *timer)
377 {
378  can_timer_stop(timer);
379 }
380 
381 can_timer_t *
383 {
384  int errc = 0;
385 
386  can_timer_t *timer = __can_timer_alloc();
387  if (!timer) {
388  errc = get_errc();
389  goto error_alloc_timer;
390  }
391 
392  if (!__can_timer_init(timer)) {
393  errc = get_errc();
394  goto error_init_timer;
395  }
396 
397  return timer;
398 
399 error_init_timer:
400  __can_timer_free(timer);
401 error_alloc_timer:
402  set_errc(errc);
403  return NULL;
404 }
405 
406 void
408 {
409  if (timer) {
410  __can_timer_fini(timer);
411  __can_timer_free(timer);
412  }
413 }
414 
415 void
417  void **pdata)
418 {
419  assert(timer);
420 
421  if (pfunc)
422  *pfunc = timer->func;
423  if (pdata)
424  *pdata = timer->data;
425 }
426 
427 void
429 {
430  assert(timer);
431 
432  timer->func = func;
433  timer->data = data;
434 }
435 
436 void
438  const struct timespec *start, const struct timespec *interval)
439 {
440  assert(timer);
441  assert(net);
442 
443  can_timer_stop(timer);
444 
445  if (!start && !interval)
446  return;
447 
448  if (interval)
449  timer->interval = *interval;
450  else
451  timer->interval = (struct timespec){ 0, 0 };
452 
453  if (start) {
454  timer->start = *start;
455  } else {
456  can_net_get_time(net, &timer->start);
457  timespec_add(&timer->start, &timer->interval);
458  }
459 
460  timer->net = net;
461 
462  pheap_insert(&timer->net->timer_heap, &timer->node);
463 
464  can_net_set_next(net);
465 }
466 
467 void
469 {
470  assert(timer);
471 
472  can_net_t *net = timer->net;
473  if (!net)
474  return;
475 
476  pheap_remove(&timer->net->timer_heap, &timer->node);
477 
478  timer->net = NULL;
479 
480  can_net_set_next(net);
481 }
482 
483 void
484 can_timer_timeout(can_timer_t *timer, can_net_t *net, int timeout)
485 {
486  if (timeout < 0) {
487  can_timer_stop(timer);
488  } else {
489  struct timespec start = { 0, 0 };
490  can_net_get_time(net, &start);
491  timespec_add_msec(&start, timeout);
492 
493  can_timer_start(timer, net, &start, NULL);
494  }
495 }
496 
497 void *
498 __can_recv_alloc(void)
499 {
500  void *ptr = malloc(sizeof(struct __can_recv));
501  if (!ptr)
502  set_errc(errno2c(errno));
503  return ptr;
504 }
505 
506 void
507 __can_recv_free(void *ptr)
508 {
509  free(ptr);
510 }
511 
512 struct __can_recv *
513 __can_recv_init(struct __can_recv *recv)
514 {
515  assert(recv);
516 
517  rbnode_init(&recv->node, &recv->key);
518  dlnode_init(&recv->list);
519 
520  recv->net = NULL;
521 
522  recv->key = 0;
523 
524  recv->func = NULL;
525  recv->data = NULL;
526 
527  return recv;
528 }
529 
530 void
531 __can_recv_fini(struct __can_recv *recv)
532 {
533  can_recv_stop(recv);
534 }
535 
536 can_recv_t *
538 {
539  int errc = 0;
540 
541  can_recv_t *recv = __can_recv_alloc();
542  if (!recv) {
543  errc = get_errc();
544  goto error_alloc_recv;
545  }
546 
547  if (!__can_recv_init(recv)) {
548  errc = get_errc();
549  goto error_init_recv;
550  }
551 
552  return recv;
553 
554 error_init_recv:
555  __can_recv_free(recv);
556 error_alloc_recv:
557  set_errc(errc);
558  return NULL;
559 }
560 
561 void
563 {
564  if (recv) {
565  __can_recv_fini(recv);
566  __can_recv_free(recv);
567  }
568 }
569 
570 void
571 can_recv_get_func(const can_recv_t *recv, can_recv_func_t **pfunc, void **pdata)
572 {
573  assert(recv);
574 
575  if (pfunc)
576  *pfunc = recv->func;
577  if (pdata)
578  *pdata = recv->data;
579 }
580 
581 void
583 {
584  assert(recv);
585 
586  recv->func = func;
587  recv->data = data;
588 }
589 
590 void
591 can_recv_start(can_recv_t *recv, can_net_t *net, uint_least32_t id,
592  uint_least8_t flags)
593 {
594  assert(recv);
595  assert(net);
596 
597  can_recv_stop(recv);
598 
599  recv->net = net;
600 
601  recv->key = can_recv_key(id, flags);
602  struct rbnode *node = rbtree_find(&recv->net->recv_tree, &recv->key);
603  if (node) {
604  can_recv_t *prev = structof(node, can_recv_t, node);
605  dlnode_insert_after(&prev->list, &recv->list);
606  } else {
607  rbtree_insert(&recv->net->recv_tree, &recv->node);
608  dlnode_init(&recv->list);
609  }
610 }
611 
612 void
614 {
615  assert(recv);
616 
617  if (!recv->net)
618  return;
619 
620  struct dlnode *prev = recv->list.prev;
621  struct dlnode *next = recv->list.next;
622 
623  if (!prev)
624  rbtree_remove(&recv->net->recv_tree, &recv->node);
625  dlnode_remove(&recv->list);
626  dlnode_init(&recv->list);
627 
628  recv->net = NULL;
629 
630  if (!prev && next) {
631  recv = structof(next, can_recv_t, list);
632  rbtree_insert(&recv->net->recv_tree, &recv->node);
633  }
634 }
635 
636 static void
638 {
639  assert(net);
640 
641  struct pnode *node = pheap_first(&net->timer_heap);
642  if (!node)
643  return;
644  can_timer_t *timer = structof(node, can_timer_t, node);
645 
646  net->next = timer->start;
647  if (net->next_func)
648  net->next_func(&net->next, net->next_data);
649 }
650 
651 static inline can_recv_key_t
652 can_recv_key(uint_least32_t id, uint_least8_t flags)
653 {
654  id &= (flags & CAN_FLAG_IDE) ? CAN_MASK_EID : CAN_MASK_BID;
655  return (can_recv_key_t)id | ((can_recv_key_t)flags << 29);
656 }
657 
658 static int
659 can_recv_key_cmp(const void *p1, const void *p2)
660 {
661 #ifdef LELY_NO_CANFD
662  return uint32_cmp(p1, p2);
663 #else
664  return uint64_cmp(p1, p2);
665 #endif
666 }
@ CAN_FLAG_IDE
The Identifier Extension (IDE) flag.
Definition: msg.h:43
#define CAN_MASK_EID
The mask used to extract the 29-bit Extended Identifier from a CAN frame.
Definition: msg.h:34
#define CAN_MASK_BID
The mask used to extract the 11-bit Base Identifier from a CAN frame.
Definition: msg.h:31
This header file is part of the utilities library; it contains the comparison function definitions.
This header file is part of the utilities library; it contains the doubly-linked list declarations.
void dlnode_remove(struct dlnode *node)
Removes node from a doubly-list.
Definition: dllist.h:254
int dlnode_insert_after(struct dlnode *prev, struct dlnode *node)
Inserts node after prev.
Definition: dllist.h:234
#define dlnode_foreach(first, node)
Iterates in order over each node in a doubly-linked list.
Definition: dllist.h:106
void dlnode_init(struct dlnode *node)
Initializes a node in a doubly-linked list.
Definition: dllist.h:227
This header file is part of the utilities library; it contains the native and platform-independent er...
@ ERRNUM_NOSYS
Function not supported.
Definition: errnum.h:181
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function.
Definition: errnum.c:947
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
void set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:375
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
void can_timer_stop(can_timer_t *timer)
Stops a CAN timer and unregisters it with a network interface.
Definition: net.c:468
int can_net_send(can_net_t *net, const struct can_msg *msg)
Sends a CAN frame from a network interface.
Definition: net.c:308
void can_timer_start(can_timer_t *timer, can_net_t *net, const struct timespec *start, const struct timespec *interval)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:437
static int can_recv_key_cmp(const void *p1, const void *p2)
The function used to compare to CAN receiver keys.
Definition: net.c:659
void can_net_get_time(const can_net_t *net, struct timespec *tp)
Retrieves the current time of a CAN network interface.
Definition: net.c:204
static can_recv_key_t can_recv_key(uint_least32_t id, uint_least8_t flags)
Computes a CAN receiver key from a CAN identifier and flags.
Definition: net.c:652
void can_timer_set_func(can_timer_t *timer, can_timer_func_t *func, void *data)
Sets the callback function invoked when a CAN timer is triggered.
Definition: net.c:428
void can_timer_get_func(const can_timer_t *timer, can_timer_func_t **pfunc, void **pdata)
Retrieves the callback function invoked when a CAN timer is triggered.
Definition: net.c:416
void can_net_set_send_func(can_net_t *net, can_send_func_t *func, void *data)
Sets the callback function used to send CAN frames from a network interface.
Definition: net.c:334
can_recv_t * can_recv_create(void)
Creates a new CAN frame receiver.
Definition: net.c:537
can_net_t * can_net_create(void)
Creates a new CAN network interface.
Definition: net.c:170
void can_recv_stop(can_recv_t *recv)
Stops a CAN frame receiver from processing frames and unregisters it with the network interface.
Definition: net.c:613
void can_net_destroy(can_net_t *net)
Destroys a CAN network interface.
Definition: net.c:195
void can_net_get_send_func(const can_net_t *net, can_send_func_t **pfunc, void **pdata)
Retrieves the callback function used to send CAN frames from a network interface.
Definition: net.c:322
static void can_net_set_next(can_net_t *net)
Invokes the callback function if the time at which the next CAN timer triggers has been updated.
Definition: net.c:637
void can_recv_set_func(can_recv_t *recv, can_recv_func_t *func, void *data)
Sets the callback function used to process CAN frames with a receiver.
Definition: net.c:582
void can_recv_destroy(can_recv_t *recv)
Destroys a CAN frame receiver.
Definition: net.c:562
can_timer_t * can_timer_create(void)
Creates a new CAN timer.
Definition: net.c:382
void can_timer_timeout(can_timer_t *timer, can_net_t *net, int timeout)
Starts a CAN timer and registers it with a network interface.
Definition: net.c:484
uint_least64_t can_recv_key_t
The type of the key used to match CAN frame receivers to CAN frames.
Definition: net.c:88
void can_net_get_next_func(const can_net_t *net, can_timer_func_t **pfunc, void **pdata)
Retrieves the callback function invoked when the time at which the next CAN timer triggers is updated...
Definition: net.c:257
void can_net_set_next_func(can_net_t *net, can_timer_func_t *func, void *data)
Sets the callback function invoked when the time at which the next CAN timer triggers is updated.
Definition: net.c:269
void can_recv_start(can_recv_t *recv, can_net_t *net, uint_least32_t id, uint_least8_t flags)
Registers a CAN frame receiver with a network interface and starts processing frames.
Definition: net.c:591
void can_recv_get_func(const can_recv_t *recv, can_recv_func_t **pfunc, void **pdata)
Retrieves the callback function used to process CAN frames with a receiver.
Definition: net.c:571
int can_net_recv(can_net_t *net, const struct can_msg *msg)
Receives a CAN frame with a network interface and processes it with the corresponding receiver(s).
Definition: net.c:278
int can_net_set_time(can_net_t *net, const struct timespec *tp)
Sets the current time of a CAN network interface.
Definition: net.c:213
void can_timer_destroy(can_timer_t *timer)
Destroys a CAN timer.
Definition: net.c:407
This header file is part of the CAN library; it contains the CAN network interface declarations.
int can_send_func_t(const struct can_msg *msg, void *data)
The type of a CAN send callback function, invoked by a CAN network interface when a frame needs to be...
Definition: net.h:85
int can_recv_func_t(const struct can_msg *msg, void *data)
The type of a CAN receive callback function, invoked by a CAN frame receiver when a frame is received...
Definition: net.h:73
int can_timer_func_t(const struct timespec *tp, void *data)
The type of a CAN timer callback function, invoked by a CAN timer when the time is updated,...
Definition: net.h:61
This header file is part of the utilities library; it contains the pairing heap declarations.
struct pnode * pheap_first(const struct pheap *heap)
Returns a pointer to the first (minimum) node in a pairing heap.
Definition: pheap.h:234
void pheap_insert(struct pheap *heap, struct pnode *node)
Inserts a node into a pairing heap.
Definition: pheap.c:37
void pheap_init(struct pheap *heap, pheap_cmp_t *cmp)
Initializes a pairing heap.
Definition: pheap.h:214
void pheap_remove(struct pheap *heap, struct pnode *node)
Removes a node from a pairing heap.
Definition: pheap.c:65
This header file is part of the utilities library; it contains the red-black tree declarations.
void rbnode_init(struct rbnode *node, const void *key)
Initializes a node in a red-black tree.
Definition: rbtree.h:229
void rbtree_insert(struct rbtree *tree, struct rbnode *node)
Inserts a node into a red-black tree.
Definition: rbtree.c:108
void rbtree_init(struct rbtree *tree, rbtree_cmp_t *cmp)
Initializes a red-black tree.
Definition: rbtree.h:238
void rbtree_remove(struct rbtree *tree, struct rbnode *node)
Removes a node from a red-black tree.
Definition: rbtree.c:187
#define rbtree_foreach(tree, node)
Iterates over each node in a red-black tree in ascending order.
Definition: rbtree.h:226
struct rbnode * rbtree_find(const struct rbtree *tree, const void *key)
Finds a node in a red-black tree.
Definition: rbtree.c:306
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 <stdlib....
A CAN network interface.
Definition: net.c:37
void * send_data
A pointer to the user-specified data for send_func.
Definition: net.c:53
can_send_func_t * send_func
A pointer to the callback function invoked by can_net_send().
Definition: net.c:51
struct timespec time
The current time.
Definition: net.c:41
struct rbtree recv_tree
The tree containing all receivers.
Definition: net.c:49
struct pheap timer_heap
The tree containing all timers.
Definition: net.c:39
struct timespec next
The time at which the next timer triggers.
Definition: net.c:43
void * next_data
A pointer to user-specified data for next_func.
Definition: net.c:47
can_timer_func_t * next_func
A pointer to the callback function invoked by can_net_set_next().
Definition: net.c:45
A CAN frame receiver.
Definition: net.c:99
struct rbnode node
The node of this receiver in the tree of receivers.
Definition: net.c:101
can_recv_key_t key
The key used in node.
Definition: net.c:110
void * data
A pointer to the user-specified data for func.
Definition: net.c:114
can_recv_func_t * func
A pointer to the callback function invoked by can_net_recv().
Definition: net.c:112
struct dlnode list
The list of CAN frame receivers with the same key.
Definition: net.c:103
can_net_t * net
A pointer to the network interface with which this receiver is registered.
Definition: net.c:108
A CAN timer.
Definition: net.c:63
can_net_t * net
A pointer to the network interface with which this timer is registered.
Definition: net.c:70
can_timer_func_t * func
A pointer to the callback function invoked by can_net_set_time().
Definition: net.c:76
struct timespec interval
The interval between successive triggers.
Definition: net.c:74
struct timespec start
The time at which the timer should trigger.
Definition: net.c:72
void * data
A pointer to the user-specified data for func.
Definition: net.c:78
struct pnode node
The node of this timer in the tree of timers.
Definition: net.c:65
A CAN or CAN FD format frame.
Definition: msg.h:87
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
A node in a doubly-linked list.
Definition: dllist.h:39
struct dlnode * next
A pointer to the next node in the list.
Definition: dllist.h:43
struct dlnode * prev
A pointer to the previous node in the list.
Definition: dllist.h:41
A pairing heap.
Definition: pheap.h:86
A node in a pairing heap.
Definition: pheap.h:51
const void * key
A pointer to the key of this node.
Definition: pheap.h:57
A node in a red-black tree.
Definition: rbtree.h:52
const void * key
A pointer to the key for this node.
Definition: rbtree.h:58
A red-black tree.
Definition: rbtree.h:90
This header file is part of the utilities library; it contains the time function declarations.
int timespec_cmp(const void *p1, const void *p2)
Compares two times.
Definition: time.h:228
void timespec_add(struct timespec *tp, const struct timespec *inc)
Adds the time interval *inc to the time at tp.
Definition: time.h:117
void timespec_add_msec(struct timespec *tp, uint_least64_t msec)
Adds msec milliseconds to the time at tp.
Definition: time.h:135