1 /* SPDX-License-Identifier: LGPL-2.1-or-later */
2 
3 #include <arpa/inet.h>
4 #include <linux/sockios.h>
5 #include <sys/ioctl.h>
6 
7 #include "sd-lldp-rx.h"
8 
9 #include "alloc-util.h"
10 #include "ether-addr-util.h"
11 #include "event-util.h"
12 #include "fd-util.h"
13 #include "lldp-neighbor.h"
14 #include "lldp-network.h"
15 #include "lldp-rx-internal.h"
16 #include "memory-util.h"
17 #include "network-common.h"
18 #include "socket-util.h"
19 #include "sort-util.h"
20 #include "string-table.h"
21 
22 #define LLDP_DEFAULT_NEIGHBORS_MAX 128U
23 
24 static const char * const lldp_rx_event_table[_SD_LLDP_RX_EVENT_MAX] = {
25         [SD_LLDP_RX_EVENT_ADDED]     = "added",
26         [SD_LLDP_RX_EVENT_REMOVED]   = "removed",
27         [SD_LLDP_RX_EVENT_UPDATED]   = "updated",
28         [SD_LLDP_RX_EVENT_REFRESHED] = "refreshed",
29 };
30 
31 DEFINE_STRING_TABLE_LOOKUP(lldp_rx_event, sd_lldp_rx_event_t);
32 
lldp_rx_flush_neighbors(sd_lldp_rx * lldp_rx)33 static void lldp_rx_flush_neighbors(sd_lldp_rx *lldp_rx) {
34         assert(lldp_rx);
35 
36         hashmap_clear(lldp_rx->neighbor_by_id);
37 }
38 
lldp_rx_callback(sd_lldp_rx * lldp_rx,sd_lldp_rx_event_t event,sd_lldp_neighbor * n)39 static void lldp_rx_callback(sd_lldp_rx *lldp_rx, sd_lldp_rx_event_t event, sd_lldp_neighbor *n) {
40         assert(lldp_rx);
41         assert(event >= 0 && event < _SD_LLDP_RX_EVENT_MAX);
42 
43         if (!lldp_rx->callback)
44                 return (void) log_lldp_rx(lldp_rx, "Received '%s' event.", lldp_rx_event_to_string(event));
45 
46         log_lldp_rx(lldp_rx, "Invoking callback for '%s' event.", lldp_rx_event_to_string(event));
47         lldp_rx->callback(lldp_rx, event, n, lldp_rx->userdata);
48 }
49 
lldp_rx_make_space(sd_lldp_rx * lldp_rx,size_t extra)50 static int lldp_rx_make_space(sd_lldp_rx *lldp_rx, size_t extra) {
51         usec_t t = USEC_INFINITY;
52         bool changed = false;
53 
54         assert(lldp_rx);
55 
56         /* Remove all entries that are past their TTL, and more until at least the specified number of extra entries
57          * are free. */
58 
59         for (;;) {
60                 _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
61 
62                 n = prioq_peek(lldp_rx->neighbor_by_expiry);
63                 if (!n)
64                         break;
65 
66                 sd_lldp_neighbor_ref(n);
67 
68                 if (hashmap_size(lldp_rx->neighbor_by_id) > LESS_BY(lldp_rx->neighbors_max, extra))
69                         goto remove_one;
70 
71                 if (t == USEC_INFINITY)
72                         t = now(CLOCK_BOOTTIME);
73 
74                 if (n->until > t)
75                         break;
76 
77         remove_one:
78                 lldp_neighbor_unlink(n);
79                 lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, n);
80                 changed = true;
81         }
82 
83         return changed;
84 }
85 
lldp_rx_keep_neighbor(sd_lldp_rx * lldp_rx,sd_lldp_neighbor * n)86 static bool lldp_rx_keep_neighbor(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) {
87         assert(lldp_rx);
88         assert(n);
89 
90         /* Don't keep data with a zero TTL */
91         if (n->ttl <= 0)
92                 return false;
93 
94         /* Filter out data from the filter address */
95         if (!ether_addr_is_null(&lldp_rx->filter_address) &&
96             ether_addr_equal(&lldp_rx->filter_address, &n->source_address))
97                 return false;
98 
99         /* Only add if the neighbor has a capability we are interested in. Note that we also store all neighbors with
100          * no caps field set. */
101         if (n->has_capabilities &&
102             (n->enabled_capabilities & lldp_rx->capability_mask) == 0)
103                 return false;
104 
105         /* Keep everything else */
106         return true;
107 }
108 
109 static int lldp_rx_start_timer(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *neighbor);
110 
lldp_rx_add_neighbor(sd_lldp_rx * lldp_rx,sd_lldp_neighbor * n)111 static int lldp_rx_add_neighbor(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) {
112         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *old = NULL;
113         bool keep;
114         int r;
115 
116         assert(lldp_rx);
117         assert(n);
118         assert(!n->lldp_rx);
119 
120         keep = lldp_rx_keep_neighbor(lldp_rx, n);
121 
122         /* First retrieve the old entry for this MSAP */
123         old = hashmap_get(lldp_rx->neighbor_by_id, &n->id);
124         if (old) {
125                 sd_lldp_neighbor_ref(old);
126 
127                 if (!keep) {
128                         lldp_neighbor_unlink(old);
129                         lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, old);
130                         return 0;
131                 }
132 
133                 if (lldp_neighbor_equal(n, old)) {
134                         /* Is this equal, then restart the TTL counter, but don't do anything else. */
135                         old->timestamp = n->timestamp;
136                         lldp_rx_start_timer(lldp_rx, old);
137                         lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REFRESHED, old);
138                         return 0;
139                 }
140 
141                 /* Data changed, remove the old entry, and add a new one */
142                 lldp_neighbor_unlink(old);
143 
144         } else if (!keep)
145                 return 0;
146 
147         /* Then, make room for at least one new neighbor */
148         lldp_rx_make_space(lldp_rx, 1);
149 
150         r = hashmap_ensure_put(&lldp_rx->neighbor_by_id, &lldp_neighbor_hash_ops, &n->id, n);
151         if (r < 0)
152                 goto finish;
153 
154         r = prioq_ensure_put(&lldp_rx->neighbor_by_expiry, lldp_neighbor_prioq_compare_func, n, &n->prioq_idx);
155         if (r < 0) {
156                 assert_se(hashmap_remove(lldp_rx->neighbor_by_id, &n->id) == n);
157                 goto finish;
158         }
159 
160         n->lldp_rx = lldp_rx;
161 
162         lldp_rx_start_timer(lldp_rx, n);
163         lldp_rx_callback(lldp_rx, old ? SD_LLDP_RX_EVENT_UPDATED : SD_LLDP_RX_EVENT_ADDED, n);
164 
165         return 1;
166 
167 finish:
168         if (old)
169                 lldp_rx_callback(lldp_rx, SD_LLDP_RX_EVENT_REMOVED, old);
170 
171         return r;
172 }
173 
lldp_rx_handle_datagram(sd_lldp_rx * lldp_rx,sd_lldp_neighbor * n)174 static int lldp_rx_handle_datagram(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *n) {
175         int r;
176 
177         assert(lldp_rx);
178         assert(n);
179 
180         r = lldp_neighbor_parse(n);
181         if (r < 0)
182                 return r;
183 
184         r = lldp_rx_add_neighbor(lldp_rx, n);
185         if (r < 0)
186                 return log_lldp_rx_errno(lldp_rx, r, "Failed to add datagram. Ignoring.");
187 
188         log_lldp_rx(lldp_rx, "Successfully processed LLDP datagram.");
189         return 0;
190 }
191 
lldp_rx_receive_datagram(sd_event_source * s,int fd,uint32_t revents,void * userdata)192 static int lldp_rx_receive_datagram(sd_event_source *s, int fd, uint32_t revents, void *userdata) {
193         _cleanup_(sd_lldp_neighbor_unrefp) sd_lldp_neighbor *n = NULL;
194         ssize_t space, length;
195         sd_lldp_rx *lldp_rx = userdata;
196         struct timespec ts;
197 
198         assert(fd >= 0);
199         assert(lldp_rx);
200 
201         space = next_datagram_size_fd(fd);
202         if (space < 0) {
203                 if (ERRNO_IS_TRANSIENT(space) || ERRNO_IS_DISCONNECT(space))
204                         return 0;
205 
206                 log_lldp_rx_errno(lldp_rx, space, "Failed to determine datagram size to read, ignoring: %m");
207                 return 0;
208         }
209 
210         n = lldp_neighbor_new(space);
211         if (!n) {
212                 log_oom_debug();
213                 return 0;
214         }
215 
216         length = recv(fd, LLDP_NEIGHBOR_RAW(n), n->raw_size, MSG_DONTWAIT);
217         if (length < 0) {
218                 if (ERRNO_IS_TRANSIENT(errno) || ERRNO_IS_DISCONNECT(errno))
219                         return 0;
220 
221                 log_lldp_rx_errno(lldp_rx, errno, "Failed to read LLDP datagram, ignoring: %m");
222                 return 0;
223         }
224 
225         if ((size_t) length != n->raw_size) {
226                 log_lldp_rx(lldp_rx, "Packet size mismatch, ignoring");
227                 return 0;
228         }
229 
230         /* Try to get the timestamp of this packet if it is known */
231         if (ioctl(fd, SIOCGSTAMPNS, &ts) >= 0)
232                 triple_timestamp_from_realtime(&n->timestamp, timespec_load(&ts));
233         else
234                 triple_timestamp_get(&n->timestamp);
235 
236         (void) lldp_rx_handle_datagram(lldp_rx, n);
237         return 0;
238 }
239 
lldp_rx_reset(sd_lldp_rx * lldp_rx)240 static void lldp_rx_reset(sd_lldp_rx *lldp_rx) {
241         assert(lldp_rx);
242 
243         (void) event_source_disable(lldp_rx->timer_event_source);
244         lldp_rx->io_event_source = sd_event_source_disable_unref(lldp_rx->io_event_source);
245         lldp_rx->fd = safe_close(lldp_rx->fd);
246 }
247 
sd_lldp_rx_is_running(sd_lldp_rx * lldp_rx)248 int sd_lldp_rx_is_running(sd_lldp_rx *lldp_rx) {
249         if (!lldp_rx)
250                 return false;
251 
252         return lldp_rx->fd >= 0;
253 }
254 
sd_lldp_rx_start(sd_lldp_rx * lldp_rx)255 int sd_lldp_rx_start(sd_lldp_rx *lldp_rx) {
256         int r;
257 
258         assert_return(lldp_rx, -EINVAL);
259         assert_return(lldp_rx->event, -EINVAL);
260         assert_return(lldp_rx->ifindex > 0, -EINVAL);
261 
262         if (sd_lldp_rx_is_running(lldp_rx))
263                 return 0;
264 
265         assert(!lldp_rx->io_event_source);
266 
267         lldp_rx->fd = lldp_network_bind_raw_socket(lldp_rx->ifindex);
268         if (lldp_rx->fd < 0)
269                 return lldp_rx->fd;
270 
271         r = sd_event_add_io(lldp_rx->event, &lldp_rx->io_event_source, lldp_rx->fd, EPOLLIN, lldp_rx_receive_datagram, lldp_rx);
272         if (r < 0)
273                 goto fail;
274 
275         r = sd_event_source_set_priority(lldp_rx->io_event_source, lldp_rx->event_priority);
276         if (r < 0)
277                 goto fail;
278 
279         (void) sd_event_source_set_description(lldp_rx->io_event_source, "lldp-rx-io");
280 
281         log_lldp_rx(lldp_rx, "Started LLDP client");
282         return 1;
283 
284 fail:
285         lldp_rx_reset(lldp_rx);
286         return r;
287 }
288 
sd_lldp_rx_stop(sd_lldp_rx * lldp_rx)289 int sd_lldp_rx_stop(sd_lldp_rx *lldp_rx) {
290         if (!sd_lldp_rx_is_running(lldp_rx))
291                 return 0;
292 
293         log_lldp_rx(lldp_rx, "Stopping LLDP client");
294 
295         lldp_rx_reset(lldp_rx);
296         lldp_rx_flush_neighbors(lldp_rx);
297 
298         return 1;
299 }
300 
sd_lldp_rx_attach_event(sd_lldp_rx * lldp_rx,sd_event * event,int64_t priority)301 int sd_lldp_rx_attach_event(sd_lldp_rx *lldp_rx, sd_event *event, int64_t priority) {
302         int r;
303 
304         assert_return(lldp_rx, -EINVAL);
305         assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY);
306         assert_return(!lldp_rx->event, -EBUSY);
307 
308         if (event)
309                 lldp_rx->event = sd_event_ref(event);
310         else {
311                 r = sd_event_default(&lldp_rx->event);
312                 if (r < 0)
313                         return r;
314         }
315 
316         lldp_rx->event_priority = priority;
317 
318         return 0;
319 }
320 
sd_lldp_rx_detach_event(sd_lldp_rx * lldp_rx)321 int sd_lldp_rx_detach_event(sd_lldp_rx *lldp_rx) {
322         assert_return(lldp_rx, -EINVAL);
323         assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY);
324 
325         lldp_rx->io_event_source = sd_event_source_disable_unref(lldp_rx->io_event_source);
326         lldp_rx->timer_event_source = sd_event_source_disable_unref(lldp_rx->timer_event_source);
327         lldp_rx->event = sd_event_unref(lldp_rx->event);
328         return 0;
329 }
330 
sd_lldp_rx_get_event(sd_lldp_rx * lldp_rx)331 sd_event* sd_lldp_rx_get_event(sd_lldp_rx *lldp_rx) {
332         assert_return(lldp_rx, NULL);
333 
334         return lldp_rx->event;
335 }
336 
sd_lldp_rx_set_callback(sd_lldp_rx * lldp_rx,sd_lldp_rx_callback_t cb,void * userdata)337 int sd_lldp_rx_set_callback(sd_lldp_rx *lldp_rx, sd_lldp_rx_callback_t cb, void *userdata) {
338         assert_return(lldp_rx, -EINVAL);
339 
340         lldp_rx->callback = cb;
341         lldp_rx->userdata = userdata;
342 
343         return 0;
344 }
345 
sd_lldp_rx_set_ifindex(sd_lldp_rx * lldp_rx,int ifindex)346 int sd_lldp_rx_set_ifindex(sd_lldp_rx *lldp_rx, int ifindex) {
347         assert_return(lldp_rx, -EINVAL);
348         assert_return(ifindex > 0, -EINVAL);
349         assert_return(!sd_lldp_rx_is_running(lldp_rx), -EBUSY);
350 
351         lldp_rx->ifindex = ifindex;
352         return 0;
353 }
354 
sd_lldp_rx_set_ifname(sd_lldp_rx * lldp_rx,const char * ifname)355 int sd_lldp_rx_set_ifname(sd_lldp_rx *lldp_rx, const char *ifname) {
356         assert_return(lldp_rx, -EINVAL);
357         assert_return(ifname, -EINVAL);
358 
359         if (!ifname_valid_full(ifname, IFNAME_VALID_ALTERNATIVE))
360                 return -EINVAL;
361 
362         return free_and_strdup(&lldp_rx->ifname, ifname);
363 }
364 
sd_lldp_rx_get_ifname(sd_lldp_rx * lldp_rx,const char ** ret)365 int sd_lldp_rx_get_ifname(sd_lldp_rx *lldp_rx, const char **ret) {
366         int r;
367 
368         assert_return(lldp_rx, -EINVAL);
369 
370         r = get_ifname(lldp_rx->ifindex, &lldp_rx->ifname);
371         if (r < 0)
372                 return r;
373 
374         if (ret)
375                 *ret = lldp_rx->ifname;
376 
377         return 0;
378 }
379 
lldp_rx_free(sd_lldp_rx * lldp_rx)380 static sd_lldp_rx *lldp_rx_free(sd_lldp_rx *lldp_rx) {
381         if (!lldp_rx)
382                 return NULL;
383 
384         lldp_rx_reset(lldp_rx);
385 
386         sd_lldp_rx_detach_event(lldp_rx);
387 
388         lldp_rx_flush_neighbors(lldp_rx);
389 
390         hashmap_free(lldp_rx->neighbor_by_id);
391         prioq_free(lldp_rx->neighbor_by_expiry);
392         free(lldp_rx->ifname);
393         return mfree(lldp_rx);
394 }
395 
396 DEFINE_PUBLIC_TRIVIAL_REF_UNREF_FUNC(sd_lldp_rx, sd_lldp_rx, lldp_rx_free);
397 
sd_lldp_rx_new(sd_lldp_rx ** ret)398 int sd_lldp_rx_new(sd_lldp_rx **ret) {
399         _cleanup_(sd_lldp_rx_unrefp) sd_lldp_rx *lldp_rx = NULL;
400 
401         assert_return(ret, -EINVAL);
402 
403         lldp_rx = new(sd_lldp_rx, 1);
404         if (!lldp_rx)
405                 return -ENOMEM;
406 
407         *lldp_rx = (sd_lldp_rx) {
408                 .n_ref = 1,
409                 .fd = -1,
410                 .neighbors_max = LLDP_DEFAULT_NEIGHBORS_MAX,
411                 .capability_mask = UINT16_MAX,
412         };
413 
414         *ret = TAKE_PTR(lldp_rx);
415         return 0;
416 }
417 
on_timer_event(sd_event_source * s,uint64_t usec,void * userdata)418 static int on_timer_event(sd_event_source *s, uint64_t usec, void *userdata) {
419         sd_lldp_rx *lldp_rx = userdata;
420         int r;
421 
422         r = lldp_rx_make_space(lldp_rx, 0);
423         if (r < 0) {
424                 log_lldp_rx_errno(lldp_rx, r, "Failed to make space, ignoring: %m");
425                 return 0;
426         }
427 
428         r = lldp_rx_start_timer(lldp_rx, NULL);
429         if (r < 0) {
430                 log_lldp_rx_errno(lldp_rx, r, "Failed to restart timer, ignoring: %m");
431                 return 0;
432         }
433 
434         return 0;
435 }
436 
lldp_rx_start_timer(sd_lldp_rx * lldp_rx,sd_lldp_neighbor * neighbor)437 static int lldp_rx_start_timer(sd_lldp_rx *lldp_rx, sd_lldp_neighbor *neighbor) {
438         sd_lldp_neighbor *n;
439 
440         assert(lldp_rx);
441         assert(lldp_rx->event);
442 
443         if (neighbor)
444                 lldp_neighbor_start_ttl(neighbor);
445 
446         n = prioq_peek(lldp_rx->neighbor_by_expiry);
447         if (!n)
448                 return event_source_disable(lldp_rx->timer_event_source);
449 
450         return event_reset_time(lldp_rx->event, &lldp_rx->timer_event_source,
451                                 CLOCK_BOOTTIME,
452                                 n->until, 0,
453                                 on_timer_event, lldp_rx,
454                                 lldp_rx->event_priority, "lldp-rx-timer", true);
455 }
456 
neighbor_compare_func(sd_lldp_neighbor * const * a,sd_lldp_neighbor * const * b)457 static inline int neighbor_compare_func(sd_lldp_neighbor * const *a, sd_lldp_neighbor * const *b) {
458         assert(a);
459         assert(b);
460         assert(*a);
461         assert(*b);
462 
463         return lldp_neighbor_id_compare_func(&(*a)->id, &(*b)->id);
464 }
465 
sd_lldp_rx_get_neighbors(sd_lldp_rx * lldp_rx,sd_lldp_neighbor *** ret)466 int sd_lldp_rx_get_neighbors(sd_lldp_rx *lldp_rx, sd_lldp_neighbor ***ret) {
467         _cleanup_free_ sd_lldp_neighbor **l = NULL;
468         sd_lldp_neighbor *n;
469         int k = 0;
470 
471         assert_return(lldp_rx, -EINVAL);
472         assert_return(ret, -EINVAL);
473 
474         if (hashmap_isempty(lldp_rx->neighbor_by_id)) { /* Special shortcut */
475                 *ret = NULL;
476                 return 0;
477         }
478 
479         l = new0(sd_lldp_neighbor*, hashmap_size(lldp_rx->neighbor_by_id));
480         if (!l)
481                 return -ENOMEM;
482 
483         HASHMAP_FOREACH(n, lldp_rx->neighbor_by_id)
484                 l[k++] = sd_lldp_neighbor_ref(n);
485 
486         assert((size_t) k == hashmap_size(lldp_rx->neighbor_by_id));
487 
488         /* Return things in a stable order */
489         typesafe_qsort(l, k, neighbor_compare_func);
490         *ret = TAKE_PTR(l);
491 
492         return k;
493 }
494 
sd_lldp_rx_set_neighbors_max(sd_lldp_rx * lldp_rx,uint64_t m)495 int sd_lldp_rx_set_neighbors_max(sd_lldp_rx *lldp_rx, uint64_t m) {
496         assert_return(lldp_rx, -EINVAL);
497         assert_return(m > 0, -EINVAL);
498 
499         lldp_rx->neighbors_max = m;
500         lldp_rx_make_space(lldp_rx, 0);
501 
502         return 0;
503 }
504 
sd_lldp_rx_match_capabilities(sd_lldp_rx * lldp_rx,uint16_t mask)505 int sd_lldp_rx_match_capabilities(sd_lldp_rx *lldp_rx, uint16_t mask) {
506         assert_return(lldp_rx, -EINVAL);
507         assert_return(mask != 0, -EINVAL);
508 
509         lldp_rx->capability_mask = mask;
510 
511         return 0;
512 }
513 
sd_lldp_rx_set_filter_address(sd_lldp_rx * lldp_rx,const struct ether_addr * addr)514 int sd_lldp_rx_set_filter_address(sd_lldp_rx *lldp_rx, const struct ether_addr *addr) {
515         assert_return(lldp_rx, -EINVAL);
516 
517         /* In order to deal nicely with bridges that send back our own packets, allow one address to be filtered, so
518          * that our own can be filtered out here. */
519 
520         if (addr)
521                 lldp_rx->filter_address = *addr;
522         else
523                 zero(lldp_rx->filter_address);
524 
525         return 0;
526 }
527