Lely core libraries 2.3.4
dev.c
Go to the documentation of this file.
1
24#include "co.h"
25#include <lely/co/detail/dev.h>
26#include <lely/co/detail/obj.h>
27#include <lely/util/cmp.h>
28#include <lely/util/diag.h>
29#if !LELY_NO_CO_TPDO
30#include <lely/co/pdo.h>
31#endif
32
33#include <assert.h>
34#if !LELY_NO_MALLOC
35#include <stdlib.h>
36#endif
37
38static void co_obj_set_id(
39 co_obj_t *obj, co_unsigned8_t new_id, co_unsigned8_t old_id);
40static void co_sub_set_id(
41 co_sub_t *sub, co_unsigned8_t new_id, co_unsigned8_t old_id);
42static void co_val_set_id(co_unsigned16_t type, void *val,
43 co_unsigned8_t new_id, co_unsigned8_t old_id);
44
45#if !LELY_NO_MALLOC
46
47void *
48__co_dev_alloc(void)
49{
50 void *ptr = malloc(sizeof(struct __co_dev));
51#if !LELY_NO_ERRNO
52 if (!ptr)
53 set_errc(errno2c(errno));
54#endif
55 return ptr;
56}
57
58void
59__co_dev_free(void *ptr)
60{
61 free(ptr);
62}
63
64#endif // !LELY_NO_MALLOC
65
66struct __co_dev *
67__co_dev_init(struct __co_dev *dev, co_unsigned8_t id)
68{
69 assert(dev);
70
71 dev->netid = 0;
72
73 if (!id || (id > CO_NUM_NODES && id != 0xff)) {
75 return NULL;
76 }
77 dev->id = id;
78
79 rbtree_init(&dev->tree, &uint16_cmp);
80
81#if !LELY_NO_CO_OBJ_NAME
82 dev->name = NULL;
83
84 dev->vendor_name = NULL;
85#endif
86 dev->vendor_id = 0;
87#if !LELY_NO_CO_OBJ_NAME
88 dev->product_name = NULL;
89#endif
90 dev->product_code = 0;
91 dev->revision = 0;
92#if !LELY_NO_CO_OBJ_NAME
93 dev->order_code = NULL;
94#endif
95
96 dev->baud = 0;
97 dev->rate = 0;
98
99 dev->lss = 0;
100
101 dev->dummy = 0;
102
103#if !LELY_NO_CO_TPDO
104 dev->tpdo_event_ind = NULL;
105 dev->tpdo_event_data = NULL;
106#if !LELY_NO_CO_MPDO
107 dev->sam_mpdo_event_ind = NULL;
108 dev->sam_mpdo_event_data = NULL;
109#endif
110#endif // !LELY_NO_CO_TPDO
111
112 return dev;
113}
114
115void
116__co_dev_fini(struct __co_dev *dev)
117{
118 assert(dev);
119#if LELY_NO_MALLOC
120 (void)dev;
121#else
122 rbtree_foreach (&dev->tree, node)
123 co_obj_destroy(structof(node, co_obj_t, node));
124
125#if !LELY_NO_CO_OBJ_NAME
126 free(dev->vendor_name);
127 free(dev->product_name);
128 free(dev->order_code);
129
130 free(dev->name);
131#endif
132#endif // LELY_NO_MALLOC
133}
134
135#if !LELY_NO_MALLOC
136
137co_dev_t *
138co_dev_create(co_unsigned8_t id)
139{
140 int errc = 0;
141
142 co_dev_t *dev = __co_dev_alloc();
143 if (!dev) {
144 errc = get_errc();
145 goto error_alloc_dev;
146 }
147
148 if (!__co_dev_init(dev, id)) {
149 errc = get_errc();
150 goto error_init_dev;
151 }
152
153 return dev;
154
155error_init_dev:
156 __co_dev_free(dev);
157error_alloc_dev:
158 set_errc(errc);
159 return NULL;
160}
161
162void
164{
165 if (dev) {
166 __co_dev_fini(dev);
167 __co_dev_free(dev);
168 }
169}
170
171#endif // !LELY_NO_MALLOC
172
173co_unsigned8_t
175{
176 assert(dev);
177
178 return dev->netid;
179}
180
181int
182co_dev_set_netid(co_dev_t *dev, co_unsigned8_t id)
183{
184 assert(dev);
185
186 if (id > CO_NUM_NETWORKS && id != 0xff) {
188 return -1;
189 }
190
191 dev->netid = id;
192
193 return 0;
194}
195
196co_unsigned8_t
198{
199 assert(dev);
200
201 return dev->id;
202}
203
204int
205co_dev_set_id(co_dev_t *dev, co_unsigned8_t id)
206{
207 assert(dev);
208
209 if (!id || (id > CO_NUM_NODES && id != 0xff)) {
211 return -1;
212 }
213
214 rbtree_foreach (&dev->tree, node)
215 co_obj_set_id(structof(node, co_obj_t, node), id, dev->id);
216
217 dev->id = id;
218
219 return 0;
220}
221
222co_unsigned16_t
223co_dev_get_idx(const co_dev_t *dev, co_unsigned16_t maxidx,
224 co_unsigned16_t *idx)
225{
226 assert(dev);
227
228 if (!idx)
229 maxidx = 0;
230
231 if (maxidx) {
232 struct rbnode *node = rbtree_first(&dev->tree);
233 for (size_t i = 0; node && i < maxidx;
234 node = rbnode_next(node), i++)
235 idx[i] = co_obj_get_idx(structof(node, co_obj_t, node));
236 }
237
238 return (co_unsigned16_t)rbtree_size(&dev->tree);
239}
240
241int
243{
244 assert(dev);
245 assert(obj);
246
247 if (obj->dev && obj->dev != dev)
248 return -1;
249
250 if (obj->dev == dev)
251 return 0;
252
253 if (rbtree_find(&dev->tree, obj->node.key))
254 return -1;
255
256 obj->dev = dev;
257 rbtree_insert(&obj->dev->tree, &obj->node);
258
259 return 0;
260}
261
262int
264{
265 assert(dev);
266 assert(obj);
267
268 if (obj->dev != dev)
269 return -1;
270
271 rbtree_remove(&obj->dev->tree, &obj->node);
272 rbnode_init(&obj->node, &obj->idx);
273 obj->dev = NULL;
274
275 return 0;
276}
277
278co_obj_t *
279co_dev_find_obj(const co_dev_t *dev, co_unsigned16_t idx)
280{
281 assert(dev);
282
283 struct rbnode *node = rbtree_find(&dev->tree, &idx);
284 if (!node)
285 return NULL;
286 return structof(node, co_obj_t, node);
287}
288
289co_sub_t *
290co_dev_find_sub(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
291{
292 co_obj_t *obj = co_dev_find_obj(dev, idx);
293 return obj ? co_obj_find_sub(obj, subidx) : NULL;
294}
295
296co_obj_t *
298{
299 assert(dev);
300
301 struct rbnode *node = rbtree_first(&dev->tree);
302 return node ? structof(node, co_obj_t, node) : NULL;
303}
304
305co_obj_t *
307{
308 assert(dev);
309
310 struct rbnode *node = rbtree_last(&dev->tree);
311 return node ? structof(node, co_obj_t, node) : NULL;
312}
313
314#if !LELY_NO_CO_OBJ_NAME
315
316const char *
318{
319 assert(dev);
320
321 return dev->name;
322}
323
324int
325co_dev_set_name(co_dev_t *dev, const char *name)
326{
327 assert(dev);
328
329 if (!name || !*name) {
330 free(dev->name);
331 dev->name = NULL;
332 return 0;
333 }
334
335 void *ptr = realloc(dev->name, strlen(name) + 1);
336 if (!ptr) {
337#if !LELY_NO_ERRNO
338 set_errc(errno2c(errno));
339#endif
340 return -1;
341 }
342 dev->name = ptr;
343 strcpy(dev->name, name);
344
345 return 0;
346}
347
348const char *
350{
351 assert(dev);
352
353 return dev->vendor_name;
354}
355
356int
357co_dev_set_vendor_name(co_dev_t *dev, const char *vendor_name)
358{
359 assert(dev);
360
361 if (!vendor_name || !*vendor_name) {
362 free(dev->vendor_name);
363 dev->vendor_name = NULL;
364 return 0;
365 }
366
367 void *ptr = realloc(dev->vendor_name, strlen(vendor_name) + 1);
368 if (!ptr) {
369#if !LELY_NO_ERRNO
370 set_errc(errno2c(errno));
371#endif
372 return -1;
373 }
374 dev->vendor_name = ptr;
375 strcpy(dev->vendor_name, vendor_name);
376
377 return 0;
378}
379
380#endif // !LELY_NO_CO_OBJ_NAME
381
382co_unsigned32_t
384{
385 assert(dev);
386
387 return dev->vendor_id;
388}
389
390void
391co_dev_set_vendor_id(co_dev_t *dev, co_unsigned32_t vendor_id)
392{
393 assert(dev);
394
395 dev->vendor_id = vendor_id;
396}
397
398#if !LELY_NO_CO_OBJ_NAME
399
400const char *
402{
403 assert(dev);
404
405 return dev->product_name;
406}
407
408int
409co_dev_set_product_name(co_dev_t *dev, const char *product_name)
410{
411 assert(dev);
412
413 if (!product_name || !*product_name) {
414 free(dev->product_name);
415 dev->product_name = NULL;
416 return 0;
417 }
418
419 void *ptr = realloc(dev->product_name, strlen(product_name) + 1);
420 if (!ptr) {
421#if !LELY_NO_ERRNO
422 set_errc(errno2c(errno));
423#endif
424 return -1;
425 }
426 dev->product_name = ptr;
427 strcpy(dev->product_name, product_name);
428
429 return 0;
430}
431
432#endif // !LELY_NO_CO_OBJ_NAME
433
434co_unsigned32_t
436{
437 assert(dev);
438
439 return dev->product_code;
440}
441
442void
443co_dev_set_product_code(co_dev_t *dev, co_unsigned32_t product_code)
444{
445 assert(dev);
446
447 dev->product_code = product_code;
448}
449
450co_unsigned32_t
452{
453 assert(dev);
454
455 return dev->revision;
456}
457
458void
459co_dev_set_revision(co_dev_t *dev, co_unsigned32_t revision)
460{
461 assert(dev);
462
463 dev->revision = revision;
464}
465
466#if !LELY_NO_CO_OBJ_NAME
467
468const char *
470{
471 assert(dev);
472
473 return dev->order_code;
474}
475
476int
477co_dev_set_order_code(co_dev_t *dev, const char *order_code)
478{
479 assert(dev);
480
481 if (!order_code || !*order_code) {
482 free(dev->order_code);
483 dev->order_code = NULL;
484 return 0;
485 }
486
487 void *ptr = realloc(dev->order_code, strlen(order_code) + 1);
488 if (!ptr) {
489#if !LELY_NO_ERRNO
490 set_errc(errno2c(errno));
491#endif
492 return -1;
493 }
494 dev->order_code = ptr;
495 strcpy(dev->order_code, order_code);
496
497 return 0;
498}
499
500#endif // !LELY_NO_CO_OBJ_NAME
501
502unsigned int
504{
505 assert(dev);
506
507 return dev->baud;
508}
509
510void
511co_dev_set_baud(co_dev_t *dev, unsigned int baud)
512{
513 assert(dev);
514
515 dev->baud = baud;
516}
517
518co_unsigned16_t
520{
521 assert(dev);
522
523 return dev->rate;
524}
525
526void
527co_dev_set_rate(co_dev_t *dev, co_unsigned16_t rate)
528{
529 assert(dev);
530
531 dev->rate = rate;
532}
533
534int
536{
537 assert(dev);
538
539 return dev->lss;
540}
541
542void
544{
545 assert(dev);
546
547 dev->lss = !!lss;
548}
549
550co_unsigned32_t
552{
553 assert(dev);
554
555 return dev->dummy;
556}
557
558void
559co_dev_set_dummy(co_dev_t *dev, co_unsigned32_t dummy)
560{
561 assert(dev);
562
563 dev->dummy = dummy;
564}
565
566const void *
567co_dev_get_val(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
568{
569 co_sub_t *sub = dev ? co_dev_find_sub(dev, idx, subidx) : NULL;
570 return co_sub_get_val(sub);
571}
572
573size_t
574co_dev_set_val(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx,
575 const void *ptr, size_t n)
576{
577 assert(dev);
578
579 co_sub_t *sub = co_dev_find_sub(dev, idx, subidx);
580 if (!sub) {
582 return 0;
583 }
584
585 return co_sub_set_val(sub, ptr, n);
586}
587
588#define LELY_CO_DEFINE_TYPE(a, b, c, d) \
589 co_##b##_t co_dev_get_val_##c(const co_dev_t *dev, \
590 co_unsigned16_t idx, co_unsigned8_t subidx) \
591 { \
592 /* clang-format off */ \
593 co_sub_t *sub = dev \
594 ? co_dev_find_sub(dev, idx, subidx) \
595 : NULL; \
596 /* clang-format on */ \
597 return co_sub_get_val_##c(sub); \
598 } \
599\
600 size_t co_dev_set_val_##c(co_dev_t *dev, co_unsigned16_t idx, \
601 co_unsigned8_t subidx, co_##b##_t c) \
602 { \
603 assert(dev); \
604\
605 co_sub_t *sub = co_dev_find_sub(dev, idx, subidx); \
606 if (!sub) { \
607 set_errnum(ERRNUM_INVAL); \
608 return 0; \
609 } \
610\
611 return co_sub_set_val_##c(sub, c); \
612 }
613#include <lely/co/def/basic.def>
614#undef LELY_CO_DEFINE_TYPE
615
616size_t
617co_dev_read_sub(co_dev_t *dev, co_unsigned16_t *pidx, co_unsigned8_t *psubidx,
618 const uint_least8_t *begin, const uint_least8_t *end)
619{
620 if (!begin || !end || end - begin < 2 + 1 + 4)
621 return 0;
622
623 // Read the object index.
624 co_unsigned16_t idx;
625 if (co_val_read(CO_DEFTYPE_UNSIGNED16, &idx, begin, end) != 2)
626 return 0;
627 begin += 2;
628 // Read the object sub-index.
629 co_unsigned8_t subidx;
630 if (co_val_read(CO_DEFTYPE_UNSIGNED8, &subidx, begin, end) != 1)
631 return 0;
632 begin += 1;
633 // Read the value size (in bytes).
634 co_unsigned32_t size;
635 if (co_val_read(CO_DEFTYPE_UNSIGNED32, &size, begin, end) != 4)
636 return 0;
637 begin += 4;
638
639 if (end - begin < (ptrdiff_t)size)
640 return 0;
641
642 // Read the value into the sub-object, if it exists.
643 co_sub_t *sub = co_dev_find_sub(dev, idx, subidx);
644 if (sub) {
645 co_unsigned16_t type = co_sub_get_type(sub);
646 union co_val val;
647#if LELY_NO_MALLOC
648 struct co_array array = CO_ARRAY_INIT;
649 if (co_type_is_array(type))
650 co_val_init_array(&val, &array);
651 else
652#endif
653 co_val_init(type, &val);
654 if (co_val_read(type, &val, begin, begin + size) == size)
655 co_sub_set_val(sub, co_val_addressof(type, &val),
656 co_val_sizeof(type, &val));
657#if !LELY_NO_MALLOC
658 co_val_fini(type, &val);
659#endif
660 }
661
662 if (pidx)
663 *pidx = idx;
664 if (psubidx)
665 *psubidx = subidx;
666
667 return 2 + 1 + 4 + size;
668}
669
670size_t
671co_dev_write_sub(const co_dev_t *dev, co_unsigned16_t idx,
672 co_unsigned8_t subidx, uint_least8_t *begin, uint_least8_t *end)
673{
674 co_sub_t *sub = co_dev_find_sub(dev, idx, subidx);
675 if (!sub)
676 return 0;
677 co_unsigned16_t type = co_sub_get_type(sub);
678 const void *val = co_sub_get_val(sub);
679
680 co_unsigned32_t size = co_val_write(type, val, NULL, NULL);
681 if (!size && co_val_sizeof(type, val))
682 return 0;
683
684 if (begin && (!end || end - begin >= (ptrdiff_t)(2 + 1 + 4 + size))) {
685 // Write the object index.
686 if (co_val_write(CO_DEFTYPE_UNSIGNED16, &idx, begin, end) != 2)
687 return 0;
688 begin += 2;
689 // Write the object sub-index.
690 if (co_val_write(CO_DEFTYPE_UNSIGNED8, &subidx, begin, end)
691 != 1)
692 return 0;
693 begin += 1;
694 // Write the value size (in bytes).
695 if (co_val_write(CO_DEFTYPE_UNSIGNED32, &size, begin, end) != 4)
696 return 0;
697 begin += 4;
698 // Write the value.
699 if (co_val_write(type, val, begin, end) != size)
700 return 0;
701 }
702
703 return 2 + 1 + 4 + size;
704}
705
706int
707co_dev_read_dcf(co_dev_t *dev, co_unsigned16_t *pmin, co_unsigned16_t *pmax,
708 void *const *ptr)
709{
710 assert(dev);
711 assert(ptr);
712
713 co_unsigned16_t min = CO_UNSIGNED16_MAX;
714 co_unsigned16_t max = CO_UNSIGNED16_MIN;
715
716 size_t size = co_val_sizeof(CO_DEFTYPE_DOMAIN, ptr);
717 const uint_least8_t *begin = *ptr;
718 const uint_least8_t *end = begin + size;
719
720 // Read the total number of sub-indices.
721 co_unsigned32_t n;
722 size = co_val_read(CO_DEFTYPE_UNSIGNED32, &n, begin, end);
723 if (size != 4)
724 return -1;
725 begin += size;
726
727 for (size_t i = 0; i < n; i++) {
728 // Read the value of the sub-object.
729 co_unsigned16_t idx;
730 size = co_dev_read_sub(dev, &idx, NULL, begin, end);
731 if (!size)
732 return -1;
733 begin += size;
734
735 // Keep track of the index range.
736 min = MIN(min, idx);
737 max = MAX(max, idx);
738 }
739
740 if (pmin)
741 *pmin = min;
742 if (pmax)
743 *pmax = max;
744
745 return 0;
746}
747
748#if !LELY_NO_STDIO
749int
750co_dev_read_dcf_file(co_dev_t *dev, co_unsigned16_t *pmin,
751 co_unsigned16_t *pmax, const char *filename)
752{
753 void *dom = NULL;
754 if (!co_val_read_file(CO_DEFTYPE_DOMAIN, &dom, filename))
755 return -1;
756
757 if (co_dev_read_dcf(dev, pmin, pmax, &dom) == -1) {
759 return -1;
760 }
761
763 return 0;
764}
765#endif
766
767int
768co_dev_write_dcf(const co_dev_t *dev, co_unsigned16_t min, co_unsigned16_t max,
769 void **ptr)
770{
771 assert(dev);
772 assert(ptr);
773
774 size_t size = 4;
775 co_unsigned32_t n = 0;
776
777 // Count the number of matching sub-objects and compute the total size
778 // (in bytes).
779 for (co_obj_t *obj = co_dev_first_obj(dev); obj;
780 obj = co_obj_next(obj)) {
781 co_unsigned16_t idx = co_obj_get_idx(obj);
782 if (idx < min)
783 continue;
784 if (idx > max)
785 break;
786 for (co_sub_t *sub = co_obj_first_sub(obj); sub;
787 sub = co_sub_next(sub)) {
788 co_unsigned8_t subidx = co_sub_get_subidx(sub);
789 size += co_dev_write_sub(dev, idx, subidx, NULL, NULL);
790 n++;
791 }
792 }
793
794 // Create a DOMAIN for the concise DCF.
795 if (co_val_init_dom(ptr, NULL, size) == -1)
796 return -1;
797
798 uint_least8_t *begin = *ptr;
799 uint_least8_t *end = begin + size;
800
801 // Write the total number of sub-indices.
802 begin += co_val_write(CO_DEFTYPE_UNSIGNED32, &n, begin, end);
803
804 // Write the sub-objects.
805 for (co_obj_t *obj = co_dev_first_obj(dev); obj;
806 obj = co_obj_next(obj)) {
807 co_unsigned16_t idx = co_obj_get_idx(obj);
808 if (idx < min)
809 continue;
810 if (idx > max)
811 break;
812 for (co_sub_t *sub = co_obj_first_sub(obj); sub;
813 sub = co_sub_next(sub)) {
814 co_unsigned8_t subidx = co_sub_get_subidx(sub);
815 begin += co_dev_write_sub(dev, idx, subidx, begin, end);
816 }
817 }
818
819 return 0;
820}
821
822#if !LELY_NO_STDIO
823int
824co_dev_write_dcf_file(const co_dev_t *dev, co_unsigned16_t min,
825 co_unsigned16_t max, const char *filename)
826{
827 void *dom = NULL;
828 if (co_dev_write_dcf(dev, min, max, &dom) == -1)
829 return -1;
830
831 size_t nbyte = co_val_sizeof(CO_DEFTYPE_DOMAIN, &dom);
832 if (co_val_write_file(CO_DEFTYPE_DOMAIN, &dom, filename) != nbyte) {
834 return -1;
835 }
836
838 return 0;
839}
840#endif
841
842#if !LELY_NO_CO_TPDO
843
844void
846 void **pdata)
847{
848 assert(dev);
849
850 if (pind)
851 *pind = dev->tpdo_event_ind;
852 if (pdata)
853 *pdata = dev->tpdo_event_data;
854}
855
856void
858 co_dev_t *dev, co_dev_tpdo_event_ind_t *ind, void *data)
859{
860 assert(dev);
861
862 dev->tpdo_event_ind = ind;
863 dev->tpdo_event_data = data;
864}
865
866void
867co_dev_tpdo_event(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
868{
869 assert(dev);
870
871 // Check if the specified sub-object can be mapped into a TPDO.
872 if (co_dev_chk_tpdo(dev, idx, subidx))
873 return;
874
875 const co_obj_t *obj_1800 = NULL;
876 // Find the first TPDO.
877 for (co_unsigned16_t i = 0; i < CO_NUM_PDOS && !obj_1800; i++)
878 obj_1800 = co_dev_find_obj(dev, 0x1800 + i);
879 for (; obj_1800; obj_1800 = co_obj_next(obj_1800)) {
880 co_unsigned16_t i = co_obj_get_idx(obj_1800) - 0x1800;
881 if (i >= CO_NUM_PDOS)
882 break;
883 // Check if this is a valid acyclic or event-driven PDO.
884 const struct co_pdo_comm_par *comm =
885 co_obj_addressof_val(obj_1800);
886 assert(comm);
887 if (comm->n < 2 || (comm->cobid & CO_PDO_COBID_VALID)
888 || !(!comm->trans || comm->trans >= 0xfe))
889 continue;
890 // Check if the sub-object is mapped into this PDO.
891 const co_obj_t *obj_1a00 = co_dev_find_obj(dev, 0x1a00 + i);
892 if (!obj_1a00)
893 continue;
894 const struct co_pdo_map_par *map =
895 co_obj_addressof_val(obj_1a00);
896 assert(map);
897 // Check whether this is an MPDO.
898 if (map->n > 0x40)
899 continue;
900 for (size_t j = 0; j < map->n; j++) {
901 if (((map->map[j] >> 16) & 0xffff) != idx)
902 continue;
903 if (((map->map[j] >> 8) & 0xff) != subidx)
904 continue;
905 // Issue a single indication for this PDO.
906 if (dev->tpdo_event_ind)
907 dev->tpdo_event_ind(
908 i + 1, dev->tpdo_event_data);
909 break;
910 }
911 }
912}
913
914#if !LELY_NO_CO_MPDO
915
916void
918 co_dev_sam_mpdo_event_ind_t **pind, void **pdata)
919{
920 assert(dev);
921
922 if (pind)
923 *pind = dev->sam_mpdo_event_ind;
924 if (pdata)
925 *pdata = dev->sam_mpdo_event_data;
926}
927
928void
930 co_dev_t *dev, co_dev_sam_mpdo_event_ind_t *ind, void *data)
931{
932 assert(dev);
933
934 dev->sam_mpdo_event_ind = ind;
935 dev->sam_mpdo_event_data = data;
936}
937
938void
939co_dev_sam_mpdo_event(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
940{
941 assert(dev);
942
943 // Check whether the sub-object can be mapped into a SAM-MPDO.
944 if (!co_dev_chk_sam_mpdo(dev, idx, subidx))
945 return;
946
947 // Check if the specified sub-object can be mapped into a TPDO.
948 if (co_dev_chk_tpdo(dev, idx, subidx))
949 return;
950
951 const co_obj_t *obj_1800 = NULL;
952 // Find the first TPDO.
953 for (co_unsigned16_t i = 0; i < CO_NUM_PDOS && !obj_1800; i++)
954 obj_1800 = co_dev_find_obj(dev, 0x1800 + i);
955 for (; obj_1800; obj_1800 = co_obj_next(obj_1800)) {
956 co_unsigned16_t i = co_obj_get_idx(obj_1800) - 0x1800;
957 if (i >= CO_NUM_PDOS)
958 break;
959 // Check if this is a valid event-driven PDO.
960 const struct co_pdo_comm_par *comm =
961 co_obj_addressof_val(obj_1800);
962 assert(comm);
963 if (comm->n < 2 || (comm->cobid & CO_PDO_COBID_VALID)
964 || comm->trans < 0xfe)
965 continue;
966 // Check if this is a SAM-MPDO.
967 const co_obj_t *obj_1a00 = co_dev_find_obj(dev, 0x1a00 + i);
968 if (!obj_1a00)
969 continue;
970 const struct co_pdo_map_par *map =
971 co_obj_addressof_val(obj_1a00);
972 assert(map);
973 if (map->n != CO_PDO_MAP_SAM_MPDO)
974 continue;
975 // Issue an indication for this PDO.
976 if (dev->sam_mpdo_event_ind)
977 dev->sam_mpdo_event_ind(i + 1, idx, subidx,
979 // Only a single SAM-MPDO producer per CANopen device is
980 // allowed.
981 break;
982 }
983}
984
985#endif // !LELY_NO_CO_MPDO
986
987#endif // !LELY_NO_CO_TPDO
988
989static void
990co_obj_set_id(co_obj_t *obj, co_unsigned8_t new_id, co_unsigned8_t old_id)
991{
992 assert(obj);
993
994 rbtree_foreach (&obj->tree, node)
995 co_sub_set_id(structof(node, co_sub_t, node), new_id, old_id);
996}
997
998static void
999co_sub_set_id(co_sub_t *sub, co_unsigned8_t new_id, co_unsigned8_t old_id)
1000{
1001 assert(sub);
1002
1003 unsigned int flags = co_sub_get_flags(sub);
1004 co_unsigned16_t type = co_sub_get_type(sub);
1005#if !LELY_NO_CO_OBJ_LIMITS
1006 if (flags & CO_OBJ_FLAGS_MIN_NODEID)
1007 co_val_set_id(type, &sub->min, new_id, old_id);
1008 if (flags & CO_OBJ_FLAGS_MAX_NODEID)
1009 co_val_set_id(type, &sub->max, new_id, old_id);
1010#endif
1011#if !LELY_NO_CO_OBJ_DEFAULT
1012 if (flags & CO_OBJ_FLAGS_DEF_NODEID)
1013 co_val_set_id(type, &sub->def, new_id, old_id);
1014#endif
1015 if (flags & CO_OBJ_FLAGS_VAL_NODEID)
1016 co_val_set_id(type, sub->val, new_id, old_id);
1017}
1018
1019static void
1020co_val_set_id(co_unsigned16_t type, void *val, co_unsigned8_t new_id,
1021 co_unsigned8_t old_id)
1022{
1023 assert(val);
1024
1025 union co_val *u = val;
1026 switch (type) {
1027#define LELY_CO_DEFINE_TYPE(a, b, c, d) \
1028 case CO_DEFTYPE_##a: \
1029 u->c += new_id - old_id; \
1030 break;
1031#include <lely/co/def/basic.def>
1032#undef LELY_CO_DEFINE_TYPE
1033 }
1034}
This header file is part of the utilities library; it contains the comparison function definitions.
This is the internal header file of the device description.
void co_dev_sam_mpdo_event(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Checks if the specified sub-object in the object dictionary of a CANopen device can be mapped into a ...
Definition: dev.c:939
void co_dev_get_tpdo_event_ind(const co_dev_t *dev, co_dev_tpdo_event_ind_t **pind, void **pdata)
Retrieves the indication function invoked by co_dev_tpdo_event() when an event is indicated for (a su...
Definition: dev.c:845
co_obj_t * co_dev_find_obj(const co_dev_t *dev, co_unsigned16_t idx)
Finds an object in the object dictionary of a CANopen device.
Definition: dev.c:279
co_unsigned16_t co_dev_get_idx(const co_dev_t *dev, co_unsigned16_t maxidx, co_unsigned16_t *idx)
Retrieves a list of object indices in the object dictionary of a CANopen device.
Definition: dev.c:223
int co_dev_read_dcf(co_dev_t *dev, co_unsigned16_t *pmin, co_unsigned16_t *pmax, void *const *ptr)
Reads the values of a range of objects from a memory buffer, in the concise DCF format,...
Definition: dev.c:707
size_t co_dev_read_sub(co_dev_t *dev, co_unsigned16_t *pidx, co_unsigned8_t *psubidx, const uint_least8_t *begin, const uint_least8_t *end)
Reads a value from a memory buffer, in the concise DCF format, and stores it in a sub-object in the o...
Definition: dev.c:617
co_unsigned8_t co_dev_get_id(const co_dev_t *dev)
Returns the node-ID of a CANopen device.
Definition: dev.c:197
int co_dev_set_vendor_name(co_dev_t *dev, const char *vendor_name)
Sets the vendor name of a CANopen device.
Definition: dev.c:357
void co_dev_get_sam_mpdo_event_ind(const co_dev_t *dev, co_dev_sam_mpdo_event_ind_t **pind, void **pdata)
Retrieves the indication function invoked by co_dev_sam_mpdo_event() when an event is indicated for (...
Definition: dev.c:917
void co_dev_set_product_code(co_dev_t *dev, co_unsigned32_t product_code)
Sets the product code of a CANopen device.
Definition: dev.c:443
co_obj_t * co_dev_last_obj(const co_dev_t *dev)
Finds the last object (with the highest index) in the object dictionary of a CANopen device.
Definition: dev.c:306
void co_dev_set_dummy(co_dev_t *dev, co_unsigned32_t dummy)
Sets the data types supported by a CANopen device for mapping dummy entries in PDOs.
Definition: dev.c:559
int co_dev_insert_obj(co_dev_t *dev, co_obj_t *obj)
Inserts an object into the object dictionary of a CANopen device.
Definition: dev.c:242
const char * co_dev_get_product_name(const co_dev_t *dev)
Returns a pointer to the product name of a CANopen device.
Definition: dev.c:401
void co_dev_set_vendor_id(co_dev_t *dev, co_unsigned32_t vendor_id)
Sets the vendor ID of a CANopen device.
Definition: dev.c:391
int co_dev_set_id(co_dev_t *dev, co_unsigned8_t id)
Sets the node-ID of a CANopen device.
Definition: dev.c:205
int co_dev_set_netid(co_dev_t *dev, co_unsigned8_t id)
Sets the network-ID of a CANopen device.
Definition: dev.c:182
const char * co_dev_get_vendor_name(const co_dev_t *dev)
Returns a pointer to the vendor name of a CANopen device.
Definition: dev.c:349
void co_dev_set_revision(co_dev_t *dev, co_unsigned32_t revision)
Sets the revision number of a CANopen device.
Definition: dev.c:459
co_sub_t * co_dev_find_sub(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Finds a sub-object in the object dictionary of a CANopen device.
Definition: dev.c:290
co_unsigned16_t co_dev_get_rate(const co_dev_t *dev)
Returns the (pending) baudrate of a CANopen device (in kbit/s).
Definition: dev.c:519
int co_dev_write_dcf(const co_dev_t *dev, co_unsigned16_t min, co_unsigned16_t max, void **ptr)
Loads the values of a range of objects in the object dictionary of a CANopen device,...
Definition: dev.c:768
int co_dev_remove_obj(co_dev_t *dev, co_obj_t *obj)
Removes an object from the object dictionary a CANopen device.
Definition: dev.c:263
int co_dev_get_lss(const co_dev_t *dev)
Returns 1 if LSS is supported and 0 if not.
Definition: dev.c:535
void co_dev_set_sam_mpdo_event_ind(co_dev_t *dev, co_dev_sam_mpdo_event_ind_t *ind, void *data)
Sets the indication function invoked by co_dev_sam_mpdo_event() when an event is indicated for (a sub...
Definition: dev.c:929
co_unsigned8_t co_dev_get_netid(const co_dev_t *dev)
Returns the network-ID of a CANopen device.
Definition: dev.c:174
co_unsigned32_t co_dev_get_vendor_id(const co_dev_t *dev)
Returns the vendor ID of a CANopen device.
Definition: dev.c:383
void co_dev_set_lss(co_dev_t *dev, int lss)
Sets the LSS support flag.
Definition: dev.c:543
int co_dev_set_order_code(co_dev_t *dev, const char *order_code)
Sets the order code of a CANopen device.
Definition: dev.c:477
co_unsigned32_t co_dev_get_dummy(const co_dev_t *dev)
Returns the data types supported by a CANopen device for mapping dummy entries in PDOs (one bit for e...
Definition: dev.c:551
const char * co_dev_get_name(const co_dev_t *dev)
Returns the name of a CANopen device.
Definition: dev.c:317
int co_dev_write_dcf_file(const co_dev_t *dev, co_unsigned16_t min, co_unsigned16_t max, const char *filename)
Loads the values of a range of objects in the object dictionary of a CANopen device,...
Definition: dev.c:824
void co_dev_destroy(co_dev_t *dev)
Destroys a CANopen device, including all objects in its object dictionary.
Definition: dev.c:163
void co_dev_tpdo_event(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Checks if the specified sub-object in the object dictionary of a CANopen device can be mapped into a ...
Definition: dev.c:867
void co_dev_set_rate(co_dev_t *dev, co_unsigned16_t rate)
Sets the (pending) baudrate of a CANopen device.
Definition: dev.c:527
size_t co_dev_set_val(co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx, const void *ptr, size_t n)
Sets the current value of a CANopen sub-object.
Definition: dev.c:574
unsigned int co_dev_get_baud(const co_dev_t *dev)
Returns the supported bit rates of a CANopen device (any combination of CO_BAUD_1000,...
Definition: dev.c:503
co_unsigned32_t co_dev_get_product_code(const co_dev_t *dev)
Returns the product code of a CANopen device.
Definition: dev.c:435
const void * co_dev_get_val(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Returns a pointer to the current value of a CANopen sub-object.
Definition: dev.c:567
int co_dev_set_product_name(co_dev_t *dev, const char *product_name)
Sets the product name of a CANopen device.
Definition: dev.c:409
co_obj_t * co_dev_first_obj(const co_dev_t *dev)
Finds the first object (with the lowest index) in the object dictionary of a CANopen device.
Definition: dev.c:297
void co_dev_set_baud(co_dev_t *dev, unsigned int baud)
Sets the supported bit rates of a CANopen device.
Definition: dev.c:511
void co_dev_set_tpdo_event_ind(co_dev_t *dev, co_dev_tpdo_event_ind_t *ind, void *data)
Sets the indication function invoked by co_dev_tpdo_event() when an event is indicated for (a sub-obj...
Definition: dev.c:857
int co_dev_read_dcf_file(co_dev_t *dev, co_unsigned16_t *pmin, co_unsigned16_t *pmax, const char *filename)
Reads the values of a range of objects from a file, in the concise DCF format, and stores them in the...
Definition: dev.c:750
size_t co_dev_write_sub(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx, uint_least8_t *begin, uint_least8_t *end)
Loads the value of a sub-object from the object dictionary of a CANopen device, and writes it to a me...
Definition: dev.c:671
co_dev_t * co_dev_create(co_unsigned8_t id)
Creates a new CANopen device.
Definition: dev.c:138
const char * co_dev_get_order_code(const co_dev_t *dev)
Returns a pointer to the order code of a CANopen device.
Definition: dev.c:469
int co_dev_set_name(co_dev_t *dev, const char *name)
Sets the name of a CANopen device.
Definition: dev.c:325
co_unsigned32_t co_dev_get_revision(const co_dev_t *dev)
Returns the revision number of a CANopen device.
Definition: dev.c:451
void co_dev_tpdo_event_ind_t(co_unsigned16_t num, void *data)
The type of a CANopen Transmit-PDO event indication function, invoked by co_dev_tpdo_event() when an ...
Definition: dev.h:97
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
#define CO_NUM_NETWORKS
The maximum number of CANopen networks.
Definition: dev.h:53
void co_dev_sam_mpdo_event_ind_t(co_unsigned16_t num, co_unsigned16_t idx, co_unsigned8_t subidx, void *data)
The type of a CANopen source address mode multiplex PDO event indication function,...
Definition: dev.h:109
This is the internal header file of the object dictionary.
This header file is part of the utilities library; it contains the diagnostic declarations.
@ ERRNUM_INVAL
Invalid argument.
Definition: errnum.h:132
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
#define MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
co_sub_t * co_obj_first_sub(const co_obj_t *obj)
Finds the first sub-object (with the lowest sub-index) in a CANopen object.
Definition: obj.c:249
co_unsigned8_t co_sub_get_subidx(const co_sub_t *sub)
Returns the sub-index of a CANopen sub-object.
Definition: obj.c:559
#define CO_OBJ_FLAGS_MAX_NODEID
The upper limit of the object value is of the form $NODEID { "+" number }.
Definition: obj.h:108
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
Definition: obj.c:328
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:164
const void * co_sub_get_val(const co_sub_t *sub)
Returns a pointer to the current value of a CANopen sub-object.
Definition: obj.c:712
co_sub_t * co_obj_find_sub(const co_obj_t *obj, co_unsigned8_t subidx)
Finds a sub-object in a CANopen object.
Definition: obj.c:240
size_t co_sub_set_val(co_sub_t *sub, const void *ptr, size_t n)
Sets the current value of a CANopen sub-object.
Definition: obj.c:718
#define CO_OBJ_FLAGS_DEF_NODEID
The default object value is of the form $NODEID { "+" number }.
Definition: obj.h:111
unsigned int co_sub_get_flags(const co_sub_t *sub)
Returns the object flags of a CANopen sub-object.
Definition: obj.c:818
co_obj_t * co_obj_next(const co_obj_t *obj)
Finds the next object in the object dictionary of a CANopen device.
Definition: obj.c:147
#define CO_OBJ_FLAGS_MIN_NODEID
The lower limit of the object value is of the form $NODEID { "+" number }.
Definition: obj.h:105
#define CO_OBJ_FLAGS_VAL_NODEID
The current object value is of the form $NODEID { "+" number }.
Definition: obj.h:114
void co_obj_destroy(co_obj_t *obj)
Destroys a CANopen object, including its sub-objects.
Definition: obj.c:126
co_sub_t * co_sub_next(const co_sub_t *sub)
Finds the next sub-object in a CANopen object.
Definition: obj.c:542
co_unsigned16_t co_sub_get_type(const co_sub_t *sub)
Returns the data type of a CANopen sub-object.
Definition: obj.c:603
This header file is part of the CANopen library; it contains the Process Data Object (PDO) declaratio...
co_unsigned32_t co_dev_chk_tpdo(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Checks if the specified object is valid and can be mapped into a Transmit-PDO.
Definition: pdo.c:130
#define CO_PDO_COBID_VALID
The bit in the PDO COB-ID specifying whether the PDO exists and is valid.
Definition: pdo.h:37
#define CO_NUM_PDOS
The maximum number of Receive/Transmit-PDOs.
Definition: pdo.h:28
int co_dev_chk_sam_mpdo(const co_dev_t *dev, co_unsigned16_t idx, co_unsigned8_t subidx)
Checks if the specified object is part of the object scanner list (objects 1FA0..1FCF) and can be tra...
Definition: pdo.c:203
#define CO_PDO_MAP_SAM_MPDO
The value of sub-index 0 of the PDO mapping parameter record indicating a a source address mode multi...
Definition: pdo.h:52
void rbnode_init(struct rbnode *node, const void *key)
Initializes a node in a red-black tree.
Definition: rbtree.h:237
struct rbnode * rbtree_last(const struct rbtree *tree)
Returns a pointer to the last (rightmost) node in a red-black tree.
Definition: rbtree.c:343
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
struct rbnode * rbtree_first(const struct rbtree *tree)
Returns a pointer to the first (leftmost) node in a red-black tree.
Definition: rbtree.c:335
struct rbnode * rbnode_next(const struct rbnode *node)
Returns a pointer to the next (in-order) node in a red-black tree with respect to node.
Definition: rbtree.c:91
void rbtree_init(struct rbtree *tree, rbtree_cmp_t *cmp)
Initializes a red-black tree.
Definition: rbtree.h:248
size_t rbtree_size(const struct rbtree *tree)
Returns the size (in number of nodes) of a red-black tree.
Definition: rbtree.h:265
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 CANopen library.
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib....
A CANopen device.
Definition: dev.h:30
struct rbtree tree
The tree containing the object dictionary.
Definition: dev.h:36
co_unsigned32_t product_code
The product code.
Definition: dev.h:50
char * product_name
A pointer to the product name.
Definition: dev.h:47
co_unsigned16_t rate
The (pending) baudrate (in kbit/s).
Definition: dev.h:60
char * vendor_name
A pointer to the vendor name.
Definition: dev.h:41
char * order_code
A pointer to the order code.
Definition: dev.h:55
co_dev_tpdo_event_ind_t * tpdo_event_ind
A pointer to the Transmit-PDO event indication function.
Definition: dev.h:67
co_unsigned8_t netid
The network-ID.
Definition: dev.h:32
co_unsigned8_t id
The node-ID.
Definition: dev.h:34
void * sam_mpdo_event_data
A pointer to user-specified data for sam_mpdo_event_ind.
Definition: dev.h:74
unsigned baud
The supported bit rates.
Definition: dev.h:58
int lss
A flag specifying whether LSS is supported (1) or not (0).
Definition: dev.h:62
co_unsigned32_t vendor_id
The vendor ID.
Definition: dev.h:44
co_dev_sam_mpdo_event_ind_t * sam_mpdo_event_ind
A pointer to the SAM-MPDO event indication function.
Definition: dev.h:72
co_unsigned32_t revision
The revision number.
Definition: dev.h:52
co_unsigned32_t dummy
The data types supported for mapping dummy entries in PDOs.
Definition: dev.h:64
void * tpdo_event_data
A pointer to user-specified data for tpdo_event_ind.
Definition: dev.h:69
char * name
A pointer to the name of the device.
Definition: dev.h:39
A CANopen object.
Definition: obj.h:31
co_unsigned16_t idx
The object index.
Definition: obj.h:37
struct rbtree tree
The tree containing all the sub-objects.
Definition: obj.h:45
struct rbnode node
The node of this object in the tree of objects.
Definition: obj.h:33
co_dev_t * dev
A pointer to the CANopen device containing this object.
Definition: obj.h:35
A CANopen sub-object.
Definition: obj.h:53
union co_val def
The default value.
Definition: obj.h:74
union co_val min
The lower limit of the object value.
Definition: obj.h:68
union co_val max
The upper limit of the object value.
Definition: obj.h:70
void * val
A pointer to the sub-object value.
Definition: obj.h:77
A PDO communication parameter record.
Definition: pdo.h:64
co_unsigned32_t cobid
COB-ID.
Definition: pdo.h:68
co_unsigned8_t trans
Transmission type.
Definition: pdo.h:70
co_unsigned8_t n
Highest sub-index supported.
Definition: pdo.h:66
A PDO mapping parameter record.
Definition: pdo.h:90
co_unsigned32_t map[CO_PDO_NUM_MAPS]
An array of objects to be mapped.
Definition: pdo.h:94
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
#define CO_DEFTYPE_UNSIGNED16
The data type (and object index) of a 16-bit unsigned integer.
Definition: type.h:47
int co_type_is_array(co_unsigned16_t type)
Returns 1 if the specified (static) data type is an array, and 0 if not.
Definition: type.c:40
#define CO_DEFTYPE_DOMAIN
The data type (and object index) of an arbitrary large block of data.
Definition: type.h:77
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
A union of the CANopen static data types.
Definition: val.h:273
size_t co_val_read(co_unsigned16_t type, void *val, const uint_least8_t *begin, const uint_least8_t *end)
Reads a value of the specified data type from a memory buffer.
Definition: val.c:451
int co_val_init(co_unsigned16_t type, void *val)
Initializes a value of the specified data type to zero.
Definition: val.c:62
const void * co_val_addressof(co_unsigned16_t type, const void *val)
Returns the address of the first byte in a value of the specified data type.
Definition: val.c:260
#define CO_UNSIGNED16_MIN
The minimum value of a 16-bit unsigned integer.
Definition: val.h:87
size_t co_val_write(co_unsigned16_t type, const void *val, uint_least8_t *begin, uint_least8_t *end)
Writes a value of the specified data type to a memory buffer.
Definition: val.c:791
int co_val_init_dom(void **val, const void *dom, size_t n)
Initializes an arbitrary large block of data (CO_DEFTYPE_DOMAIN).
Definition: val.c:230
size_t co_val_sizeof(co_unsigned16_t type, const void *val)
Returns the size (in bytes) of a value of the specified data type.
Definition: val.c:269
size_t co_val_read_file(co_unsigned16_t type, void *val, const char *filename)
Reads a value of the specified data type from a file.
Definition: val.c:671
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:249
size_t co_val_write_file(co_unsigned16_t type, const void *val, const char *filename)
Writes a value of the specified data type to a file.
Definition: val.c:943
#define CO_UNSIGNED16_MAX
The maximum value of a 16-bit unsigned integer.
Definition: val.h:90