Coverage Report

Created: 2022-07-22 12:05

/libfido2/src/netlink.c
Line
Count
Source (jump to first uncovered line)
1
/*
2
 * Copyright (c) 2020 Yubico AB. All rights reserved.
3
 * Use of this source code is governed by a BSD-style
4
 * license that can be found in the LICENSE file.
5
 */
6
7
#include <sys/socket.h>
8
9
#include <linux/genetlink.h>
10
#include <linux/netlink.h>
11
#include <linux/nfc.h>
12
13
#include <errno.h>
14
#include <limits.h>
15
16
#include "fido.h"
17
#include "netlink.h"
18
19
#ifdef FIDO_FUZZ
20
static ssize_t (*fuzz_read)(int, void *, size_t);
21
static ssize_t (*fuzz_write)(int, const void *, size_t);
22
444k
# define READ   fuzz_read
23
444k
# define WRITE  fuzz_write
24
#else
25
# define READ   read
26
# define WRITE  write
27
#endif
28
29
#ifndef SOL_NETLINK
30
#define SOL_NETLINK     270
31
#endif
32
33
451
#define NETLINK_POLL_MS 100
34
35
/* XXX avoid signed NLA_ALIGNTO */
36
#undef NLA_HDRLEN
37
#define NLA_HDRLEN      NLMSG_ALIGN(sizeof(struct nlattr))
38
39
typedef struct nlmsgbuf {
40
        size_t         siz; /* alloc size */
41
        size_t         len; /* of payload */
42
        unsigned char *ptr; /* in payload */
43
        union {
44
                struct nlmsghdr   nlmsg;
45
                char              buf[NLMSG_HDRLEN]; /* align */
46
        }              u;
47
        unsigned char  payload[];
48
} nlmsgbuf_t;
49
50
typedef struct genlmsgbuf {
51
        union {
52
                struct genlmsghdr genl;
53
                char              buf[GENL_HDRLEN];  /* align */
54
        }              u;
55
} genlmsgbuf_t;
56
57
typedef struct nlamsgbuf {
58
        size_t         siz; /* alloc size */
59
        size_t         len; /* of payload */
60
        unsigned char *ptr; /* in payload */
61
        union {
62
                struct nlattr     nla;
63
                char              buf[NLA_HDRLEN];   /* align */
64
        }              u;
65
        unsigned char  payload[];
66
} nlamsgbuf_t;
67
68
typedef struct nl_family {
69
        uint16_t id;
70
        uint32_t mcastgrp;
71
} nl_family_t;
72
73
typedef struct nl_poll {
74
        uint32_t     dev;
75
        unsigned int eventcnt;
76
} nl_poll_t;
77
78
typedef struct nl_target {
79
        int       found;
80
        uint32_t *value;
81
} nl_target_t;
82
83
static const void *
84
nlmsg_ptr(const nlmsgbuf_t *m)
85
888k
{
86
888k
        return (&m->u.nlmsg);
87
888k
}
88
89
static size_t
90
nlmsg_len(const nlmsgbuf_t *m)
91
1.33M
{
92
1.33M
        return (m->u.nlmsg.nlmsg_len);
93
1.33M
}
94
95
static uint16_t
96
nlmsg_type(const nlmsgbuf_t *m)
97
8.00k
{
98
8.00k
        return (m->u.nlmsg.nlmsg_type);
99
8.00k
}
100
101
static nlmsgbuf_t *
102
nlmsg_new(uint16_t type, uint16_t flags, size_t len)
103
453k
{
104
453k
        nlmsgbuf_t *m;
105
453k
        size_t siz;
106
107
453k
        if (len > SIZE_MAX - sizeof(*m) ||
108
453k
            (siz = sizeof(*m) + len) > UINT16_MAX ||
109
453k
            (m = calloc(1, siz)) == NULL)
110
1.17k
                return (NULL);
111
112
451k
        m->siz = siz;
113
451k
        m->len = len;
114
451k
        m->ptr = m->payload;
115
451k
        m->u.nlmsg.nlmsg_type = type;
116
451k
        m->u.nlmsg.nlmsg_flags = NLM_F_REQUEST | flags;
117
451k
        m->u.nlmsg.nlmsg_len = NLMSG_HDRLEN;
118
119
451k
        return (m);
120
453k
}
121
122
static nlamsgbuf_t *
123
nla_from_buf(const unsigned char **ptr, size_t *len)
124
17.0k
{
125
17.0k
        nlamsgbuf_t h, *a;
126
17.0k
        size_t nlalen, skip;
127
128
17.0k
        if (*len < sizeof(h.u))
129
2.34k
                return (NULL);
130
131
14.7k
        memset(&h, 0, sizeof(h));
132
14.7k
        memcpy(&h.u, *ptr, sizeof(h.u));
133
134
14.7k
        if ((nlalen = h.u.nla.nla_len) < sizeof(h.u) || nlalen > *len ||
135
14.7k
            nlalen - sizeof(h.u) > UINT16_MAX ||
136
14.7k
            nlalen > SIZE_MAX - sizeof(*a) ||
137
14.7k
            (skip = NLMSG_ALIGN(nlalen)) > *len ||
138
14.7k
            (a = calloc(1, sizeof(*a) + nlalen - sizeof(h.u))) == NULL)
139
2.04k
                return (NULL);
140
141
12.6k
        memcpy(&a->u, *ptr, nlalen);
142
12.6k
        a->siz = sizeof(*a) + nlalen - sizeof(h.u);
143
12.6k
        a->ptr = a->payload;
144
12.6k
        a->len = nlalen - sizeof(h.u);
145
12.6k
        *ptr += skip;
146
12.6k
        *len -= skip;
147
148
12.6k
        return (a);
149
14.7k
}
150
151
static nlamsgbuf_t *
152
nla_getattr(nlamsgbuf_t *a)
153
6.60k
{
154
6.60k
        return (nla_from_buf((void *)&a->ptr, &a->len));
155
6.60k
}
156
157
static uint16_t
158
nla_type(const nlamsgbuf_t *a)
159
17.9k
{
160
17.9k
        return (a->u.nla.nla_type);
161
17.9k
}
162
163
static nlamsgbuf_t *
164
nlmsg_getattr(nlmsgbuf_t *m)
165
10.4k
{
166
10.4k
        return (nla_from_buf((void *)&m->ptr, &m->len));
167
10.4k
}
168
169
static int
170
nla_read(nlamsgbuf_t *a, void *buf, size_t cnt)
171
2.22k
{
172
2.22k
        if (cnt > a->u.nla.nla_len ||
173
2.22k
            fido_buf_read((void *)&a->ptr, &a->len, buf, cnt) < 0)
174
18
                return (-1);
175
176
2.20k
        a->u.nla.nla_len = (uint16_t)(a->u.nla.nla_len - cnt);
177
178
2.20k
        return (0);
179
2.22k
}
180
181
static nlmsgbuf_t *
182
nlmsg_from_buf(const unsigned char **ptr, size_t *len)
183
5.24k
{
184
5.24k
        nlmsgbuf_t h, *m;
185
5.24k
        size_t msglen, skip;
186
187
5.24k
        if (*len < sizeof(h.u))
188
398
                return (NULL);
189
190
4.84k
        memset(&h, 0, sizeof(h));
191
4.84k
        memcpy(&h.u, *ptr, sizeof(h.u));
192
193
4.84k
        if ((msglen = h.u.nlmsg.nlmsg_len) < sizeof(h.u) || msglen > *len ||
194
4.84k
            msglen - sizeof(h.u) > UINT16_MAX ||
195
4.84k
            (skip = NLMSG_ALIGN(msglen)) > *len ||
196
4.84k
            (m = nlmsg_new(0, 0, msglen - sizeof(h.u))) == NULL)
197
415
                return (NULL);
198
199
4.43k
        memcpy(&m->u, *ptr, msglen);
200
4.43k
        *ptr += skip;
201
4.43k
        *len -= skip;
202
203
4.43k
        return (m);
204
4.84k
}
205
206
static int
207
nlmsg_read(nlmsgbuf_t *m, void *buf, size_t cnt)
208
3.02k
{
209
3.02k
        if (cnt > m->u.nlmsg.nlmsg_len ||
210
3.02k
            fido_buf_read((void *)&m->ptr, &m->len, buf, cnt) < 0)
211
379
                return (-1);
212
213
2.64k
        m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len - cnt);
214
215
2.64k
        return (0);
216
3.02k
}
217
218
static int
219
nlmsg_write(nlmsgbuf_t *m, const void *buf, size_t cnt)
220
3.11M
{
221
3.11M
        if (cnt > UINT32_MAX - m->u.nlmsg.nlmsg_len ||
222
3.11M
            fido_buf_write(&m->ptr, &m->len, buf, cnt) < 0)
223
0
                return (-1);
224
225
3.11M
        m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len + cnt);
226
227
3.11M
        return (0);
228
3.11M
}
229
230
static int
231
nlmsg_set_genl(nlmsgbuf_t *m, uint8_t cmd)
232
447k
{
233
447k
        genlmsgbuf_t g;
234
235
447k
        memset(&g, 0, sizeof(g));
236
447k
        g.u.genl.cmd = cmd;
237
447k
        g.u.genl.version = NFC_GENL_VERSION;
238
239
447k
        return (nlmsg_write(m, &g, sizeof(g)));
240
447k
}
241
242
static int
243
nlmsg_get_genl(nlmsgbuf_t *m, uint8_t cmd)
244
2.16k
{
245
2.16k
        genlmsgbuf_t g;
246
247
2.16k
        memset(&g, 0, sizeof(g));
248
249
2.16k
        if (nlmsg_read(m, &g, sizeof(g)) < 0 || g.u.genl.cmd != cmd)
250
664
                return (-1);
251
252
1.50k
        return (0);
253
2.16k
}
254
255
static int
256
nlmsg_get_status(nlmsgbuf_t *m)
257
862
{
258
862
        int status;
259
260
862
        if (nlmsg_read(m, &status, sizeof(status)) < 0 || status == INT_MIN)
261
19
                return (-1);
262
843
        if (status < 0)
263
140
                status = -status;
264
265
843
        return (status);
266
862
}
267
268
static int
269
nlmsg_setattr(nlmsgbuf_t *m, uint16_t type, const void *ptr, size_t len)
270
893k
{
271
893k
        int r;
272
893k
        char *padding;
273
893k
        size_t skip;
274
893k
        nlamsgbuf_t a;
275
276
893k
        if ((skip = NLMSG_ALIGN(len)) > UINT16_MAX - sizeof(a.u) ||
277
893k
            skip < len || (padding = calloc(1, skip - len)) == NULL)
278
2.46k
                return (-1);
279
280
890k
        memset(&a, 0, sizeof(a));
281
890k
        a.u.nla.nla_type = type;
282
890k
        a.u.nla.nla_len = (uint16_t)(len + sizeof(a.u));
283
890k
        r = nlmsg_write(m, &a.u, sizeof(a.u)) < 0 ||
284
890k
            nlmsg_write(m, ptr, len) < 0 ||
285
890k
            nlmsg_write(m, padding, skip - len) < 0 ? -1 : 0;
286
287
890k
        free(padding);
288
289
890k
        return (r);
290
893k
}
291
292
static int
293
nlmsg_set_u16(nlmsgbuf_t *m, uint16_t type, uint16_t val)
294
446k
{
295
446k
        return (nlmsg_setattr(m, type, &val, sizeof(val)));
296
446k
}
297
298
static int
299
nlmsg_set_u32(nlmsgbuf_t *m, uint16_t type, uint32_t val)
300
1.68k
{
301
1.68k
        return (nlmsg_setattr(m, type, &val, sizeof(val)));
302
1.68k
}
303
304
static int
305
nlmsg_set_str(nlmsgbuf_t *m, uint16_t type, const char *val)
306
445k
{
307
445k
        return (nlmsg_setattr(m, type, val, strlen(val) + 1));
308
445k
}
309
310
static int
311
nla_get_u16(nlamsgbuf_t *a, uint16_t *v)
312
894
{
313
894
        return (nla_read(a, v, sizeof(*v)));
314
894
}
315
316
static int
317
nla_get_u32(nlamsgbuf_t *a, uint32_t *v)
318
1.05k
{
319
1.05k
        return (nla_read(a, v, sizeof(*v)));
320
1.05k
}
321
322
static char *
323
nla_get_str(nlamsgbuf_t *a)
324
310
{
325
310
        size_t n;
326
310
        char *s = NULL;
327
328
310
        if ((n = a->len) < 1 || a->ptr[n - 1] != '\0' ||
329
310
            (s = calloc(1, n)) == NULL || nla_read(a, s, n) < 0) {
330
38
                free(s);
331
38
                return (NULL);
332
38
        }
333
272
        s[n - 1] = '\0';
334
335
272
        return (s);
336
310
}
337
338
static int
339
nlmsg_tx(int fd, const nlmsgbuf_t *m)
340
444k
{
341
444k
        ssize_t r;
342
343
444k
        if ((r = WRITE(fd, nlmsg_ptr(m), nlmsg_len(m))) == -1) {
344
1.11k
                fido_log_error(errno, "%s: write", __func__);
345
1.11k
                return (-1);
346
1.11k
        }
347
443k
        if (r < 0 || (size_t)r != nlmsg_len(m)) {
348
0
                fido_log_debug("%s: %zd != %zu", __func__, r, nlmsg_len(m));
349
0
                return (-1);
350
0
        }
351
443k
        fido_log_xxd(nlmsg_ptr(m), nlmsg_len(m), "%s", __func__);
352
353
443k
        return (0);
354
443k
}
355
356
static ssize_t
357
nlmsg_rx(int fd, unsigned char *ptr, size_t len, int ms)
358
444k
{
359
444k
        ssize_t r;
360
361
444k
        if (len > SSIZE_MAX) {
362
0
                fido_log_debug("%s: len", __func__);
363
0
                return (-1);
364
0
        }
365
444k
        if (fido_hid_unix_wait(fd, ms, NULL) < 0) {
366
0
                fido_log_debug("%s: fido_hid_unix_wait", __func__);
367
0
                return (-1);
368
0
        }
369
444k
        if ((r = READ(fd, ptr, len)) == -1) {
370
1.05k
                fido_log_error(errno, "%s: read %zd", __func__, r);
371
1.05k
                return (-1);
372
1.05k
        }
373
443k
        fido_log_xxd(ptr, (size_t)r, "%s", __func__);
374
375
443k
        return (r);
376
444k
}
377
378
static int
379
nlmsg_iter(nlmsgbuf_t *m, void *arg, int (*parser)(nlamsgbuf_t *, void *))
380
1.46k
{
381
1.46k
        nlamsgbuf_t *a;
382
1.46k
        int r;
383
384
10.4k
        while ((a = nlmsg_getattr(m)) != NULL) {
385
9.06k
                r = parser(a, arg);
386
9.06k
                free(a);
387
9.06k
                if (r < 0) {
388
91
                        fido_log_debug("%s: parser", __func__);
389
91
                        return (-1);
390
91
                }
391
9.06k
        }
392
393
1.37k
        return (0);
394
1.46k
}
395
396
static int
397
nla_iter(nlamsgbuf_t *g, void *arg, int (*parser)(nlamsgbuf_t *, void *))
398
3.16k
{
399
3.16k
        nlamsgbuf_t *a;
400
3.16k
        int r;
401
402
6.60k
        while ((a = nla_getattr(g)) != NULL) {
403
3.59k
                r = parser(a, arg);
404
3.59k
                free(a);
405
3.59k
                if (r < 0) {
406
152
                        fido_log_debug("%s: parser", __func__);
407
152
                        return (-1);
408
152
                }
409
3.59k
        }
410
411
3.01k
        return (0);
412
3.16k
}
413
414
static int
415
nl_parse_reply(const uint8_t *blob, size_t blob_len, uint16_t msg_type,
416
    uint8_t genl_cmd, void *arg, int (*parser)(nlamsgbuf_t *, void *))
417
443k
{
418
443k
        nlmsgbuf_t *m;
419
443k
        int r;
420
421
446k
        while (blob_len) {
422
5.24k
                if ((m = nlmsg_from_buf(&blob, &blob_len)) == NULL) {
423
813
                        fido_log_debug("%s: nlmsg", __func__);
424
813
                        return (-1);
425
813
                }
426
4.43k
                if (nlmsg_type(m) == NLMSG_ERROR) {
427
862
                        r = nlmsg_get_status(m);
428
862
                        free(m);
429
862
                        return (r);
430
862
                }
431
3.57k
                if (nlmsg_type(m) != msg_type ||
432
3.57k
                    nlmsg_get_genl(m, genl_cmd) < 0) {
433
2.07k
                        fido_log_debug("%s: skipping", __func__);
434
2.07k
                        free(m);
435
2.07k
                        continue;
436
2.07k
                }
437
1.50k
                if (parser != NULL && nlmsg_iter(m, arg, parser) < 0) {
438
91
                        fido_log_debug("%s: nlmsg_iter", __func__);
439
91
                        free(m);
440
91
                        return (-1);
441
91
                }
442
1.40k
                free(m);
443
1.40k
        }
444
445
441k
        return (0);
446
443k
}
447
448
static int
449
parse_mcastgrp(nlamsgbuf_t *a, void *arg)
450
1.74k
{
451
1.74k
        nl_family_t *family = arg;
452
1.74k
        char *name;
453
454
1.74k
        switch (nla_type(a)) {
455
310
        case CTRL_ATTR_MCAST_GRP_NAME:
456
310
                if ((name = nla_get_str(a)) == NULL ||
457
310
                    strcmp(name, NFC_GENL_MCAST_EVENT_NAME) != 0) {
458
73
                        free(name);
459
73
                        return (-1); /* XXX skip? */
460
73
                }
461
237
                free(name);
462
237
                return (0);
463
1.19k
        case CTRL_ATTR_MCAST_GRP_ID:
464
1.19k
                if (family->mcastgrp)
465
392
                        break;
466
799
                if (nla_get_u32(a, &family->mcastgrp) < 0) {
467
3
                        fido_log_debug("%s: group", __func__);
468
3
                        return (-1);
469
3
                }
470
796
                return (0);
471
1.74k
        }
472
473
640
        fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
474
475
640
        return (0);
476
1.74k
}
477
478
static int
479
parse_mcastgrps(nlamsgbuf_t *a, void *arg)
480
1.84k
{
481
1.84k
        return (nla_iter(a, arg, parse_mcastgrp));
482
1.84k
}
483
484
static int
485
parse_family(nlamsgbuf_t *a, void *arg)
486
8.47k
{
487
8.47k
        nl_family_t *family = arg;
488
489
8.47k
        switch (nla_type(a)) {
490
1.37k
        case CTRL_ATTR_FAMILY_ID:
491
1.37k
                if (family->id)
492
477
                        break;
493
894
                if (nla_get_u16(a, &family->id) < 0) {
494
12
                        fido_log_debug("%s: id", __func__);
495
12
                        return (-1);
496
12
                }
497
882
                return (0);
498
1.32k
        case CTRL_ATTR_MCAST_GROUPS:
499
1.32k
                return (nla_iter(a, family, parse_mcastgrps));
500
8.47k
        }
501
502
6.25k
        fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
503
504
6.25k
        return (0);
505
8.47k
}
506
507
static int
508
nl_get_nfc_family(int fd, uint16_t *type, uint32_t *mcastgrp)
509
447k
{
510
447k
        nlmsgbuf_t *m;
511
447k
        uint8_t reply[512];
512
447k
        nl_family_t family;
513
447k
        ssize_t r;
514
447k
        int ok;
515
516
447k
        if ((m = nlmsg_new(GENL_ID_CTRL, 0, 64)) == NULL ||
517
447k
            nlmsg_set_genl(m, CTRL_CMD_GETFAMILY) < 0 ||
518
447k
            nlmsg_set_u16(m, CTRL_ATTR_FAMILY_ID, GENL_ID_CTRL) < 0 ||
519
447k
            nlmsg_set_str(m, CTRL_ATTR_FAMILY_NAME, NFC_GENL_NAME) < 0 ||
520
447k
            nlmsg_tx(fd, m) < 0) {
521
4.57k
                free(m);
522
4.57k
                return (-1);
523
4.57k
        }
524
442k
        free(m);
525
442k
        memset(&family, 0, sizeof(family));
526
442k
        if ((r = nlmsg_rx(fd, reply, sizeof(reply), -1)) < 0) {
527
1.04k
                fido_log_debug("%s: nlmsg_rx", __func__);
528
1.04k
                return (-1);
529
1.04k
        }
530
441k
        if ((ok = nl_parse_reply(reply, (size_t)r, GENL_ID_CTRL,
531
441k
            CTRL_CMD_NEWFAMILY, &family, parse_family)) != 0) {
532
934
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
533
934
                return (-1);
534
934
        }
535
440k
        if (family.id == 0 || family.mcastgrp == 0) {
536
440k
                fido_log_debug("%s: missing attr", __func__);
537
440k
                return (-1);
538
440k
        }
539
617
        *type = family.id;
540
617
        *mcastgrp = family.mcastgrp;
541
542
617
        return (0);
543
440k
}
544
545
static int
546
parse_target(nlamsgbuf_t *a, void *arg)
547
168
{
548
168
        nl_target_t *t = arg;
549
550
168
        if (t->found || nla_type(a) != NFC_ATTR_TARGET_INDEX) {
551
159
                fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
552
159
                return (0);
553
159
        }
554
9
        if (nla_get_u32(a, t->value) < 0) {
555
1
                fido_log_debug("%s: target", __func__);
556
1
                return (-1);
557
1
        }
558
8
        t->found = 1;
559
560
8
        return (0);
561
9
}
562
563
int
564
fido_nl_power_nfc(fido_nl_t *nl, uint32_t dev)
565
617
{
566
617
        nlmsgbuf_t *m;
567
617
        uint8_t reply[512];
568
617
        ssize_t r;
569
617
        int ok;
570
571
617
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||
572
617
            nlmsg_set_genl(m, NFC_CMD_DEV_UP) < 0 ||
573
617
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
574
617
            nlmsg_tx(nl->fd, m) < 0) {
575
155
                free(m);
576
155
                return (-1);
577
155
        }
578
462
        free(m);
579
462
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
580
4
                fido_log_debug("%s: nlmsg_rx", __func__);
581
4
                return (-1);
582
4
        }
583
458
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
584
458
            NFC_CMD_DEV_UP, NULL, NULL)) != 0 && ok != EALREADY) {
585
151
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
586
151
                return (-1);
587
151
        }
588
589
307
        return (0);
590
458
}
591
592
static int
593
nl_nfc_poll(fido_nl_t *nl, uint32_t dev)
594
552
{
595
552
        nlmsgbuf_t *m;
596
552
        uint8_t reply[512];
597
552
        ssize_t r;
598
552
        int ok;
599
600
552
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||
601
552
            nlmsg_set_genl(m, NFC_CMD_START_POLL) < 0 ||
602
552
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
603
552
            nlmsg_set_u32(m, NFC_ATTR_PROTOCOLS, NFC_PROTO_ISO14443_MASK) < 0 ||
604
552
            nlmsg_tx(nl->fd, m) < 0) {
605
14
                free(m);
606
14
                return (-1);
607
14
        }
608
538
        free(m);
609
538
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
610
4
                fido_log_debug("%s: nlmsg_rx", __func__);
611
4
                return (-1);
612
4
        }
613
534
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
614
534
            NFC_CMD_START_POLL, NULL, NULL)) != 0) {
615
83
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
616
83
                return (-1);
617
83
        }
618
619
451
        return (0);
620
534
}
621
622
static int
623
nl_dump_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target, int ms)
624
68
{
625
68
        nlmsgbuf_t *m;
626
68
        nl_target_t t;
627
68
        uint8_t reply[512];
628
68
        ssize_t r;
629
68
        int ok;
630
631
68
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_DUMP, 64)) == NULL ||
632
68
            nlmsg_set_genl(m, NFC_CMD_GET_TARGET) < 0 ||
633
68
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
634
68
            nlmsg_tx(nl->fd, m) < 0) {
635
3
                free(m);
636
3
                return (-1);
637
3
        }
638
65
        free(m);
639
65
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), ms)) < 0) {
640
1
                fido_log_debug("%s: nlmsg_rx", __func__);
641
1
                return (-1);
642
1
        }
643
64
        memset(&t, 0, sizeof(t));
644
64
        t.value = target;
645
64
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
646
64
            NFC_CMD_GET_TARGET, &t, parse_target)) != 0) {
647
37
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
648
37
                return (-1);
649
37
        }
650
27
        if (!t.found) {
651
19
                fido_log_debug("%s: target not found", __func__);
652
19
                return (-1);
653
19
        }
654
655
8
        return (0);
656
27
}
657
658
static int
659
parse_nfc_event(nlamsgbuf_t *a, void *arg)
660
418
{
661
418
        nl_poll_t *ctx = arg;
662
418
        uint32_t dev;
663
664
418
        if (nla_type(a) != NFC_ATTR_DEVICE_INDEX) {
665
172
                fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
666
172
                return (0);
667
172
        }
668
246
        if (nla_get_u32(a, &dev) < 0) {
669
2
                fido_log_debug("%s: dev", __func__);
670
2
                return (-1);
671
2
        }
672
244
        if (dev == ctx->dev)
673
90
                ctx->eventcnt++;
674
154
        else
675
154
                fido_log_debug("%s: ignoring dev 0x%x", __func__, dev);
676
677
244
        return (0);
678
246
}
679
680
int
681
fido_nl_get_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target)
682
552
{
683
552
        uint8_t reply[512];
684
552
        nl_poll_t ctx;
685
552
        ssize_t r;
686
552
        int ok;
687
688
552
        if (nl_nfc_poll(nl, dev) < 0) {
689
101
                fido_log_debug("%s: nl_nfc_poll", __func__);
690
101
                return (-1);
691
101
        }
692
#ifndef FIDO_FUZZ
693
        if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,
694
            &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) {
695
                fido_log_error(errno, "%s: setsockopt add", __func__);
696
                return (-1);
697
        }
698
#endif
699
451
        r = nlmsg_rx(nl->fd, reply, sizeof(reply), NETLINK_POLL_MS);
700
#ifndef FIDO_FUZZ
701
        if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP,
702
            &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) {
703
                fido_log_error(errno, "%s: setsockopt drop", __func__);
704
                return (-1);
705
        }
706
#endif
707
451
        if (r < 0) {
708
2
                fido_log_debug("%s: nlmsg_rx", __func__);
709
2
                return (-1);
710
2
        }
711
449
        memset(&ctx, 0, sizeof(ctx));
712
449
        ctx.dev = dev;
713
449
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
714
449
            NFC_EVENT_TARGETS_FOUND, &ctx, parse_nfc_event)) != 0) {
715
129
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
716
129
                return (-1);
717
129
        }
718
320
        if (ctx.eventcnt == 0) {
719
252
                fido_log_debug("%s: dev 0x%x not observed", __func__, dev);
720
252
                return (-1);
721
252
        }
722
68
        if (nl_dump_nfc_target(nl, dev, target, -1) < 0) {
723
60
                fido_log_debug("%s: nl_dump_nfc_target", __func__);
724
60
                return (-1);
725
60
        }
726
727
8
        return (0);
728
68
}
729
730
void
731
fido_nl_free(fido_nl_t **nlp)
732
448k
{
733
448k
        fido_nl_t *nl;
734
735
448k
        if (nlp == NULL || (nl = *nlp) == NULL)
736
0
                return;
737
448k
        if (nl->fd != -1 && close(nl->fd) == -1)
738
0
                fido_log_error(errno, "%s: close", __func__);
739
740
448k
        free(nl);
741
448k
        *nlp = NULL;
742
448k
}
743
744
fido_nl_t *
745
fido_nl_new(void)
746
449k
{
747
449k
        fido_nl_t *nl;
748
449k
        int ok = -1;
749
750
449k
        if ((nl = calloc(1, sizeof(*nl))) == NULL)
751
1.06k
                return (NULL);
752
448k
        if ((nl->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC,
753
448k
            NETLINK_GENERIC)) == -1) {
754
0
                fido_log_error(errno, "%s: socket", __func__);
755
0
                goto fail;
756
0
        }
757
448k
        nl->saddr.nl_family = AF_NETLINK;
758
448k
        if (bind(nl->fd, (struct sockaddr *)&nl->saddr,
759
448k
            sizeof(nl->saddr)) == -1) {
760
1.05k
                fido_log_error(errno, "%s: bind", __func__);
761
1.05k
                goto fail;
762
1.05k
        }
763
447k
        if (nl_get_nfc_family(nl->fd, &nl->nfc_type, &nl->nfc_mcastgrp) < 0) {
764
446k
                fido_log_debug("%s: nl_get_nfc_family", __func__);
765
446k
                goto fail;
766
446k
        }
767
768
617
        ok = 0;
769
448k
fail:
770
448k
        if (ok < 0)
771
447k
                fido_nl_free(&nl);
772
773
448k
        return (nl);
774
617
}
775
776
#ifdef FIDO_FUZZ
777
void
778
set_netlink_io_functions(ssize_t (*read_f)(int, void *, size_t),
779
    ssize_t (*write_f)(int, const void *, size_t))
780
1.94k
{
781
1.94k
        fuzz_read = read_f;
782
1.94k
        fuzz_write = write_f;
783
1.94k
}
784
#endif