Coverage Report

Created: 2021-07-20 18:14

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