1 files changed, 167 insertions, 0 deletions
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 <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <err.h>
+
+#include <iconv.h>
+
+_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));
+}