hydrolink/gen_c/hydrolink_gen_c.py

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"
{"\n".join(f"#include \"{GENERIC_MSG_NAME}_{msg.name_lower}.h\"" for msg in msgs)}
""")

def generate_protocol(xml, 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)

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}

inline void byte_swap_2(char *dst, const char *src) {{
  dst[0] = src[1];
  dst[1] = src[0];
}}

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];
}}

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("""
inline void crc_xmodem_init(uint16_t *crc) { *crc = 0; }

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;
    }
  }
}

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;
}
""")
        f.write(f"""
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) {{
  msg->id = msg_id;
  msg->payload_length = payload_length;
  msg->dst_id = dst_id;
  msg->src_id = src_id;
  uint16_t crc;
  crc_xmodem_init(&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;
}}

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;
}}

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)
    for msg in msgs:
        generate_message_h(os.path.join(out_dir), msg)
    generate_main_header(msgs, out_dir)
    generate_protocol(xmls[0], out_dir)
    generate_enums_h(out_dir, xmls[0].enums)