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