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