27 #ifndef LELY_NO_CO_DCF
42 static struct __co_dev *__co_dev_init_from_dcf_cfg(
47 static int co_obj_parse_cfg(
49 #ifndef LELY_NO_CO_OBJ_NAME
55 static int co_sub_parse_cfg(
58 co_unsigned16_t type,
const char *
name);
60 static int co_rpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask);
61 static int co_tpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask);
63 size_t co_val_lex_dcf(co_unsigned16_t type,
void *val,
const char *begin,
64 const char *end,
struct floc *at);
65 static size_t co_val_lex_id(
66 const char *begin,
const char *end,
struct floc *at);
67 static void co_val_set_id(co_unsigned16_t type,
void *val, co_unsigned8_t
id);
69 static co_unsigned16_t config_get_idx(
const config_t *cfg,
const char *section,
70 co_unsigned16_t maxidx, co_unsigned16_t *idx);
73 __co_dev_init_from_dcf_file(
struct __co_dev *dev,
const char *filename)
78 "unable to create configuration struct");
79 goto error_create_cfg;
83 goto error_parse_ini_file;
85 if (!__co_dev_init_from_dcf_cfg(dev, cfg))
107 goto error_alloc_dev;
110 if (!__co_dev_init_from_dcf_file(dev, filename)) {
125 __co_dev_init_from_dcf_text(
struct __co_dev *dev,
const char *begin,
126 const char *end,
struct floc *at)
131 "unable to create configuration struct");
132 goto error_create_cfg;
136 goto error_parse_ini_text;
138 if (!__co_dev_init_from_dcf_cfg(dev, cfg))
146 error_parse_ini_text:
160 goto error_alloc_dev;
163 if (!__co_dev_init_from_dcf_text(dev, begin, end, at)) {
183 if (!__co_dev_init(dev, 0xff)) {
185 "unable to initialize device description");
189 if (co_dev_parse_cfg(dev, cfg) == -1)
190 goto error_parse_cfg;
210 config_get(cfg,
"DeviceInfo",
"VendorName")) == -1) {
213 goto error_parse_dev;
216 val =
config_get(cfg,
"DeviceInfo",
"VendorNumber");
222 config_get(cfg,
"DeviceInfo",
"ProductName")) == -1) {
225 goto error_parse_dev;
228 val =
config_get(cfg,
"DeviceInfo",
"ProductNumber");
232 val =
config_get(cfg,
"DeviceInfo",
"RevisionNumber");
238 config_get(cfg,
"DeviceInfo",
"OrderCode")) == -1) {
240 goto error_parse_dev;
244 unsigned int baud = 0;
245 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_10");
246 if (val && *val && strtoul(val, NULL, 0))
248 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_20");
249 if (val && *val && strtoul(val, NULL, 0))
251 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_50");
252 if (val && *val && strtoul(val, NULL, 0))
254 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_125");
255 if (val && *val && strtoul(val, NULL, 0))
257 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_250");
258 if (val && *val && strtoul(val, NULL, 0))
260 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_500");
261 if (val && *val && strtoul(val, NULL, 0))
263 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_800");
264 if (val && *val && strtoul(val, NULL, 0))
266 val =
config_get(cfg,
"DeviceInfo",
"BaudRate_1000");
267 if (val && *val && strtoul(val, NULL, 0))
271 val =
config_get(cfg,
"DeviceInfo",
"LSS_Supported");
277 co_unsigned32_t
dummy = 0;
278 for (
int i = 0; i < 0x20; i++) {
281 snprintf(key,
sizeof(key),
"Dummy%04X", (co_unsigned16_t)i);
284 if (val && *val && strtoul(val, NULL, 0))
290 co_unsigned16_t n = 0;
291 n += config_get_idx(cfg,
"MandatoryObjects", 0, NULL);
292 n += config_get_idx(cfg,
"OptionalObjects", 0, NULL);
293 n += config_get_idx(cfg,
"ManufacturerObjects", 0, NULL);
296 co_unsigned16_t *idx = malloc(n *
sizeof(co_unsigned16_t));
299 "unable to create object list");
300 goto error_parse_idx;
302 co_unsigned16_t i = 0;
303 i += config_get_idx(cfg,
"MandatoryObjects", n - i, idx + i);
304 i += config_get_idx(cfg,
"OptionalObjects", n - i, idx + i);
305 config_get_idx(cfg,
"ManufacturerObjects", n - i, idx + i);
307 for (i = 0; i < n; i++) {
311 goto error_parse_obj;
316 snprintf(section,
sizeof(section),
"%X", idx[i]);
319 co_obj_t *obj = co_obj_build(dev, idx[i]);
321 goto error_parse_obj;
324 if (co_obj_parse_cfg(obj, cfg, section) == -1)
325 goto error_parse_obj;
337 val =
config_get(cfg,
"DeviceInfo",
"CompactPDO");
338 unsigned int mask = val && *val ? strtoul(val, NULL, 0) : 0;
340 co_unsigned16_t nrpdo = 0;
341 val =
config_get(cfg,
"DeviceInfo",
"NrOfRxPDO");
343 nrpdo = (co_unsigned16_t)strtoul(val, NULL, 0);
345 for (co_unsigned16_t i = 0; i < 512 && nrpdo; i++) {
350 for (co_unsigned16_t i = 0; i < 512; i++) {
358 if (co_rpdo_build(dev, i + 1, mask) == -1)
359 goto error_parse_pdo;
362 co_unsigned16_t ntpdo = 0;
363 val =
config_get(cfg,
"DeviceInfo",
"NrOfTxPDO");
365 ntpdo = (co_unsigned16_t)strtoul(val, NULL, 0);
367 for (co_unsigned16_t i = 0; i < 512 && ntpdo; i++) {
372 for (co_unsigned16_t i = 0; i < 512; i++) {
380 if (co_tpdo_build(dev, i + 1, mask) == -1)
381 goto error_parse_pdo;
385 val =
config_get(cfg,
"DeviceComissioning",
"NodeID");
388 (co_unsigned8_t)strtoul(val, NULL, 0)) == -1) {
392 goto error_parse_dcf;
395 val =
config_get(cfg,
"DeviceComissioning",
"NetNumber");
398 (co_unsigned32_t)strtoul(val, NULL, 0)) == -1) {
401 "invalid network-ID (%s) specified", val);
402 goto error_parse_dcf;
407 config_get(cfg,
"DeviceComissioning",
"NodeName"))
411 goto error_parse_dcf;
414 val =
config_get(cfg,
"DeviceComissioning",
"Baudrate");
418 val =
config_get(cfg,
"DeviceComissioning",
"LSS_SerialNumber");
420 if (val && *val && !co_dev_set_val_u32(dev, 0x1018, 0x04,
421 strtoul(val, NULL, 0))) {
424 goto error_parse_dcf;
447 struct floc at = { section, 0, 0 };
451 const char *name =
config_get(cfg, section,
"ParameterName");
454 "ParameterName not specified for object 0x%04X",
458 #ifndef LELY_NO_CO_OBJ_NAME
464 "unable to set name of object 0x%04X", idx);
472 code = (co_unsigned8_t)strtoul(val, NULL, 0);
475 "ObjectType = 0x%x for object 0x%04X",
483 co_unsigned8_t subnum = 0;
486 subnum = (co_unsigned8_t)strtoul(val, NULL, 0);
487 co_unsigned8_t subobj = 0;
488 val =
config_get(cfg, section,
"CompactSubObj");
490 subobj = (co_unsigned8_t)strtoul(val, NULL, 0);
491 if (!subnum && !subobj) {
493 "neither SubNumber nor CompactSubObj specified for object 0x%04X",
497 if (subnum && subobj) {
499 "both SubNumber and CompactSubObj specified for object 0x%04X",
505 for (
size_t subidx = 0; subnum && subidx < 0xff; subidx++) {
508 snprintf(section,
sizeof(section),
"%Xsub%X",
509 (co_unsigned16_t)idx,
510 (co_unsigned8_t)subidx);
515 cfg, section,
"ParameterName");
530 "DataType not specified");
533 co_unsigned16_t type =
534 (co_unsigned16_t)strtoul(val, NULL, 0);
538 (co_unsigned8_t)subidx, type, name);
543 if (co_sub_parse_cfg(sub, cfg, section) == -1)
549 co_sub_t *sub = co_sub_build(obj, 0,
555 #ifndef LELY_NO_CO_OBJ_DEFAULT
560 name =
config_get(cfg, section,
"ParameterName");
566 "DataType not specified");
569 co_unsigned16_t type =
570 (co_unsigned16_t)strtoul(val, NULL, 0);
573 for (
size_t subidx = 1; subidx <= subobj; subidx++) {
575 char *subname = NULL;
577 if (
asprintf(&subname,
"%s%u", name,
578 (co_unsigned8_t)subidx) < 0)
583 sub = co_sub_build(obj, (co_unsigned8_t)subidx,
591 if (co_sub_parse_cfg(sub, cfg, section) == -1)
595 #ifndef LELY_NO_CO_OBJ_NAME
597 if (co_obj_parse_names(obj, cfg) == -1)
602 if (co_obj_parse_values(obj, cfg) == -1)
609 "object 0x%04X does not provide the highest sub-index implemented",
621 type = (co_unsigned16_t)strtoul(val, NULL, 0);
628 co_sub_t *sub = co_sub_build(obj, 0, type, name);
633 if (co_sub_parse_cfg(sub, cfg, section) == -1)
640 #ifndef LELY_NO_CO_OBJ_NAME
651 snprintf(section,
sizeof(section),
"%XName", idx);
653 const char *val =
config_get(cfg, section,
"NrOfEntries");
657 co_unsigned8_t n = (co_unsigned8_t)strtoul(val, NULL, 0);
658 for (
size_t subidx = 1; n && subidx < 0xff; subidx++) {
660 snprintf(key,
sizeof(key),
"%u", (co_unsigned8_t)subidx);
666 obj, (co_unsigned8_t)subidx);
669 "unable to set name of sub-object %Xsub%X",
670 (co_unsigned16_t)idx,
671 (co_unsigned8_t)subidx);
679 #endif // LELY_NO_CO_OBJ_NAME
692 snprintf(section,
sizeof(section),
"%XValue", (co_unsigned16_t)idx);
693 struct floc at = { section, 0, 0 };
695 const char *val =
config_get(cfg, section,
"NrOfEntries");
699 co_unsigned8_t n = (co_unsigned8_t)strtoul(val, NULL, 0);
700 for (
size_t subidx = 1; n && subidx < 0xff; subidx++) {
702 snprintf(key,
sizeof(key),
"%u", (co_unsigned8_t)subidx);
708 obj, (co_unsigned8_t)subidx);
711 size_t chars = co_val_lex_id(val, NULL, &at);
716 if (!co_val_lex_dcf(type, sub->
val, val, NULL, &at)) {
718 "unable to set value of sub-object %Xsub%X",
719 (co_unsigned16_t)idx,
720 (co_unsigned8_t)subidx);
724 co_val_set_id(type, sub->
val,
id);
732 co_obj_build(
co_dev_t *dev, co_unsigned16_t idx)
745 "unable to insert object 0x%04X into the object dictionary",
763 struct floc at = { section, 0, 0 };
768 #ifdef LELY_NO_CO_OBJ_DEFAULT
773 #ifndef LELY_NO_CO_OBJ_LIMITS
776 size_t chars = co_val_lex_id(val, NULL, &at);
781 if (!co_val_lex_dcf(type, &sub->
min, val, NULL, &at)) {
783 "unable to parse LowLimit");
787 co_val_set_id(type, &sub->
min,
id);
792 size_t chars = co_val_lex_id(val, NULL, &at);
797 if (!co_val_lex_dcf(type, &sub->
max, val, NULL, &at)) {
799 "unable to parse HighLimit");
803 co_val_set_id(type, &sub->
max,
id);
805 #endif // LELY_NO_CO_OBJ_LIMITS
810 if (!strcasecmp(val,
"ro")) {
812 }
else if (!strcasecmp(val,
"wo")) {
814 }
else if (!strcasecmp(val,
"rw")) {
816 }
else if (!strcasecmp(val,
"rwr")) {
818 }
else if (!strcasecmp(val,
"rww")) {
820 }
else if (!strcasecmp(val,
"const")) {
832 val =
config_get(cfg, section,
"DefaultValue");
834 size_t chars = co_val_lex_id(val, NULL, &at);
839 #ifdef LELY_NO_CO_OBJ_DEFAULT
840 if (!co_val_lex_dcf(type, &def, val, NULL, &at)) {
842 if (!co_val_lex_dcf(type, &sub->
def, val, NULL, &at)) {
845 "unable to parse DefaultValue");
849 #ifdef LELY_NO_CO_OBJ_DEFAULT
850 co_val_set_id(type, &def,
id);
852 co_val_set_id(type, &sub->
def,
id);
862 sub->
flags |= strtoul(val, NULL, 0);
864 val =
config_get(cfg, section,
"ParameterValue");
866 size_t chars = co_val_lex_id(val, NULL, &at);
871 if (!co_val_lex_dcf(type, sub->
val, val, NULL, &at)) {
873 "unable to parse ParameterValue");
877 co_val_set_id(type, sub->
val,
id);
878 #ifndef LELY_NO_CO_OBJ_FILE
880 && (val =
config_get(cfg, section,
"UploadFile"))
884 "AccessType must be 'ro' or 'const' when using UploadFile");
895 "unable to parse UploadFile");
899 && (val =
config_get(cfg, section,
"DownloadFile"))
903 "AccessType must be 'wo' when using DownloadFile");
913 "unable to parse DownloadFile");
920 #ifdef LELY_NO_CO_OBJ_DEFAULT
927 #ifndef LELY_NO_CO_OBJ_LIMITS
931 #ifndef LELY_NO_CO_OBJ_DEFAULT
937 "LowLimit exceeds HighLimit");
938 #ifndef LELY_NO_CO_OBJ_DEFAULT
946 "ParameterValue underflow");
949 "ParameterValue overflow");
956 #ifdef LELY_NO_CO_OBJ_DEFAULT
963 co_sub_build(
co_obj_t *obj, co_unsigned8_t subidx, co_unsigned16_t type,
973 "unable to create sub-object %Xsub%X", idx,
980 "unable to insert sub-object %Xsub%X into the object dictionary",
985 #ifndef LELY_NO_CO_OBJ_NAME
988 "unable to set name of sub-object %Xsub%X", idx,
1004 co_rpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask)
1007 assert(num && num <= 512);
1011 co_unsigned8_t n = 0;
1012 for (
int i = 0; i < 6; i++) {
1013 if (mask & (1 << i))
1019 co_obj_t *obj = co_obj_build(dev, 0x1400 + num - 1);
1022 #ifndef LELY_NO_CO_OBJ_NAME
1033 "Highest sub-index supported");
1037 #ifndef LELY_NO_CO_OBJ_DEFAULT
1044 "COB-ID used by RPDO");
1049 cobid = num * 0x100 + 0x100 + 0xff;
1054 #ifndef LELY_NO_CO_OBJ_DEFAULT
1062 "Transmission type");
1078 "Compatibility entry");
1094 "SYNC start value");
1103 co_obj_t *obj = co_obj_build(dev, 0x1600 + num - 1);
1106 #ifndef LELY_NO_CO_OBJ_NAME
1116 "Highest sub-index supported");
1121 for (co_unsigned8_t i = 1; i <= 0x40; i++) {
1123 snprintf(name,
sizeof(name),
"Application object %u",
1138 co_tpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask)
1141 assert(num && num <= 512);
1145 co_unsigned8_t n = 0;
1146 for (
int i = 0; i < 6; i++) {
1147 if (mask & (1 << i))
1153 co_obj_t *obj = co_obj_build(dev, 0x1800 + num - 1);
1156 #ifndef LELY_NO_CO_OBJ_NAME
1167 "Highest sub-index supported");
1171 #ifndef LELY_NO_CO_OBJ_DEFAULT
1178 "COB-ID used by TPDO");
1183 cobid = num * 0x100 + 0x80 + 0xff;
1188 #ifndef LELY_NO_CO_OBJ_DEFAULT
1196 "Transmission type");
1228 "SYNC start value");
1237 co_obj_t *obj = co_obj_build(dev, 0x1a00 + num - 1);
1240 #ifndef LELY_NO_CO_OBJ_NAME
1250 "Highest sub-index supported");
1255 for (co_unsigned8_t i = 1; i <= 0x40; i++) {
1257 snprintf(name,
sizeof(name),
"Application object %u",
1272 co_val_lex_dcf(co_unsigned16_t type,
void *val,
const char *begin,
1273 const char *end,
struct floc *at)
1276 assert(!end || end >= begin);
1281 size_t chars =
lex_hex(begin, end, NULL, NULL, &n);
1285 "unable to create value of type OCTET_STRING");
1288 uint_least8_t *os = *(
void **)val;
1290 lex_hex(begin, end, NULL, os, &n);
1292 return floc_lex(at, begin, begin + chars);
1298 for (cp = begin; (!end || cp < end) && *cp;) {
1300 cp +=
lex_utf8(cp, end, NULL, &c32);
1301 assert(c32 < 0xd800 || c32 > 0xdfff);
1302 n += c32 <= 0xffff ? 1 : 2;
1307 "unable to create value of type UNICODE_STRING");
1311 char16_t *us = *(
void **)val;
1313 for (cp = begin; (!end || cp < end) && *cp;) {
1315 cp +=
lex_utf8(cp, end, NULL, &c32);
1316 assert(c32 < 0xd800 || c32 > 0xdfff);
1318 if (c32 <= 0xffff) {
1322 *us++ = 0xd800 + ((c32 >> 10) & 0x3ff);
1323 *us++ = 0xdc00 + (c32 & 0x3ff);
1331 size_t chars =
lex_hex(begin, end, NULL, NULL, &n);
1335 "unable to create value of type DOMAIN");
1338 void *dom = *(
void **)val;
1340 lex_hex(begin, end, NULL, dom, &n);
1342 return floc_lex(at, begin, begin + chars);
1344 default:
return co_val_lex(type, val, begin, end, at);
1349 co_val_lex_id(
const char *begin,
const char *end,
struct floc *at)
1353 const char *cp = begin;
1355 if ((end && cp - end < 7) || strncasecmp(begin,
"$NODEID", 7))
1369 co_val_set_id(co_unsigned16_t type,
void *val, co_unsigned8_t
id)
1375 #define LELY_CO_DEFINE_TYPE(a, b, c, d) \
1376 case CO_DEFTYPE_##a: \
1379 #include <lely/co/def/basic.def>
1380 #undef LELY_CO_DEFINE_TYPE
1384 static co_unsigned16_t
1385 config_get_idx(
const config_t *cfg,
const char *section, co_unsigned16_t maxidx,
1386 co_unsigned16_t *idx)
1393 const char *val =
config_get(cfg, section,
"SupportedObjects");
1397 co_unsigned16_t n = (co_unsigned16_t)strtoul(val, NULL, 0);
1398 for (
size_t i = 0; i < (size_t)
MIN(n, maxidx); i++) {
1400 snprintf(key,
sizeof(key),
"%u", (co_unsigned16_t)(i + 1));
1404 idx[i] = val && *val
1405 ? (co_unsigned16_t)strtoul(val, NULL, 0) : 0;
1412 #endif // !LELY_NO_CO_DCF