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.
7 #include <sys/socket.h>
9 #include <linux/genetlink.h>
10 #include <linux/netlink.h>
11 #include <linux/nfc.h>
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
30 #define SOL_NETLINK 270
33 /* XXX avoid signed NLA_ALIGNTO */
35 #define NLA_HDRLEN NLMSG_ALIGN(sizeof(struct nlattr))
37 typedef struct nlmsgbuf {
38 size_t siz; /* alloc size */
39 size_t len; /* of payload */
40 unsigned char *ptr; /* in payload */
42 struct nlmsghdr nlmsg;
43 char buf[NLMSG_HDRLEN]; /* align */
45 unsigned char payload[];
48 typedef struct genlmsgbuf {
50 struct genlmsghdr genl;
51 char buf[GENL_HDRLEN]; /* align */
55 typedef struct nlamsgbuf {
56 size_t siz; /* alloc size */
57 size_t len; /* of payload */
58 unsigned char *ptr; /* in payload */
61 char buf[NLA_HDRLEN]; /* align */
63 unsigned char payload[];
66 typedef struct nl_family {
71 typedef struct nl_poll {
73 unsigned int eventcnt;
76 typedef struct nl_target {
82 nlmsg_ptr(const nlmsgbuf_t *m)
88 nlmsg_len(const nlmsgbuf_t *m)
90 return (m->u.nlmsg.nlmsg_len);
94 nlmsg_type(const nlmsgbuf_t *m)
96 return (m->u.nlmsg.nlmsg_type);
100 nlmsg_new(uint16_t type, uint16_t flags, size_t len)
105 if (len > SIZE_MAX - sizeof(*m) ||
106 (siz = sizeof(*m) + len) > UINT16_MAX ||
107 (m = calloc(1, siz)) == NULL)
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;
121 nla_from_buf(const unsigned char **ptr, size_t *len)
126 if (*len < sizeof(h.u))
129 memset(&h, 0, sizeof(h));
130 memcpy(&h.u, *ptr, sizeof(h.u));
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)
139 memcpy(&a->u, *ptr, nlalen);
140 a->siz = sizeof(*a) + nlalen - sizeof(h.u);
142 a->len = nlalen - sizeof(h.u);
150 nla_getattr(nlamsgbuf_t *a)
152 return (nla_from_buf((void *)&a->ptr, &a->len));
156 nla_type(const nlamsgbuf_t *a)
158 return (a->u.nla.nla_type);
162 nlmsg_getattr(nlmsgbuf_t *m)
164 return (nla_from_buf((void *)&m->ptr, &m->len));
168 nla_read(nlamsgbuf_t *a, void *buf, size_t cnt)
170 if (cnt > a->u.nla.nla_len ||
171 fido_buf_read((void *)&a->ptr, &a->len, buf, cnt) < 0)
174 a->u.nla.nla_len = (uint16_t)(a->u.nla.nla_len - cnt);
180 nlmsg_from_buf(const unsigned char **ptr, size_t *len)
185 if (*len < sizeof(h.u))
188 memset(&h, 0, sizeof(h));
189 memcpy(&h.u, *ptr, sizeof(h.u));
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)
197 memcpy(&m->u, *ptr, msglen);
205 nlmsg_read(nlmsgbuf_t *m, void *buf, size_t cnt)
207 if (cnt > m->u.nlmsg.nlmsg_len ||
208 fido_buf_read((void *)&m->ptr, &m->len, buf, cnt) < 0)
211 m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len - cnt);
217 nlmsg_write(nlmsgbuf_t *m, const void *buf, size_t cnt)
219 if (cnt > UINT32_MAX - m->u.nlmsg.nlmsg_len ||
220 fido_buf_write(&m->ptr, &m->len, buf, cnt) < 0)
223 m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len + cnt);
229 nlmsg_set_genl(nlmsgbuf_t *m, uint8_t cmd)
233 memset(&g, 0, sizeof(g));
235 g.u.genl.version = NFC_GENL_VERSION;
237 return (nlmsg_write(m, &g, sizeof(g)));
241 nlmsg_get_genl(nlmsgbuf_t *m, uint8_t cmd)
245 memset(&g, 0, sizeof(g));
247 if (nlmsg_read(m, &g, sizeof(g)) < 0 || g.u.genl.cmd != cmd)
254 nlmsg_get_status(nlmsgbuf_t *m)
258 if (nlmsg_read(m, &status, sizeof(status)) < 0 || status == INT_MIN)
267 nlmsg_setattr(nlmsgbuf_t *m, uint16_t type, const void *ptr, size_t len)
274 if ((skip = NLMSG_ALIGN(len)) > UINT16_MAX - sizeof(a.u) ||
275 skip < len || (padding = calloc(1, skip - len)) == NULL)
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;
291 nlmsg_set_u16(nlmsgbuf_t *m, uint16_t type, uint16_t val)
293 return (nlmsg_setattr(m, type, &val, sizeof(val)));
297 nlmsg_set_u32(nlmsgbuf_t *m, uint16_t type, uint32_t val)
299 return (nlmsg_setattr(m, type, &val, sizeof(val)));
303 nlmsg_set_str(nlmsgbuf_t *m, uint16_t type, const char *val)
305 return (nlmsg_setattr(m, type, val, strlen(val) + 1));
309 nla_get_u16(nlamsgbuf_t *a, uint16_t *v)
311 return (nla_read(a, v, sizeof(*v)));
315 nla_get_u32(nlamsgbuf_t *a, uint32_t *v)
317 return (nla_read(a, v, sizeof(*v)));
321 nla_get_str(nlamsgbuf_t *a)
326 if ((n = a->len) < 1 || a->ptr[n - 1] != '\0' ||
327 (s = calloc(1, n)) == NULL || nla_read(a, s, n) < 0) {
337 nlmsg_tx(int fd, const nlmsgbuf_t *m)
341 if ((r = WRITE(fd, nlmsg_ptr(m), nlmsg_len(m))) == -1) {
342 fido_log_error(errno, "%s: write", __func__);
345 if (r < 0 || (size_t)r != nlmsg_len(m)) {
346 fido_log_debug("%s: %zd != %zu", __func__, r, nlmsg_len(m));
349 fido_log_xxd(nlmsg_ptr(m), nlmsg_len(m), "%s", __func__);
355 nlmsg_rx(int fd, unsigned char *ptr, size_t len, int ms)
359 if (len > SSIZE_MAX) {
360 fido_log_debug("%s: len", __func__);
363 if (fido_hid_unix_wait(fd, ms, NULL) < 0) {
364 fido_log_debug("%s: fido_hid_unix_wait", __func__);
367 if ((r = READ(fd, ptr, len)) == -1) {
368 fido_log_error(errno, "%s: read %zd", __func__, r);
371 fido_log_xxd(ptr, (size_t)r, "%s", __func__);
377 nlmsg_iter(nlmsgbuf_t *m, void *arg, int (*parser)(nlamsgbuf_t *, void *))
382 while ((a = nlmsg_getattr(m)) != NULL) {
386 fido_log_debug("%s: parser", __func__);
395 nla_iter(nlamsgbuf_t *g, void *arg, int (*parser)(nlamsgbuf_t *, void *))
400 while ((a = nla_getattr(g)) != NULL) {
404 fido_log_debug("%s: parser", __func__);
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 *))
420 if ((m = nlmsg_from_buf(&blob, &blob_len)) == NULL) {
421 fido_log_debug("%s: nlmsg", __func__);
424 if (nlmsg_type(m) == NLMSG_ERROR) {
425 r = nlmsg_get_status(m);
429 if (nlmsg_type(m) != msg_type ||
430 nlmsg_get_genl(m, genl_cmd) < 0) {
431 fido_log_debug("%s: skipping", __func__);
435 if (parser != NULL && nlmsg_iter(m, arg, parser) < 0) {
436 fido_log_debug("%s: nlmsg_iter", __func__);
447 parse_mcastgrp(nlamsgbuf_t *a, void *arg)
449 nl_family_t *family = arg;
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) {
457 return (-1); /* XXX skip? */
461 case CTRL_ATTR_MCAST_GRP_ID:
462 if (family->mcastgrp)
464 if (nla_get_u32(a, &family->mcastgrp) < 0) {
465 fido_log_debug("%s: group", __func__);
471 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
477 parse_mcastgrps(nlamsgbuf_t *a, void *arg)
479 return (nla_iter(a, arg, parse_mcastgrp));
483 parse_family(nlamsgbuf_t *a, void *arg)
485 nl_family_t *family = arg;
487 switch (nla_type(a)) {
488 case CTRL_ATTR_FAMILY_ID:
491 if (nla_get_u16(a, &family->id) < 0) {
492 fido_log_debug("%s: id", __func__);
496 case CTRL_ATTR_MCAST_GROUPS:
497 return (nla_iter(a, family, parse_mcastgrps));
500 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
506 nl_get_nfc_family(int fd, uint16_t *type, uint32_t *mcastgrp)
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) {
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__);
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);
533 if (family.id == 0 || family.mcastgrp == 0) {
534 fido_log_debug("%s: missing attr", __func__);
538 *mcastgrp = family.mcastgrp;
544 parse_target(nlamsgbuf_t *a, void *arg)
546 nl_target_t *t = arg;
548 if (t->found || nla_type(a) != NFC_ATTR_TARGET_INDEX) {
549 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
552 if (nla_get_u32(a, t->value) < 0) {
553 fido_log_debug("%s: target", __func__);
562 fido_nl_power_nfc(fido_nl_t *nl, uint32_t dev)
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) {
577 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
578 fido_log_debug("%s: nlmsg_rx", __func__);
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);
591 nl_nfc_poll(fido_nl_t *nl, uint32_t dev)
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) {
607 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {
608 fido_log_debug("%s: nlmsg_rx", __func__);
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);
621 nl_dump_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target, int ms)
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) {
637 if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), ms)) < 0) {
638 fido_log_debug("%s: nlmsg_rx", __func__);
641 memset(&t, 0, sizeof(t));
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);
649 fido_log_debug("%s: target not found", __func__);
657 parse_nfc_event(nlamsgbuf_t *a, void *arg)
659 nl_poll_t *ctx = arg;
662 if (nla_type(a) != NFC_ATTR_DEVICE_INDEX) {
663 fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));
666 if (nla_get_u32(a, &dev) < 0) {
667 fido_log_debug("%s: dev", __func__);
673 fido_log_debug("%s: ignoring dev 0x%x", __func__, dev);
679 fido_nl_get_nfc_target(fido_nl_t *nl, uint32_t dev, uint32_t *target)
686 if (nl_nfc_poll(nl, dev) < 0) {
687 fido_log_debug("%s: nl_nfc_poll", __func__);
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__);
697 r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1);
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__);
706 fido_log_debug("%s: nlmsg_rx", __func__);
709 memset(&ctx, 0, sizeof(ctx));
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);
716 if (ctx.eventcnt == 0) {
717 fido_log_debug("%s: dev 0x%x not observed", __func__, dev);
720 if (nl_dump_nfc_target(nl, dev, target, -1) < 0) {
721 fido_log_debug("%s: nl_dump_nfc_target", __func__);
729 fido_nl_free(fido_nl_t **nlp)
733 if (nlp == NULL || (nl = *nlp) == NULL)
735 if (nl->fd != -1 && close(nl->fd) == -1)
736 fido_log_error(errno, "%s: close", __func__);
748 if ((nl = calloc(1, sizeof(*nl))) == NULL)
750 if ((nl->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC,
751 NETLINK_GENERIC)) == -1) {
752 fido_log_error(errno, "%s: socket", __func__);
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__);
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__);
776 set_netlink_io_functions(ssize_t (*read_f)(int, void *, size_t),
777 ssize_t (*write_f)(int, const void *, size_t))
780 fuzz_write = write_f;