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