1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
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 };
_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 , 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));
}
|
