git.meshlink.io Git - meshlink/blob - src/mdns.c

   1 // SPDX-FileCopyrightText: 2020 Guus Sliepen <guus@meshlink.io>
   2 // SPDX-License-Identifier: GPL-2.0-or-later
   3
   4 #include "system.h"
   5
   6 #include <stddef.h>
   7
   8 #include "mdns.h"
   9 #include "xalloc.h"
  10
  11 // Creating a buffer
  12
  13 typedef struct {
  14         uint8_t *ptr;
  15         ptrdiff_t len;
  16 } buf_t;
  17
  18 static void buf_add(buf_t *buf, const void *data, uint32_t len) {
  19         if(buf->len >= len) {
  20                 memcpy(buf->ptr, data, len);
  21                 buf->ptr += len;
  22                 buf->len -= len;
  23         } else {
  24                 buf->len = -1;
  25         }
  26 }
  27
  28 static void buf_add_uint8(buf_t *buf, uint8_t val) {
  29         if(buf->len >= 1) {
  30                 buf->ptr[0] = val;
  31                 buf->ptr++;
  32                 buf->len--;
  33         } else {
  34                 buf->len = -1;
  35         }
  36 }
  37
  38 static void buf_add_uint16(buf_t *buf, uint16_t val) {
  39         uint16_t nval = htons(val);
  40         buf_add(buf, &nval, sizeof(nval));
  41 }
  42
  43 static void buf_add_uint32(buf_t *buf, uint32_t val) {
  44         uint32_t nval = htonl(val);
  45         buf_add(buf, &nval, sizeof(nval));
  46 }
  47
  48 static void buf_add_label(buf_t *buf, const char *str) {
  49         size_t len = strlen(str);
  50
  51         if(len < 256) {
  52                 buf_add_uint8(buf, len);
  53                 buf_add(buf, str, len);
  54         } else {
  55                 buf->len = -1;
  56         }
  57 }
  58
  59 static void buf_add_ulabel(buf_t *buf, const char *str) {
  60         size_t len = strlen(str);
  61
  62         if(len + 1 < 256) {
  63                 buf_add_uint8(buf, len + 1);
  64                 buf_add_uint8(buf, '_');
  65                 buf_add(buf, str, len);
  66         } else {
  67                 buf->len = -1;
  68         }
  69 }
  70
  71 static void buf_add_kvp(buf_t *buf, const char *key, const char *val) {
  72         size_t key_len = strlen(key);
  73         size_t val_len = strlen(val);
  74
  75         if(key_len + val_len + 1 < 256) {
  76                 buf_add_uint8(buf, key_len + val_len + 1);
  77                 buf_add(buf, key, key_len);
  78                 buf_add_uint8(buf, '=');
  79                 buf_add(buf, val, val_len);
  80         } else {
  81                 buf->len = -1;
  82         }
  83 }
  84
  85 static uint8_t *buf_len_start(buf_t *buf) {
  86         if(buf->len < 2) {
  87                 buf->len = -1;
  88                 return NULL;
  89         } else {
  90                 uint8_t *ptr = buf->ptr;
  91                 buf->ptr += 2;
  92                 buf->len -= 2;
  93                 return ptr;
  94         }
  95 }
  96
  97 static void buf_len_end(buf_t *buf, uint8_t *ptr) {
  98         if(buf->len < 0) {
  99                 return;
 100         }
 101
 102         uint16_t len = htons(buf->ptr - ptr - 2);
 103         memcpy(ptr, &len, sizeof(len));
 104 }
 105
 106 // Functions reading a buffer
 107
 108 typedef struct {
 109         const uint8_t *ptr;
 110         ptrdiff_t len;
 111 } cbuf_t;
 112
 113 static void buf_check(cbuf_t *buf, const void *data, uint32_t len) {
 114         if(buf->len >= len && !memcmp(buf->ptr, data, len)) {
 115                 buf->ptr += len;
 116                 buf->len -= len;
 117         } else {
 118                 buf->len = -1;
 119         }
 120 }
 121
 122 static void buf_check_uint8(cbuf_t *buf, uint8_t val) {
 123         if(buf->len >= 1 && buf->ptr[0] == val) {
 124                 buf->ptr++;
 125                 buf->len--;
 126         } else {
 127                 buf->len = -1;
 128         }
 129 }
 130
 131 static void buf_check_uint16(cbuf_t *buf, uint16_t val) {
 132         uint16_t nval = htons(val);
 133         buf_check(buf, &nval, sizeof(nval));
 134 }
 135
 136 static uint16_t buf_get_uint16(cbuf_t *buf) {
 137         uint16_t nval;
 138
 139         if(buf->len >= 2) {
 140                 memcpy(&nval, buf->ptr, 2);
 141                 buf->ptr += 2;
 142                 buf->len -= 2;
 143                 return ntohs(nval);
 144         } else {
 145                 buf->len = -1;
 146                 return 0;
 147         }
 148 }
 149
 150 static void buf_check_uint32(cbuf_t *buf, uint32_t val) {
 151         uint32_t nval = htonl(val);
 152         buf_check(buf, &nval, sizeof(nval));
 153 }
 154
 155 static void buf_check_label(cbuf_t *buf, const char *str) {
 156         size_t len = strlen(str);
 157
 158         if(len < 256) {
 159                 buf_check_uint8(buf, len);
 160                 buf_check(buf, str, len);
 161         } else {
 162                 buf->len = -1;
 163         }
 164 }
 165
 166 static char *buf_get_label(cbuf_t *buf) {
 167         if(buf->len < 1) {
 168                 buf->len = -1;
 169                 return NULL;
 170         }
 171
 172         uint8_t len = buf->ptr[0];
 173         buf->ptr++;
 174         buf->len--;
 175
 176         if(buf->len < len) {
 177                 buf->len = -1;
 178                 return NULL;
 179         }
 180
 181         char *label = xmalloc(len + 1);
 182         memcpy(label, buf->ptr, len);
 183         label[len] = 0;
 184         buf->ptr += len;
 185         buf->len -= len;
 186         return label;
 187 }
 188
 189 static void buf_check_ulabel(cbuf_t *buf, const char *str) {
 190         size_t len = strlen(str);
 191
 192         if(len + 1 < 256) {
 193                 buf_check_uint8(buf, len + 1);
 194                 buf_check_uint8(buf, '_');
 195                 buf_check(buf, str, len);
 196         } else {
 197                 buf->len = -1;
 198         }
 199 }
 200
 201 static void buf_get_kvp(cbuf_t *buf, const char *key, char **val) {
 202         char *kvp = buf_get_label(buf);
 203
 204         if(buf->len == -1) {
 205                 return;
 206         }
 207
 208         char *split = strchr(kvp, '=');
 209
 210         if(!split) {
 211                 buf->len = -1;
 212                 return;
 213         }
 214
 215         *split++ = 0;
 216
 217         if(strcmp(kvp, key)) {
 218                 buf->len = -1;
 219                 return;
 220         }
 221
 222         memmove(kvp, split, strlen(split) + 1);
 223         *val = kvp;
 224 }
 225
 226 static const uint8_t *buf_check_len_start(cbuf_t *buf) {
 227         if(buf->len < 2) {
 228                 buf->len = -1;
 229                 return NULL;
 230         } else {
 231                 const uint8_t *ptr = buf->ptr;
 232                 buf->ptr += 2;
 233                 buf->len -= 2;
 234                 return ptr;
 235         }
 236 }
 237
 238 static void buf_check_len_end(cbuf_t *buf, const uint8_t *ptr) {
 239         if(buf->len < 0) {
 240                 return;
 241         }
 242
 243         uint16_t len = htons(buf->ptr - ptr - 2);
 244
 245         if(memcmp(ptr, &len, sizeof(len))) {
 246                 buf->len = -1;
 247         }
 248 }
 249
 250 size_t prepare_request(void *vdata, size_t size, const char *protocol, const char *transport) {
 251         uint8_t *data = vdata;
 252         buf_t buf = {data, size};
 253
 254         // Header
 255         buf_add_uint16(&buf, 0); // TX ID
 256         buf_add_uint16(&buf, 0); // flags
 257         buf_add_uint16(&buf, 1); // 1 question
 258         buf_add_uint16(&buf, 0); // 0 answer RR
 259         buf_add_uint16(&buf, 0); // 0 authority RRs
 260         buf_add_uint16(&buf, 0); // 0 additional RR
 261
 262         // Question section: _protocol._transport.local PTR IN
 263         buf_add_ulabel(&buf, protocol);
 264         buf_add_ulabel(&buf, transport);
 265         buf_add_label(&buf, "local");
 266         buf_add_uint8(&buf, 0);
 267         buf_add_uint16(&buf, 0xc); // PTR
 268         buf_add_uint16(&buf, 0x1); // IN
 269
 270         // Done.
 271         if(buf.len < 0) {
 272                 return 0;
 273         } else {
 274                 return buf.ptr - data;
 275         }
 276 }
 277
 278 bool parse_request(const void *vdata, size_t size, const char *protocol, const char *transport) {
 279         const uint8_t *data = vdata;
 280         cbuf_t buf = {data, size};
 281
 282         // Header
 283         buf_get_uint16(&buf); // TX ID
 284         buf_check_uint16(&buf, 0); // flags
 285         buf_check_uint16(&buf, 1); // 1 question
 286         buf_get_uint16(&buf); // ? answer RR
 287         buf_get_uint16(&buf); // ? authority RRs
 288         buf_get_uint16(&buf); // ? additional RR
 289
 290         if(buf.len == -1) {
 291                 return false;
 292         }
 293
 294         // Question section: _protocol._transport.local PTR IN
 295         buf_check_ulabel(&buf, protocol);
 296         buf_check_ulabel(&buf, transport);
 297         buf_check_label(&buf, "local");
 298         buf_check_uint8(&buf, 0);
 299         buf_check_uint16(&buf, 0xc); // PTR
 300         buf_check_uint16(&buf, 0x1); // IN
 301
 302         if(buf.len == -1) {
 303                 return false;
 304         }
 305
 306         // Done.
 307         return buf.len != -1;
 308 }
 309
 310 size_t prepare_response(void *vdata, size_t size, const char *name, const char *protocol, const char *transport, uint16_t port, int nkeys, const char **keys, const char **values) {
 311         uint8_t *data = vdata;
 312         buf_t buf = {data, size};
 313
 314         // Header
 315         buf_add_uint16(&buf, 0); // TX ID
 316         buf_add_uint16(&buf, 0x8400); // flags
 317         buf_add_uint16(&buf, 0); // 1 question
 318         buf_add_uint16(&buf, 3); // 1 answer RR
 319         buf_add_uint16(&buf, 0); // 0 authority RRs
 320         buf_add_uint16(&buf, 0); // 1 additional RR
 321
 322         // Add the TXT record: _protocol._transport local TXT IN 3600 name._protocol._transport key=value...
 323         uint16_t full_name = buf.ptr - data; // remember start of full name
 324         buf_add_label(&buf, name);
 325         uint16_t protocol_offset = buf.ptr - data; // remember start of _protocol
 326         buf_add_ulabel(&buf, protocol);
 327         buf_add_ulabel(&buf, transport);
 328         uint16_t local_offset = buf.ptr - data; // remember start of local
 329         buf_add_label(&buf, "local");
 330         buf_add_uint8(&buf, 0);
 331         buf_add_uint16(&buf, 0x10); // TXT
 332         buf_add_uint16(&buf, 0x1); // IN
 333         buf_add_uint32(&buf, 3600); // TTL
 334
 335         uint8_t *len_ptr = buf_len_start(&buf);
 336
 337         for(int i = 0; i < nkeys; i++) {
 338                 buf_add_kvp(&buf, keys[i], values[i]);
 339         }
 340
 341         buf_len_end(&buf, len_ptr);
 342
 343         // Add the PTR record: _protocol._transport.local PTR IN 3600 name._protocol._transport.local
 344         buf_add_uint16(&buf, 0xc000 | protocol_offset);
 345         buf_add_uint16(&buf, 0xc); // PTR
 346         buf_add_uint16(&buf, 0x8001); // IN (flush)
 347         buf_add_uint32(&buf, 3600); // TTL
 348         len_ptr = buf_len_start(&buf);
 349         buf_add_uint16(&buf, 0xc000 | full_name);
 350         buf_len_end(&buf, len_ptr);
 351
 352         // Add the SRV record: name._protocol._transport.local SRV IN 120 0 0 port name.local
 353         buf_add_uint16(&buf, 0xc000 | full_name);
 354         buf_add_uint16(&buf, 0x21); // SRV
 355         buf_add_uint16(&buf, 0x8001); // IN (flush)
 356         buf_add_uint32(&buf, 120); // TTL
 357         len_ptr = buf_len_start(&buf);
 358         buf_add_uint16(&buf, 0); // priority
 359         buf_add_uint16(&buf, 0); // weight
 360         buf_add_uint16(&buf, port); // port
 361         buf_add_label(&buf, name);
 362         buf_add_uint16(&buf, 0xc000 | local_offset);
 363         buf_len_end(&buf, len_ptr);
 364
 365         // Done.
 366         if(buf.len < 0) {
 367                 return 0;
 368         } else {
 369                 return buf.ptr - data;
 370         }
 371 }
 372
 373 bool parse_response(const void *vdata, size_t size, char **name, const char *protocol, const char *transport, uint16_t *port, int nkeys, const char **keys, char **values) {
 374         const uint8_t *data = vdata;
 375         cbuf_t buf = {data, size};
 376
 377         // Header
 378         buf_check_uint16(&buf, 0); // TX ID
 379         buf_check_uint16(&buf, 0x8400); // flags
 380         buf_check_uint16(&buf, 0); // 0 question
 381         buf_check_uint16(&buf, 3); // 1 answer RR
 382         buf_check_uint16(&buf, 0); // 0 authority RRs
 383         buf_check_uint16(&buf, 0); // 0 checkitional RR
 384
 385         if(buf.len == -1) {
 386                 return false;
 387         }
 388
 389         // Check the TXT record: _protocol._transport local TXT IN 3600 name._protocol._transport key=value...
 390         uint16_t full_name = buf.ptr - data; // remember start of full name
 391         *name = buf_get_label(&buf);
 392         uint16_t protocol_offset = buf.ptr - data; // remember start of _protocol
 393         buf_check_ulabel(&buf, protocol);
 394         buf_check_ulabel(&buf, transport);
 395         uint16_t local_offset = buf.ptr - data; // remember start of local
 396         buf_check_label(&buf, "local");
 397         buf_check_uint8(&buf, 0);
 398         buf_check_uint16(&buf, 0x10); // TXT
 399         buf_check_uint16(&buf, 0x1); // IN
 400         buf_check_uint32(&buf, 3600); // TTL
 401         const uint8_t *len_ptr = buf_check_len_start(&buf);
 402
 403         for(int i = 0; i < nkeys; i++) {
 404                 buf_get_kvp(&buf, keys[i], &values[i]);
 405         }
 406
 407         buf_check_len_end(&buf, len_ptr);
 408
 409         if(buf.len == -1) {
 410                 return false;
 411         }
 412
 413         // Check the PTR record: _protocol._transport.local PTR IN 3600 name._protocol._transport.local
 414         buf_check_uint16(&buf, 0xc000 | protocol_offset);
 415         buf_check_uint16(&buf, 0xc); // PTR
 416         buf_check_uint16(&buf, 0x8001); // IN (flush)
 417         buf_check_uint32(&buf, 3600); // TTL
 418         len_ptr = buf_check_len_start(&buf);
 419         buf_check_uint16(&buf, 0xc000 | full_name);
 420         buf_check_len_end(&buf, len_ptr);
 421
 422         if(buf.len == -1) {
 423                 return false;
 424         }
 425
 426         // Check the SRV record: name._protocol._transport.local SRV IN 120 0 0 port name.local
 427         buf_check_uint16(&buf, 0xc000 | full_name);
 428         buf_check_uint16(&buf, 0x21); // SRV
 429         buf_check_uint16(&buf, 0x8001); // IN (flush)
 430         buf_check_uint32(&buf, 120); // TTL
 431         len_ptr = buf_check_len_start(&buf);
 432         buf_check_uint16(&buf, 0); // priority
 433         buf_check_uint16(&buf, 0); // weight
 434         *port = buf_get_uint16(&buf); // port
 435         buf_check_label(&buf, *name);
 436         buf_check_uint16(&buf, 0xc000 | local_offset);
 437         buf_check_len_end(&buf, len_ptr);
 438
 439         // Done.
 440         return buf.len == 0;
 441 }