Coverage Report

Created: 2021-10-21 13:35

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