3  * Use of this source code is governed by a BSD-style
5  * SPDX-License-Identifier: BSD-2-Clause
85 nlmsg_ptr(const nlmsgbuf_t *m)  in nlmsg_ptr()  argument
87 	return (&m->u.nlmsg);  in nlmsg_ptr()
91 nlmsg_len(const nlmsgbuf_t *m)  in nlmsg_len()  argument
93 	return (m->u.nlmsg.nlmsg_len);  in nlmsg_len()
97 nlmsg_type(const nlmsgbuf_t *m)  in nlmsg_type()  argument
99 	return (m->u.nlmsg.nlmsg_type);  in nlmsg_type()
105 	nlmsgbuf_t *m;  in nlmsg_new()  local
108 	if (len > SIZE_MAX - sizeof(*m) ||  in nlmsg_new()
109 	    (siz = sizeof(*m) + len) > UINT16_MAX ||  in nlmsg_new()
110 	    (m = calloc(1, siz)) == NULL)  in nlmsg_new()
113 	m->siz = siz;  in nlmsg_new()
114 	m->len = len;  in nlmsg_new()
115 	m->ptr = m->payload;  in nlmsg_new()
116 	m->u.nlmsg.nlmsg_type = type;  in nlmsg_new()
117 	m->u.nlmsg.nlmsg_flags = NLM_F_REQUEST | flags;  in nlmsg_new()
118 	m->u.nlmsg.nlmsg_len = NLMSG_HDRLEN;  in nlmsg_new()
120 	return (m);  in nlmsg_new()
126 	nlamsgbuf_t h, *a;  in nla_from_buf()  local
136 	    nlalen - sizeof(h.u) > UINT16_MAX ||  in nla_from_buf()
137 	    nlalen > SIZE_MAX - sizeof(*a) ||  in nla_from_buf()
139 	    (a = calloc(1, sizeof(*a) + nlalen - sizeof(h.u))) == NULL)  in nla_from_buf()
142 	memcpy(&a->u, *ptr, nlalen);  in nla_from_buf()
143 	a->siz = sizeof(*a) + nlalen - sizeof(h.u);  in nla_from_buf()
144 	a->ptr = a->payload;  in nla_from_buf()
145 	a->len = nlalen - sizeof(h.u);  in nla_from_buf()
147 	*len -= skip;  in nla_from_buf()
149 	return (a);  in nla_from_buf()
153 nla_getattr(nlamsgbuf_t *a)  in nla_getattr()  argument
155 	return (nla_from_buf((void *)&a->ptr, &a->len));  in nla_getattr()
159 nla_type(const nlamsgbuf_t *a)  in nla_type()  argument
161 	return (a->u.nla.nla_type);  in nla_type()
165 nlmsg_getattr(nlmsgbuf_t *m)  in nlmsg_getattr()  argument
167 	return (nla_from_buf((void *)&m->ptr, &m->len));  in nlmsg_getattr()
171 nla_read(nlamsgbuf_t *a, void *buf, size_t cnt)  in nla_read()  argument
173 	if (cnt > a->u.nla.nla_len ||  in nla_read()
174 	    fido_buf_read((void *)&a->ptr, &a->len, buf, cnt) < 0)  in nla_read()
175 		return (-1);  in nla_read()
177 	a->u.nla.nla_len = (uint16_t)(a->u.nla.nla_len - cnt);  in nla_read()
185 	nlmsgbuf_t h, *m;  in nlmsg_from_buf()  local
195 	    msglen - sizeof(h.u) > UINT16_MAX ||  in nlmsg_from_buf()
197 	    (m = nlmsg_new(0, 0, msglen - sizeof(h.u))) == NULL)  in nlmsg_from_buf()
200 	memcpy(&m->u, *ptr, msglen);  in nlmsg_from_buf()
202 	*len -= skip;  in nlmsg_from_buf()
204 	return (m);  in nlmsg_from_buf()
208 nlmsg_read(nlmsgbuf_t *m, void *buf, size_t cnt)  in nlmsg_read()  argument
210 	if (cnt > m->u.nlmsg.nlmsg_len ||  in nlmsg_read()
211 	    fido_buf_read((void *)&m->ptr, &m->len, buf, cnt) < 0)  in nlmsg_read()
212 		return (-1);  in nlmsg_read()
214 	m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len - cnt);  in nlmsg_read()
220 nlmsg_write(nlmsgbuf_t *m, const void *buf, size_t cnt)  in nlmsg_write()  argument
222 	if (cnt > UINT32_MAX - m->u.nlmsg.nlmsg_len ||  in nlmsg_write()
223 	    fido_buf_write(&m->ptr, &m->len, buf, cnt) < 0)  in nlmsg_write()
224 		return (-1);  in nlmsg_write()
226 	m->u.nlmsg.nlmsg_len = (uint32_t)(m->u.nlmsg.nlmsg_len + cnt);  in nlmsg_write()
232 nlmsg_set_genl(nlmsgbuf_t *m, uint8_t cmd)  in nlmsg_set_genl()  argument
240 	return (nlmsg_write(m, &g, sizeof(g)));  in nlmsg_set_genl()
244 nlmsg_get_genl(nlmsgbuf_t *m, uint8_t cmd)  in nlmsg_get_genl()  argument
250 	if (nlmsg_read(m, &g, sizeof(g)) < 0 || g.u.genl.cmd != cmd)  in nlmsg_get_genl()
251 		return (-1);  in nlmsg_get_genl()
257 nlmsg_get_status(nlmsgbuf_t *m)  in nlmsg_get_status()  argument
261 	if (nlmsg_read(m, &status, sizeof(status)) < 0 || status == INT_MIN)  in nlmsg_get_status()
262 		return (-1);  in nlmsg_get_status()
264 		status = -status;  in nlmsg_get_status()
270 nlmsg_setattr(nlmsgbuf_t *m, uint16_t type, const void *ptr, size_t len)  in nlmsg_setattr()  argument
275 	nlamsgbuf_t a;  in nlmsg_setattr()  local
277 	if ((skip = NLMSG_ALIGN(len)) > UINT16_MAX - sizeof(a.u) ||  in nlmsg_setattr()
278 	    skip < len || (padding = calloc(1, skip - len)) == NULL)  in nlmsg_setattr()
279 		return (-1);  in nlmsg_setattr()
281 	memset(&a, 0, sizeof(a));  in nlmsg_setattr()
282 	a.u.nla.nla_type = type;  in nlmsg_setattr()
283 	a.u.nla.nla_len = (uint16_t)(len + sizeof(a.u));  in nlmsg_setattr()
284 	r = nlmsg_write(m, &a.u, sizeof(a.u)) < 0 ||  in nlmsg_setattr()
285 	    nlmsg_write(m, ptr, len) < 0 ||  in nlmsg_setattr()
286 	    nlmsg_write(m, padding, skip - len) < 0 ? -1 : 0;  in nlmsg_setattr()
294 nlmsg_set_u16(nlmsgbuf_t *m, uint16_t type, uint16_t val)  in nlmsg_set_u16()  argument
296 	return (nlmsg_setattr(m, type, &val, sizeof(val)));  in nlmsg_set_u16()
300 nlmsg_set_u32(nlmsgbuf_t *m, uint16_t type, uint32_t val)  in nlmsg_set_u32()  argument
302 	return (nlmsg_setattr(m, type, &val, sizeof(val)));  in nlmsg_set_u32()
306 nlmsg_set_str(nlmsgbuf_t *m, uint16_t type, const char *val)  in nlmsg_set_str()  argument
308 	return (nlmsg_setattr(m, type, val, strlen(val) + 1));  in nlmsg_set_str()
312 nla_get_u16(nlamsgbuf_t *a, uint16_t *v)  in nla_get_u16()  argument
314 	return (nla_read(a, v, sizeof(*v)));  in nla_get_u16()
318 nla_get_u32(nlamsgbuf_t *a, uint32_t *v)  in nla_get_u32()  argument
320 	return (nla_read(a, v, sizeof(*v)));  in nla_get_u32()
324 nla_get_str(nlamsgbuf_t *a)  in nla_get_str()  argument
329 	if ((n = a->len) < 1 || a->ptr[n - 1] != '\0' ||  in nla_get_str()
330 	    (s = calloc(1, n)) == NULL || nla_read(a, s, n) < 0) {  in nla_get_str()
334 	s[n - 1] = '\0';  in nla_get_str()
340 nlmsg_tx(int fd, const nlmsgbuf_t *m)  in nlmsg_tx()  argument
344 	if ((r = WRITE(fd, nlmsg_ptr(m), nlmsg_len(m))) == -1) {  in nlmsg_tx()
346 		return (-1);  in nlmsg_tx()
348 	if (r < 0 || (size_t)r != nlmsg_len(m)) {  in nlmsg_tx()
349 		fido_log_debug("%s: %zd != %zu", __func__, r, nlmsg_len(m));  in nlmsg_tx()
350 		return (-1);  in nlmsg_tx()
352 	fido_log_xxd(nlmsg_ptr(m), nlmsg_len(m), "%s", __func__);  in nlmsg_tx()
364 		return (-1);  in nlmsg_rx()
368 		return (-1);  in nlmsg_rx()
370 	if ((r = READ(fd, ptr, len)) == -1) {  in nlmsg_rx()
372 		return (-1);  in nlmsg_rx()
380 nlmsg_iter(nlmsgbuf_t *m, void *arg, int (*parser)(nlamsgbuf_t *, void *))  in nlmsg_iter()  argument
382 	nlamsgbuf_t *a;  in nlmsg_iter()  local
385 	while ((a = nlmsg_getattr(m)) != NULL) {  in nlmsg_iter()
386 		r = parser(a, arg);  in nlmsg_iter()
387 		free(a);  in nlmsg_iter()
390 			return (-1);  in nlmsg_iter()
400 	nlamsgbuf_t *a;  in nla_iter()  local
403 	while ((a = nla_getattr(g)) != NULL) {  in nla_iter()
404 		r = parser(a, arg);  in nla_iter()
405 		free(a);  in nla_iter()
408 			return (-1);  in nla_iter()
419 	nlmsgbuf_t *m;  in nl_parse_reply()  local
423 		if ((m = nlmsg_from_buf(&blob, &blob_len)) == NULL) {  in nl_parse_reply()
425 			return (-1);  in nl_parse_reply()
427 		if (nlmsg_type(m) == NLMSG_ERROR) {  in nl_parse_reply()
428 			r = nlmsg_get_status(m);  in nl_parse_reply()
429 			free(m);  in nl_parse_reply()
432 		if (nlmsg_type(m) != msg_type ||  in nl_parse_reply()
433 		    nlmsg_get_genl(m, genl_cmd) < 0) {  in nl_parse_reply()
435 			free(m);  in nl_parse_reply()
438 		if (parser != NULL && nlmsg_iter(m, arg, parser) < 0) {  in nl_parse_reply()
440 			free(m);  in nl_parse_reply()
441 			return (-1);  in nl_parse_reply()
443 		free(m);  in nl_parse_reply()
450 parse_mcastgrp(nlamsgbuf_t *a, void *arg)  in parse_mcastgrp()  argument
455 	switch (nla_type(a)) {  in parse_mcastgrp()
457 		if ((name = nla_get_str(a)) == NULL ||  in parse_mcastgrp()
460 			return (-1); /* XXX skip? */  in parse_mcastgrp()
465 		if (family->mcastgrp)  in parse_mcastgrp()
467 		if (nla_get_u32(a, &family->mcastgrp) < 0) {  in parse_mcastgrp()
469 			return (-1);  in parse_mcastgrp()
474 	fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));  in parse_mcastgrp()
480 parse_mcastgrps(nlamsgbuf_t *a, void *arg)  in parse_mcastgrps()  argument
482 	return (nla_iter(a, arg, parse_mcastgrp));  in parse_mcastgrps()
486 parse_family(nlamsgbuf_t *a, void *arg)  in parse_family()  argument
490 	switch (nla_type(a)) {  in parse_family()
492 		if (family->id)  in parse_family()
494 		if (nla_get_u16(a, &family->id) < 0) {  in parse_family()
496 			return (-1);  in parse_family()
500 		return (nla_iter(a, family, parse_mcastgrps));  in parse_family()
503 	fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));  in parse_family()
511 	nlmsgbuf_t *m;  in nl_get_nfc_family()  local
517 	if ((m = nlmsg_new(GENL_ID_CTRL, 0, 64)) == NULL ||  in nl_get_nfc_family()
518 	    nlmsg_set_genl(m, CTRL_CMD_GETFAMILY) < 0 ||  in nl_get_nfc_family()
519 	    nlmsg_set_u16(m, CTRL_ATTR_FAMILY_ID, GENL_ID_CTRL) < 0 ||  in nl_get_nfc_family()
520 	    nlmsg_set_str(m, CTRL_ATTR_FAMILY_NAME, NFC_GENL_NAME) < 0 ||  in nl_get_nfc_family()
521 	    nlmsg_tx(fd, m) < 0) {  in nl_get_nfc_family()
522 		free(m);  in nl_get_nfc_family()
523 		return (-1);  in nl_get_nfc_family()
525 	free(m);  in nl_get_nfc_family()
527 	if ((r = nlmsg_rx(fd, reply, sizeof(reply), -1)) < 0) {  in nl_get_nfc_family()
529 		return (-1);  in nl_get_nfc_family()
534 		return (-1);  in nl_get_nfc_family()
538 		return (-1);  in nl_get_nfc_family()
547 parse_target(nlamsgbuf_t *a, void *arg)  in parse_target()  argument
551 	if (t->found || nla_type(a) != NFC_ATTR_TARGET_INDEX) {  in parse_target()
552 		fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));  in parse_target()
555 	if (nla_get_u32(a, t->value) < 0) {  in parse_target()
557 		return (-1);  in parse_target()
559 	t->found = 1;  in parse_target()
567 	nlmsgbuf_t *m;  in fido_nl_power_nfc()  local
572 	if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||  in fido_nl_power_nfc()
573 	    nlmsg_set_genl(m, NFC_CMD_DEV_UP) < 0 ||  in fido_nl_power_nfc()
574 	    nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||  in fido_nl_power_nfc()
575 	    nlmsg_tx(nl->fd, m) < 0) {  in fido_nl_power_nfc()
576 		free(m);  in fido_nl_power_nfc()
577 		return (-1);  in fido_nl_power_nfc()
579 	free(m);  in fido_nl_power_nfc()
580 	if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {  in fido_nl_power_nfc()
582 		return (-1);  in fido_nl_power_nfc()
584 	if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,  in fido_nl_power_nfc()
587 		return (-1);  in fido_nl_power_nfc()
596 	nlmsgbuf_t *m;  in nl_nfc_poll()  local
601 	if ((m = nlmsg_new(nl->nfc_type, NLM_F_ACK, 64)) == NULL ||  in nl_nfc_poll()
602 	    nlmsg_set_genl(m, NFC_CMD_START_POLL) < 0 ||  in nl_nfc_poll()
603 	    nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||  in nl_nfc_poll()
604 	    nlmsg_set_u32(m, NFC_ATTR_PROTOCOLS, NFC_PROTO_ISO14443_MASK) < 0 ||  in nl_nfc_poll()
605 	    nlmsg_tx(nl->fd, m) < 0) {  in nl_nfc_poll()
606 		free(m);  in nl_nfc_poll()
607 		return (-1);  in nl_nfc_poll()
609 	free(m);  in nl_nfc_poll()
610 	if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), -1)) < 0) {  in nl_nfc_poll()
612 		return (-1);  in nl_nfc_poll()
614 	if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,  in nl_nfc_poll()
617 		return (-1);  in nl_nfc_poll()
626 	nlmsgbuf_t *m;  in nl_dump_nfc_target()  local
632 	if ((m = nlmsg_new(nl->nfc_type, NLM_F_DUMP, 64)) == NULL ||  in nl_dump_nfc_target()
633 	    nlmsg_set_genl(m, NFC_CMD_GET_TARGET) < 0 ||  in nl_dump_nfc_target()
634 	    nlmsg_set_u32(m, NFC_ATTR_DEVICE_INDEX, dev) < 0 ||  in nl_dump_nfc_target()
635 	    nlmsg_tx(nl->fd, m) < 0) {  in nl_dump_nfc_target()
636 		free(m);  in nl_dump_nfc_target()
637 		return (-1);  in nl_dump_nfc_target()
639 	free(m);  in nl_dump_nfc_target()
640 	if ((r = nlmsg_rx(nl->fd, reply, sizeof(reply), ms)) < 0) {  in nl_dump_nfc_target()
642 		return (-1);  in nl_dump_nfc_target()
646 	if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,  in nl_dump_nfc_target()
649 		return (-1);  in nl_dump_nfc_target()
653 		return (-1);  in nl_dump_nfc_target()
660 parse_nfc_event(nlamsgbuf_t *a, void *arg)  in parse_nfc_event()  argument
665 	if (nla_type(a) != NFC_ATTR_DEVICE_INDEX) {  in parse_nfc_event()
666 		fido_log_debug("%s: ignoring nla 0x%x", __func__, nla_type(a));  in parse_nfc_event()
669 	if (nla_get_u32(a, &dev) < 0) {  in parse_nfc_event()
671 		return (-1);  in parse_nfc_event()
673 	if (dev == ctx->dev)  in parse_nfc_event()
674 		ctx->eventcnt++;  in parse_nfc_event()
691 		return (-1);  in fido_nl_get_nfc_target()
694 	if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_ADD_MEMBERSHIP,  in fido_nl_get_nfc_target()
695 	    &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) {  in fido_nl_get_nfc_target()
697 		return (-1);  in fido_nl_get_nfc_target()
700 	r = nlmsg_rx(nl->fd, reply, sizeof(reply), NETLINK_POLL_MS);  in fido_nl_get_nfc_target()
702 	if (setsockopt(nl->fd, SOL_NETLINK, NETLINK_DROP_MEMBERSHIP,  in fido_nl_get_nfc_target()
703 	    &nl->nfc_mcastgrp, sizeof(nl->nfc_mcastgrp)) == -1) {  in fido_nl_get_nfc_target()
705 		return (-1);  in fido_nl_get_nfc_target()
710 		return (-1);  in fido_nl_get_nfc_target()
714 	if ((ok = nl_parse_reply(reply, (size_t)r, nl->nfc_type,  in fido_nl_get_nfc_target()
717 		return (-1);  in fido_nl_get_nfc_target()
721 		return (-1);  in fido_nl_get_nfc_target()
723 	if (nl_dump_nfc_target(nl, dev, target, -1) < 0) {  in fido_nl_get_nfc_target()
725 		return (-1);  in fido_nl_get_nfc_target()
738 	if (nl->fd != -1 && close(nl->fd) == -1)  in fido_nl_free()
749 	int ok = -1;  in fido_nl_new()
753 	if ((nl->fd = socket(AF_NETLINK, SOCK_RAW | SOCK_CLOEXEC,  in fido_nl_new()
754 	    NETLINK_GENERIC)) == -1) {  in fido_nl_new()
758 	nl->saddr.nl_family = AF_NETLINK;  in fido_nl_new()
759 	if (bind(nl->fd, (struct sockaddr *)&nl->saddr,  in fido_nl_new()
760 	    sizeof(nl->saddr)) == -1) {  in fido_nl_new()
764 	if (nl_get_nfc_family(nl->fd, &nl->nfc_type, &nl->nfc_mcastgrp) < 0) {  in fido_nl_new()