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