Coverage Report

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