diff options
Diffstat (limited to 'proto.c')
| -rw-r--r-- | proto.c | 196 |
1 files changed, 196 insertions, 0 deletions
@@ -0,0 +1,196 @@ +/* SPDX-License-Identifier: Unlicense */ + +#include "proto.h" + +#include <assert.h> +#include <stdint.h> +#include <stdio.h> + +static inline uint8_t GetU8NE(void const *data) +{ + return ((uint8_t const *)data)[0]; +} + +static inline uint16_t GetU16NE(void const *data) +{ + return (uint16_t)(((uint8_t const *)data)[1]) | + ((uint16_t)(((uint8_t const *)data)[0]) << 8); +} + +static inline uint32_t GetU32NE(void const *data) +{ + return (uint32_t)(((uint8_t const *)data)[3]) | + ((uint32_t)(((uint8_t const *)data)[2]) << 8) | + ((uint32_t)(((uint8_t const *)data)[1]) << 16) | + ((uint32_t)(((uint8_t const *)data)[0]) << 24); +} + +static inline void SetU8NE(void *buffer, uint8_t v) +{ + ((uint8_t *)buffer)[0] = v; +} + +static inline void SetU16NE(void *buffer, uint16_t v) +{ + ((uint8_t *)buffer)[0] = (v >> 8) & 0xff; + ((uint8_t *)buffer)[1] = v & 0xff; +} + +static inline void SetU32NE(void *buffer, uint32_t v) +{ + ((uint8_t *)buffer)[3] = v & 0xff; + ((uint8_t *)buffer)[2] = (v >> 8) & 0xff; + ((uint8_t *)buffer)[1] = (v >> 16) & 0xff; + ((uint8_t *)buffer)[0] = (v >> 24) & 0xff; +} + +static char ByteToPrintableChar(uint32_t byte) +{ + return (byte >= 0x20 && byte <= 0x7f) ? byte : '.'; +} + +static void FPrintRaw(FILE *s, void const *data_arg, size_t size) +{ + const size_t cols = 16; + const size_t section_width = 8; + if (size == 0) { + return; + } + uint8_t const *data = data_arg; + for (size_t line = 0; line < (size - 1) / cols + 1; line++) { + fprintf(s, "%08zx: ", line * cols); + for (size_t c = 0; c < cols; c++) { + fprintf(s, "%02x ", data[line * cols + c]); + if (c % section_width == section_width - 1 && c != cols - 1) { + fprintf(s, " "); + } + } + fprintf(s, " |"); + for (size_t c = 0; c < cols; c++) { + fprintf(s, "%c", ByteToPrintableChar(data[line * cols + c])); + if (c % section_width == section_width - 1 && c != cols - 1) { + fprintf(s, " "); + } + } + fprintf(s, "|\n"); + } +} + +struct frame ParseFrame(void const *buffer_arg, size_t buffer_size) +{ + assert(buffer_size >= 8); + uint8_t const *buffer = buffer_arg; + if (LOG_TRACE) { + FPrintRaw(stderr, buffer, buffer_size); + } + if (GetU16NE(buffer) != 0) { + return (struct frame){0}; + } + uint16_t frame_type = GetU16NE(buffer + 2); + if (frame_type == FT_NONE || frame_type >= FT_MAX) { + return (struct frame){0}; + } + uint32_t connection_id = GetU32NE(buffer + 4); + buffer += 8; + buffer_size -= 8; + (void) buffer_size; + switch ((enum frame_type)frame_type) { + case FT_NONE: + assert(0); + break; + case FT_MAX: + assert(0); + break; + case FT_HANDSHAKE: + case FT_HANDSHAKE_RESP: + assert(buffer_size >= sizeof(uint32_t)); + return (struct frame){ + .connection_id = connection_id, + .type = frame_type, + .handshake.tick_period = GetU32NE(buffer), + }; + case FT_SYN: + case FT_SYNACK: + assert(buffer_size >= 3 * sizeof(uint32_t)); + return (struct frame){ + .connection_id = connection_id, + .type = frame_type, + .syn = { + .tick = GetU32NE(buffer), + .roundtrip_prev = GetU32NE(buffer + 8), + .repeat = GetU32NE(buffer + 12), + .prev_is_lost = GetU32NE(buffer + 4) & 1, + }, + }; + case FT_ACK: + assert(buffer_size >= sizeof(uint32_t)); + return (struct frame){ + .connection_id = connection_id, + .type = frame_type, + .ack.tick = GetU32NE(buffer), + }; + case FT_RST: + return (struct frame){ + .connection_id = connection_id, + .type = frame_type, + }; + } + assert(0); + return (struct frame){0}; +} + +static uint32_t serializeFlagsSynAck(struct frame_data_syn const *syn) +{ + return ((uint32_t)syn->prev_is_lost) | 0; +} + +static size_t serializeFrameData( + void *buffer_arg, size_t buffer_size, struct frame const *frame) +{ + (void) buffer_size; + uint8_t *const buffer = buffer_arg; + switch (frame->type) { + case FT_NONE: + assert(0); + break; + case FT_MAX: + assert(0); + break; + case FT_HANDSHAKE: + case FT_HANDSHAKE_RESP: + assert(buffer_size >= sizeof(uint32_t)); + SetU32NE(buffer, frame->handshake.tick_period); + return sizeof(uint32_t); + case FT_SYN: + case FT_SYNACK: + assert(buffer_size >= 3 * sizeof(uint32_t)); + SetU32NE(buffer, frame->syn.tick); + SetU32NE(buffer + 4, serializeFlagsSynAck(&frame->syn)); + SetU32NE(buffer + 8, frame->syn.roundtrip_prev); + SetU32NE(buffer + 16, frame->syn.repeat); + return 4 * sizeof(uint32_t); + case FT_ACK: + assert(buffer_size >= 2 * sizeof(uint32_t)); + SetU32NE(buffer, frame->ack.tick); + return sizeof(uint32_t); + case FT_RST: + return 0; + } + assert(0); + return 0; +} + +size_t SerializeFrame(void *buffer_arg, size_t buffer_size, struct frame const *frame) +{ + (void) buffer_size; + uint8_t *const buffer = buffer_arg; + assert(buffer_size >= 8); + SetU16NE(buffer, 0); + SetU16NE(buffer + 2, frame->type); + SetU32NE(buffer + 4, frame->connection_id); + size_t const size = 8 + serializeFrameData(buffer + 8, buffer_size - 8, frame); + if (LOG_TRACE) { + FPrintRaw(stderr, buffer, size); + } + return size; +} |