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