diff --git a/dronecan_dsdlc_helpers.py b/dronecan_dsdlc_helpers.py index 010bd68..775c473 100644 --- a/dronecan_dsdlc_helpers.py +++ b/dronecan_dsdlc_helpers.py @@ -270,3 +270,145 @@ def mkdir_p(path): pass else: raise + +def build_coding_table(msg_underscored_name, msg_union, msg_max_bitlen, msg_fields): + if len(msg_fields) == 0: # cheaper to encode an empty message without a table + return None + if int((msg_max_bitlen+7)/8) > 65535: # too big to encode + return None + + table = _build_coding_table_core(msg_underscored_name, msg_union, msg_fields, True) + if table is None: return None + if len(table) > 65536: # too big to encode + return None + + # turn table description into a list of strings + table_str = [] + for kind, relative, *params in table: + if kind is None: + # number of list items must exactly equal number of entries! + table_str.append("// auxiliary entry") + else: + params = ", ".join(str(v) for v in params) + table_str.append(f"{kind}({params}),") + + return table_str + +def _build_coding_table_core(obj_underscored_name, obj_union, obj_fields, tao): + table = [] + + if obj_union: # unions are a special case + if len(obj_fields) == 0 or len(obj_fields) > 255: # out of range to encode + return None + + # entries to describe the union + table.append(("CANARD_TABLE_CODING_ENTRIES_UNION", False, + f"offsetof(struct {obj_underscored_name}, {obj_fields[0].name})", len(obj_fields), + union_msg_tag_bitlen_from_num_fields(len(obj_fields)), + f"offsetof(struct {obj_underscored_name}, union_tag)" + )) + table.append((None, None)) # dummy auxiliary entry for accurate entry count + + field_entries = [] + for field in obj_fields: + t = field.type + + if isinstance(t, dronecan.dsdl.parser.CompoundType): + sub = _build_coding_table_core(field.name, t.union, t.fields, tao) + else: + sub = _build_coding_table_core(field.name, False, [t], tao) + if sub is None: return None + if len(sub) > 255: # too many entries to encode + return None + + # entry to describe this field + table.append(("CANARD_TABLE_CODING_ENTRY_UNION_FIELD", True, len(sub))) + + for s_kind, s_relative, *s_params in sub: + if s_kind is None: + field_entries.append((None, None)) + else: + # all entries must have a relative offset + field_entries.append((s_kind, True, *s_params)) + + table.extend(field_entries) + return table + + for field_idx, field in enumerate(obj_fields): + tao_eligible = tao and field_idx == len(obj_fields)-1 + if isinstance(field, dronecan.dsdl.parser.Type): + t = field + offset = 0 + else: + t = field.type + offset = f"offsetof(struct {obj_underscored_name}, {field.name})" + field_name = t.full_name.replace('.', '_').split("[")[0] + if isinstance(t, dronecan.dsdl.parser.PrimitiveType): + if t.kind == t.KIND_BOOLEAN or t.kind == t.KIND_UNSIGNED_INT: + ts = "CANARD_TABLE_CODING_UNSIGNED" + elif t.kind == t.KIND_SIGNED_INT: + ts = "CANARD_TABLE_CODING_SIGNED" + elif t.kind == t.KIND_FLOAT: + ts = "CANARD_TABLE_CODING_FLOAT" + table.append(("CANARD_TABLE_CODING_ENTRY_PRIMITIVE", False, offset, ts, t.bitlen)) + elif isinstance(t, dronecan.dsdl.parser.VoidType): + table.append(("CANARD_TABLE_CODING_ENTRY_VOID", True, t.bitlen)) + elif isinstance(t, dronecan.dsdl.parser.CompoundType): + sub = _build_coding_table_core(field_name, t.union, t.fields, tao_eligible) # build the compound table + if sub is None: return None + + # prepend offset to each entry that's not relative + for s_kind, s_relative, *s_params in sub: + if s_kind is None: + table.append((None, None)) + else: + if not s_relative: # not relative, needs offset adjusted + if s_kind.startswith("CANARD_TABLE_CODING_ENTRIES_ARRAY_DYNAMIC") \ + or s_kind == "CANARD_TABLE_CODING_ENTRIES_UNION": # has a second offset + s_params = (*s_params[:-1], f"{offset}+{s_params[-1]}") + table.append((s_kind, s_relative, f"{offset}+{s_params[0]}", *s_params[1:])) + else: + table.append((s_kind, s_relative, *s_params)) + elif isinstance(t, dronecan.dsdl.parser.ArrayType): + if t.max_size > 65535: # too big to encode + return None + + use_tao = t.mode == t.MODE_DYNAMIC and tao_eligible and t.value_type.get_min_bitlen() >= 8 + + if isinstance(t.value_type, dronecan.dsdl.parser.CompoundType): + sub = _build_coding_table_core(field_name, t.value_type.union, t.value_type.fields, tao_eligible and not use_tao) + else: + sub = _build_coding_table_core(field_name, False, [t.value_type], tao_eligible and not use_tao) + if sub is None: return None + if len(sub) == 0: # array of empty objects??? + assert False + if len(sub) > 256: # too many entries to encode + return None + + # entries for the array itself + if t.mode == t.MODE_STATIC: + table.append(("CANARD_TABLE_CODING_ENTRIES_ARRAY_STATIC", False, + offset, len(sub), f"sizeof({dronecan_type_to_ctype(t.value_type)})", t.max_size + )) + table.append((None, None)) # dummy auxiliary entry for accurate entry count + elif t.mode == t.MODE_DYNAMIC: + # same cdef as field_cdef + cdef = 'struct { uint%u_t len; %s data[%u]; }' % (c_int_type_bitlen(array_len_field_bitlen(t)), dronecan_type_to_ctype(t.value_type), t.max_size) + table.append((f"CANARD_TABLE_CODING_ENTRIES_ARRAY_DYNAMIC", False, + f"{offset}+offsetof({cdef}, data)", 1 if use_tao else 0, len(sub), f"sizeof({dronecan_type_to_ctype(t.value_type)})", t.max_size, + t.max_size.bit_length(), f"{offset}+offsetof({cdef}, len)" + )) + table.extend(((None, None),)*2) # dummy auxiliary entries for accurate entry count + else: + assert False # unknown type + + for s_kind, s_relative, *s_params in sub: + if s_kind is None: + table.append((None, None)) + else: + # all entries must have a relative offset + table.append((s_kind, True, *s_params)) + else: + assert False # unknown type + + return table diff --git a/templates/msg.c.em b/templates/msg.c.em index 1b7d9cd..341ac6b 100644 --- a/templates/msg.c.em +++ b/templates/msg.c.em @@ -11,14 +11,27 @@ #include #endif -uint32_t @(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer +@{coding_table = build_coding_table(msg_underscored_name, msg_union, msg_max_bitlen, msg_fields)} +@[if coding_table is not None] +#if CANARD_ENABLE_TABLE_ENCODING || CANARD_ENABLE_TABLE_DECODING +const CanardCodingTable _@(msg_underscored_name)_coding_table = { + .max_size = @(msg_define_name.upper())_MAX_SIZE, + .entries_max = @(len(coding_table))-1, + .entries = { +@("\n".join(" "+e for e in coding_table)) + }, +}; +#endif +@[end if] + +uint32_t _@(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer #if CANARD_ENABLE_TAO_OPTION , bool tao #endif ) { uint32_t bit_ofs = 0; memset(buffer, 0, @(msg_define_name.upper())_MAX_SIZE); - _@(msg_underscored_name)_encode(buffer, &bit_ofs, msg, + __@(msg_underscored_name)_encode(buffer, &bit_ofs, msg, #if CANARD_ENABLE_TAO_OPTION tao #else @@ -31,14 +44,14 @@ uint32_t @(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer /* return true if the decode is invalid */ -bool @(msg_underscored_name)_decode(const CanardRxTransfer* transfer, @(msg_c_type)* msg) { +bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, @(msg_c_type)* msg) { #if CANARD_ENABLE_TAO_OPTION if (transfer->tao && (transfer->payload_len > @(msg_define_name.upper())_MAX_SIZE)) { return true; /* invalid payload length */ } #endif uint32_t bit_ofs = 0; - if (_@(msg_underscored_name)_decode(transfer, &bit_ofs, msg, + if (__@(msg_underscored_name)_decode(transfer, &bit_ofs, msg, #if CANARD_ENABLE_TAO_OPTION transfer->tao #else diff --git a/templates/msg.h.em b/templates/msg.h.em index 8ec0b2d..62d7c2d 100644 --- a/templates/msg.h.em +++ b/templates/msg.h.em @@ -69,18 +69,76 @@ extern "C" { #endif -uint32_t @(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer +uint32_t _@(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer #if CANARD_ENABLE_TAO_OPTION , bool tao #endif ); -bool @(msg_underscored_name)_decode(const CanardRxTransfer* transfer, @(msg_c_type)* msg); +bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, @(msg_c_type)* msg); + +@{coding_table = build_coding_table(msg_underscored_name, msg_union, msg_max_bitlen, msg_fields)} +@[if coding_table is not None] +#if CANARD_ENABLE_TABLE_ENCODING || CANARD_ENABLE_TABLE_DECODING +extern const CanardCodingTable _@(msg_underscored_name)_coding_table; +#endif +@[end if] + +static inline uint32_t @(msg_underscored_name)_encode(@(msg_c_type)* msg, uint8_t* buffer +#if CANARD_ENABLE_TAO_OPTION + , bool tao +#endif +) { +@[if msg_max_bitlen == 0] + (void)msg; + (void)buffer; +#if CANARD_ENABLE_TAO_OPTION + (void)tao; +#endif + + return 0; // 0-length message encodes to 0 bytes +@[else] +@[if coding_table is not None] +#if CANARD_ENABLE_TABLE_ENCODING + if (sizeof(@(msg_c_type)) <= 65535) { // ensure offsets fit in table + return canardTableEncodeMessage(&_@(msg_underscored_name)_coding_table, buffer, msg +#if CANARD_ENABLE_TAO_OPTION + , tao +#endif + ); + } +#endif +@[end if] + return _@(msg_underscored_name)_encode(msg, buffer +#if CANARD_ENABLE_TAO_OPTION + , tao +#endif + ); +@[end if] +} + +static inline bool @(msg_underscored_name)_decode(const CanardRxTransfer* transfer, @(msg_c_type)* msg) { +@[if msg_max_bitlen == 0] + (void)msg; + + // all transports accurately convey a payload length of 0 bytes so any payload is an error + return transfer->payload_len != 0; +@[else] +@[if coding_table is not None] +#if CANARD_ENABLE_TABLE_DECODING + if (sizeof(@(msg_c_type)) <= 65535) { // ensure offsets fit in table + return canardTableDecodeMessage(&_@(msg_underscored_name)_coding_table, transfer, msg); + } +#endif +@[end if] + return _@(msg_underscored_name)_decode(transfer, msg); +@[end if] +} #if defined(CANARD_DSDLC_INTERNAL) @{indent = 0}@{ind = ' '*indent}@ -static inline void _@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao); -static inline bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao); -void _@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao) { +static inline void __@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao); +static inline bool __@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao); +void __@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao) { @{indent += 1}@{ind = ' '*indent}@ @(ind)(void)buffer; @(ind)(void)bit_ofs; @@ -101,7 +159,7 @@ void _@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c @{indent += 1}@{ind = ' '*indent}@ @[ end if]@ @[ if field.type.category == field.type.CATEGORY_COMPOUND]@ -@(ind)_@(underscored_name(field.type))_encode(buffer, bit_ofs, &msg->@(field.name), @('tao' if (field == msg_fields[-1] or msg_union) else 'false')); +@(ind)__@(underscored_name(field.type))_encode(buffer, bit_ofs, &msg->@(field.name), @('tao' if (field == msg_fields[-1] or msg_union) else 'false')); @[ elif field.type.category == field.type.CATEGORY_PRIMITIVE]@ @[ if field.type.kind == field.type.KIND_FLOAT and field.type.bitlen == 16]@ @(ind){ @@ -144,7 +202,7 @@ void _@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c @[ end if]@ @(ind)*bit_ofs += @(field.type.value_type.bitlen); @[ elif field.type.value_type.category == field.type.value_type.CATEGORY_COMPOUND]@ -@(ind)_@(underscored_name(field.type.value_type))_encode(buffer, bit_ofs, &msg->@(field_get_data(field))[i], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@); +@(ind)__@(underscored_name(field.type.value_type))_encode(buffer, bit_ofs, &msg->@(field_get_data(field))[i], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@); @[ end if]@ @{indent -= 1}@{ind = ' '*indent}@ @(ind)} @@ -167,7 +225,7 @@ void _@(msg_underscored_name)_encode(uint8_t* buffer, uint32_t* bit_ofs, @(msg_c /* decode @(msg_underscored_name), return true on failure, false on success */ -bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao) { +bool __@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* bit_ofs, @(msg_c_type)* msg, bool tao) { @{indent += 1}@{ind = ' '*indent}@ @(ind)(void)transfer; @(ind)(void)bit_ofs; @@ -194,7 +252,7 @@ bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* @{indent += 1}@{ind = ' '*indent}@ @[ end if]@ @[ if field.type.category == field.type.CATEGORY_COMPOUND]@ -@(ind)if (_@(underscored_name(field.type))_decode(transfer, bit_ofs, &msg->@(field.name), @('tao' if (field == msg_fields[-1] or msg_union) else 'false'))) {return true;} +@(ind)if (__@(underscored_name(field.type))_decode(transfer, bit_ofs, &msg->@(field.name), @('tao' if (field == msg_fields[-1] or msg_union) else 'false'))) {return true;} @[ elif field.type.category == field.type.CATEGORY_PRIMITIVE]@ @[ if field.type.kind == field.type.KIND_FLOAT and field.type.bitlen == 16]@ @(ind){ @@ -235,7 +293,7 @@ bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* @(ind)uint32_t max_bits = (transfer->payload_len*8)-7; // TAO elements must be >= 8 bits @(ind)while (max_bits > *bit_ofs) { @{indent += 1}@{ind = ' '*indent}@ -@(ind)if (!max_len-- || _@(underscored_name(field.type.value_type))_decode(transfer, bit_ofs, &msg->@(field_get_data(field))[msg->@(field.name).len], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@)) {return true;} +@(ind)if (!max_len-- || __@(underscored_name(field.type.value_type))_decode(transfer, bit_ofs, &msg->@(field_get_data(field))[msg->@(field.name).len], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@)) {return true;} @(ind)msg->@(field.name).len++; @{indent -= 1}@{ind = ' '*indent}@ @(ind)} @@ -269,7 +327,7 @@ bool _@(msg_underscored_name)_decode(const CanardRxTransfer* transfer, uint32_t* @[ end if]@ @(ind)*bit_ofs += @(field.type.value_type.bitlen); @[ elif field.type.value_type.category == field.type.value_type.CATEGORY_COMPOUND]@ -@(ind)if (_@(underscored_name(field.type.value_type))_decode(transfer, bit_ofs, &msg->@(field_get_data(field))[i], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@)) {return true;} +@(ind)if (__@(underscored_name(field.type.value_type))_decode(transfer, bit_ofs, &msg->@(field_get_data(field))[i], @[if field == msg_fields[-1] and field.type.value_type.get_min_bitlen() < 8]tao && i==msg->@(field.name).len@[else]false@[end if]@)) {return true;} @[ end if]@ @{indent -= 1}@{ind = ' '*indent}@ @(ind)}