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
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
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
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
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
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
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 }
int lss
A flag specifying whether LSS is supported (1) or not (0).
Definition: dev.c:67
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
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
A PDO mapping parameter record.
Definition: pdo.h:69
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
#define CO_NUM_NODES
The maximum number of nodes in a CANopen network.
Definition: dev.h:56
const void * key
A pointer to the key for this node.
Definition: rbtree.h:58
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
void * tpdo_event_data
A pointer to user-specified data for tpdo_event_ind.
Definition: dev.c:74
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
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, and writes them to a memory buffer, in the concise DCF format.
Definition: dev.c:783
A CANopen sub-object.
Definition: obj.h:54
#define CO_UNSIGNED16_MAX
The maximum value of a 16-bit unsigned integer.
Definition: val.h:64
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_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
#define CO_DEFTYPE_DOMAIN
The data type (and object index) of an arbitrary large block of data.
Definition: type.h:77
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
char * order_code
A pointer to the order code.
Definition: dev.c:61
This header file is part of the utilities library; it contains the comparison function definitions...
co_unsigned32_t revision
The revision number.
Definition: dev.c:59
size_t rbtree_size(const struct rbtree *tree)
Returns the size (in number of nodes) of a red-black tree.
Definition: rbtree.h:252
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 set_errnum(errnum_t errnum)
Sets the current (thread-specific) platform-independent error number to errnum.
Definition: errnum.h:375
co_dev_t * dev
A pointer to the CANopen device containing this object.
Definition: obj.h:36
void diag(enum diag_severity severity, int errc, const char *format,...)
Emits a diagnostic message.
Definition: diag.c:156
co_unsigned8_t n
Number of mapped objects in PDO.
Definition: pdo.h:71
A union of the CANopen static data types.
Definition: val.h:163
A PDO communication parameter record.
Definition: pdo.h:43
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
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
#define CO_NUM_NETWORKS
The maximum number of CANopen networks.
Definition: dev.h:53
#define MIN(a, b)
Returns the minimum of a and b.
Definition: util.h:57
#define CO_UNSIGNED16_MIN
The minimum value of a 16-bit unsigned integer.
Definition: val.h:61
co_unsigned32_t cobid
COB-ID.
Definition: pdo.h:47
co_unsigned8_t co_dev_get_id(const co_dev_t *dev)
Returns the node-ID of a CANopen device.
Definition: dev.c:207
union co_val max
The upper limit of the object value.
Definition: obj.h:71
struct rbtree tree
The tree containing all the sub-objects.
Definition: obj.h:46
union co_val min
The lower limit of the object value.
Definition: obj.h:69
This is the internal header file of the object dictionary.
unsigned baud
The supported bit rates.
Definition: dev.c:63
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
char * name
A pointer to the name of the device.
Definition: dev.c:49
struct rbtree tree
The tree containing the object dictionary.
Definition: dev.c:47
void set_errc(int errc)
Sets the current (thread-specific) native error code to errc.
Definition: errnum.c:957
union co_val def
The default value.
Definition: obj.h:75
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
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
#define CO_DEFTYPE_UNSIGNED32
The data type (and object index) of a 32-bit unsigned integer.
Definition: type.h:50
co_unsigned8_t netid
The network-ID.
Definition: dev.c:43
co_unsigned32_t vendor_id
The vendor ID.
Definition: dev.c:53
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 * val
A pointer to the sub-object value.
Definition: obj.h:78
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
#define rbtree_foreach(tree, node)
Iterates over each node in a red-black tree in ascending order.
Definition: rbtree.h:226
co_unsigned32_t co_dev_get_revision(const co_dev_t *dev)
Returns the revision number of a CANopen device.
Definition: dev.c:446
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
A red-black tree.
Definition: rbtree.h:90
int co_dev_get_lss(const co_dev_t *dev)
Returns 1 if LSS is supported and 0 if not.
Definition: dev.c:524
struct rbnode node
The node of this object in the tree of objects.
Definition: obj.h:34
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
#define CO_DEFTYPE_UNSIGNED16
The data type (and object index) of a 16-bit unsigned integer.
Definition: type.h:47
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
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
frbuf_t * frbuf_create(const char *filename)
Creates a new read file buffer.
Definition: frbuf.c:133
This is the internal header file of the CANopen library.
fwbuf_t * fwbuf_create(const char *filename)
Creates a new (atomic) write file buffer.
Definition: fwbuf.c:302
#define CO_PDO_COBID_VALID
The bit in the PDO COB-ID specifying whether the PDO exists and is valid.
Definition: pdo.h:28
co_unsigned8_t n
Highest sub-index supported.
Definition: pdo.h:45
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
char * product_name
A pointer to the product name.
Definition: dev.c:55
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
int get_errc(void)
Returns the last (thread-specific) native error code set by a system call or library function...
Definition: errnum.c:947
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
#define MAX(a, b)
Returns the maximum of a and b.
Definition: util.h:65
const char * co_dev_get_name(const co_dev_t *dev)
Returns the name of a CANopen device.
Definition: dev.c:324
#define CO_DEFTYPE_UNSIGNED8
The data type (and object index) of an 8-bit unsigned integer.
Definition: type.h:44
An read file buffer struct.
Definition: frbuf.c:49
struct rbnode * rbtree_find(const struct rbtree *tree, const void *key)
Finds a node in a red-black tree.
Definition: rbtree.c:306
co_sub_t * co_sub_next(const co_sub_t *sub)
Finds the next sub-object in a CANopen object.
Definition: obj.c:511
void frbuf_destroy(frbuf_t *buf)
Destroys a read file buffer.
Definition: frbuf.c:158
co_unsigned32_t dummy
The data types supported for mapping dummy entries in PDOs.
Definition: dev.c:69
An error.
Definition: diag.h:49
Invalid argument.
Definition: errnum.h:129
void rbtree_remove(struct rbtree *tree, struct rbnode *node)
Removes a node from a red-black tree.
Definition: rbtree.c:187
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
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
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, CO_BAUD_800, CO_BAUD_500, CO_BAUD_250, CO_BAUD_125, CO_BAUD_50, CO_BAUD_20, CO_BAUD_10 and CO_BAUD_AUTO).
Definition: dev.c:492
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_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
void co_dev_set_lss(co_dev_t *dev, int lss)
Sets the LSS support flag.
Definition: dev.c:532
co_unsigned8_t co_dev_get_netid(const co_dev_t *dev)
Returns the network-ID of a CANopen device.
Definition: dev.c:184
int errno2c(int errnum)
Transforms a standard C error number to a native error code.
Definition: errnum.c:43
void co_dev_destroy(co_dev_t *dev)
Destroys a CANopen device, including all objects in its object dictionary.
Definition: dev.c:175
This header file is part of the utilities library; it contains the diagnostic declarations.
#define CO_OBJ_FLAGS_VAL_NODEID
The current object value is of the form $NODEID { "+" number }.
Definition: obj.h:114
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
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
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_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
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
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
A CANopen device.
Definition: dev.c:41
An (atomic) write file buffer struct.
Definition: fwbuf.c:56
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
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
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
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
void co_obj_destroy(co_obj_t *obj)
Destroys a CANopen object, including its sub-objects.
Definition: obj.c:118
#define structof(ptr, type, member)
Obtains the address of a structure from the address of one of its members.
Definition: util.h:93
co_unsigned8_t id
The node-ID.
Definition: dev.c:45
co_unsigned32_t product_code
The product code.
Definition: dev.c:57
int co_val_init(co_unsigned16_t type, void *val)
Initializes a value of the specified data type to zero.
Definition: val.c:122
int co_dev_set_name(co_dev_t *dev, const char *name)
Sets the name of a CANopen device.
Definition: dev.c:332
void co_val_fini(co_unsigned16_t type, void *val)
Finalizes a value of the specified data type.
Definition: val.c:275
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
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
char * vendor_name
A pointer to the vendor name.
Definition: dev.c:51
This header file is part of the utilities library; it contains the read file buffer declarations...
This header file is part of the CANopen library; it contains the device description declarations...
void rbtree_init(struct rbtree *tree, rbtree_cmp_t *cmp)
Initializes a red-black tree.
Definition: rbtree.h:238
#define CO_OBJ_FLAGS_DEF_NODEID
The default object value is of the form $NODEID { "+" number }.
Definition: obj.h:111
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, and stores them in the object dictionary of a CANopen device.
Definition: dev.c:688
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
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
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 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
void rbtree_insert(struct rbtree *tree, struct rbnode *node)
Inserts a node into a red-black tree.
Definition: rbtree.c:108
co_unsigned32_t map[0x40]
An array of objects to be mapped.
Definition: pdo.h:73
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
This header file is part of the utilities library; it contains the (atomic) write file buffer declara...
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
This header file is part of the C11 and POSIX compatibility library; it includes <stdlib.h> and defines any missing functionality.
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
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
co_dev_tpdo_event_ind_t * tpdo_event_ind
A pointer to the Transmit-PDO event indication function.
Definition: dev.c:72
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
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
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
co_unsigned16_t co_obj_get_idx(const co_obj_t *obj)
Returns the index of a CANopen object.
Definition: obj.c:154
#define CO_OBJ_FLAGS_MAX_NODEID
The upper limit of the object value is of the form $NODEID { "+" number }.
Definition: obj.h:108
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
A CANopen object.
Definition: obj.h:32
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, and writes them to a file, in the concise DCF format.
Definition: dev.c:839
#define CO_OBJ_FLAGS_MIN_NODEID
The lower limit of the object value is of the form $NODEID { "+" number }.
Definition: obj.h:105
co_unsigned8_t trans
Transmission type.
Definition: pdo.h:49
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
co_unsigned16_t rate
The (pending) baudrate (in kbit/s).
Definition: dev.c:65
void * co_obj_addressof_val(const co_obj_t *obj)
Returns the address of the value of a CANopen object.
Definition: obj.c:311
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
co_dev_t * co_dev_create(co_unsigned8_t id)
Creates a new CANopen device.
Definition: dev.c:150
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
A node in a red-black tree.
Definition: rbtree.h:52
This header file is part of the CANopen library; it contains the Process Data Object (PDO) declaratio...