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