diff options
| -rw-r--r-- | include/tlv.h | 36 | ||||
| -rwxr-xr-x[-rw-r--r--] | src/common/tlv.c | 86 | ||||
| -rw-r--r-- | src/device/main.ino | 1 |
3 files changed, 101 insertions, 22 deletions
diff --git a/include/tlv.h b/include/tlv.h index b52763a..75aa133 100644 --- a/include/tlv.h +++ b/include/tlv.h @@ -62,6 +62,8 @@ A few things to note: #include <errno.h> +#include <stdint.h> +#include <limits.h> #define E_TLV_OVERFLOW (1 << 0) #define E_UNKNOWN_TYPE (1 << 1) @@ -79,8 +81,19 @@ extern "C" { */ typedef unsigned int msg_idx_t; +#if INT32_MAX == INT_MAX +#define READ_ENUM(data) htonl((data)) +#else INT_MAX == INT16_MAX +#define READ_ENUM(data) htons((data)) +#endif + enum tlv_type { /** + * Explicitly states that this tlv is no longer valid for reading. + */ + INVALID = 0, + + /** * NULL-terminated string. To be put in a queue to display on the * screen. */ @@ -132,8 +145,8 @@ enum tlv_type { * */ struct tlv { enum tlv_type type; - size_t length; - void *data; + uint16_t length; + const void *data; }; /** @@ -142,15 +155,22 @@ struct tlv { * Related functions return one tlv at a time. */ struct tlv_parser { - char *data; - size_t offset; - size_t size; + const char *data; + uint16_t offset; }; /** + * Initialises parser to begin parsing the data. + * + * Returns the first tlv which should be a meta-tlv. If it is not --- you got + * problems. + */ +int tlv_parser_init(struct tlv_parser *parser, char *data, struct tlv *ret); + +/** * Fills tlv structure to represent the next tlv in the packet. * - * Returns END_OF_PACKET if all tlv's were read of E_TLV_OVERFLOW, if the last + * Returns END_OF_PACKET if all tlv's were read or E_TLV_OVERFLOW, if the last * tlv, according to its declared size should not fit in a packet. */ int tlv_get(struct tlv_parser *parser, struct tlv *ret); @@ -175,14 +195,14 @@ size_t tlv_raw_size(const struct tlv *t); * Pushes tlv to buffer as contiguous data. Check tlv size with tlv_raw_size * beforehand. If you don't do that and overflow --- tough tiddy. */ -int tlv_get_raw(struct tlv *t, char *buf); +int tlv_get_raw(const struct tlv *t, char *buf); /** * Pushes data to tlv. Returns E_TLV_OVERFLOW if pushing data would cause the * final size to be greater than TLV_SZ_MAX_RAW. In case of such error the data is left * untouched. */ -int tlv_push_data(struct tlv *t, const char *data, size_t size); +int tlv_push_data(struct tlv *t, const char *data, uint16_t size); /** * Pushes a sub-tlv into the packet. 't' can only be REGURAL, HEARTBEAT or diff --git a/src/common/tlv.c b/src/common/tlv.c index c25f008..72d0399 100644..100755 --- a/src/common/tlv.c +++ b/src/common/tlv.c @@ -31,33 +31,93 @@ #include "net.h" #include "utils.h" +#ifdef unix +#include <arpa/inet.h> +#else +#include <lwip/def.h> +#endif + +struct tlv_header { + enum tlv_type type; + uint16_t size; +}; + +/** + * Returns tlv_size from packet + */ +uint16_t tlv_size(char *buf); + +/** + * Returns tlv_type from packet. + */ +enum tlv_type tlv_get_type(char *buf); + +/* Changes to be made: + * * Trust tlv size from raw data + * * Don't fail on 0 length. In that case data points to NULL + * * tlv_parser should be opaque. + * * Make a constructor that takes a user-supplied buffer. + */ int tlv_get(struct tlv_parser *parser, struct tlv *ret) { int retval = 0; - if (parser->offset + sizeof(ret->type) + sizeof(ret->length) >= parser->size) { - retval = E_TLV_OVERFLOW; - } else if (parser -> offset == parser->size) { - retval = END_OF_PACKET; + size_t packet_size; + size_t final_size; + struct tlv *crr_tlv = parser->data + parser->offset; + + packet_size = tlv_size(parser->data); + final_size = parser->offset + crr_tlv->length + sizeof(struct tlv_header); + if (final_size >= packet_size) { + return retval = E_TLV_OVERFLOW; } else { - memcpy(&ret->type, parser->data + parser->offset, sizeof(ret->type)); - parser->size += sizeof(ret->type); - ret->length = memcpy(&ret->length, parser->data + parser->offset, sizeof(ret->length)); - parser->offset += sizeof(ret->length); - if (parser->offset + ret->length >= parser->size) { - retval = E_TLV_OVERFLOW; + if (crr_tlv->length != 0) { + ret->data = parser->data + parser->offset + sizeof(struct tlv_header); } else { - memcpy(ret->data, parser->data, ret->length); + ret->data = NULL; + } + ret->type = crr_tlv->type; + ret->length = crr_tlv->length; + parser->offset += sizeof(struct tlv_header) + ret->length; + if (final_size == packet_size) { + retval = END_OF_PACKET; } } + return retval; } +int tlv_parser_init(struct tlv_parser *parser, char *buf, struct tlv *ret) +{ + /* Data points to the beggining of the buffer for size and + * type acquisition purposes. + * */ + struct tlv_header *data = buf; + /* It is known that the first tlv will be of the meta kind. + * We can safely skip the header. + * */ + parser->data = buf + sizeof(struct tlv_header); + ret->type = READ_ENUM(data->type); + ret->length = ntohs(data->size); +} + +uint16_t tlv_size(char *buf) +{ + struct tlv_header *data = buf; + return ntohs(data->size); +} + +enum tlv_type tlv_get_type(char *buf) +{ + struct tlv_header *data = buf; + return READ_ENUM(data->type); +} + size_t tlv_data_size(struct tlv_parser *parser) { size_t size; - if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= parser->size) { + if (parser->offset + sizeof(enum tlv_type) + sizeof(size_t) >= tlv_size(parser)) { size = 0; } else { memcpy(&size, parser->data + parser->offset + sizeof(enum tlv_type), sizeof(size_t)); @@ -92,7 +152,7 @@ size_t tlv_raw_size(const struct tlv *t) return sizeof(*t) + t->length; } -int tlv_push_data(struct tlv *t, const char *data, size_t size) +int tlv_push_data(struct tlv *t, const char *data, uint16_t size) { int ret = 0; size_t final_size = tlv_raw_size(t) + size; diff --git a/src/device/main.ino b/src/device/main.ino index 24cfb9c..4376346 100644 --- a/src/device/main.ino +++ b/src/device/main.ino @@ -103,7 +103,6 @@ void loop(void) if (progstate.bytes_read > 0) { progstate.parser.data = progstate.in_packet_buf; progstate.parser.offset = 0; - progstate.parser.size = progstate.bytes_read; /* Ignore errors for now. */ while (tlv_get(&progstate.parser, &progstate.crr_data) == 0) { handle_tlv(&progstate.crr_data); |