Coverage Report

Created: 2022-04-27 14:33

/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
545k
# define READ   fuzz_read
23
546k
# 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
490
#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
1.09M
{
86
1.09M
        return (&m->u.nlmsg);
87
1.09M
}
88
89
static size_t
90
nlmsg_len(const nlmsgbuf_t *m)
91
1.63M
{
92
1.63M
        return (m->u.nlmsg.nlmsg_len);
93
1.63M
}
94
95
static uint16_t
96
nlmsg_type(const nlmsgbuf_t *m)
97
8.04k
{
98
8.04k
        return (m->u.nlmsg.nlmsg_type);
99
8.04k
}
100
101
static nlmsgbuf_t *
102
nlmsg_new(uint16_t type, uint16_t flags, size_t len)
103
555k
{
104
555k
        nlmsgbuf_t *m;
105
555k
        size_t siz;
106
107
555k
        if (len > SIZE_MAX - sizeof(*m) ||
108
555k
            (siz = sizeof(*m) + len) > UINT16_MAX ||
109
555k
            (m = calloc(1, siz)) == NULL)
110
1.40k
                return (NULL);
111
112
554k
        m->siz = siz;
113
554k
        m->len = len;
114
554k
        m->ptr = m->payload;
115
554k
        m->u.nlmsg.nlmsg_type = type;
116
554k
        m->u.nlmsg.nlmsg_flags = NLM_F_REQUEST | flags;
117
554k
        m->u.nlmsg.nlmsg_len = NLMSG_HDRLEN;
118
119
554k
        return (m);
120
555k
}
121
122
static nlamsgbuf_t *
123
nla_from_buf(const unsigned char **ptr, size_t *len)
124
18.5k
{
125
18.5k
        nlamsgbuf_t h, *a;
126
18.5k
        size_t nlalen, skip;
127
128
18.5k
        if (*len < sizeof(h.u))
129
2.45k
                return (NULL);
130
131
16.0k
        memset(&h, 0, sizeof(h));
132
16.0k
        memcpy(&h.u, *ptr, sizeof(h.u));
133
134
16.0k
        if ((nlalen = h.u.nla.nla_len) < sizeof(h.u) || nlalen > *len ||
135
16.0k
            nlalen - sizeof(h.u) > UINT16_MAX ||
136
16.0k
            nlalen > SIZE_MAX - sizeof(*a) ||
137
16.0k
            (skip = NLMSG_ALIGN(nlalen)) > *len ||
138
16.0k
            (a = calloc(1, sizeof(*a) + nlalen - sizeof(h.u))) == NULL)
139
2.00k
                return (NULL);
140
141
14.0k
        memcpy(&a->u, *ptr, nlalen);
142
14.0k
        a->siz = sizeof(*a) + nlalen - sizeof(h.u);
143
14.0k
        a->ptr = a->payload;
144
14.0k
        a->len = nlalen - sizeof(h.u);
145
14.0k
        *ptr += skip;
146
14.0k
        *len -= skip;
147
148
14.0k
        return (a);
149
16.0k
}
150
151
static nlamsgbuf_t *
152
nla_getattr(nlamsgbuf_t *a)
153
6.89k
{
154
6.89k
        return (nla_from_buf((void *)&a->ptr, &a->len));
155
6.89k
}
156
157
static uint16_t
158
nla_type(const nlamsgbuf_t *a)
159
20.1k
{
160
20.1k
        return (a->u.nla.nla_type);
161
20.1k
}
162
163
static nlamsgbuf_t *
164
nlmsg_getattr(nlmsgbuf_t *m)
165
11.6k
{
166
11.6k
        return (nla_from_buf((void *)&m->ptr, &m->len));
167
11.6k
}
168
169
static int
170
nla_read(nlamsgbuf_t *a, void *buf, size_t cnt)
171
2.51k
{
172
2.51k
        if (cnt > a->u.nla.nla_len ||
173
2.51k
            fido_buf_read((void *)&a->ptr, &a->len, buf, cnt) < 0)
174
20
                return (-1);
175
176
2.49k
        a->u.nla.nla_len = (uint16_t)(a->u.nla.nla_len - cnt);
177
178
2.49k
        return (0);
179
2.51k
}
180
181
static nlmsgbuf_t *
182
nlmsg_from_buf(const unsigned char **ptr, size_t *len)
183
5.37k
{
184
5.37k
        nlmsgbuf_t h, *m;
185
5.37k
        size_t msglen, skip;
186
187
5.37k
        if (*len < sizeof(h.u))
188
536
                return (NULL);
189
190
4.83k
        memset(&h, 0, sizeof(h));
191
4.83k
        memcpy(&h.u, *ptr, sizeof(h.u));
192
193
4.83k
        if ((msglen = h.u.nlmsg.nlmsg_len) < sizeof(h.u) || msglen > *len ||
194
4.83k
            msglen - sizeof(h.u) > UINT16_MAX ||
195
4.83k
            (skip = NLMSG_ALIGN(msglen)) > *len ||
196
4.83k
            (m = nlmsg_new(0, 0, msglen - sizeof(h.u))) == NULL)
197
391
                return (NULL);
198
199
4.44k
        memcpy(&m->u, *ptr, msglen);
200
4.44k
        *ptr += skip;
201
4.44k
        *len -= skip;
202
203
4.44k
        return (m);
204
4.83k
}
205
206
static int
207
nlmsg_read(nlmsgbuf_t *m, void *buf, size_t cnt)
208
2.97k
{
209
2.97k
        if (cnt > m->u.nlmsg.nlmsg_len ||
210
2.97k
            fido_buf_read((void *)&m->ptr, &m->len, buf, cnt) < 0)
211
211
                return (-1);
212
213
2.76k
        m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len - cnt);
214
215
2.76k
        return (0);
216
2.97k
}
217
218
static int
219
nlmsg_write(nlmsgbuf_t *m, const void *buf, size_t cnt)
220
3.83M
{
221
3.83M
        if (cnt > UINT32_MAX - m->u.nlmsg.nlmsg_len ||
222
3.83M
            fido_buf_write(&m->ptr, &m->len, buf, cnt) < 0)
223
0
                return (-1);
224
225
3.83M
        m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len + cnt);
226
227
3.83M
        return (0);
228
3.83M
}
229
230
static int
231
nlmsg_set_genl(nlmsgbuf_t *m, uint8_t cmd)
232
549k
{
233
549k
        genlmsgbuf_t g;
234
235
549k
        memset(&g, 0, sizeof(g));
236
549k
        g.u.genl.cmd = cmd;
237
549k
        g.u.genl.version = NFC_GENL_VERSION;
238
239
549k
        return (nlmsg_write(m, &g, sizeof(g)));
240
549k
}
241
242
static int
243
nlmsg_get_genl(nlmsgbuf_t *m, uint8_t cmd)
244
2.12k
{
245
2.12k
        genlmsgbuf_t g;
246
247
2.12k
        memset(&g, 0, sizeof(g));
248
249
2.12k
        if (nlmsg_read(m, &g, sizeof(g)) < 0 || g.u.genl.cmd != cmd)
250
473
                return (-1);
251
252
1.65k
        return (0);
253
2.12k
}
254
255
static int
256
nlmsg_get_status(nlmsgbuf_t *m)
257
845
{
258
845
        int status;
259
260
845
        if (nlmsg_read(m, &status, sizeof(status)) < 0 || status == INT_MIN)
261
21
                return (-1);
262
824
        if (status < 0)
263
143
                status = -status;
264
265
824
        return (status);
266
845
}
267
268
static int
269
nlmsg_setattr(nlmsgbuf_t *m, uint16_t type, const void *ptr, size_t len)
270
1.09M
{
271
1.09M
        int r;
272
1.09M
        char *padding;
273
1.09M
        size_t skip;
274
1.09M
        nlamsgbuf_t a;
275
276
1.09M
        if ((skip = NLMSG_ALIGN(len)) > UINT16_MAX - sizeof(a.u) ||
277
1.09M
            skip < len || (padding = calloc(1, skip - len)) == NULL)
278
3.05k
                return (-1);
279
280
1.09M
        memset(&a, 0, sizeof(a));
281
1.09M
        a.u.nla.nla_type = type;
282
1.09M
        a.u.nla.nla_len = (uint16_t)(len + sizeof(a.u));
283
1.09M
        r = nlmsg_write(m, &a.u, sizeof(a.u)) < 0 ||
284
1.09M
            nlmsg_write(m, ptr, len) < 0 ||
285
1.09M
            nlmsg_write(m, padding, skip - len) < 0 ? -1 : 0;
286
287
1.09M
        free(padding);
288
289
1.09M
        return (r);
290
1.09M
}
291
292
static int
293
nlmsg_set_u16(nlmsgbuf_t *m, uint16_t type, uint16_t val)
294
548k
{
295
548k
        return (nlmsg_setattr(m, type, &val, sizeof(val)));
296
548k
}
297
298
static int
299
nlmsg_set_u32(nlmsgbuf_t *m, uint16_t type, uint32_t val)
300
1.81k
{
301
1.81k
        return (nlmsg_setattr(m, type, &val, sizeof(val)));
302
1.81k
}
303
304
static int
305
nlmsg_set_str(nlmsgbuf_t *m, uint16_t type, const char *val)
306
547k
{
307
547k
        return (nlmsg_setattr(m, type, val, strlen(val) + 1));
308
547k
}
309
310
static int
311
nla_get_u16(nlamsgbuf_t *a, uint16_t *v)
312
959
{
313
959
        return (nla_read(a, v, sizeof(*v)));
314
959
}
315
316
static int
317
nla_get_u32(nlamsgbuf_t *a, uint32_t *v)
318
1.17k
{
319
1.17k
        return (nla_read(a, v, sizeof(*v)));
320
1.17k
}
321
322
static char *
323
nla_get_str(nlamsgbuf_t *a)
324
411
{
325
411
        size_t n;
326
411
        char *s = NULL;
327
328
411
        if ((n = a->len) < 1 || a->ptr[n - 1] != '\0' ||
329
411
            (s = calloc(1, n)) == NULL || nla_read(a, s, n) < 0) {
330
39
                free(s);
331
39
                return (NULL);
332
39
        }
333
372
        s[n - 1] = '\0';
334
335
372
        return (s);
336
411
}
337
338
static int
339
nlmsg_tx(int fd, const nlmsgbuf_t *m)
340
546k
{
341
546k
        ssize_t r;
342
343
546k
        if ((r = WRITE(fd, nlmsg_ptr(m), nlmsg_len(m))) == -1) {
344
1.43k
                fido_log_error(errno, "%s: write", __func__);
345
1.43k
                return (-1);
346
1.43k
        }
347
545k
        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
545k
        fido_log_xxd(nlmsg_ptr(m), nlmsg_len(m), "%s", __func__);
352
353
545k
        return (0);
354
545k
}
355
356
static ssize_t
357
nlmsg_rx(int fd, unsigned char *ptr, size_t len, int ms)
358
545k
{
359
545k
        ssize_t r;
360
361
545k
        if (len > SSIZE_MAX) {
362
0
                fido_log_debug("%s: len", __func__);
363
0
                return (-1);
364
0
        }
365
545k
        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
545k
        if ((r = READ(fd, ptr, len)) == -1) {
370
0
                fido_log_error(errno, "%s: read %zd", __func__, r);
371
0
                return (-1);
372
0
        }
373
545k
        fido_log_xxd(ptr, (size_t)r, "%s", __func__);
374
375
545k
        return (r);
376
545k
}
377
378
static int
379
nlmsg_iter(nlmsgbuf_t *m, void *arg, int (*parser)(nlamsgbuf_t *, void *))
380
1.63k
{
381
1.63k
        nlamsgbuf_t *a;
382
1.63k
        int r;
383
384
11.6k
        while ((a = nlmsg_getattr(m)) != NULL) {
385
10.1k
                r = parser(a, arg);
386
10.1k
                free(a);
387
10.1k
                if (r < 0) {
388
190
                        fido_log_debug("%s: parser", __func__);
389
190
                        return (-1);
390
190
                }
391
10.1k
        }
392
393
1.44k
        return (0);
394
1.63k
}
395
396
static int
397
nla_iter(nlamsgbuf_t *g, void *arg, int (*parser)(nlamsgbuf_t *, void *))
398
3.35k
{
399
3.35k
        nlamsgbuf_t *a;
400
3.35k
        int r;
401
402
6.89k
        while ((a = nla_getattr(g)) != NULL) {
403
3.88k
                r = parser(a, arg);
404
3.88k
                free(a);
405
3.88k
                if (r < 0) {
406
348
                        fido_log_debug("%s: parser", __func__);
407
348
                        return (-1);
408
348
                }
409
3.88k
        }
410
411
3.01k
        return (0);
412
3.35k
}
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
545k
{
418
545k
        nlmsgbuf_t *m;
419
545k
        int r;
420
421
549k
        while (blob_len) {
422
5.37k
                if ((m = nlmsg_from_buf(&blob, &blob_len)) == NULL) {
423
927
                        fido_log_debug("%s: nlmsg", __func__);
424
927
                        return (-1);
425
927
                }
426
4.44k
                if (nlmsg_type(m) == NLMSG_ERROR) {
427
845
                        r = nlmsg_get_status(m);
428
845
                        free(m);
429
845
                        return (r);
430
845
                }
431
3.60k
                if (nlmsg_type(m) != msg_type ||
432
3.60k
                    nlmsg_get_genl(m, genl_cmd) < 0) {
433
1.94k
                        fido_log_debug("%s: skipping", __func__);
434
1.94k
                        free(m);
435
1.94k
                        continue;
436
1.94k
                }
437
1.65k
                if (parser != NULL && nlmsg_iter(m, arg, parser) < 0) {
438
190
                        fido_log_debug("%s: nlmsg_iter", __func__);
439
190
                        free(m);
440
190
                        return (-1);
441
190
                }
442
1.46k
                free(m);
443
1.46k
        }
444
445
543k
        return (0);
446
545k
}
447
448
static int
449
parse_mcastgrp(nlamsgbuf_t *a, void *arg)
450
1.96k
{
451
1.96k
        nl_family_t *family = arg;
452
1.96k
        char *name;
453
454
1.96k
        switch (nla_type(a)) {
455
411
        case CTRL_ATTR_MCAST_GRP_NAME:
456
411
                if ((name = nla_get_str(a)) == NULL ||
457
411
                    strcmp(name, NFC_GENL_MCAST_EVENT_NAME) != 0) {
458
170
                        free(name);
459
170
                        return (-1); /* XXX skip? */
460
170
                }
461
241
                free(name);
462
241
                return (0);
463
1.28k
        case CTRL_ATTR_MCAST_GRP_ID:
464
1.28k
                if (family->mcastgrp)
465
409
                        break;
466
879
                if (nla_get_u32(a, &family->mcastgrp) < 0) {
467
4
                        fido_log_debug("%s: group", __func__);
468
4
                        return (-1);
469
4
                }
470
875
                return (0);
471
1.96k
        }
472
473
670
        fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
474
475
670
        return (0);
476
1.96k
}
477
478
static int
479
parse_mcastgrps(nlamsgbuf_t *a, void *arg)
480
1.92k
{
481
1.92k
        return (nla_iter(a, arg, parse_mcastgrp));
482
1.92k
}
483
484
static int
485
parse_family(nlamsgbuf_t *a, void *arg)
486
9.47k
{
487
9.47k
        nl_family_t *family = arg;
488
489
9.47k
        switch (nla_type(a)) {
490
1.51k
        case CTRL_ATTR_FAMILY_ID:
491
1.51k
                if (family->id)
492
556
                        break;
493
959
                if (nla_get_u16(a, &family->id) < 0) {
494
11
                        fido_log_debug("%s: id", __func__);
495
11
                        return (-1);
496
11
                }
497
948
                return (0);
498
1.43k
        case CTRL_ATTR_MCAST_GROUPS:
499
1.43k
                return (nla_iter(a, family, parse_mcastgrps));
500
9.47k
        }
501
502
7.08k
        fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
503
504
7.08k
        return (0);
505
9.47k
}
506
507
static int
508
nl_get_nfc_family(int fd, uint16_t *type, uint32_t *mcastgrp)
509
549k
{
510
549k
        nlmsgbuf_t *m;
511
549k
        uint8_t reply[512];
512
549k
        nl_family_t family;
513
549k
        ssize_t r;
514
549k
        int ok;
515
516
549k
        if ((m = nlmsg_new(GENL_ID_CTRL, 0, 64)) == NULL ||
517
549k
            nlmsg_set_genl(m, CTRL_CMD_GETFAMILY) < 0 ||
518
549k
            nlmsg_set_u16(m, CTRL_ATTR_FAMILY_ID, GENL_ID_CTRL) < 0 ||
519
549k
            nlmsg_set_str(m, CTRL_ATTR_FAMILY_NAME, NFC_GENL_NAME) < 0 ||
520
549k
            nlmsg_tx(fd, m) < 0) {
521
5.65k
                free(m);
522
5.65k
                return (-1);
523
5.65k
        }
524
544k
        free(m);
525
544k
        memset(&family, 0, sizeof(family));
526
544k
        if ((r = nlmsg_rx(fd, reply, sizeof(reply), -1)) < 0) {
527
0
                fido_log_debug("%s: nlmsg_rx", __func__);
528
0
                return (-1);
529
0
        }
530
544k
        if ((ok = nl_parse_reply(reply, (size_t)r, GENL_ID_CTRL,
531
544k
            CTRL_CMD_NEWFAMILY, &family, parse_family)) != 0) {
532
1.14k
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
533
1.14k
                return (-1);
534
1.14k
        }
535
543k
        if (family.id == 0 || family.mcastgrp == 0) {
536
542k
                fido_log_debug("%s: missing attr", __func__);
537
542k
                return (-1);
538
542k
        }
539
647
        *type = family.id;
540
647
        *mcastgrp = family.mcastgrp;
541
542
647
        return (0);
543
543k
}
544
545
static int
546
parse_target(nlamsgbuf_t *a, void *arg)
547
185
{
548
185
        nl_target_t *t = arg;
549
550
185
        if (t->found || nla_type(a) != NFC_ATTR_TARGET_INDEX) {
551
172
                fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
552
172
                return (0);
553
172
        }
554
13
        if (nla_get_u32(a, t->value) < 0) {
555
2
                fido_log_debug("%s: target", __func__);
556
2
                return (-1);
557
2
        }
558
11
        t->found = 1;
559
560
11
        return (0);
561
13
}
562
563
int
564
fido_nl_power_nfc(fido_nl_t *nl, uint32_t dev)
565
647
{
566
647
        nlmsgbuf_t *m;
567
647
        uint8_t reply[512];
568
647
        ssize_t r;
569
647
        int ok;
570
571
647
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||
572
647
            nlmsg_set_genl(m, NFC_CMD_DEV_UP) < 0 ||
573
647
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
574
647
            nlmsg_tx(nl->fd, m) < 0) {
575
224
                free(m);
576
224
                return (-1);
577
224
        }
578
423
        free(m);
579
423
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
580
0
                fido_log_debug("%s: nlmsg_rx", __func__);
581
0
                return (-1);
582
0
        }
583
423
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
584
423
            NFC_CMD_DEV_UP, NULL, NULL)) != 0 && ok != EALREADY) {
585
142
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
586
142
                return (-1);
587
142
        }
588
589
281
        return (0);
590
423
}
591
592
static int
593
nl_nfc_poll(fido_nl_t *nl, uint32_t dev)
594
593
{
595
593
        nlmsgbuf_t *m;
596
593
        uint8_t reply[512];
597
593
        ssize_t r;
598
593
        int ok;
599
600
593
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||
601
593
            nlmsg_set_genl(m, NFC_CMD_START_POLL) < 0 ||
602
593
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
603
593
            nlmsg_set_u32(m, NFC_ATTR_PROTOCOLS, NFC_PROTO_ISO14443_MASK) < 0 ||
604
593
            nlmsg_tx(nl->fd, m) < 0) {
605
13
                free(m);
606
13
                return (-1);
607
13
        }
608
580
        free(m);
609
580
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
610
0
                fido_log_debug("%s: nlmsg_rx", __func__);
611
0
                return (-1);
612
0
        }
613
580
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
614
580
            NFC_CMD_START_POLL, NULL, NULL)) != 0) {
615
90
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
616
90
                return (-1);
617
90
        }
618
619
490
        return (0);
620
580
}
621
622
static int
623
nl_dump_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target, int ms)
624
78
{
625
78
        nlmsgbuf_t *m;
626
78
        nl_target_t t;
627
78
        uint8_t reply[512];
628
78
        ssize_t r;
629
78
        int ok;
630
631
78
        if ((m = nlmsg_new(nl->nfc_type, NLM_F_DUMP, 64)) == NULL ||
632
78
            nlmsg_set_genl(m, NFC_CMD_GET_TARGET) < 0 ||
633
78
            nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||
634
78
            nlmsg_tx(nl->fd, m) < 0) {
635
3
                free(m);
636
3
                return (-1);
637
3
        }
638
75
        free(m);
639
75
        if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), ms)) < 0) {
640
0
                fido_log_debug("%s: nlmsg_rx", __func__);
641
0
                return (-1);
642
0
        }
643
75
        memset(&t, 0, sizeof(t));
644
75
        t.value = target;
645
75
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
646
75
            NFC_CMD_GET_TARGET, &t, parse_target)) != 0) {
647
42
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
648
42
                return (-1);
649
42
        }
650
33
        if (!t.found) {
651
23
                fido_log_debug("%s: target not found", __func__);
652
23
                return (-1);
653
23
        }
654
655
10
        return (0);
656
33
}
657
658
static int
659
parse_nfc_event(nlamsgbuf_t *a, void *arg)
660
503
{
661
503
        nl_poll_t *ctx = arg;
662
503
        uint32_t dev;
663
664
503
        if (nla_type(a) != NFC_ATTR_DEVICE_INDEX) {
665
216
                fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
666
216
                return (0);
667
216
        }
668
287
        if (nla_get_u32(a, &dev) < 0) {
669
3
                fido_log_debug("%s: dev", __func__);
670
3
                return (-1);
671
3
        }
672
284
        if (dev == ctx->dev)
673
101
                ctx->eventcnt++;
674
183
        else
675
183
                fido_log_debug("%s: ignoring dev 0x%x", __func__, dev);
676
677
284
        return (0);
678
287
}
679
680
int
681
fido_nl_get_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target)
682
593
{
683
593
        uint8_t reply[512];
684
593
        nl_poll_t ctx;
685
593
        ssize_t r;
686
593
        int ok;
687
688
593
        if (nl_nfc_poll(nl, dev) < 0) {
689
103
                fido_log_debug("%s: nl_nfc_poll", __func__);
690
103
                return (-1);
691
103
        }
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
490
        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
490
        if (r < 0) {
708
0
                fido_log_debug("%s: nlmsg_rx", __func__);
709
0
                return (-1);
710
0
        }
711
490
        memset(&ctx, 0, sizeof(ctx));
712
490
        ctx.dev = dev;
713
490
        if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,
714
490
            NFC_EVENT_TARGETS_FOUND, &ctx, parse_nfc_event)) != 0) {
715
132
                fido_log_debug("%s: nl_parse_reply: %d", __func__, ok);
716
132
                return (-1);
717
132
        }
718
358
        if (ctx.eventcnt == 0) {
719
280
                fido_log_debug("%s: dev 0x%x not observed", __func__, dev);
720
280
                return (-1);
721
280
        }
722
78
        if (nl_dump_nfc_target(nl, dev, target, -1) < 0) {
723
68
                fido_log_debug("%s: nl_dump_nfc_target", __func__);
724
68
                return (-1);
725
68
        }
726
727
10
        return (0);
728
78
}
729
730
void
731
fido_nl_free(fido_nl_t **nlp)
732
551k
{
733
551k
        fido_nl_t *nl;
734
735
551k
        if (nlp == NULL || (nl = *nlp) == NULL)
736
0
                return;
737
551k
        if (nl->fd != -1 && close(nl->fd) == -1)
738
0
                fido_log_error(errno, "%s: close", __func__);
739
740
551k
        free(nl);
741
551k
        *nlp = NULL;
742
551k
}
743
744
fido_nl_t *
745
fido_nl_new(void)
746
552k
{
747
552k
        fido_nl_t *nl;
748
552k
        int ok = -1;
749
750
552k
        if ((nl = calloc(1, sizeof(*nl))) == NULL)
751
1.39k
                return (NULL);
752
551k
        if ((nl->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC,
753
551k
            NETLINK_GENERIC)) == -1) {
754
0
                fido_log_error(errno, "%s: socket", __func__);
755
0
                goto fail;
756
0
        }
757
551k
        nl->saddr.nl_family = AF_NETLINK;
758
551k
        if (bind(nl->fd, (struct sockaddr *)&nl->saddr,
759
551k
            sizeof(nl->saddr)) == -1) {
760
1.41k
                fido_log_error(errno, "%s: bind", __func__);
761
1.41k
                goto fail;
762
1.41k
        }
763
549k
        if (nl_get_nfc_family(nl->fd, &nl->nfc_type, &nl->nfc_mcastgrp) < 0) {
764
549k
                fido_log_debug("%s: nl_get_nfc_family", __func__);
765
549k
                goto fail;
766
549k
        }
767
768
647
        ok = 0;
769
551k
fail:
770
551k
        if (ok < 0)
771
550k
                fido_nl_free(&nl);
772
773
551k
        return (nl);
774
647
}
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
2.20k
{
781
2.20k
        fuzz_read = read_f;
782
2.20k
        fuzz_write = write_f;
783
2.20k
}
784
#endif