203 lines
6.3 KiB
C
203 lines
6.3 KiB
C
|
|
#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 6
|
|
#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 {{1, 73}, {2, 58}, {3, 250}, {4, 38}, {5, 28}}
|
|
|
|
/**
|
|
* @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 hydrolink_msg_s;
|
|
|
|
typedef struct hydrolink_msg_s hydrolink_msg_t;
|
|
|
|
struct hydrolink_msg_s {
|
|
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])))
|
|
|
|
#define hydrolink_put_float(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_double(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_char(buf, wire_offset, b) buf[wire_offset] = (char)b
|
|
#define hydrolink_put_int8_t(buf, wire_offset, b) buf[wire_offset] = (int8_t)b
|
|
#define hydrolink_put_uint8_t(buf, wire_offset, b) buf[wire_offset] = (uint8_t)b
|
|
#define hydrolink_put_int16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_uint16_t(buf, wire_offset, b) byte_swap_2(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_int32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_uint32_t(buf, wire_offset, b) byte_swap_4(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_int64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
|
|
#define hydrolink_put_uint64_t(buf, wire_offset, b) byte_swap_8(&buf[wire_offset], (const char *)&b)
|
|
|
|
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];
|
|
}
|
|
|
|
#define HYDROLINK_MSG_RETURN_TYPE(TYPE, SIZE) \
|
|
static inline TYPE HYDROLINK_RETURN_## TYPE(const hydrolink_msg_t *msg, uint8_t offset) {\
|
|
TYPE ret; \
|
|
byte_swap_## SIZE((char*)&ret, &HYDROLINK_PAYLOAD(msg)[offset]); \
|
|
return ret; \
|
|
}
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(float, 4)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(double, 8)
|
|
|
|
#define HYDROLINK_RETURN_char(msg, wire_offset) (char)HYDROLINK_PAYLOAD(msg)[wire_offset]
|
|
#define HYDROLINK_RETURN_int8_t(msg, wire_offset) (int8_t)HYDROLINK_PAYLOAD(msg)[wire_offset]
|
|
#define HYDROLINK_RETURN_uint8_t(msg, wire_offset) (uint8_t)HYDROLINK_PAYLOAD(msg)[wire_offset]
|
|
HYDROLINK_MSG_RETURN_TYPE(int16_t, 2)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(uint16_t, 2)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(int32_t, 4)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(uint32_t, 4)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(int64_t, 8)
|
|
|
|
HYDROLINK_MSG_RETURN_TYPE(uint64_t, 8)
|
|
|
|
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;
|
|
}
|
|
|
|
static inline void hydrolink_fill_header_and_crc(hydrolink_msg_t *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;
|
|
}
|
|
|
|
static inline uint8_t hydrolink_serialize_message(hydrolink_msg_t *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(hydrolink_msg_t *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;
|
|
}
|