From 662d78b9a5bebdb98dc178ebd1f8e3384793c104 Mon Sep 17 00:00:00 2001 From: Jari Vetoniemi Date: Sun, 17 Mar 2019 15:30:43 +0200 Subject: Initial commit --- src/packet.c | 167 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 167 insertions(+) create mode 100644 src/packet.c (limited to 'src/packet.c') diff --git a/src/packet.c b/src/packet.c new file mode 100644 index 0000000..8ab6ae6 --- /dev/null +++ b/src/packet.c @@ -0,0 +1,167 @@ +#include "packet.h" +#include +#include +#include +#include + +#include + +_Static_assert(sizeof(((union packet*)0)->buf) >= sizeof(((union packet*)0)->hdr), "packet.hdr must fit packet"); +_Static_assert(sizeof(((union packet*)0)->buf) == sizeof(*((union packet*)0)), "packet.buf must be sizeof(packet)"); + +bool +packet_verify(const union packet *packet) +{ + assert(packet); + + if (packet->hdr.size < sizeof(packet->hdr) || packet->hdr.size > sizeof(packet->buf)) + return false; + + static const uint8_t hdr[] = { 0x02, 0x00, 0xff, 0xff, 0xff, 0xff, 0xff }; // 0xff are wildcards + _Static_assert(sizeof(packet->hdr) >= sizeof(hdr), "sizeof(packet->hdr) != sizeof(hdr)"); + + for (const uint8_t *b = packet->buf, *h = hdr; (size_t)(h - hdr) < sizeof(hdr) ; ++b, ++h) { + if (*h != 0xff && *b != *h) + return false; + } + + return true; +} + +void +packet_crypt(union packet *packet) +{ + assert(packet); + static const uint8_t key[] = { 0x57, 0x19, 0xc6, 0x2d, 0x56, 0x68, 0x3a, 0xcc, 0x60, 0x3b, 0x0b, 0xb1, 0x90, 0x5c, 0x4a, 0xf8, 0x80, 0x28, 0xb1, 0x45, 0xb6, 0x85, 0xe7, 0x4c, 0x06, 0x2d, 0x55, 0x83, 0xaf, 0x44, 0x99 }; + const int16_t salt = packet->hdr.salt + packet->hdr.salt + packet->hdr.salt * 2; + for (size_t i = 5 /* skip salt && size && unknown[0] */, c = 0; i < packet->hdr.size; ++i, ++c) { + const uint32_t hi = (uint64_t)(0x55555556 * c) >> 32; + int32_t magic = (hi >> sizeof(key)) + hi; + magic = salt - (magic + magic * 2); + assert(magic + c < sizeof(key)); + packet->buf[i] ^= key[magic + c]; + } +} + +static size_t +utf16_to_utf8(const uint8_t *utf16, const size_t utf16_sz, uint8_t *utf8, const size_t utf8_sz) +{ + assert(utf16 && utf8 && utf8_sz); + + if (!utf16_sz) { + utf8[0] = 0; + return 0; + } + + static iconv_t cd; + if (!cd && !(cd = iconv_open("utf8", "utf16le"))) + err(EXIT_FAILURE, "iconv_open"); + + size_t isz = utf16_sz, osz = utf8_sz - 1; + iconv(cd, (char*[]){(uint8_t*)utf16}, &isz, (char*[]){utf8}, &osz); + const size_t len = (utf8_sz - 1) - osz; + utf8[len] = 0; + return len; +} + +static size_t +utf8_to_utf16(const uint8_t *utf8, const size_t utf8_sz, uint8_t *utf16, const size_t utf16_sz) +{ + assert(utf8 && utf16 && utf16_sz); + + if (!utf8_sz) + return 0; + + static iconv_t cd; + if (!cd && !(cd = iconv_open("utf16le", "utf8"))) + err(EXIT_FAILURE, "iconv_open"); + + size_t isz = utf8_sz, osz = utf16_sz; + iconv(cd, (char*[]){(uint8_t*)utf8}, &isz, (char*[]){utf16}, &osz); + const size_t len = utf16_sz - osz; + return len; +} + +void +pbuf_flush(struct pbuf *pbuf) +{ + pbuf->packet->hdr.size = pbuf->cursor; + pbuf->cursor = 0; +} + +void +pbuf_write(struct pbuf *pbuf, const void *data, const size_t sz) +{ + assert(pbuf && pbuf->packet); + assert(sz <= sizeof(pbuf->packet->buf) && pbuf->cursor <= sizeof(pbuf->packet->buf) - sz); + memcpy(pbuf->packet->buf + pbuf->cursor, data, sz); + pbuf->cursor += sz; +} + +void +pbuf_write_str_len(struct pbuf *pbuf, const void *str, const uint16_t len) +{ + pbuf_write(pbuf, &len, sizeof(len)); + pbuf_write(pbuf, str, len); +} + +void +pbuf_write_str(struct pbuf *pbuf, const char *str) +{ + assert(str); + pbuf_write_str_len(pbuf, str, strlen(str)); +} + +void +pbuf_write_str_len_utf16(struct pbuf *pbuf, const void *str, const uint16_t len) +{ + struct pbuf_string u16; + u16.len = utf8_to_utf16(str, len, u16.data, sizeof(u16.data)); + pbuf_write(pbuf, &u16.len, sizeof(u16.len)); + pbuf_write(pbuf, u16.data, u16.len); +} + +void +pbuf_write_str_utf16(struct pbuf *pbuf, const char *str) +{ + assert(str); + pbuf_write_str_len_utf16(pbuf, str, strlen(str)); +} + +size_t +pbuf_read_safe(struct pbuf *pbuf, void *data, const size_t data_sz, const size_t sz) +{ + assert(pbuf && pbuf->packet); + assert(sz <= sizeof(pbuf->packet->buf) && pbuf->cursor <= sizeof(pbuf->packet->buf) - sz); + const size_t to_copy = (sz > data_sz ? data_sz : sz); + memcpy(data, pbuf->packet->buf + pbuf->cursor, to_copy); + pbuf->cursor += sz; + return to_copy; +} + +void +pbuf_read(struct pbuf *pbuf, void *data, const size_t sz) +{ + pbuf_read_safe(pbuf, data, sz, sz); +} + +void +pbuf_read_str(struct pbuf *pbuf, struct pbuf_string *str) +{ + assert(str); + pbuf_read(pbuf, &str->len, sizeof(str->len)); + str->len = pbuf_read_safe(pbuf, str->data, sizeof(str->data) - 1, str->len); + assert(str->len < sizeof(str->data)); + str->data[str->len] = 0; +} + +void +pbuf_read_str_utf16(struct pbuf *pbuf, struct pbuf_string *str) +{ + assert(str); + struct pbuf_string u16; + pbuf_read(pbuf, &u16.len, sizeof(u16.len)); + u16.len = pbuf_read_safe(pbuf, u16.data, sizeof(u16.data) - 1, u16.len); + assert(u16.len < sizeof(u16.data)); + str->len = utf16_to_utf8(u16.data, u16.len, str->data, sizeof(str->data)); +} -- cgit v1.2.3