2 * This program is free software; you can redistribute it and/or modify
3 * it under the terms of the GNU General Public License as published by
4 * the Free Software Foundation; either version 2 of the License.
6 * This program is distributed in the hope that it will be useful,
7 * but WITHOUT ANY WARRANTY; without even the implied warranty of
8 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
9 * GNU General Public License for more details.
11 * You should have received a copy of the GNU General Public License
12 * along with this program; if not, write to the Free Software
13 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
15 * Copyright (C) 2020 embedd.ch
16 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
17 * Copyright (C) 2020 John Crispin <john@phrozen.org>
22 #include <sys/types.h>
23 #include <sys/socket.h>
24 #include <netinet/in.h>
26 #include <arpa/inet.h>
30 #include <libubox/vlist.h>
31 #include <libubox/avl-cmp.h>
32 #include <libubox/usock.h>
37 static uint32_t local_id
;
38 static struct uloop_fd remote_fd
;
39 static struct uloop_timeout remote_timer
;
40 static struct uloop_timeout reload_timer
;
42 static struct blob_buf buf
;
43 static uint32_t msg_seq
;
46 struct vlist_node node
;
51 interfaces_update_cb(struct vlist_tree
*tree
,
52 struct vlist_node
*node_new
,
53 struct vlist_node
*node_old
);
55 static int remote_host_cmp(const void *k1
, const void *k2
, void *ptr
)
57 unsigned long v1
= (unsigned long) k1
;
58 unsigned long v2
= (unsigned long) k2
;
63 static VLIST_TREE(interfaces
, avl_strcmp
, interfaces_update_cb
, true, true);
64 LIST_HEAD(remote_nodes
);
65 AVL_TREE(remote_hosts
, remote_host_cmp
, false, NULL
);
68 interface_name(struct interface
*iface
)
70 return iface
->node
.avl
.key
;
74 interface_check(struct interface
*iface
)
76 iface
->ifindex
= if_nametoindex(interface_name(iface
));
77 uloop_timeout_set(&reload_timer
, 1);
81 interface_init(struct interface
*iface
)
83 interface_check(iface
);
87 interface_free(struct interface
*iface
)
89 avl_delete(&interfaces
.avl
, &iface
->node
.avl
);
94 interfaces_update_cb(struct vlist_tree
*tree
,
95 struct vlist_node
*node_new
,
96 struct vlist_node
*node_old
)
98 struct interface
*iface
;
100 if (node_new
&& node_old
) {
101 iface
= container_of(node_new
, struct interface
, node
);
103 iface
= container_of(node_old
, struct interface
, node
);
104 interface_check(iface
);
105 } else if (node_old
) {
106 iface
= container_of(node_old
, struct interface
, node
);
107 interface_free(iface
);
109 iface
= container_of(node_new
, struct interface
, node
);
110 interface_init(iface
);
114 void usteer_interface_add(const char *name
)
116 struct interface
*iface
;
119 iface
= calloc_a(sizeof(*iface
), &name_buf
, strlen(name
) + 1);
120 strcpy(name_buf
, name
);
121 vlist_add(&interfaces
, &iface
->node
, name_buf
);
124 void config_set_interfaces(struct blob_attr
*data
)
126 struct blob_attr
*cur
;
132 if (!blobmsg_check_attr_list(data
, BLOBMSG_TYPE_STRING
))
135 vlist_update(&interfaces
);
136 blobmsg_for_each_attr(cur
, data
, rem
) {
137 usteer_interface_add(blobmsg_data(cur
));
139 vlist_flush(&interfaces
);
142 void config_get_interfaces(struct blob_buf
*buf
)
144 struct interface
*iface
;
147 c
= blobmsg_open_array(buf
, "interfaces");
148 vlist_for_each_element(&interfaces
, iface
, node
) {
149 blobmsg_add_string(buf
, NULL
, interface_name(iface
));
151 blobmsg_close_array(buf
, c
);
155 interface_add_station(struct usteer_remote_node
*node
, struct blob_attr
*data
)
158 struct sta_info
*si
, *local_si
;
159 struct apmsg_sta msg
;
160 struct usteer_node
*local_node
;
164 if (!parse_apmsg_sta(&msg
, data
)) {
165 MSG(DEBUG
, "Cannot parse station in message\n");
169 if (msg
.timeout
<= 0) {
170 MSG(DEBUG
, "Refuse to add an already expired station entry\n");
174 sta
= usteer_sta_get(msg
.addr
, true);
178 si
= usteer_sta_info_get(sta
, &node
->node
, &create
);
182 connect_change
= si
->connected
!= msg
.connected
;
183 si
->connected
= msg
.connected
;
184 si
->signal
= msg
.signal
;
185 si
->seen
= current_time
- msg
.seen
;
186 si
->last_connected
= current_time
- msg
.last_connected
;
188 /* Check if client roamed to this foreign node */
189 if ((connect_change
|| create
) && si
->connected
== STA_CONNECTED
) {
190 for_each_local_node(local_node
) {
191 local_si
= usteer_sta_info_get(sta
, local_node
, NULL
);
195 if (current_time
- local_si
->last_connected
< config
.roam_process_timeout
) {
196 node
->node
.roam_events
.target
++;
202 usteer_sta_info_update_timeout(si
, msg
.timeout
);
206 remote_node_free(struct usteer_remote_node
*node
)
208 struct usteer_remote_host
*host
= node
->host
;
210 list_del(&node
->list
);
211 list_del(&node
->host_list
);
212 usteer_sta_node_cleanup(&node
->node
);
213 usteer_measurement_report_node_cleanup(&node
->node
);
216 if (!list_empty(&host
->nodes
))
219 avl_delete(&remote_hosts
, &host
->avl
);
224 static struct usteer_remote_host
*
225 interface_get_host(const char *addr
, unsigned long id
)
227 struct usteer_remote_host
*host
;
229 host
= avl_find_element(&remote_hosts
, (void *)id
, host
, avl
);
233 host
= calloc(1, sizeof(*host
));
234 host
->avl
.key
= (void *)id
;
235 INIT_LIST_HEAD(&host
->nodes
);
236 avl_insert(&remote_hosts
, &host
->avl
);
239 if (host
->addr
&& !strcmp(host
->addr
, addr
))
243 host
->addr
= strdup(addr
);
248 static struct usteer_remote_node
*
249 interface_get_node(struct usteer_remote_host
*host
, const char *name
)
251 struct usteer_remote_node
*node
;
252 int addr_len
= strlen(host
->addr
);
255 list_for_each_entry(node
, &host
->nodes
, host_list
)
256 if (!strcmp(node
->name
, name
))
259 node
= calloc_a(sizeof(*node
), &buf
, addr_len
+ 1 + strlen(name
) + 1);
260 node
->node
.type
= NODE_TYPE_REMOTE
;
261 node
->node
.created
= current_time
;
263 sprintf(buf
, "%s#%s", host
->addr
, name
);
264 node
->node
.avl
.key
= buf
;
265 node
->name
= buf
+ addr_len
+ 1;
267 INIT_LIST_HEAD(&node
->node
.sta_info
);
268 INIT_LIST_HEAD(&node
->node
.measurements
);
270 list_add_tail(&node
->list
, &remote_nodes
);
271 list_add_tail(&node
->host_list
, &host
->nodes
);
277 interface_add_node(struct usteer_remote_host
*host
, struct blob_attr
*data
)
279 struct usteer_remote_node
*node
;
280 struct apmsg_node msg
;
281 struct blob_attr
*cur
;
284 if (!parse_apmsg_node(&msg
, data
)) {
285 MSG(DEBUG
, "Cannot parse node in message\n");
289 node
= interface_get_node(host
, msg
.name
);
291 node
->node
.freq
= msg
.freq
;
292 node
->node
.channel
= msg
.channel
;
293 node
->node
.op_class
= msg
.op_class
;
294 node
->node
.n_assoc
= msg
.n_assoc
;
295 node
->node
.max_assoc
= msg
.max_assoc
;
296 node
->node
.noise
= msg
.noise
;
297 node
->node
.load
= msg
.load
;
299 memcpy(node
->node
.bssid
, msg
.bssid
, sizeof(node
->node
.bssid
));
301 snprintf(node
->node
.ssid
, sizeof(node
->node
.ssid
), "%s", msg
.ssid
);
302 usteer_node_set_blob(&node
->node
.rrm_nr
, msg
.rrm_nr
);
303 usteer_node_set_blob(&node
->node
.node_info
, msg
.node_info
);
305 blob_for_each_attr(cur
, msg
.stations
, rem
)
306 interface_add_station(node
, cur
);
310 interface_recv_msg(struct interface
*iface
, char *addr_str
, void *buf
, int len
)
312 struct usteer_remote_host
*host
;
313 struct blob_attr
*data
= buf
;
315 struct blob_attr
*cur
;
318 if (config
.local_mode
)
321 if (blob_pad_len(data
) != len
) {
322 MSG(DEBUG
, "Invalid message length (header: %d, real: %d)\n", blob_pad_len(data
), len
);
326 if (!parse_apmsg(&msg
, data
)) {
327 MSG(DEBUG
, "Missing fields in message\n");
331 if (msg
.id
== local_id
)
334 MSG(NETWORK
, "Received message on %s (id=%08x->%08x seq=%d len=%d)\n",
335 interface_name(iface
), msg
.id
, local_id
, msg
.seq
, len
);
337 host
= interface_get_host(addr_str
, msg
.id
);
338 usteer_node_set_blob(&host
->host_info
, msg
.host_info
);
340 blob_for_each_attr(cur
, msg
.nodes
, rem
)
341 interface_add_node(host
, cur
);
344 static struct interface
*
345 interface_find_by_ifindex(int index
)
347 struct interface
*iface
;
349 vlist_for_each_element(&interfaces
, iface
, node
) {
350 if (iface
->ifindex
== index
)
358 interface_recv_v4(struct uloop_fd
*u
, unsigned int events
)
360 static char buf
[APMGR_BUFLEN
];
361 static char cmsg_buf
[( CMSG_SPACE(sizeof(struct in_pktinfo
)) + sizeof(int)) + 1];
362 static struct sockaddr_in sin
;
363 char addr_str
[INET_ADDRSTRLEN
];
364 static struct iovec iov
= {
366 .iov_len
= sizeof(buf
)
368 static struct msghdr msg
= {
370 .msg_namelen
= sizeof(sin
),
373 .msg_control
= cmsg_buf
,
374 .msg_controllen
= sizeof(cmsg_buf
),
376 struct cmsghdr
*cmsg
;
380 struct in_pktinfo
*pkti
= NULL
;
381 struct interface
*iface
;
383 len
= recvmsg(u
->fd
, &msg
, 0);
397 for (cmsg
= CMSG_FIRSTHDR(&msg
); cmsg
; cmsg
= CMSG_NXTHDR(&msg
, cmsg
)) {
398 if (cmsg
->cmsg_type
!= IP_PKTINFO
)
401 pkti
= (struct in_pktinfo
*) CMSG_DATA(cmsg
);
405 MSG(DEBUG
, "Received packet without ifindex\n");
409 iface
= interface_find_by_ifindex(pkti
->ipi_ifindex
);
411 MSG(DEBUG
, "Received packet from unconfigured interface %d\n", pkti
->ipi_ifindex
);
415 inet_ntop(AF_INET
, &sin
.sin_addr
, addr_str
, sizeof(addr_str
));
417 interface_recv_msg(iface
, addr_str
, buf
, len
);
422 static void interface_recv_v6(struct uloop_fd
*u
, unsigned int events
){
423 static char buf
[APMGR_BUFLEN
];
424 static char cmsg_buf
[( CMSG_SPACE(sizeof(struct in6_pktinfo
)) + sizeof(int)) + 1];
425 static struct sockaddr_in6 sin
;
426 static struct iovec iov
= {
428 .iov_len
= sizeof(buf
)
430 static struct msghdr msg
= {
432 .msg_namelen
= sizeof(sin
),
435 .msg_control
= cmsg_buf
,
436 .msg_controllen
= sizeof(cmsg_buf
),
438 struct cmsghdr
*cmsg
;
439 char addr_str
[INET6_ADDRSTRLEN
];
443 struct in6_pktinfo
*pkti
= NULL
;
444 struct interface
*iface
;
446 len
= recvmsg(u
->fd
, &msg
, 0);
460 for (cmsg
= CMSG_FIRSTHDR(&msg
); cmsg
; cmsg
= CMSG_NXTHDR(&msg
, cmsg
)) {
461 if (cmsg
->cmsg_type
!= IPV6_PKTINFO
)
464 pkti
= (struct in6_pktinfo
*) CMSG_DATA(cmsg
);
468 MSG(DEBUG
, "Received packet without ifindex\n");
472 iface
= interface_find_by_ifindex(pkti
->ipi6_ifindex
);
474 MSG(DEBUG
, "Received packet from unconfigured interface %d\n", pkti
->ipi6_ifindex
);
478 inet_ntop(AF_INET6
, &sin
.sin6_addr
, addr_str
, sizeof(addr_str
));
479 if (sin
.sin6_addr
.s6_addr
[0] == 0) {
480 /* IPv4 mapped address. Ignore. */
484 interface_recv_msg(iface
, addr_str
, buf
, len
);
488 static void interface_send_msg_v4(struct interface
*iface
, struct blob_attr
*data
)
490 static size_t cmsg_data
[( CMSG_SPACE(sizeof(struct in_pktinfo
)) / sizeof(size_t)) + 1];
491 static struct sockaddr_in a
;
492 static struct iovec iov
;
493 static struct msghdr m
= {
494 .msg_name
= (struct sockaddr
*) &a
,
495 .msg_namelen
= sizeof(a
),
498 .msg_control
= cmsg_data
,
499 .msg_controllen
= CMSG_LEN(sizeof(struct in_pktinfo
)),
501 struct in_pktinfo
*pkti
;
502 struct cmsghdr
*cmsg
;
504 a
.sin_family
= AF_INET
;
505 a
.sin_port
= htons(APMGR_PORT
);
506 a
.sin_addr
.s_addr
= ~0;
508 memset(cmsg_data
, 0, sizeof(cmsg_data
));
509 cmsg
= CMSG_FIRSTHDR(&m
);
510 cmsg
->cmsg_len
= m
.msg_controllen
;
511 cmsg
->cmsg_level
= IPPROTO_IP
;
512 cmsg
->cmsg_type
= IP_PKTINFO
;
514 pkti
= (struct in_pktinfo
*) CMSG_DATA(cmsg
);
515 pkti
->ipi_ifindex
= iface
->ifindex
;
518 iov
.iov_len
= blob_pad_len(data
);
520 if (sendmsg(remote_fd
.fd
, &m
, 0) < 0)
525 static void interface_send_msg_v6(struct interface
*iface
, struct blob_attr
*data
) {
526 static struct sockaddr_in6 groupSock
= {};
528 groupSock
.sin6_family
= AF_INET6
;
529 inet_pton(AF_INET6
, APMGR_V6_MCAST_GROUP
, &groupSock
.sin6_addr
);
530 groupSock
.sin6_port
= htons(APMGR_PORT
);
532 setsockopt(remote_fd
.fd
, IPPROTO_IPV6
, IPV6_MULTICAST_IF
, &iface
->ifindex
, sizeof(iface
->ifindex
));
534 if (sendto(remote_fd
.fd
, data
, blob_pad_len(data
), 0, (const struct sockaddr
*)&groupSock
, sizeof(groupSock
)) < 0)
538 static void interface_send_msg(struct interface
*iface
, struct blob_attr
*data
){
540 interface_send_msg_v6(iface
, data
);
542 interface_send_msg_v4(iface
, data
);
546 static void usteer_send_sta_info(struct sta_info
*sta
)
548 int seen
= current_time
- sta
->seen
;
549 int last_connected
= !!sta
->connected
? 0 : current_time
- sta
->last_connected
;
552 c
= blob_nest_start(&buf
, 0);
553 blob_put(&buf
, APMSG_STA_ADDR
, sta
->sta
->addr
, 6);
554 blob_put_int8(&buf
, APMSG_STA_CONNECTED
, !!sta
->connected
);
555 blob_put_int32(&buf
, APMSG_STA_SIGNAL
, sta
->signal
);
556 blob_put_int32(&buf
, APMSG_STA_SEEN
, seen
);
557 blob_put_int32(&buf
, APMSG_STA_LAST_CONNECTED
, last_connected
);
558 blob_put_int32(&buf
, APMSG_STA_TIMEOUT
, config
.local_sta_timeout
- seen
);
559 blob_nest_end(&buf
, c
);
562 static void usteer_send_node(struct usteer_node
*node
, struct sta_info
*sta
)
566 c
= blob_nest_start(&buf
, 0);
568 blob_put_string(&buf
, APMSG_NODE_NAME
, usteer_node_name(node
));
569 blob_put_string(&buf
, APMSG_NODE_SSID
, node
->ssid
);
570 blob_put_int32(&buf
, APMSG_NODE_FREQ
, node
->freq
);
571 blob_put_int32(&buf
, APMSG_NODE_NOISE
, node
->noise
);
572 blob_put_int32(&buf
, APMSG_NODE_LOAD
, node
->load
);
573 blob_put_int32(&buf
, APMSG_NODE_N_ASSOC
, node
->n_assoc
);
574 blob_put_int32(&buf
, APMSG_NODE_MAX_ASSOC
, node
->max_assoc
);
575 blob_put_int32(&buf
, APMSG_NODE_OP_CLASS
, node
->op_class
);
576 blob_put_int32(&buf
, APMSG_NODE_CHANNEL
, node
->channel
);
577 blob_put(&buf
, APMSG_NODE_BSSID
, node
->bssid
, sizeof(node
->bssid
));
579 r
= blob_nest_start(&buf
, APMSG_NODE_RRM_NR
);
580 blobmsg_add_field(&buf
, BLOBMSG_TYPE_ARRAY
, "",
581 blobmsg_data(node
->rrm_nr
),
582 blobmsg_data_len(node
->rrm_nr
));
583 blob_nest_end(&buf
, r
);
587 blob_put(&buf
, APMSG_NODE_NODE_INFO
,
588 blob_data(node
->node_info
),
589 blob_len(node
->node_info
));
591 s
= blob_nest_start(&buf
, APMSG_NODE_STATIONS
);
594 usteer_send_sta_info(sta
);
596 list_for_each_entry(sta
, &node
->sta_info
, node_list
)
597 usteer_send_sta_info(sta
);
600 blob_nest_end(&buf
, s
);
602 blob_nest_end(&buf
, c
);
606 usteer_check_timeout(void)
608 struct usteer_remote_node
*node
, *tmp
;
609 int timeout
= config
.remote_node_timeout
;
611 list_for_each_entry_safe(node
, tmp
, &remote_nodes
, list
) {
612 if (config
.local_mode
|| node
->check
++ > timeout
)
613 remote_node_free(node
);
618 usteer_update_init(void)
620 blob_buf_init(&buf
, 0);
621 blob_put_int32(&buf
, APMSG_ID
, local_id
);
622 blob_put_int32(&buf
, APMSG_SEQ
, ++msg_seq
);
624 blob_put(&buf
, APMSG_HOST_INFO
,
625 blob_data(host_info_blob
),
626 blob_len(host_info_blob
));
628 return blob_nest_start(&buf
, APMSG_NODES
);
632 usteer_update_send(void *c
)
634 struct interface
*iface
;
636 blob_nest_end(&buf
, c
);
638 vlist_for_each_element(&interfaces
, iface
, node
)
639 interface_send_msg(iface
, buf
.head
);
643 usteer_send_sta_update(struct sta_info
*si
)
645 void *c
= usteer_update_init();
646 usteer_send_node(si
->node
, si
);
647 usteer_update_send(c
);
651 usteer_send_update_timer(struct uloop_timeout
*t
)
653 struct usteer_node
*node
;
656 usteer_update_time();
657 uloop_timeout_set(t
, config
.remote_update_interval
);
659 if (!config
.local_mode
&&
660 (!avl_is_empty(&local_nodes
) || host_info_blob
)) {
661 c
= usteer_update_init();
662 for_each_local_node(node
)
663 usteer_send_node(node
, NULL
);
665 usteer_update_send(c
);
667 usteer_check_timeout();
671 usteer_init_local_id(void)
675 f
= fopen("/dev/urandom", "r");
677 perror("fopen(/dev/urandom)");
681 if (fread(&local_id
, sizeof(local_id
), 1, f
) < 1)
688 static int usteer_create_v4_socket() {
692 fd
= usock(USOCK_UDP
| USOCK_SERVER
| USOCK_NONBLOCK
|
693 USOCK_NUMERIC
| USOCK_IPV4ONLY
,
694 "0.0.0.0", APMGR_PORT_STR
);
700 if (setsockopt(fd
, IPPROTO_IP
, IP_PKTINFO
, &yes
, sizeof(yes
)) < 0)
701 perror("setsockopt(IP_PKTINFO)");
703 if (setsockopt(fd
, SOL_SOCKET
, SO_BROADCAST
, &yes
, sizeof(yes
)) < 0)
704 perror("setsockopt(SO_BROADCAST)");
710 static int usteer_create_v6_socket() {
711 struct interface
*iface
;
712 struct ipv6_mreq group
;
716 fd
= usock(USOCK_UDP
| USOCK_SERVER
| USOCK_NONBLOCK
|
717 USOCK_NUMERIC
| USOCK_IPV6ONLY
,
718 "::", APMGR_PORT_STR
);
724 if (!inet_pton(AF_INET6
, APMGR_V6_MCAST_GROUP
, &group
.ipv6mr_multiaddr
.s6_addr
))
725 perror("inet_pton(AF_INET6)");
727 /* Membership has to be added for every interface we listen on. */
728 vlist_for_each_element(&interfaces
, iface
, node
) {
729 group
.ipv6mr_interface
= iface
->ifindex
;
730 if(setsockopt(fd
, IPPROTO_IPV6
, IPV6_ADD_MEMBERSHIP
, (char *)&group
, sizeof group
) < 0)
731 perror("setsockopt(IPV6_ADD_MEMBERSHIP)");
734 if (setsockopt(fd
, IPPROTO_IPV6
, IPV6_RECVPKTINFO
, &yes
, sizeof(yes
)) < 0)
735 perror("setsockopt(IPV6_RECVPKTINFO)");
737 if (setsockopt(fd
, SOL_SOCKET
, SO_BROADCAST
, &yes
, sizeof(yes
)) < 0)
738 perror("setsockopt(SO_BROADCAST)");
743 static void usteer_reload_timer(struct uloop_timeout
*t
) {
744 /* Remove uloop descriptor */
745 if (remote_fd
.fd
&& remote_fd
.registered
) {
746 uloop_fd_delete(&remote_fd
);
751 remote_fd
.fd
= usteer_create_v6_socket();
752 remote_fd
.cb
= interface_recv_v6
;
754 remote_fd
.fd
= usteer_create_v4_socket();
755 remote_fd
.cb
= interface_recv_v4
;
758 if (remote_fd
.fd
< 0)
761 uloop_fd_add(&remote_fd
, ULOOP_READ
);
764 int usteer_interface_init(void)
766 if (usteer_init_local_id())
769 remote_timer
.cb
= usteer_send_update_timer
;
770 remote_timer
.cb(&remote_timer
);
772 reload_timer
.cb
= usteer_reload_timer
;
773 reload_timer
.cb(&reload_timer
);