42 static struct __co_dev *__co_dev_init_from_dcf_cfg(
47 static int co_obj_parse_cfg(
49 #if !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));
304 goto error_parse_idx;
306 co_unsigned16_t i = 0;
307 i += config_get_idx(cfg,
"MandatoryObjects", n - i, idx + i);
308 i += config_get_idx(cfg,
"OptionalObjects", n - i, idx + i);
309 config_get_idx(cfg,
"ManufacturerObjects", n - i, idx + i);
311 for (i = 0; i < n; i++) {
315 goto error_parse_obj;
320 snprintf(section,
sizeof(section),
"%X", idx[i]);
323 co_obj_t *obj = co_obj_build(dev, idx[i]);
325 goto error_parse_obj;
328 if (co_obj_parse_cfg(obj, cfg, section) == -1)
329 goto error_parse_obj;
341 val =
config_get(cfg,
"DeviceInfo",
"CompactPDO");
342 unsigned int mask = val && *val ? strtoul(val, NULL, 0) : 0;
344 co_unsigned16_t nrpdo = 0;
345 val =
config_get(cfg,
"DeviceInfo",
"NrOfRxPDO");
347 nrpdo = (co_unsigned16_t)strtoul(val, NULL, 0);
349 for (co_unsigned16_t i = 0; i <
CO_NUM_PDOS && nrpdo; i++) {
354 for (co_unsigned16_t i = 0; i <
CO_NUM_PDOS; i++) {
362 if (co_rpdo_build(dev, i + 1, mask) == -1)
363 goto error_parse_pdo;
366 co_unsigned16_t ntpdo = 0;
367 val =
config_get(cfg,
"DeviceInfo",
"NrOfTxPDO");
369 ntpdo = (co_unsigned16_t)strtoul(val, NULL, 0);
371 for (co_unsigned16_t i = 0; i <
CO_NUM_PDOS && ntpdo; i++) {
376 for (co_unsigned16_t i = 0; i <
CO_NUM_PDOS; i++) {
384 if (co_tpdo_build(dev, i + 1, mask) == -1)
385 goto error_parse_pdo;
389 val =
config_get(cfg,
"DeviceComissioning",
"NodeID");
392 (co_unsigned8_t)strtoul(val, NULL, 0)) == -1) {
396 goto error_parse_dcf;
399 val =
config_get(cfg,
"DeviceComissioning",
"NetNumber");
402 (co_unsigned32_t)strtoul(val, NULL, 0)) == -1) {
405 "invalid network-ID (%s) specified", val);
406 goto error_parse_dcf;
411 config_get(cfg,
"DeviceComissioning",
"NodeName"))
415 goto error_parse_dcf;
418 val =
config_get(cfg,
"DeviceComissioning",
"Baudrate");
422 val =
config_get(cfg,
"DeviceComissioning",
"LSS_SerialNumber");
424 if (val && *val && !co_dev_set_val_u32(dev, 0x1018, 0x04,
425 strtoul(val, NULL, 0))) {
428 goto error_parse_dcf;
451 struct floc at = { section, 0, 0 };
455 const char *name =
config_get(cfg, section,
"ParameterName");
458 "ParameterName not specified for object 0x%04X",
462 #if !LELY_NO_CO_OBJ_NAME
468 "unable to set name of object 0x%04X", idx);
476 code = (co_unsigned8_t)strtoul(val, NULL, 0);
479 "ObjectType = 0x%x for object 0x%04X",
487 co_unsigned8_t subnum = 0;
490 subnum = (co_unsigned8_t)strtoul(val, NULL, 0);
491 co_unsigned8_t subobj = 0;
492 val =
config_get(cfg, section,
"CompactSubObj");
494 subobj = (co_unsigned8_t)strtoul(val, NULL, 0);
495 if (!subnum && !subobj) {
497 "neither SubNumber nor CompactSubObj specified for object 0x%04X",
501 if (subnum && subobj) {
503 "both SubNumber and CompactSubObj specified for object 0x%04X",
509 for (
size_t subidx = 0; subnum && subidx < 0xff; subidx++) {
512 snprintf(section,
sizeof(section),
"%Xsub%X",
513 (co_unsigned16_t)idx,
514 (co_unsigned8_t)subidx);
519 cfg, section,
"ParameterName");
534 "DataType not specified");
537 co_unsigned16_t type =
538 (co_unsigned16_t)strtoul(val, NULL, 0);
542 (co_unsigned8_t)subidx, type, name);
547 if (co_sub_parse_cfg(sub, cfg, section) == -1)
553 co_sub_t *sub = co_sub_build(obj, 0,
559 #if !LELY_NO_CO_OBJ_DEFAULT
564 name =
config_get(cfg, section,
"ParameterName");
570 "DataType not specified");
573 co_unsigned16_t type =
574 (co_unsigned16_t)strtoul(val, NULL, 0);
577 for (
size_t subidx = 1; subidx <= subobj; subidx++) {
579 char *subname = NULL;
581 if (
asprintf(&subname,
"%s%u", name,
582 (co_unsigned8_t)subidx) < 0)
587 sub = co_sub_build(obj, (co_unsigned8_t)subidx,
595 if (co_sub_parse_cfg(sub, cfg, section) == -1)
599 #if !LELY_NO_CO_OBJ_NAME
601 if (co_obj_parse_names(obj, cfg) == -1)
606 if (co_obj_parse_values(obj, cfg) == -1)
613 "object 0x%04X does not provide the highest sub-index implemented",
623 type = (co_unsigned16_t)strtoul(val, NULL, 0);
630 co_sub_t *sub = co_sub_build(obj, 0, type, name);
635 if (co_sub_parse_cfg(sub, cfg, section) == -1)
642 #if !LELY_NO_CO_OBJ_NAME
653 snprintf(section,
sizeof(section),
"%XName", idx);
655 const char *val =
config_get(cfg, section,
"NrOfEntries");
659 co_unsigned8_t n = (co_unsigned8_t)strtoul(val, NULL, 0);
660 for (
size_t subidx = 1; n && subidx < 0xff; subidx++) {
662 snprintf(key,
sizeof(key),
"%u", (co_unsigned8_t)subidx);
668 obj, (co_unsigned8_t)subidx);
671 "unable to set name of sub-object %Xsub%X",
672 (co_unsigned16_t)idx,
673 (co_unsigned8_t)subidx);
681 #endif // LELY_NO_CO_OBJ_NAME
694 snprintf(section,
sizeof(section),
"%XValue", (co_unsigned16_t)idx);
695 struct floc at = { section, 0, 0 };
697 const char *val =
config_get(cfg, section,
"NrOfEntries");
701 co_unsigned8_t n = (co_unsigned8_t)strtoul(val, NULL, 0);
702 for (
size_t subidx = 1; n && subidx < 0xff; subidx++) {
704 snprintf(key,
sizeof(key),
"%u", (co_unsigned8_t)subidx);
710 obj, (co_unsigned8_t)subidx);
713 size_t chars = co_val_lex_id(val, NULL, &at);
718 if (!co_val_lex_dcf(type, sub->
val, val, NULL, &at)) {
720 "unable to set value of sub-object %Xsub%X",
721 (co_unsigned16_t)idx,
722 (co_unsigned8_t)subidx);
726 co_val_set_id(type, sub->
val,
id);
734 co_obj_build(
co_dev_t *dev, co_unsigned16_t idx)
747 "unable to insert object 0x%04X into the object dictionary",
765 struct floc at = { section, 0, 0 };
770 #if LELY_NO_CO_OBJ_DEFAULT
775 #if !LELY_NO_CO_OBJ_LIMITS
778 size_t chars = co_val_lex_id(val, NULL, &at);
783 if (!co_val_lex_dcf(type, &sub->
min, val, NULL, &at)) {
785 "unable to parse LowLimit");
789 co_val_set_id(type, &sub->
min,
id);
794 size_t chars = co_val_lex_id(val, NULL, &at);
799 if (!co_val_lex_dcf(type, &sub->
max, val, NULL, &at)) {
801 "unable to parse HighLimit");
805 co_val_set_id(type, &sub->
max,
id);
807 #endif // LELY_NO_CO_OBJ_LIMITS
812 if (!strcasecmp(val,
"ro")) {
814 }
else if (!strcasecmp(val,
"wo")) {
816 }
else if (!strcasecmp(val,
"rw")) {
818 }
else if (!strcasecmp(val,
"rwr")) {
820 }
else if (!strcasecmp(val,
"rww")) {
822 }
else if (!strcasecmp(val,
"const")) {
834 val =
config_get(cfg, section,
"DefaultValue");
836 size_t chars = co_val_lex_id(val, NULL, &at);
841 #if LELY_NO_CO_OBJ_DEFAULT
842 if (!co_val_lex_dcf(type, &def, val, NULL, &at)) {
844 if (!co_val_lex_dcf(type, &sub->
def, val, NULL, &at)) {
847 "unable to parse DefaultValue");
851 #if LELY_NO_CO_OBJ_DEFAULT
852 co_val_set_id(type, &def,
id);
854 co_val_set_id(type, &sub->
def,
id);
864 sub->
flags |= strtoul(val, NULL, 0);
866 val =
config_get(cfg, section,
"ParameterValue");
869 size_t chars = co_val_lex_id(val, NULL, &at);
874 if (!co_val_lex_dcf(type, sub->
val, val, NULL, &at)) {
876 "unable to parse ParameterValue");
880 co_val_set_id(type, sub->
val,
id);
881 #if !LELY_NO_CO_OBJ_FILE
883 && (val =
config_get(cfg, section,
"UploadFile"))
887 "AccessType must be 'ro' or 'const' when using UploadFile");
898 "unable to parse UploadFile");
902 && (val =
config_get(cfg, section,
"DownloadFile"))
906 "AccessType must be 'wo' when using DownloadFile");
916 "unable to parse DownloadFile");
923 #if LELY_NO_CO_OBJ_DEFAULT
930 #if !LELY_NO_CO_OBJ_LIMITS
934 #if !LELY_NO_CO_OBJ_DEFAULT
940 "LowLimit exceeds HighLimit");
941 #if !LELY_NO_CO_OBJ_DEFAULT
949 "ParameterValue underflow");
952 "ParameterValue overflow");
959 #if LELY_NO_CO_OBJ_DEFAULT
966 co_sub_build(
co_obj_t *obj, co_unsigned8_t subidx, co_unsigned16_t type,
976 "unable to create sub-object %Xsub%X", idx,
983 "unable to insert sub-object %Xsub%X into the object dictionary",
988 #if !LELY_NO_CO_OBJ_NAME
991 "unable to set name of sub-object %Xsub%X", idx,
1007 co_rpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask)
1014 co_unsigned8_t n = 0;
1015 for (
int i = 0; i < 6; i++) {
1016 if (mask & (1 << i))
1022 co_obj_t *obj = co_obj_build(dev, 0x1400 + num - 1);
1025 #if !LELY_NO_CO_OBJ_NAME
1036 "Highest sub-index supported");
1040 #if !LELY_NO_CO_OBJ_DEFAULT
1047 "COB-ID used by RPDO");
1052 cobid = num * 0x100 + 0x100 + 0xff;
1057 #if !LELY_NO_CO_OBJ_DEFAULT
1065 "Transmission type");
1081 "Compatibility entry");
1097 "SYNC start value");
1106 co_obj_t *obj = co_obj_build(dev, 0x1600 + num - 1);
1109 #if !LELY_NO_CO_OBJ_NAME
1119 "Highest sub-index supported");
1124 for (co_unsigned8_t i = 1; i <= 0x40; i++) {
1126 snprintf(name,
sizeof(name),
"Application object %u",
1141 co_tpdo_build(
co_dev_t *dev, co_unsigned16_t num,
int mask)
1148 co_unsigned8_t n = 0;
1149 for (
int i = 0; i < 6; i++) {
1150 if (mask & (1 << i))
1156 co_obj_t *obj = co_obj_build(dev, 0x1800 + num - 1);
1159 #if !LELY_NO_CO_OBJ_NAME
1170 "Highest sub-index supported");
1174 #if !LELY_NO_CO_OBJ_DEFAULT
1181 "COB-ID used by TPDO");
1186 cobid = num * 0x100 + 0x80 + 0xff;
1191 #if !LELY_NO_CO_OBJ_DEFAULT
1199 "Transmission type");
1231 "SYNC start value");
1240 co_obj_t *obj = co_obj_build(dev, 0x1a00 + num - 1);
1243 #if !LELY_NO_CO_OBJ_NAME
1253 "Highest sub-index supported");
1258 for (co_unsigned8_t i = 1; i <= 0x40; i++) {
1260 snprintf(name,
sizeof(name),
"Application object %u",
1275 co_val_lex_dcf(co_unsigned16_t type,
void *val,
const char *begin,
1276 const char *end,
struct floc *at)
1279 assert(!end || end >= begin);
1284 size_t chars =
lex_hex(begin, end, NULL, NULL, &n);
1288 "unable to create value of type OCTET_STRING");
1291 uint_least8_t *os = *(
void **)val;
1293 lex_hex(begin, end, NULL, os, &n);
1295 return floc_lex(at, begin, begin + chars);
1301 for (cp = begin; (!end || cp < end) && *cp;) {
1303 cp +=
lex_utf8(cp, end, NULL, &c32);
1304 assert(c32 < 0xd800 || c32 > 0xdfff);
1305 n += c32 <= 0xffff ? 1 : 2;
1310 "unable to create value of type UNICODE_STRING");
1314 char16_t *us = *(
void **)val;
1316 for (cp = begin; (!end || cp < end) && *cp;) {
1318 cp +=
lex_utf8(cp, end, NULL, &c32);
1319 assert(c32 < 0xd800 || c32 > 0xdfff);
1321 if (c32 <= 0xffff) {
1325 *us++ = 0xd800 + ((c32 >> 10) & 0x3ff);
1326 *us++ = 0xdc00 + (c32 & 0x3ff);
1334 size_t chars =
lex_hex(begin, end, NULL, NULL, &n);
1338 "unable to create value of type DOMAIN");
1341 void *dom = *(
void **)val;
1343 lex_hex(begin, end, NULL, dom, &n);
1345 return floc_lex(at, begin, begin + chars);
1347 default:
return co_val_lex(type, val, begin, end, at);
1352 co_val_lex_id(
const char *begin,
const char *end,
struct floc *at)
1356 const char *cp = begin;
1358 if ((end && cp - end < 7) || strncasecmp(begin,
"$NODEID", 7))
1372 co_val_set_id(co_unsigned16_t type,
void *val, co_unsigned8_t
id)
1378 #define LELY_CO_DEFINE_TYPE(a, b, c, d) \
1379 case CO_DEFTYPE_##a: \
1382 #include <lely/co/def/basic.def>
1383 #undef LELY_CO_DEFINE_TYPE
1387 static co_unsigned16_t
1388 config_get_idx(
const config_t *cfg,
const char *section, co_unsigned16_t maxidx,
1389 co_unsigned16_t *idx)
1396 const char *val =
config_get(cfg, section,
"SupportedObjects");
1400 co_unsigned16_t n = (co_unsigned16_t)strtoul(val, NULL, 0);
1401 for (
size_t i = 0; i < (size_t)
MIN(n, maxidx); i++) {
1403 snprintf(key,
sizeof(key),
"%u", (co_unsigned16_t)(i + 1));
1407 idx[i] = val && *val
1408 ? (co_unsigned16_t)strtoul(val, NULL, 0) : 0;
1415 #endif // !LELY_NO_CO_DCF