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