Line data Source code
1 : /* protocol.c - Wire protocol implementation for PeerTalk */
2 :
3 : #include "protocol.h"
4 : #include "pt_internal.h"
5 : #include "pt_compat.h"
6 : #include "../../include/peertalk.h"
7 :
8 : /* ========================================================================
9 : * CRC-16 Lookup Table
10 : * ======================================================================== */
11 :
12 : /* CRC-16 table for polynomial 0x8408 (reflected 0x1021)
13 : * Generated with init=0x0000
14 : * Check value: pt_crc16("123456789", 9) == 0x2189
15 : */
16 : static const uint16_t crc16_table[256] = {
17 : 0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
18 : 0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
19 : 0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
20 : 0x9CC9, 0x8D40, 0xBFDB, 0xAE52, 0xDAED, 0xCB64, 0xF9FF, 0xE876,
21 : 0x2102, 0x308B, 0x0210, 0x1399, 0x6726, 0x76AF, 0x4434, 0x55BD,
22 : 0xAD4A, 0xBCC3, 0x8E58, 0x9FD1, 0xEB6E, 0xFAE7, 0xC87C, 0xD9F5,
23 : 0x3183, 0x200A, 0x1291, 0x0318, 0x77A7, 0x662E, 0x54B5, 0x453C,
24 : 0xBDCB, 0xAC42, 0x9ED9, 0x8F50, 0xFBEF, 0xEA66, 0xD8FD, 0xC974,
25 : 0x4204, 0x538D, 0x6116, 0x709F, 0x0420, 0x15A9, 0x2732, 0x36BB,
26 : 0xCE4C, 0xDFC5, 0xED5E, 0xFCD7, 0x8868, 0x99E1, 0xAB7A, 0xBAF3,
27 : 0x5285, 0x430C, 0x7197, 0x601E, 0x14A1, 0x0528, 0x37B3, 0x263A,
28 : 0xDECD, 0xCF44, 0xFDDF, 0xEC56, 0x98E9, 0x8960, 0xBBFB, 0xAA72,
29 : 0x6306, 0x728F, 0x4014, 0x519D, 0x2522, 0x34AB, 0x0630, 0x17B9,
30 : 0xEF4E, 0xFEC7, 0xCC5C, 0xDDD5, 0xA96A, 0xB8E3, 0x8A78, 0x9BF1,
31 : 0x7387, 0x620E, 0x5095, 0x411C, 0x35A3, 0x242A, 0x16B1, 0x0738,
32 : 0xFFCF, 0xEE46, 0xDCDD, 0xCD54, 0xB9EB, 0xA862, 0x9AF9, 0x8B70,
33 : 0x8408, 0x9581, 0xA71A, 0xB693, 0xC22C, 0xD3A5, 0xE13E, 0xF0B7,
34 : 0x0840, 0x19C9, 0x2B52, 0x3ADB, 0x4E64, 0x5FED, 0x6D76, 0x7CFF,
35 : 0x9489, 0x8500, 0xB79B, 0xA612, 0xD2AD, 0xC324, 0xF1BF, 0xE036,
36 : 0x18C1, 0x0948, 0x3BD3, 0x2A5A, 0x5EE5, 0x4F6C, 0x7DF7, 0x6C7E,
37 : 0xA50A, 0xB483, 0x8618, 0x9791, 0xE32E, 0xF2A7, 0xC03C, 0xD1B5,
38 : 0x2942, 0x38CB, 0x0A50, 0x1BD9, 0x6F66, 0x7EEF, 0x4C74, 0x5DFD,
39 : 0xB58B, 0xA402, 0x9699, 0x8710, 0xF3AF, 0xE226, 0xD0BD, 0xC134,
40 : 0x39C3, 0x284A, 0x1AD1, 0x0B58, 0x7FE7, 0x6E6E, 0x5CF5, 0x4D7C,
41 : 0xC60C, 0xD785, 0xE51E, 0xF497, 0x8028, 0x91A1, 0xA33A, 0xB2B3,
42 : 0x4A44, 0x5BCD, 0x6956, 0x78DF, 0x0C60, 0x1DE9, 0x2F72, 0x3EFB,
43 : 0xD68D, 0xC704, 0xF59F, 0xE416, 0x90A9, 0x8120, 0xB3BB, 0xA232,
44 : 0x5AC5, 0x4B4C, 0x79D7, 0x685E, 0x1CE1, 0x0D68, 0x3FF3, 0x2E7A,
45 : 0xE70E, 0xF687, 0xC41C, 0xD595, 0xA12A, 0xB0A3, 0x8238, 0x93B1,
46 : 0x6B46, 0x7ACF, 0x4854, 0x59DD, 0x2D62, 0x3CEB, 0x0E70, 0x1FF9,
47 : 0xF78F, 0xE606, 0xD49D, 0xC514, 0xB1AB, 0xA022, 0x92B9, 0x8330,
48 : 0x7BC7, 0x6A4E, 0x58D5, 0x495C, 0x3DE3, 0x2C6A, 0x1EF1, 0x0F78
49 : };
50 :
51 : /* ========================================================================
52 : * CRC-16 Functions
53 : * ======================================================================== */
54 :
55 20296 : uint16_t pt_crc16(const uint8_t *data, size_t len)
56 : {
57 20296 : uint16_t crc = 0x0000;
58 572223 : while (len--) {
59 551927 : crc = (crc >> 8) ^ crc16_table[(crc ^ *data++) & 0xFF];
60 : }
61 20296 : return crc;
62 : }
63 :
64 65 : uint16_t pt_crc16_update(uint16_t crc, const uint8_t *data, size_t len)
65 : {
66 1368 : while (len--) {
67 1303 : crc = (crc >> 8) ^ crc16_table[(crc ^ *data++) & 0xFF];
68 : }
69 65 : return crc;
70 : }
71 :
72 3 : int pt_crc16_check(const uint8_t *data, size_t len, uint16_t expected)
73 : {
74 3 : return pt_crc16(data, len) == expected ? 1 : 0;
75 : }
76 :
77 : /* ========================================================================
78 : * Discovery Packet Functions
79 : * ======================================================================== */
80 :
81 10060 : int pt_discovery_encode(const pt_discovery_packet *pkt, uint8_t *buf,
82 : size_t buf_len)
83 : {
84 : uint16_t crc;
85 : size_t packet_size;
86 :
87 : /* Validate name length */
88 10060 : if (pkt->name_len > PT_PEER_NAME_MAX) {
89 0 : return PT_ERR_INVALID;
90 : }
91 :
92 : /* Validate type */
93 10060 : if (pkt->type < PT_DISC_TYPE_ANNOUNCE || pkt->type > PT_DISC_TYPE_GOODBYE) {
94 0 : return PT_ERR_INVALID;
95 : }
96 :
97 : /* Calculate packet size */
98 10060 : packet_size = 12 + pkt->name_len + 2; /* header + name + CRC */
99 :
100 : /* Check buffer size */
101 10060 : if (buf_len < packet_size) {
102 0 : return PT_ERR_BUFFER_FULL;
103 : }
104 :
105 : /* Magic: "PTLK" */
106 10060 : buf[0] = 'P';
107 10060 : buf[1] = 'T';
108 10060 : buf[2] = 'L';
109 10060 : buf[3] = 'K';
110 :
111 : /* Version, Type */
112 10060 : buf[4] = pkt->version;
113 10060 : buf[5] = pkt->type;
114 :
115 : /* Flags (big-endian) */
116 10060 : buf[6] = (pkt->flags >> 8) & 0xFF;
117 10060 : buf[7] = pkt->flags & 0xFF;
118 :
119 : /* Sender Port (big-endian) */
120 10060 : buf[8] = (pkt->sender_port >> 8) & 0xFF;
121 10060 : buf[9] = pkt->sender_port & 0xFF;
122 :
123 : /* Transports, Name Length */
124 10060 : buf[10] = pkt->transports;
125 10060 : buf[11] = pkt->name_len;
126 :
127 : /* Peer Name */
128 10060 : pt_memcpy(buf + 12, pkt->name, pkt->name_len);
129 :
130 : /* CRC-16 over header + name */
131 10060 : crc = pt_crc16(buf, 12 + pkt->name_len);
132 10060 : buf[12 + pkt->name_len] = (crc >> 8) & 0xFF;
133 10060 : buf[12 + pkt->name_len + 1] = crc & 0xFF;
134 :
135 10060 : return (int)packet_size;
136 : }
137 :
138 11230 : int pt_discovery_decode(struct pt_context *ctx, const uint8_t *buf, size_t len,
139 : pt_discovery_packet *pkt)
140 : {
141 : uint16_t crc_computed, crc_received;
142 : size_t expected_len;
143 :
144 : /* Minimum size check */
145 11230 : if (len < 14) {
146 224 : if (ctx) {
147 1 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
148 : "Discovery packet too short: %zu bytes (min 14)", len);
149 : }
150 224 : return PT_ERR_TRUNCATED;
151 : }
152 :
153 : /* Magic validation */
154 11006 : if (buf[0] != 'P' || buf[1] != 'T' || buf[2] != 'L' || buf[3] != 'K') {
155 830 : if (ctx) {
156 1 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
157 : "Invalid discovery magic: 0x%02X%02X%02X%02X",
158 : buf[0], buf[1], buf[2], buf[3]);
159 : }
160 830 : return PT_ERR_MAGIC;
161 : }
162 :
163 : /* Extract version */
164 10176 : pkt->version = buf[4];
165 10176 : if (pkt->version != PT_PROTOCOL_VERSION) {
166 10 : if (ctx) {
167 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
168 : "Protocol version mismatch: got %u, expected %u",
169 : pkt->version, PT_PROTOCOL_VERSION);
170 : }
171 10 : return PT_ERR_VERSION;
172 : }
173 :
174 : /* Extract type */
175 10166 : pkt->type = buf[5];
176 10166 : if (pkt->type < PT_DISC_TYPE_ANNOUNCE || pkt->type > PT_DISC_TYPE_GOODBYE) {
177 19 : if (ctx) {
178 6 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
179 : "Invalid discovery type: 0x%02X", pkt->type);
180 : }
181 19 : return PT_ERR_INVALID;
182 : }
183 :
184 : /* Extract flags (big-endian) */
185 10147 : pkt->flags = ((uint16_t)buf[6] << 8) | buf[7];
186 :
187 : /* Extract sender port (big-endian) */
188 10147 : pkt->sender_port = ((uint16_t)buf[8] << 8) | buf[9];
189 :
190 : /* Extract transports and name length */
191 10147 : pkt->transports = buf[10];
192 10147 : pkt->name_len = buf[11];
193 :
194 : /* Validate name length */
195 10147 : if (pkt->name_len > PT_PEER_NAME_MAX) {
196 5 : if (ctx) {
197 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
198 : "Name length too long: %u (max %u)",
199 : pkt->name_len, PT_PEER_NAME_MAX);
200 : }
201 5 : return PT_ERR_INVALID;
202 : }
203 :
204 : /* Check complete packet length */
205 10142 : expected_len = 12 + pkt->name_len + 2;
206 10142 : if (len < expected_len) {
207 7 : if (ctx) {
208 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
209 : "Discovery packet truncated: %zu bytes (expected %zu)",
210 : len, expected_len);
211 : }
212 7 : return PT_ERR_TRUNCATED;
213 : }
214 :
215 : /* Extract name */
216 10135 : pt_memcpy(pkt->name, buf + 12, pkt->name_len);
217 10135 : pkt->name[pkt->name_len] = '\0'; /* Null-terminate */
218 :
219 : /* Extract CRC (big-endian) */
220 10135 : crc_received = ((uint16_t)buf[12 + pkt->name_len] << 8) |
221 10135 : buf[12 + pkt->name_len + 1];
222 :
223 : /* Verify CRC */
224 10135 : crc_computed = pt_crc16(buf, 12 + pkt->name_len);
225 10135 : if (crc_computed != crc_received) {
226 124 : if (ctx) {
227 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
228 : "Discovery CRC mismatch: got 0x%04X, expected 0x%04X",
229 : crc_received, crc_computed);
230 : }
231 124 : return PT_ERR_CRC;
232 : }
233 :
234 10011 : if (ctx) {
235 1 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
236 : "Discovery packet decoded: type=%u, name='%s', port=%u",
237 : pkt->type, pkt->name, pkt->sender_port);
238 : }
239 :
240 10011 : return 0;
241 : }
242 :
243 : /* ========================================================================
244 : * Message Frame Functions
245 : * ======================================================================== */
246 :
247 50 : int pt_message_encode_header(const pt_message_header *hdr, uint8_t *buf)
248 : {
249 : /* Magic: "PTMG" */
250 50 : buf[0] = 'P';
251 50 : buf[1] = 'T';
252 50 : buf[2] = 'M';
253 50 : buf[3] = 'G';
254 :
255 : /* Version, Type, Flags, Sequence */
256 50 : buf[4] = hdr->version;
257 50 : buf[5] = hdr->type;
258 50 : buf[6] = hdr->flags;
259 50 : buf[7] = hdr->sequence;
260 :
261 : /* Payload Length (big-endian) */
262 50 : buf[8] = (hdr->payload_len >> 8) & 0xFF;
263 50 : buf[9] = hdr->payload_len & 0xFF;
264 :
265 50 : return PT_MESSAGE_HEADER_SIZE;
266 : }
267 :
268 1055 : int pt_message_decode_header(struct pt_context *ctx, const uint8_t *buf,
269 : size_t len, pt_message_header *hdr)
270 : {
271 : /* Minimum size check */
272 1055 : if (len < PT_MESSAGE_HEADER_SIZE) {
273 283 : if (ctx) {
274 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
275 : "Message header too short: %zu bytes (min %d)",
276 : len, PT_MESSAGE_HEADER_SIZE);
277 : }
278 283 : return PT_ERR_TRUNCATED;
279 : }
280 :
281 : /* Magic validation */
282 772 : if (buf[0] != 'P' || buf[1] != 'T' || buf[2] != 'M' || buf[3] != 'G') {
283 734 : if (ctx) {
284 1 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
285 : "Invalid message magic: 0x%02X%02X%02X%02X",
286 : buf[0], buf[1], buf[2], buf[3]);
287 : }
288 734 : return PT_ERR_MAGIC;
289 : }
290 :
291 : /* Extract version */
292 38 : hdr->version = buf[4];
293 38 : if (hdr->version != PT_PROTOCOL_VERSION) {
294 1 : if (ctx) {
295 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
296 : "Protocol version mismatch: got %u, expected %u",
297 : hdr->version, PT_PROTOCOL_VERSION);
298 : }
299 1 : return PT_ERR_VERSION;
300 : }
301 :
302 : /* Extract type */
303 37 : hdr->type = buf[5];
304 37 : if (hdr->type < PT_MSG_TYPE_DATA || hdr->type > PT_MSG_TYPE_CAPABILITY) {
305 6 : if (ctx) {
306 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
307 : "Invalid message type: 0x%02X", hdr->type);
308 : }
309 6 : return PT_ERR_INVALID;
310 : }
311 :
312 : /* Extract flags, sequence */
313 31 : hdr->flags = buf[6];
314 31 : hdr->sequence = buf[7];
315 :
316 : /* Extract payload length (big-endian) */
317 31 : hdr->payload_len = ((uint16_t)buf[8] << 8) | buf[9];
318 :
319 31 : if (ctx) {
320 27 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
321 : "Message header decoded: type=%u, seq=%u, len=%u",
322 : hdr->type, hdr->sequence, hdr->payload_len);
323 : }
324 :
325 31 : return 0;
326 : }
327 :
328 : /* ========================================================================
329 : * Compact Header Functions
330 : * ======================================================================== */
331 :
332 46 : int pt_message_encode_compact(const pt_compact_header *hdr, uint8_t *buf)
333 : {
334 : /* Marker byte: 'P' (0x50) */
335 46 : buf[0] = PT_COMPACT_MARKER;
336 :
337 : /* TypeFlags: high nibble = type (0-7), low nibble = flags (0-15) */
338 46 : buf[1] = ((hdr->type & 0x0F) << 4) | (hdr->flags & 0x0F);
339 :
340 : /* Payload Length (big-endian) */
341 46 : buf[2] = (hdr->payload_len >> 8) & 0xFF;
342 46 : buf[3] = hdr->payload_len & 0xFF;
343 :
344 46 : return PT_COMPACT_HEADER_SIZE;
345 : }
346 :
347 38 : int pt_message_decode_compact(const uint8_t *buf, size_t len, pt_compact_header *hdr)
348 : {
349 : /* Minimum size check */
350 38 : if (len < PT_COMPACT_HEADER_SIZE) {
351 1 : return PT_ERR_TRUNCATED;
352 : }
353 :
354 : /* Marker validation */
355 37 : if (buf[0] != PT_COMPACT_MARKER) {
356 1 : return PT_ERR_MAGIC;
357 : }
358 :
359 : /* Extract type and flags from TypeFlags byte */
360 36 : hdr->type = (buf[1] >> 4) & 0x0F;
361 36 : hdr->flags = buf[1] & 0x0F;
362 :
363 : /* Validate type */
364 36 : if (hdr->type < PT_MSG_TYPE_DATA || hdr->type > PT_MSG_TYPE_CAPABILITY) {
365 0 : return PT_ERR_INVALID;
366 : }
367 :
368 : /* Extract payload length (big-endian) */
369 36 : hdr->payload_len = ((uint16_t)buf[2] << 8) | buf[3];
370 :
371 36 : return 0;
372 : }
373 :
374 49 : int pt_message_is_compact(const uint8_t *buf, size_t len)
375 : {
376 49 : if (len < 2) {
377 1 : return 0;
378 : }
379 :
380 : /* Compact header starts with 'P' (0x50) followed by TypeFlags byte.
381 : * Full header starts with 'P' 'T' 'M' 'G'.
382 : * If second byte is NOT 'T', it's a compact header. */
383 48 : if (buf[0] == PT_COMPACT_MARKER && buf[1] != 'T') {
384 15 : return 1;
385 : }
386 :
387 33 : return 0;
388 : }
389 :
390 : /* ========================================================================
391 : * UDP Message Functions
392 : * ======================================================================== */
393 :
394 5 : int pt_udp_encode(const void *payload, uint16_t payload_len,
395 : uint16_t sender_port, uint8_t *buf, size_t buf_len)
396 : {
397 5 : size_t packet_size = PT_UDP_HEADER_SIZE + payload_len;
398 :
399 : /* Check buffer size */
400 5 : if (buf_len < packet_size) {
401 0 : return PT_ERR_BUFFER_FULL;
402 : }
403 :
404 : /* Magic: "PTUD" */
405 5 : buf[0] = 'P';
406 5 : buf[1] = 'T';
407 5 : buf[2] = 'U';
408 5 : buf[3] = 'D';
409 :
410 : /* Sender Port (big-endian) */
411 5 : buf[4] = (sender_port >> 8) & 0xFF;
412 5 : buf[5] = sender_port & 0xFF;
413 :
414 : /* Payload Length (big-endian) */
415 5 : buf[6] = (payload_len >> 8) & 0xFF;
416 5 : buf[7] = payload_len & 0xFF;
417 :
418 : /* Payload */
419 5 : if (payload_len > 0) {
420 5 : pt_memcpy(buf + PT_UDP_HEADER_SIZE, payload, payload_len);
421 : }
422 :
423 5 : return (int)packet_size;
424 : }
425 :
426 1017 : int pt_udp_decode(struct pt_context *ctx, const uint8_t *buf, size_t len,
427 : uint16_t *sender_port_out, const void **payload_out,
428 : uint16_t *payload_len_out)
429 : {
430 : uint16_t payload_len;
431 :
432 : /* Minimum size check */
433 1017 : if (len < PT_UDP_HEADER_SIZE) {
434 121 : if (ctx) {
435 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
436 : "UDP message too short: %zu bytes (min %d)",
437 : len, PT_UDP_HEADER_SIZE);
438 : }
439 121 : return PT_ERR_TRUNCATED;
440 : }
441 :
442 : /* Magic validation */
443 896 : if (buf[0] != 'P' || buf[1] != 'T' || buf[2] != 'U' || buf[3] != 'D') {
444 893 : if (ctx) {
445 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
446 : "Invalid UDP magic: 0x%02X%02X%02X%02X",
447 : buf[0], buf[1], buf[2], buf[3]);
448 : }
449 893 : return PT_ERR_MAGIC;
450 : }
451 :
452 : /* Extract sender port (big-endian) */
453 3 : *sender_port_out = ((uint16_t)buf[4] << 8) | buf[5];
454 :
455 : /* Extract payload length (big-endian) */
456 3 : payload_len = ((uint16_t)buf[6] << 8) | buf[7];
457 3 : *payload_len_out = payload_len;
458 :
459 : /* Verify complete packet length */
460 3 : if (len < PT_UDP_HEADER_SIZE + payload_len) {
461 0 : if (ctx) {
462 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
463 : "UDP packet truncated: %zu bytes (expected %u)",
464 : len, PT_UDP_HEADER_SIZE + payload_len);
465 : }
466 0 : return PT_ERR_TRUNCATED;
467 : }
468 :
469 : /* Set payload pointer */
470 3 : *payload_out = buf + PT_UDP_HEADER_SIZE;
471 :
472 3 : if (ctx) {
473 2 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
474 : "UDP message decoded: port=%u, len=%u",
475 : *sender_port_out, payload_len);
476 : }
477 :
478 3 : return 0;
479 : }
480 :
481 : /* ========================================================================
482 : * Capability Message Functions
483 : * ======================================================================== */
484 :
485 37 : int pt_capability_encode(const pt_capability_msg *caps, uint8_t *buf, size_t buf_len)
486 : {
487 37 : size_t offset = 0;
488 :
489 : /* Calculate required size: 6 TLVs
490 : * - MAX_MESSAGE: 1 + 1 + 2 = 4
491 : * - PREFERRED_CHUNK: 1 + 1 + 2 = 4
492 : * - BUFFER_PRESSURE: 1 + 1 + 1 = 3
493 : * - FLAGS: 1 + 1 + 2 = 4
494 : * - RECV_BUFFER_SIZE: 1 + 1 + 2 = 4
495 : * - OPTIMAL_CHUNK: 1 + 1 + 2 = 4
496 : * Total: 23 bytes
497 : */
498 37 : if (buf_len < 23) {
499 1 : return PT_ERR_BUFFER_FULL;
500 : }
501 :
502 : /* TLV 1: Max message size (2 bytes) */
503 36 : buf[offset++] = PT_CAP_MAX_MESSAGE;
504 36 : buf[offset++] = 2;
505 36 : buf[offset++] = (caps->max_message_size >> 8) & 0xFF;
506 36 : buf[offset++] = caps->max_message_size & 0xFF;
507 :
508 : /* TLV 2: Preferred chunk (2 bytes) */
509 36 : buf[offset++] = PT_CAP_PREFERRED_CHUNK;
510 36 : buf[offset++] = 2;
511 36 : buf[offset++] = (caps->preferred_chunk >> 8) & 0xFF;
512 36 : buf[offset++] = caps->preferred_chunk & 0xFF;
513 :
514 : /* TLV 3: Buffer pressure (1 byte) */
515 36 : buf[offset++] = PT_CAP_BUFFER_PRESSURE;
516 36 : buf[offset++] = 1;
517 36 : buf[offset++] = caps->buffer_pressure;
518 :
519 : /* TLV 4: Capability flags (2 bytes) */
520 36 : buf[offset++] = PT_CAP_FLAGS;
521 36 : buf[offset++] = 2;
522 36 : buf[offset++] = (caps->capability_flags >> 8) & 0xFF;
523 36 : buf[offset++] = caps->capability_flags & 0xFF;
524 :
525 : /* TLV 5: Receive buffer size (2 bytes) */
526 36 : buf[offset++] = PT_CAP_RECV_BUFFER_SIZE;
527 36 : buf[offset++] = 2;
528 36 : buf[offset++] = (caps->recv_buffer_size >> 8) & 0xFF;
529 36 : buf[offset++] = caps->recv_buffer_size & 0xFF;
530 :
531 : /* TLV 6: Optimal chunk size (2 bytes)
532 : * This is 25% of recv_buffer - the MacTCP completion threshold.
533 : * Tells sender the ideal per-send size for this receiver. */
534 36 : buf[offset++] = PT_CAP_OPTIMAL_CHUNK;
535 36 : buf[offset++] = 2;
536 36 : buf[offset++] = (caps->optimal_chunk >> 8) & 0xFF;
537 36 : buf[offset++] = caps->optimal_chunk & 0xFF;
538 :
539 36 : return (int)offset;
540 : }
541 :
542 21 : int pt_capability_decode(struct pt_context *ctx, const uint8_t *buf, size_t len,
543 : pt_capability_msg *caps)
544 : {
545 21 : size_t offset = 0;
546 21 : int tlvs_parsed = 0;
547 :
548 : /* Initialize with defaults (for missing TLVs) */
549 21 : caps->max_message_size = PT_CAP_DEFAULT_MAX_MSG;
550 21 : caps->preferred_chunk = PT_CAP_DEFAULT_CHUNK;
551 21 : caps->buffer_pressure = PT_CAP_DEFAULT_PRESSURE;
552 21 : caps->capability_flags = 0;
553 21 : caps->recv_buffer_size = 8192; /* Default to typical TCP buffer */
554 21 : caps->optimal_chunk = 2048; /* Default: 25% of 8KB buffer */
555 21 : caps->reserved = 0;
556 :
557 : /* Log payload info for debugging capability exchange issues */
558 21 : if (ctx && len < 23) {
559 : /* Expected payload is 23 bytes (6 TLVs). Log if shorter. */
560 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
561 : "Capability payload short: len=%zu (expected 23), bytes: %02X %02X %02X %02X",
562 : len,
563 : (len > 0) ? buf[0] : 0, (len > 1) ? buf[1] : 0,
564 : (len > 2) ? buf[2] : 0, (len > 3) ? buf[3] : 0);
565 : }
566 :
567 : /* Parse TLVs */
568 147 : while (offset + 2 <= len) {
569 126 : uint8_t type = buf[offset++];
570 126 : uint8_t tlv_len = buf[offset++];
571 :
572 : /* Check TLV fits in buffer */
573 126 : if (offset + tlv_len > len) {
574 0 : if (ctx) {
575 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
576 : "Capability TLV truncated: type=%u, len=%u, remaining=%zu",
577 : type, tlv_len, len - offset);
578 : }
579 0 : return PT_ERR_TRUNCATED;
580 : }
581 :
582 126 : switch (type) {
583 21 : case PT_CAP_MAX_MESSAGE:
584 21 : if (tlv_len >= 2) {
585 21 : caps->max_message_size = ((uint16_t)buf[offset] << 8) | buf[offset + 1];
586 : /* Clamp to valid range */
587 21 : if (caps->max_message_size < PT_CAP_MIN_MAX_MSG) {
588 0 : caps->max_message_size = PT_CAP_MIN_MAX_MSG;
589 : }
590 21 : if (caps->max_message_size > PT_CAP_MAX_MAX_MSG) {
591 1 : caps->max_message_size = PT_CAP_MAX_MAX_MSG;
592 : }
593 21 : tlvs_parsed++;
594 : }
595 21 : break;
596 :
597 21 : case PT_CAP_PREFERRED_CHUNK:
598 21 : if (tlv_len >= 2) {
599 21 : caps->preferred_chunk = ((uint16_t)buf[offset] << 8) | buf[offset + 1];
600 : }
601 21 : break;
602 :
603 21 : case PT_CAP_BUFFER_PRESSURE:
604 21 : if (tlv_len >= 1) {
605 21 : caps->buffer_pressure = buf[offset];
606 21 : if (caps->buffer_pressure > 100) {
607 1 : caps->buffer_pressure = 100;
608 : }
609 : }
610 21 : break;
611 :
612 21 : case PT_CAP_FLAGS:
613 21 : if (tlv_len >= 2) {
614 21 : caps->capability_flags = ((uint16_t)buf[offset] << 8) | buf[offset + 1];
615 : }
616 21 : break;
617 :
618 21 : case PT_CAP_RECV_BUFFER_SIZE:
619 21 : if (tlv_len >= 2) {
620 21 : caps->recv_buffer_size = ((uint16_t)buf[offset] << 8) | buf[offset + 1];
621 : /* Minimum 4KB, maximum 64KB */
622 21 : if (caps->recv_buffer_size < 4096) {
623 4 : caps->recv_buffer_size = 4096;
624 : }
625 : }
626 21 : break;
627 :
628 21 : case PT_CAP_OPTIMAL_CHUNK:
629 21 : if (tlv_len >= 2) {
630 21 : caps->optimal_chunk = ((uint16_t)buf[offset] << 8) | buf[offset + 1];
631 : /* Minimum 512 bytes, maximum 16KB */
632 21 : if (caps->optimal_chunk < 512) {
633 4 : caps->optimal_chunk = 512;
634 : }
635 21 : if (caps->optimal_chunk > 16384) {
636 0 : caps->optimal_chunk = 16384;
637 : }
638 : }
639 21 : break;
640 :
641 0 : default:
642 : /* Unknown TLV - skip it (forward compatibility) */
643 0 : if (ctx) {
644 0 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
645 : "Unknown capability TLV: type=%u, len=%u", type, tlv_len);
646 : }
647 0 : break;
648 : }
649 :
650 126 : offset += tlv_len;
651 : }
652 :
653 : /* Warn if no TLVs were parsed - indicates empty or malformed payload */
654 21 : if (ctx && tlvs_parsed == 0 && len > 0) {
655 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
656 : "Capability decode: no MAX_MESSAGE TLV found in %zu bytes", len);
657 : }
658 :
659 21 : if (ctx) {
660 17 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
661 : "Capability decoded: max=%u, chunk=%u, pressure=%u, flags=0x%04X, recv_buf=%u, optimal=%u",
662 : caps->max_message_size, caps->preferred_chunk,
663 : caps->buffer_pressure, caps->capability_flags,
664 : caps->recv_buffer_size, caps->optimal_chunk);
665 : }
666 :
667 21 : return 0;
668 : }
669 :
670 : /* ========================================================================
671 : * Fragment Header Functions
672 : * ======================================================================== */
673 :
674 4 : int pt_fragment_encode(const pt_fragment_header *hdr, uint8_t *buf)
675 : {
676 : /* Message ID (big-endian) */
677 4 : buf[0] = (hdr->message_id >> 8) & 0xFF;
678 4 : buf[1] = hdr->message_id & 0xFF;
679 :
680 : /* Total length (big-endian) */
681 4 : buf[2] = (hdr->total_length >> 8) & 0xFF;
682 4 : buf[3] = hdr->total_length & 0xFF;
683 :
684 : /* Fragment offset (big-endian) */
685 4 : buf[4] = (hdr->fragment_offset >> 8) & 0xFF;
686 4 : buf[5] = hdr->fragment_offset & 0xFF;
687 :
688 : /* Flags and reserved */
689 4 : buf[6] = hdr->fragment_flags;
690 4 : buf[7] = 0; /* Reserved */
691 :
692 4 : return PT_FRAGMENT_HEADER_SIZE;
693 : }
694 :
695 5 : int pt_fragment_decode(const uint8_t *buf, size_t len, pt_fragment_header *hdr)
696 : {
697 5 : if (len < PT_FRAGMENT_HEADER_SIZE) {
698 1 : return PT_ERR_TRUNCATED;
699 : }
700 :
701 : /* Message ID (big-endian) */
702 4 : hdr->message_id = ((uint16_t)buf[0] << 8) | buf[1];
703 :
704 : /* Total length (big-endian) */
705 4 : hdr->total_length = ((uint16_t)buf[2] << 8) | buf[3];
706 :
707 : /* Fragment offset (big-endian) */
708 4 : hdr->fragment_offset = ((uint16_t)buf[4] << 8) | buf[5];
709 :
710 : /* Flags and reserved */
711 4 : hdr->fragment_flags = buf[6];
712 4 : hdr->reserved = buf[7];
713 :
714 4 : return 0;
715 : }
716 :
717 : /* ========================================================================
718 : * Fragment Reassembly
719 : *
720 : * These functions handle transparent fragment reassembly. When fragments
721 : * arrive, they're accumulated in the peer's recv_direct buffer. When the
722 : * last fragment arrives, the complete message is delivered to the app
723 : * callback. Applications never see individual fragments.
724 : *
725 : * DOD: Reassembly state is stored in pt_peer_cold (rarely accessed after
726 : * negotiation). The actual data goes in recv_direct buffer.
727 : * ======================================================================== */
728 :
729 0 : int pt_reassembly_process(struct pt_context *ctx, struct pt_peer *peer,
730 : const uint8_t *fragment_data, uint16_t fragment_len,
731 : const pt_fragment_header *frag_hdr,
732 : const uint8_t **complete_data, uint16_t *complete_len)
733 : {
734 0 : pt_reassembly_state *rs = &peer->cold.reassembly;
735 0 : pt_direct_buffer *buf = &peer->recv_direct;
736 : const uint8_t *payload;
737 : uint16_t payload_len;
738 :
739 : /* Extract fragment payload (skip fragment header in data) */
740 0 : if (fragment_len < PT_FRAGMENT_HEADER_SIZE) {
741 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
742 : "Fragment too short: %u bytes", fragment_len);
743 0 : return PT_ERR_TRUNCATED;
744 : }
745 0 : payload = fragment_data + PT_FRAGMENT_HEADER_SIZE;
746 0 : payload_len = fragment_len - PT_FRAGMENT_HEADER_SIZE;
747 :
748 : /* Validate fragment offset + length doesn't exceed total */
749 0 : if (frag_hdr->fragment_offset + payload_len > frag_hdr->total_length) {
750 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
751 : "Fragment exceeds total: offset=%u len=%u total=%u",
752 : frag_hdr->fragment_offset, payload_len, frag_hdr->total_length);
753 0 : rs->active = 0;
754 0 : return PT_ERR_INVALID_PARAM;
755 : }
756 :
757 : /* Check if this is the first fragment */
758 0 : if (frag_hdr->fragment_flags & PT_FRAGMENT_FLAG_FIRST) {
759 : /* Start new reassembly */
760 0 : if (rs->active && rs->message_id != frag_hdr->message_id) {
761 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
762 : "Dropping incomplete reassembly for msg %u (new msg %u)",
763 : rs->message_id, frag_hdr->message_id);
764 : }
765 :
766 : /* Validate total length fits in buffer */
767 0 : if (frag_hdr->total_length > buf->capacity) {
768 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
769 : "Reassembly too large: %u > %u",
770 : frag_hdr->total_length, buf->capacity);
771 0 : return PT_ERR_MESSAGE_TOO_LARGE;
772 : }
773 :
774 : /* Initialize reassembly state */
775 0 : rs->message_id = frag_hdr->message_id;
776 0 : rs->total_length = frag_hdr->total_length;
777 0 : rs->received_length = 0;
778 0 : rs->active = 1;
779 :
780 0 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
781 : "Starting reassembly: msg_id=%u total=%u",
782 : frag_hdr->message_id, frag_hdr->total_length);
783 : }
784 :
785 : /* Validate we're reassembling the right message */
786 0 : if (!rs->active) {
787 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
788 : "Fragment received without FIRST: msg_id=%u offset=%u",
789 : frag_hdr->message_id, frag_hdr->fragment_offset);
790 0 : return PT_ERR_INVALID_STATE;
791 : }
792 :
793 0 : if (rs->message_id != frag_hdr->message_id) {
794 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
795 : "Fragment msg_id mismatch: expected %u got %u",
796 : rs->message_id, frag_hdr->message_id);
797 0 : return PT_ERR_INVALID_PARAM;
798 : }
799 :
800 : /* Validate offset matches what we've received */
801 0 : if (frag_hdr->fragment_offset != rs->received_length) {
802 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
803 : "Fragment offset mismatch: expected %u got %u",
804 : rs->received_length, frag_hdr->fragment_offset);
805 : /* Could be out-of-order - for now, abort reassembly */
806 0 : rs->active = 0;
807 0 : return PT_ERR_INVALID_PARAM;
808 : }
809 :
810 : /* Copy fragment data to buffer */
811 0 : pt_memcpy(buf->data + rs->received_length, payload, payload_len);
812 0 : rs->received_length += payload_len;
813 :
814 0 : PT_CTX_DEBUG(ctx, PT_LOG_CAT_PROTOCOL,
815 : "Fragment received: msg_id=%u offset=%u len=%u (%u/%u)",
816 : frag_hdr->message_id, frag_hdr->fragment_offset, payload_len,
817 : rs->received_length, rs->total_length);
818 :
819 : /* Check if this is the last fragment */
820 0 : if (frag_hdr->fragment_flags & PT_FRAGMENT_FLAG_LAST) {
821 0 : if (rs->received_length != rs->total_length) {
822 0 : PT_CTX_WARN(ctx, PT_LOG_CAT_PROTOCOL,
823 : "LAST fragment but length mismatch: received=%u total=%u",
824 : rs->received_length, rs->total_length);
825 0 : rs->active = 0;
826 0 : return PT_ERR_TRUNCATED;
827 : }
828 :
829 : /* Reassembly complete! */
830 0 : *complete_data = buf->data;
831 0 : *complete_len = rs->total_length;
832 :
833 0 : PT_CTX_INFO(ctx, PT_LOG_CAT_PROTOCOL,
834 : "Reassembly complete: msg_id=%u total=%u bytes",
835 : rs->message_id, rs->total_length);
836 :
837 0 : rs->active = 0;
838 0 : return 1; /* Complete message ready */
839 : }
840 :
841 : /* More fragments expected */
842 0 : *complete_data = NULL;
843 0 : *complete_len = 0;
844 0 : return 0; /* Not complete yet */
845 : }
846 :
847 0 : void pt_reassembly_reset(struct pt_peer *peer)
848 : {
849 0 : peer->cold.reassembly.active = 0;
850 0 : peer->cold.reassembly.received_length = 0;
851 0 : }
|
