]>
Commit | Line | Data |
---|---|---|
b1c097af YW |
1 | /* SPDX-License-Identifier: LGPL-2.1+ */ |
2 | ||
3 | #include <errno.h> | |
4 | #include <linux/filter.h> | |
5 | #include <linux/netlink.h> | |
6 | #include <sys/socket.h> | |
7 | ||
8 | #include "sd-device.h" | |
9 | #include "sd-event.h" | |
10 | ||
11 | #include "MurmurHash2.h" | |
12 | #include "alloc-util.h" | |
13 | #include "device-monitor-private.h" | |
14 | #include "device-private.h" | |
15 | #include "device-util.h" | |
16 | #include "fd-util.h" | |
17 | #include "format-util.h" | |
18 | #include "hashmap.h" | |
19 | #include "missing.h" | |
20 | #include "mount-util.h" | |
21 | #include "set.h" | |
22 | #include "socket-util.h" | |
23 | #include "string-util.h" | |
24 | #include "strv.h" | |
25 | ||
26 | struct sd_device_monitor { | |
27 | unsigned n_ref; | |
28 | ||
29 | int sock; | |
30 | union sockaddr_union snl; | |
31 | union sockaddr_union snl_trusted_sender; | |
32 | bool bound; | |
33 | ||
34 | Hashmap *subsystem_filter; | |
35 | Set *tag_filter; | |
36 | bool filter_uptodate; | |
37 | ||
38 | sd_event *event; | |
39 | sd_event_source *event_source; | |
40 | int64_t event_priority; | |
41 | sd_device_monitor_handler_t callback; | |
42 | void *userdata; | |
43 | }; | |
44 | ||
45 | #define UDEV_MONITOR_MAGIC 0xfeedcafe | |
46 | ||
47 | typedef struct monitor_netlink_header { | |
48 | /* "libudev" prefix to distinguish libudev and kernel messages */ | |
49 | char prefix[8]; | |
50 | /* Magic to protect against daemon <-> Library message format mismatch | |
51 | * Used in the kernel from socket filter rules; needs to be stored in network order */ | |
52 | unsigned magic; | |
53 | /* Total length of header structure known to the sender */ | |
54 | unsigned header_size; | |
55 | /* Properties string buffer */ | |
56 | unsigned properties_off; | |
57 | unsigned properties_len; | |
58 | /* Hashes of primary device properties strings, to let libudev subscribers | |
59 | * use in-kernel socket filters; values need to be stored in network order */ | |
60 | unsigned filter_subsystem_hash; | |
61 | unsigned filter_devtype_hash; | |
62 | unsigned filter_tag_bloom_hi; | |
63 | unsigned filter_tag_bloom_lo; | |
64 | } monitor_netlink_header; | |
65 | ||
66 | static int monitor_set_nl_address(sd_device_monitor *m) { | |
67 | union sockaddr_union snl; | |
68 | socklen_t addrlen; | |
69 | ||
70 | assert(m); | |
71 | ||
72 | /* Get the address the kernel has assigned us. | |
73 | * It is usually, but not necessarily the pid. */ | |
74 | addrlen = sizeof(struct sockaddr_nl); | |
75 | if (getsockname(m->sock, &snl.sa, &addrlen) < 0) | |
76 | return -errno; | |
77 | ||
78 | m->snl.nl.nl_pid = snl.nl.nl_pid; | |
79 | return 0; | |
80 | } | |
81 | ||
82 | int device_monitor_allow_unicast_sender(sd_device_monitor *m, sd_device_monitor *sender) { | |
83 | assert_return(m, -EINVAL); | |
84 | assert_return(sender, -EINVAL); | |
85 | ||
86 | m->snl_trusted_sender.nl.nl_pid = sender->snl.nl.nl_pid; | |
87 | return 0; | |
88 | } | |
89 | ||
90 | _public_ int sd_device_monitor_set_receive_buffer_size(sd_device_monitor *m, size_t size) { | |
9e5b6496 | 91 | int r, n = (int) size; |
b1c097af YW |
92 | |
93 | assert_return(m, -EINVAL); | |
dcdc2f61 | 94 | assert_return((size_t) n == size, -EINVAL); |
b1c097af | 95 | |
9e5b6496 YW |
96 | if (setsockopt_int(m->sock, SOL_SOCKET, SO_RCVBUF, n) < 0) { |
97 | r = setsockopt_int(m->sock, SOL_SOCKET, SO_RCVBUFFORCE, n); | |
98 | if (r < 0) | |
99 | return r; | |
100 | } | |
b1c097af YW |
101 | |
102 | return 0; | |
103 | } | |
104 | ||
105 | int device_monitor_disconnect(sd_device_monitor *m) { | |
106 | assert(m); | |
107 | ||
108 | m->sock = safe_close(m->sock); | |
109 | return 0; | |
110 | } | |
111 | ||
112 | int device_monitor_get_fd(sd_device_monitor *m) { | |
113 | assert_return(m, -EINVAL); | |
114 | ||
115 | return m->sock; | |
116 | } | |
117 | ||
118 | int device_monitor_new_full(sd_device_monitor **ret, MonitorNetlinkGroup group, int fd) { | |
119 | _cleanup_(sd_device_monitor_unrefp) sd_device_monitor *m = NULL; | |
120 | _cleanup_close_ int sock = -1; | |
121 | int r; | |
122 | ||
123 | assert_return(ret, -EINVAL); | |
124 | assert_return(group >= 0 && group < _MONITOR_NETLINK_GROUP_MAX, -EINVAL); | |
125 | ||
126 | if (group == MONITOR_GROUP_UDEV && | |
127 | access("/run/udev/control", F_OK) < 0 && | |
128 | dev_is_devtmpfs() <= 0) { | |
129 | ||
130 | /* | |
131 | * We do not support subscribing to uevents if no instance of | |
132 | * udev is running. Uevents would otherwise broadcast the | |
133 | * processing data of the host into containers, which is not | |
134 | * desired. | |
135 | * | |
136 | * Containers will currently not get any udev uevents, until | |
137 | * a supporting infrastructure is available. | |
138 | * | |
139 | * We do not set a netlink multicast group here, so the socket | |
140 | * will not receive any messages. | |
141 | */ | |
142 | ||
c7d54dae | 143 | log_debug("sd-device-monitor: The udev service seems not to be active, disabling the monitor"); |
b1c097af YW |
144 | group = MONITOR_GROUP_NONE; |
145 | } | |
146 | ||
147 | if (fd < 0) { | |
148 | sock = socket(PF_NETLINK, SOCK_RAW|SOCK_CLOEXEC|SOCK_NONBLOCK, NETLINK_KOBJECT_UEVENT); | |
149 | if (sock < 0) | |
c7d54dae | 150 | return log_debug_errno(errno, "sd-device-monitor: Failed to create socket: %m"); |
b1c097af YW |
151 | } |
152 | ||
153 | m = new(sd_device_monitor, 1); | |
154 | if (!m) | |
155 | return -ENOMEM; | |
156 | ||
157 | *m = (sd_device_monitor) { | |
158 | .n_ref = 1, | |
159 | .sock = fd >= 0 ? fd : TAKE_FD(sock), | |
160 | .bound = fd >= 0, | |
161 | .snl.nl.nl_family = AF_NETLINK, | |
162 | .snl.nl.nl_groups = group, | |
163 | }; | |
164 | ||
165 | if (fd >= 0) { | |
166 | r = monitor_set_nl_address(m); | |
167 | if (r < 0) | |
c7d54dae | 168 | return log_debug_errno(r, "sd-device-monitor: Failed to set netlink address: %m"); |
b1c097af YW |
169 | } |
170 | ||
171 | *ret = TAKE_PTR(m); | |
172 | return 0; | |
173 | } | |
174 | ||
175 | _public_ int sd_device_monitor_new(sd_device_monitor **ret) { | |
176 | return device_monitor_new_full(ret, MONITOR_GROUP_UDEV, -1); | |
177 | } | |
178 | ||
179 | _public_ int sd_device_monitor_stop(sd_device_monitor *m) { | |
180 | assert_return(m, -EINVAL); | |
181 | ||
182 | m->event_source = sd_event_source_unref(m->event_source); | |
183 | (void) device_monitor_disconnect(m); | |
184 | ||
185 | return 0; | |
186 | } | |
187 | ||
188 | static int device_monitor_event_handler(sd_event_source *s, int fd, uint32_t revents, void *userdata) { | |
189 | _cleanup_(sd_device_unrefp) sd_device *device = NULL; | |
190 | sd_device_monitor *m = userdata; | |
191 | ||
192 | assert(m); | |
193 | ||
194 | if (device_monitor_receive_device(m, &device) <= 0) | |
195 | return 0; | |
196 | ||
197 | if (m->callback) | |
198 | return m->callback(m, device, m->userdata); | |
199 | ||
200 | return 0; | |
201 | } | |
202 | ||
203 | _public_ int sd_device_monitor_start(sd_device_monitor *m, sd_device_monitor_handler_t callback, void *userdata, const char *description) { | |
204 | _cleanup_(sd_event_source_unrefp) sd_event_source *s = NULL; | |
205 | int r; | |
206 | ||
207 | assert_return(m, -EINVAL); | |
208 | ||
209 | if (!m->event) { | |
210 | r = sd_device_monitor_attach_event(m, NULL, 0); | |
211 | if (r < 0) | |
212 | return r; | |
213 | } | |
214 | ||
215 | r = device_monitor_enable_receiving(m); | |
216 | if (r < 0) | |
217 | return r; | |
218 | ||
219 | m->callback = callback; | |
220 | m->userdata = userdata; | |
221 | ||
222 | r = sd_event_add_io(m->event, &s, m->sock, EPOLLIN, device_monitor_event_handler, m); | |
223 | if (r < 0) | |
224 | return r; | |
225 | ||
226 | r = sd_event_source_set_priority(s, m->event_priority); | |
227 | if (r < 0) | |
228 | return r; | |
229 | ||
230 | if (description) { | |
231 | r = sd_event_source_set_description(s, description); | |
232 | if (r < 0) | |
233 | return r; | |
234 | } | |
235 | ||
236 | m->event_source = TAKE_PTR(s); | |
237 | ||
238 | return 0; | |
239 | } | |
240 | ||
241 | _public_ int sd_device_monitor_detach_event(sd_device_monitor *m) { | |
242 | assert_return(m, -EINVAL); | |
243 | ||
244 | (void) sd_device_monitor_stop(m); | |
245 | m->event = sd_event_unref(m->event); | |
246 | ||
247 | return 0; | |
248 | } | |
249 | ||
250 | _public_ int sd_device_monitor_attach_event(sd_device_monitor *m, sd_event *event, int64_t priority) { | |
251 | int r; | |
252 | ||
253 | assert_return(m, -EINVAL); | |
254 | assert_return(!m->event, -EBUSY); | |
255 | ||
256 | if (event) | |
257 | m->event = sd_event_ref(event); | |
258 | else { | |
259 | r = sd_event_default(&m->event); | |
260 | if (r < 0) | |
261 | return 0; | |
262 | } | |
263 | ||
264 | m->event_priority = priority; | |
265 | ||
266 | return 0; | |
267 | } | |
268 | ||
269 | _public_ sd_event *sd_device_monitor_get_event(sd_device_monitor *m) { | |
270 | assert_return(m, NULL); | |
271 | ||
272 | return m->event; | |
273 | } | |
274 | ||
275 | int device_monitor_enable_receiving(sd_device_monitor *m) { | |
276 | int r; | |
277 | ||
278 | assert_return(m, -EINVAL); | |
279 | ||
280 | if (!m->filter_uptodate) { | |
281 | r = sd_device_monitor_filter_update(m); | |
282 | if (r < 0) | |
c7d54dae | 283 | return log_debug_errno(r, "sd-device-monitor: Failed to update filter: %m"); |
b1c097af YW |
284 | } |
285 | ||
286 | if (!m->bound) { | |
287 | if (bind(m->sock, &m->snl.sa, sizeof(struct sockaddr_nl)) < 0) | |
c7d54dae | 288 | return log_debug_errno(errno, "sd-device-monitor: Failed to bind monitoring socket to event source: %m"); |
b1c097af YW |
289 | |
290 | m->bound = true; | |
291 | } | |
292 | ||
293 | r = monitor_set_nl_address(m); | |
294 | if (r < 0) | |
c7d54dae | 295 | return log_debug_errno(r, "sd-device-monitor: Failed to set address: %m"); |
b1c097af YW |
296 | |
297 | /* enable receiving of sender credentials */ | |
2ff48e98 LP |
298 | r = setsockopt_int(m->sock, SOL_SOCKET, SO_PASSCRED, true); |
299 | if (r < 0) | |
c7d54dae | 300 | return log_debug_errno(r, "sd-device-monitor: Failed to set socket option SO_PASSCRED: %m"); |
b1c097af YW |
301 | |
302 | return 0; | |
303 | } | |
304 | ||
305 | static sd_device_monitor *device_monitor_free(sd_device_monitor *m) { | |
306 | assert(m); | |
307 | ||
308 | (void) sd_device_monitor_detach_event(m); | |
309 | ||
310 | hashmap_free_free_free(m->subsystem_filter); | |
311 | set_free_free(m->tag_filter); | |
312 | ||
313 | return mfree(m); | |
314 | } | |
315 | ||
316 | DEFINE_PUBLIC_TRIVIAL_REF_UNREF_FUNC(sd_device_monitor, sd_device_monitor, device_monitor_free); | |
317 | ||
318 | static int passes_filter(sd_device_monitor *m, sd_device *device) { | |
319 | const char *tag, *subsystem, *devtype, *s, *d = NULL; | |
320 | Iterator i; | |
321 | int r; | |
322 | ||
323 | assert_return(m, -EINVAL); | |
324 | assert_return(device, -EINVAL); | |
325 | ||
326 | if (hashmap_isempty(m->subsystem_filter)) | |
327 | goto tag; | |
328 | ||
329 | r = sd_device_get_subsystem(device, &s); | |
330 | if (r < 0) | |
331 | return r; | |
332 | ||
333 | r = sd_device_get_devtype(device, &d); | |
334 | if (r < 0 && r != -ENOENT) | |
335 | return r; | |
336 | ||
337 | HASHMAP_FOREACH_KEY(devtype, subsystem, m->subsystem_filter, i) { | |
338 | if (!streq(s, subsystem)) | |
339 | continue; | |
340 | ||
341 | if (!devtype) | |
342 | goto tag; | |
343 | ||
344 | if (!d) | |
345 | continue; | |
346 | ||
347 | if (streq(d, devtype)) | |
348 | goto tag; | |
349 | } | |
350 | ||
351 | return 0; | |
352 | ||
353 | tag: | |
354 | if (set_isempty(m->tag_filter)) | |
355 | return 1; | |
356 | ||
357 | SET_FOREACH(tag, m->tag_filter, i) | |
358 | if (sd_device_has_tag(device, tag) > 0) | |
359 | return 1; | |
360 | ||
361 | return 0; | |
362 | } | |
363 | ||
364 | int device_monitor_receive_device(sd_device_monitor *m, sd_device **ret) { | |
365 | _cleanup_(sd_device_unrefp) sd_device *device = NULL; | |
366 | union { | |
367 | monitor_netlink_header nlh; | |
368 | char raw[8192]; | |
369 | } buf; | |
370 | struct iovec iov = { | |
371 | .iov_base = &buf, | |
372 | .iov_len = sizeof(buf) | |
373 | }; | |
374 | char cred_msg[CMSG_SPACE(sizeof(struct ucred))]; | |
375 | union sockaddr_union snl; | |
376 | struct msghdr smsg = { | |
377 | .msg_iov = &iov, | |
378 | .msg_iovlen = 1, | |
379 | .msg_control = cred_msg, | |
380 | .msg_controllen = sizeof(cred_msg), | |
381 | .msg_name = &snl, | |
382 | .msg_namelen = sizeof(snl), | |
383 | }; | |
384 | struct cmsghdr *cmsg; | |
385 | struct ucred *cred; | |
386 | ssize_t buflen, bufpos; | |
387 | bool is_initialized = false; | |
388 | int r; | |
389 | ||
390 | assert(ret); | |
391 | ||
392 | buflen = recvmsg(m->sock, &smsg, 0); | |
393 | if (buflen < 0) { | |
394 | if (errno != EINTR) | |
c7d54dae | 395 | log_debug_errno(errno, "sd-device-monitor: Failed to receive message: %m"); |
b1c097af YW |
396 | return -errno; |
397 | } | |
398 | ||
399 | if (buflen < 32 || (smsg.msg_flags & MSG_TRUNC)) | |
c7d54dae | 400 | return log_debug_errno(EINVAL, "sd-device-monitor: Invalid message length."); |
b1c097af YW |
401 | |
402 | if (snl.nl.nl_groups == MONITOR_GROUP_NONE) { | |
403 | /* unicast message, check if we trust the sender */ | |
404 | if (m->snl_trusted_sender.nl.nl_pid == 0 || | |
405 | snl.nl.nl_pid != m->snl_trusted_sender.nl.nl_pid) | |
c7d54dae | 406 | return log_debug_errno(EAGAIN, "sd-device-monitor: Unicast netlink message ignored."); |
b1c097af YW |
407 | |
408 | } else if (snl.nl.nl_groups == MONITOR_GROUP_KERNEL) { | |
409 | if (snl.nl.nl_pid > 0) | |
c7d54dae | 410 | return log_debug_errno(EAGAIN, "sd-device-monitor: Multicast kernel netlink message from PID %"PRIu32" ignored.", snl.nl.nl_pid); |
b1c097af YW |
411 | } |
412 | ||
413 | cmsg = CMSG_FIRSTHDR(&smsg); | |
414 | if (!cmsg || cmsg->cmsg_type != SCM_CREDENTIALS) | |
c7d54dae | 415 | return log_debug_errno(EAGAIN, "sd-device-monitor: No sender credentials received, message ignored."); |
b1c097af YW |
416 | |
417 | cred = (struct ucred*) CMSG_DATA(cmsg); | |
418 | if (cred->uid != 0) | |
c7d54dae | 419 | return log_debug_errno(EAGAIN, "sd-device-monitor: Sender uid="UID_FMT", message ignored.", cred->uid); |
b1c097af YW |
420 | |
421 | if (streq(buf.raw, "libudev")) { | |
422 | /* udev message needs proper version magic */ | |
423 | if (buf.nlh.magic != htobe32(UDEV_MONITOR_MAGIC)) | |
c7d54dae | 424 | return log_debug_errno(EAGAIN, "sd-device-monitor: Invalid message signature (%x != %x)", |
b1c097af YW |
425 | buf.nlh.magic, htobe32(UDEV_MONITOR_MAGIC)); |
426 | ||
427 | if (buf.nlh.properties_off+32 > (size_t) buflen) | |
c7d54dae | 428 | return log_debug_errno(EAGAIN, "sd-device-monitor: Invalid message length (%u > %zd)", |
b1c097af YW |
429 | buf.nlh.properties_off+32, buflen); |
430 | ||
431 | bufpos = buf.nlh.properties_off; | |
432 | ||
433 | /* devices received from udev are always initialized */ | |
434 | is_initialized = true; | |
435 | ||
436 | } else { | |
437 | /* kernel message with header */ | |
438 | bufpos = strlen(buf.raw) + 1; | |
439 | if ((size_t) bufpos < sizeof("a@/d") || bufpos >= buflen) | |
c7d54dae | 440 | return log_debug_errno(EAGAIN, "sd-device-monitor: Invalid message length"); |
b1c097af YW |
441 | |
442 | /* check message header */ | |
443 | if (!strstr(buf.raw, "@/")) | |
c7d54dae | 444 | return log_debug_errno(EAGAIN, "sd-device-monitor: Invalid message header"); |
b1c097af YW |
445 | } |
446 | ||
447 | r = device_new_from_nulstr(&device, (uint8_t*) &buf.raw[bufpos], buflen - bufpos); | |
448 | if (r < 0) | |
c7d54dae | 449 | return log_debug_errno(r, "sd-device-monitor: Failed to create device from received message: %m"); |
b1c097af YW |
450 | |
451 | if (is_initialized) | |
452 | device_set_is_initialized(device); | |
453 | ||
454 | /* Skip device, if it does not pass the current filter */ | |
455 | r = passes_filter(m, device); | |
456 | if (r < 0) | |
c7d54dae | 457 | return log_device_debug_errno(device, r, "sd-device-monitor: Failed to check received device passing filter: %m"); |
b1c097af | 458 | if (r == 0) |
c7d54dae | 459 | log_device_debug(device, "sd-device-monitor: Received device does not pass filter, ignoring"); |
b1c097af YW |
460 | else |
461 | *ret = TAKE_PTR(device); | |
462 | ||
463 | return r; | |
464 | } | |
465 | ||
466 | static uint32_t string_hash32(const char *str) { | |
467 | return MurmurHash2(str, strlen(str), 0); | |
468 | } | |
469 | ||
470 | /* Get a bunch of bit numbers out of the hash, and set the bits in our bit field */ | |
471 | static uint64_t string_bloom64(const char *str) { | |
472 | uint64_t bits = 0; | |
473 | uint32_t hash = string_hash32(str); | |
474 | ||
475 | bits |= 1LLU << (hash & 63); | |
476 | bits |= 1LLU << ((hash >> 6) & 63); | |
477 | bits |= 1LLU << ((hash >> 12) & 63); | |
478 | bits |= 1LLU << ((hash >> 18) & 63); | |
479 | return bits; | |
480 | } | |
481 | ||
482 | int device_monitor_send_device( | |
483 | sd_device_monitor *m, | |
484 | sd_device_monitor *destination, | |
485 | sd_device *device) { | |
486 | ||
487 | monitor_netlink_header nlh = { | |
488 | .prefix = "libudev", | |
489 | .magic = htobe32(UDEV_MONITOR_MAGIC), | |
490 | .header_size = sizeof nlh, | |
491 | }; | |
492 | struct iovec iov[2] = { | |
493 | { .iov_base = &nlh, .iov_len = sizeof nlh }, | |
494 | }; | |
495 | struct msghdr smsg = { | |
496 | .msg_iov = iov, | |
497 | .msg_iovlen = 2, | |
498 | }; | |
499 | /* default destination for sending */ | |
500 | union sockaddr_union default_destination = { | |
501 | .nl.nl_family = AF_NETLINK, | |
502 | .nl.nl_groups = MONITOR_GROUP_UDEV, | |
503 | }; | |
504 | uint64_t tag_bloom_bits; | |
505 | const char *buf, *val; | |
506 | ssize_t count; | |
507 | size_t blen; | |
508 | int r; | |
509 | ||
510 | assert(m); | |
511 | assert(device); | |
512 | ||
513 | r = device_get_properties_nulstr(device, (const uint8_t **) &buf, &blen); | |
514 | if (r < 0) | |
c7d54dae | 515 | return log_device_debug_errno(device, r, "sd-device-monitor: Failed to get device properties: %m"); |
b1c097af | 516 | if (blen < 32) { |
c7d54dae | 517 | log_device_debug(device, "sd-device-monitor: Length of device property nulstr is too small to contain valid device information"); |
b1c097af YW |
518 | return -EINVAL; |
519 | } | |
520 | ||
521 | /* fill in versioned header */ | |
522 | r = sd_device_get_subsystem(device, &val); | |
523 | if (r < 0) | |
c7d54dae | 524 | return log_device_debug_errno(device, r, "sd-device-monitor: Failed to get device subsystem: %m"); |
b1c097af YW |
525 | nlh.filter_subsystem_hash = htobe32(string_hash32(val)); |
526 | ||
a537eafe | 527 | if (sd_device_get_devtype(device, &val) >= 0) |
b1c097af YW |
528 | nlh.filter_devtype_hash = htobe32(string_hash32(val)); |
529 | ||
530 | /* add tag bloom filter */ | |
531 | tag_bloom_bits = 0; | |
532 | FOREACH_DEVICE_TAG(device, val) | |
533 | tag_bloom_bits |= string_bloom64(val); | |
534 | ||
535 | if (tag_bloom_bits > 0) { | |
536 | nlh.filter_tag_bloom_hi = htobe32(tag_bloom_bits >> 32); | |
537 | nlh.filter_tag_bloom_lo = htobe32(tag_bloom_bits & 0xffffffff); | |
538 | } | |
539 | ||
540 | /* add properties list */ | |
541 | nlh.properties_off = iov[0].iov_len; | |
542 | nlh.properties_len = blen; | |
543 | iov[1] = (struct iovec) { | |
544 | .iov_base = (char*) buf, | |
545 | .iov_len = blen, | |
546 | }; | |
547 | ||
548 | /* | |
549 | * Use custom address for target, or the default one. | |
550 | * | |
551 | * If we send to a multicast group, we will get | |
552 | * ECONNREFUSED, which is expected. | |
553 | */ | |
554 | smsg.msg_name = destination ? &destination->snl : &default_destination; | |
555 | smsg.msg_namelen = sizeof(struct sockaddr_nl); | |
556 | count = sendmsg(m->sock, &smsg, 0); | |
557 | if (count < 0) { | |
558 | if (!destination && errno == ECONNREFUSED) { | |
c7d54dae | 559 | log_device_debug(device, "sd-device-monitor: Passed to netlink monitor"); |
b1c097af YW |
560 | return 0; |
561 | } else | |
c7d54dae | 562 | return log_device_debug_errno(device, errno, "sd-device-monitor: Failed to send device to netlink monitor: %m"); |
b1c097af YW |
563 | } |
564 | ||
c7d54dae | 565 | log_device_debug(device, "sd-device-monitor: Passed %zi byte to netlink monitor", count); |
b1c097af YW |
566 | return count; |
567 | } | |
568 | ||
569 | static void bpf_stmt(struct sock_filter *ins, unsigned *i, | |
570 | unsigned short code, unsigned data) { | |
571 | ins[(*i)++] = (struct sock_filter) { | |
572 | .code = code, | |
573 | .k = data, | |
574 | }; | |
575 | } | |
576 | ||
577 | static void bpf_jmp(struct sock_filter *ins, unsigned *i, | |
578 | unsigned short code, unsigned data, | |
579 | unsigned short jt, unsigned short jf) { | |
580 | ins[(*i)++] = (struct sock_filter) { | |
581 | .code = code, | |
582 | .jt = jt, | |
583 | .jf = jf, | |
584 | .k = data, | |
585 | }; | |
586 | } | |
587 | ||
588 | _public_ int sd_device_monitor_filter_update(sd_device_monitor *m) { | |
589 | struct sock_filter ins[512] = {}; | |
590 | struct sock_fprog filter; | |
591 | const char *subsystem, *devtype, *tag; | |
592 | unsigned i = 0; | |
593 | Iterator it; | |
594 | ||
595 | assert_return(m, -EINVAL); | |
596 | ||
597 | if (hashmap_isempty(m->subsystem_filter) && | |
598 | set_isempty(m->tag_filter)) { | |
599 | m->filter_uptodate = true; | |
600 | return 0; | |
601 | } | |
602 | ||
603 | /* load magic in A */ | |
604 | bpf_stmt(ins, &i, BPF_LD|BPF_W|BPF_ABS, offsetof(monitor_netlink_header, magic)); | |
605 | /* jump if magic matches */ | |
606 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, UDEV_MONITOR_MAGIC, 1, 0); | |
607 | /* wrong magic, pass packet */ | |
608 | bpf_stmt(ins, &i, BPF_RET|BPF_K, 0xffffffff); | |
609 | ||
610 | if (!set_isempty(m->tag_filter)) { | |
611 | int tag_matches = set_size(m->tag_filter); | |
612 | ||
613 | /* add all tags matches */ | |
614 | SET_FOREACH(tag, m->tag_filter, it) { | |
615 | uint64_t tag_bloom_bits = string_bloom64(tag); | |
616 | uint32_t tag_bloom_hi = tag_bloom_bits >> 32; | |
617 | uint32_t tag_bloom_lo = tag_bloom_bits & 0xffffffff; | |
618 | ||
619 | /* load device bloom bits in A */ | |
620 | bpf_stmt(ins, &i, BPF_LD|BPF_W|BPF_ABS, offsetof(monitor_netlink_header, filter_tag_bloom_hi)); | |
621 | /* clear bits (tag bits & bloom bits) */ | |
622 | bpf_stmt(ins, &i, BPF_ALU|BPF_AND|BPF_K, tag_bloom_hi); | |
623 | /* jump to next tag if it does not match */ | |
624 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, tag_bloom_hi, 0, 3); | |
625 | ||
626 | /* load device bloom bits in A */ | |
627 | bpf_stmt(ins, &i, BPF_LD|BPF_W|BPF_ABS, offsetof(monitor_netlink_header, filter_tag_bloom_lo)); | |
628 | /* clear bits (tag bits & bloom bits) */ | |
629 | bpf_stmt(ins, &i, BPF_ALU|BPF_AND|BPF_K, tag_bloom_lo); | |
630 | /* jump behind end of tag match block if tag matches */ | |
631 | tag_matches--; | |
632 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, tag_bloom_lo, 1 + (tag_matches * 6), 0); | |
633 | } | |
634 | ||
635 | /* nothing matched, drop packet */ | |
636 | bpf_stmt(ins, &i, BPF_RET|BPF_K, 0); | |
637 | } | |
638 | ||
639 | /* add all subsystem matches */ | |
640 | if (!hashmap_isempty(m->subsystem_filter)) { | |
641 | HASHMAP_FOREACH_KEY(devtype, subsystem, m->subsystem_filter, it) { | |
642 | uint32_t hash = string_hash32(subsystem); | |
643 | ||
644 | /* load device subsystem value in A */ | |
645 | bpf_stmt(ins, &i, BPF_LD|BPF_W|BPF_ABS, offsetof(monitor_netlink_header, filter_subsystem_hash)); | |
646 | if (!devtype) { | |
647 | /* jump if subsystem does not match */ | |
648 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, hash, 0, 1); | |
649 | } else { | |
650 | hash = string_hash32(devtype); | |
651 | ||
652 | /* jump if subsystem does not match */ | |
653 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, hash, 0, 3); | |
654 | /* load device devtype value in A */ | |
655 | bpf_stmt(ins, &i, BPF_LD|BPF_W|BPF_ABS, offsetof(monitor_netlink_header, filter_devtype_hash)); | |
656 | /* jump if value does not match */ | |
657 | bpf_jmp(ins, &i, BPF_JMP|BPF_JEQ|BPF_K, hash, 0, 1); | |
658 | } | |
659 | ||
660 | /* matched, pass packet */ | |
661 | bpf_stmt(ins, &i, BPF_RET|BPF_K, 0xffffffff); | |
662 | ||
663 | if (i+1 >= ELEMENTSOF(ins)) | |
664 | return -E2BIG; | |
665 | } | |
666 | ||
667 | /* nothing matched, drop packet */ | |
668 | bpf_stmt(ins, &i, BPF_RET|BPF_K, 0); | |
669 | } | |
670 | ||
671 | /* matched, pass packet */ | |
672 | bpf_stmt(ins, &i, BPF_RET|BPF_K, 0xffffffff); | |
673 | ||
674 | /* install filter */ | |
675 | filter = (struct sock_fprog) { | |
676 | .len = i, | |
677 | .filter = ins, | |
678 | }; | |
679 | if (setsockopt(m->sock, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof(filter)) < 0) | |
680 | return -errno; | |
681 | ||
682 | m->filter_uptodate = true; | |
683 | return 0; | |
684 | } | |
685 | ||
686 | _public_ int sd_device_monitor_filter_add_match_subsystem_devtype(sd_device_monitor *m, const char *subsystem, const char *devtype) { | |
687 | _cleanup_free_ char *s = NULL, *d = NULL; | |
688 | int r; | |
689 | ||
690 | assert_return(m, -EINVAL); | |
691 | assert_return(subsystem, -EINVAL); | |
692 | ||
693 | s = strdup(subsystem); | |
694 | if (!s) | |
695 | return -ENOMEM; | |
696 | ||
697 | if (devtype) { | |
698 | d = strdup(devtype); | |
699 | if (!d) | |
700 | return -ENOMEM; | |
701 | } | |
702 | ||
703 | r = hashmap_ensure_allocated(&m->subsystem_filter, NULL); | |
704 | if (r < 0) | |
705 | return r; | |
706 | ||
707 | r = hashmap_put(m->subsystem_filter, s, d); | |
708 | if (r < 0) | |
709 | return r; | |
710 | ||
711 | s = d = NULL; | |
712 | m->filter_uptodate = false; | |
713 | ||
714 | return 0; | |
715 | } | |
716 | ||
717 | _public_ int sd_device_monitor_filter_add_match_tag(sd_device_monitor *m, const char *tag) { | |
718 | _cleanup_free_ char *t = NULL; | |
719 | int r; | |
720 | ||
721 | assert_return(m, -EINVAL); | |
722 | assert_return(tag, -EINVAL); | |
723 | ||
724 | t = strdup(tag); | |
725 | if (!t) | |
726 | return -ENOMEM; | |
727 | ||
728 | r = set_ensure_allocated(&m->tag_filter, &string_hash_ops); | |
729 | if (r < 0) | |
730 | return r; | |
731 | ||
732 | r = set_put(m->tag_filter, t); | |
733 | if (r == -EEXIST) | |
734 | return 0; | |
735 | if (r < 0) | |
736 | return r; | |
737 | ||
738 | TAKE_PTR(t); | |
739 | m->filter_uptodate = false; | |
740 | ||
741 | return 0; | |
742 | } | |
743 | ||
744 | _public_ int sd_device_monitor_filter_remove(sd_device_monitor *m) { | |
745 | static const struct sock_fprog filter = { 0, NULL }; | |
746 | ||
747 | assert_return(m, -EINVAL); | |
748 | ||
749 | m->subsystem_filter = hashmap_free_free_free(m->subsystem_filter); | |
750 | m->tag_filter = set_free_free(m->tag_filter); | |
751 | ||
752 | if (setsockopt(m->sock, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof(filter)) < 0) | |
753 | return -errno; | |
754 | ||
755 | m->filter_uptodate = true; | |
756 | return 0; | |
757 | } |