237 lines
6.9 KiB
Python
237 lines
6.9 KiB
Python
from hydroparse import TYPE_LENGTHS
|
|
from .gen_messages import generate_message_h
|
|
from .gen_enums import generate_enums_h
|
|
import os
|
|
|
|
NS_PREFIX_LOWER = "hydrolink_"
|
|
GENERIC_MSG_NAME = f"{NS_PREFIX_LOWER}msg"
|
|
GENERIC_MSG_STRUCT = f"{GENERIC_MSG_NAME}_s"
|
|
GENERIC_MSG_TYPE = f"{GENERIC_MSG_NAME}_t"
|
|
MSG_ID_PREFIX = "HYDROLINK_MSG_ID_"
|
|
|
|
def generate_main_header(msgs, dir):
|
|
with open(os.path.join(dir, "hydrolink.h"), "w") as f:
|
|
f.write(f"""#pragma once
|
|
#include "protocol.h"
|
|
#include "hydrolink_enums.h"
|
|
{"\n".join(f"#include \"{GENERIC_MSG_NAME}_{msg.name_lower}.h\"" for msg in msgs)}
|
|
""")
|
|
|
|
def generate_protocol(xml, msgs, dir):
|
|
put_defines = []
|
|
for typename in TYPE_LENGTHS:
|
|
length = TYPE_LENGTHS[typename]
|
|
if length == 1:
|
|
put_defines.append(
|
|
f"#define {NS_PREFIX_LOWER}put_{typename}(buf, wire_offset, b) buf[wire_offset] = ({typename})b"
|
|
)
|
|
else:
|
|
put_defines.append(
|
|
f"#define {NS_PREFIX_LOWER}put_{typename}(buf, wire_offset, b) byte_swap_{length}(&buf[wire_offset], (const char *)&b)"
|
|
)
|
|
|
|
put_defines_str = "\n".join(put_defines)
|
|
with open(os.path.join(dir, "protocol.h"), "w") as f:
|
|
f.write(f"""
|
|
#pragma once
|
|
|
|
#include <stdint.h>
|
|
#define HYDROLINK_HEADER_LEN 4
|
|
#define HYDROLINK_CRC_LEN 2
|
|
#define HYDROLINK_NON_PAYLOAD_LEN (HYDROLINK_HEADER_LEN + HYDROLINK_CRC_LEN)
|
|
#define HYDROLINK_MAX_PAYLOAD_LEN {xml.largest_payload}
|
|
#define HYDROLINK_MAX_MSG_LEN (HYDROLINK_MAX_PAYLOAD_LEN + HYDROLINK_NON_PAYLOAD_LEN)
|
|
|
|
typedef struct {{
|
|
uint8_t msg_id;
|
|
uint8_t crc_extra;
|
|
}} hydrolink_crc_extra_t;
|
|
|
|
#define HYDROLINK_MSG_CRCS {{{", ".join(f"{{{msg.id}, {msg.crc_extra}}}" for msg in msgs)}}}
|
|
|
|
/**
|
|
* @brief Get the crc_extra byte corresponding to msg_id
|
|
*
|
|
* @param msg_id the message's id
|
|
* @param crc[out] the crc_extra byte as output of this function
|
|
* @return Return 1 if successfull, 0 otherwise.
|
|
**/
|
|
static inline uint8_t hydrolink_get_crc_extra(uint8_t msg_id, uint8_t *crc) {{
|
|
static const hydrolink_crc_extra_t crcs[] = HYDROLINK_MSG_CRCS;
|
|
uint8_t left = 0;
|
|
uint8_t right = sizeof(crcs) / sizeof(crcs[0]) - 1;
|
|
while (left < right) {{
|
|
uint8_t center = (left+right+1) / 2;
|
|
if (msg_id < crcs[center].msg_id) {{
|
|
right = center - 1;
|
|
continue;
|
|
}}
|
|
if (msg_id > crcs[center].msg_id) {{
|
|
left = center;
|
|
continue;
|
|
}}
|
|
left = center;
|
|
break;
|
|
}}
|
|
if (crcs[left].msg_id != msg_id) {{
|
|
return 0;
|
|
}}
|
|
*crc = crcs[left].crc_extra;
|
|
return 1;
|
|
}}
|
|
|
|
struct {GENERIC_MSG_STRUCT};
|
|
|
|
typedef struct {GENERIC_MSG_STRUCT} {GENERIC_MSG_TYPE};
|
|
|
|
struct {GENERIC_MSG_STRUCT} {{
|
|
uint8_t id;
|
|
uint8_t payload_length;
|
|
uint8_t dst_id;
|
|
uint8_t src_id;
|
|
uint8_t payload[HYDROLINK_MAX_PAYLOAD_LEN];
|
|
uint16_t crc;
|
|
}};
|
|
|
|
#define HYDROLINK_PAYLOAD(msg) ((const char *)(&((msg)->payload[0])))
|
|
#define HYDROLINK_PAYLOAD_NON_CONST(msg) ((char *)(&((msg)->payload[0])))
|
|
|
|
{put_defines_str}
|
|
|
|
static inline void byte_swap_2(char *dst, const char *src) {{
|
|
dst[0] = src[1];
|
|
dst[1] = src[0];
|
|
}}
|
|
|
|
static inline void byte_swap_4(char *dst, const char *src) {{
|
|
dst[0] = src[3];
|
|
dst[1] = src[2];
|
|
dst[2] = src[1];
|
|
dst[3] = src[0];
|
|
}}
|
|
|
|
static inline void byte_swap_8(char *dst, const char *src) {{
|
|
dst[0] = src[7];
|
|
dst[1] = src[6];
|
|
dst[2] = src[5];
|
|
dst[3] = src[4];
|
|
dst[4] = src[3];
|
|
dst[5] = src[2];
|
|
dst[6] = src[1];
|
|
dst[7] = src[0];
|
|
}}
|
|
""")
|
|
f.write(f"""
|
|
#define HYDROLINK_MSG_RETURN_TYPE(TYPE, SIZE) \\
|
|
static inline TYPE HYDROLINK_RETURN_## TYPE(const {GENERIC_MSG_TYPE} *msg, uint8_t offset) {{\\
|
|
TYPE ret; \\
|
|
byte_swap_## SIZE((char*)&ret, &HYDROLINK_PAYLOAD(msg)[offset]); \\
|
|
return ret; \\
|
|
}}
|
|
""")
|
|
for typename in TYPE_LENGTHS:
|
|
length = TYPE_LENGTHS[typename]
|
|
if not length > 1:
|
|
f.write(f"""
|
|
#define HYDROLINK_RETURN_{typename}(msg, wire_offset) ({typename})HYDROLINK_PAYLOAD(msg)[wire_offset]""")
|
|
else:
|
|
f.write(f"""
|
|
HYDROLINK_MSG_RETURN_TYPE({typename}, {length})
|
|
""")
|
|
f.write("""
|
|
static inline void crc_xmodem_init(uint16_t *crc) { *crc = 0; }
|
|
|
|
static inline void crc_xmodem_accumulate(uint8_t data, uint16_t *crc) {
|
|
*crc = *crc ^ ((uint16_t)data << 8);
|
|
for (uint8_t j = 0; j < 8; j++) {
|
|
if (*crc & 0x8000) {
|
|
*crc = (*crc << 1) ^ 0x1021;
|
|
} else {
|
|
*crc <<= 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
static inline uint16_t crc_xmodem_calculate(const uint8_t *data, uint8_t length) {
|
|
uint16_t crc;
|
|
crc_xmodem_init(&crc);
|
|
for (uint8_t i = 0; i < length; i++) {
|
|
crc_xmodem_accumulate(*data, &crc);
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
static inline uint16_t hydrolink_calculate_crc(const uint8_t *payload, uint8_t length, uint8_t crc_extra) {
|
|
uint16_t crc;
|
|
crc_xmodem_init(&crc);
|
|
for(int i = 0; i < length; ++i) {
|
|
crc_xmodem_accumulate(payload[i], &crc);
|
|
}
|
|
crc_xmodem_accumulate(crc_extra, &crc);
|
|
return crc;
|
|
}
|
|
""")
|
|
f.write(f"""
|
|
static inline void hydrolink_fill_header_and_crc({GENERIC_MSG_TYPE} *msg, uint8_t msg_id, uint8_t payload_length, uint8_t dst_id, uint8_t src_id, uint8_t crc_extra) {{
|
|
uint16_t crc;
|
|
crc_xmodem_init(&crc);
|
|
msg->id = msg_id;
|
|
crc_xmodem_accumulate(msg->id, &crc);
|
|
msg->payload_length = payload_length;
|
|
crc_xmodem_accumulate(msg->payload_length, &crc);
|
|
msg->dst_id = dst_id;
|
|
crc_xmodem_accumulate(msg->dst_id, &crc);
|
|
msg->src_id = src_id;
|
|
crc_xmodem_accumulate(msg->src_id, &crc);
|
|
for(int i = 0; i < msg->payload_length; ++i) {{
|
|
crc_xmodem_accumulate(msg->payload[i], &crc);
|
|
}}
|
|
crc_xmodem_accumulate(crc_extra, &crc);
|
|
msg->crc = crc;
|
|
}}
|
|
|
|
static inline uint8_t hydrolink_serialize_message({GENERIC_MSG_TYPE} *msg, uint8_t *buffer) {{
|
|
buffer[0] = msg->id;
|
|
buffer[1] = msg->payload_length;
|
|
buffer[2] = msg->dst_id;
|
|
buffer[3] = msg->src_id;
|
|
uint8_t *p = &buffer[4];
|
|
for(int i =0; i<msg->payload_length;++i) {{
|
|
*p++ = msg->payload[i];
|
|
}}
|
|
*p++ = (uint8_t)(msg->crc >> 8);
|
|
*p++ = (uint8_t)(msg->crc & 0xff);
|
|
return HYDROLINK_NON_PAYLOAD_LEN + msg->payload_length;
|
|
}}
|
|
|
|
static inline uint8_t hydrolink_deserialize_header({GENERIC_MSG_TYPE} *msg, uint8_t *buffer, uint8_t buffer_length) {{
|
|
if (buffer_length < HYDROLINK_NON_PAYLOAD_LEN) {{
|
|
return 0;
|
|
}}
|
|
msg->id = buffer[0];
|
|
msg->payload_length = buffer[1];
|
|
msg->dst_id = buffer[2];
|
|
msg->src_id = buffer[3];
|
|
msg->crc = buffer[buffer_length-1] | (((uint16_t)buffer[buffer_length-2]) << 8);
|
|
if (msg->payload_length + HYDROLINK_NON_PAYLOAD_LEN != buffer_length) {{
|
|
return 0;
|
|
}}
|
|
uint8_t *p = &buffer[HYDROLINK_HEADER_LEN];
|
|
for (int i = 0; i < msg->payload_length; ++i) {{
|
|
msg->payload[i] = *p++;
|
|
}}
|
|
return msg->payload_length + HYDROLINK_NON_PAYLOAD_LEN;
|
|
}}
|
|
""")
|
|
|
|
|
|
def generate(xmls, out_dir):
|
|
msgs = []
|
|
for x in xmls:
|
|
msgs.extend(x.messages)
|
|
sorted(msgs, key=lambda msg: msg.id)
|
|
for msg in msgs:
|
|
generate_message_h(os.path.join(out_dir), msg)
|
|
generate_main_header(msgs, out_dir)
|
|
generate_protocol(xmls[0], msgs, out_dir)
|
|
generate_enums_h(out_dir, xmls[0].enums)
|