]>
Commit | Line | Data |
---|---|---|
fa31fcf2 AL |
1 | /* NHRP Multicast Support |
2 | * Copyright (c) 2020-2021 4RF Limited | |
3 | * | |
4 | * This file is free software: you may copy, redistribute and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation, either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | */ | |
9 | ||
10 | #ifdef HAVE_CONFIG_H | |
11 | #include "config.h" | |
12 | #endif | |
13 | ||
14 | #include <fcntl.h> | |
15 | #include <net/if.h> | |
16 | #include <net/ethernet.h> | |
17 | #include <netinet/if_ether.h> | |
18 | #include <linux/netlink.h> | |
19 | #include <linux/neighbour.h> | |
20 | #include <linux/netfilter/nfnetlink_log.h> | |
21 | #include <linux/if_packet.h> | |
22 | #include <sys/types.h> | |
23 | #include <sys/socket.h> | |
24 | ||
25 | #include "thread.h" | |
26 | #include "nhrpd.h" | |
27 | #include "netlink.h" | |
28 | #include "znl.h" | |
29 | #include "os.h" | |
30 | ||
31 | DEFINE_MTYPE_STATIC(NHRPD, NHRP_MULTICAST, "NHRP Multicast") | |
32 | ||
33 | static int netlink_mcast_nflog_group; | |
34 | static int netlink_mcast_log_fd = -1; | |
35 | static struct thread *netlink_mcast_log_thread; | |
36 | ||
37 | struct mcast_ctx { | |
38 | struct interface *ifp; | |
39 | struct zbuf *pkt; | |
40 | }; | |
41 | ||
42 | static void nhrp_multicast_send(struct nhrp_peer *p, struct zbuf *zb) | |
43 | { | |
44 | char buf[2][256]; | |
45 | size_t addrlen; | |
46 | int ret; | |
47 | ||
48 | addrlen = sockunion_get_addrlen(&p->vc->remote.nbma); | |
49 | ret = os_sendmsg(zb->head, zbuf_used(zb), p->ifp->ifindex, | |
50 | sockunion_get_addr(&p->vc->remote.nbma), | |
51 | addrlen, addrlen == 4 ? 0x0800 : 0x86DD); | |
52 | ||
53 | debugf(NHRP_DEBUG_COMMON, "Multicast Packet: %s -> %s, ret = %d, size = %d, addrlen = %d", | |
54 | sockunion2str(&p->vc->local.nbma, buf[0], sizeof(buf[0])), | |
55 | sockunion2str(&p->vc->remote.nbma, buf[1], sizeof(buf[1])), | |
56 | ret, zbuf_used(zb), addrlen); | |
57 | } | |
58 | ||
59 | static void nhrp_multicast_forward_nbma(union sockunion *nbma_addr, struct interface *ifp, struct zbuf *pkt) | |
60 | { | |
61 | struct nhrp_peer *p = nhrp_peer_get(ifp, nbma_addr); | |
62 | if(p && p->online) { | |
63 | /* Send packet */ | |
64 | nhrp_multicast_send(p, pkt); | |
65 | nhrp_peer_unref(p); | |
66 | } | |
67 | } | |
68 | ||
69 | static void nhrp_multicast_forward_cache(struct nhrp_cache *c, void *pctx) | |
70 | { | |
71 | struct mcast_ctx *ctx = (struct mcast_ctx *)pctx; | |
72 | ||
73 | if (c->cur.type == NHRP_CACHE_DYNAMIC && c->cur.peer) | |
74 | nhrp_multicast_forward_nbma(&c->cur.peer->vc->remote.nbma, ctx->ifp, ctx->pkt); | |
75 | } | |
76 | ||
77 | static void nhrp_multicast_forward(struct nhrp_multicast *mcast, void *pctx) | |
78 | { | |
79 | struct mcast_ctx *ctx = (struct mcast_ctx *)pctx; | |
80 | struct nhrp_interface *nifp = ctx->ifp->info; | |
81 | ||
82 | if (!nifp->enabled) | |
83 | return; | |
84 | ||
85 | /* dynamic */ | |
86 | if (sockunion_family(&mcast->nbma_addr) == AF_UNSPEC) { | |
87 | nhrp_cache_foreach(ctx->ifp, nhrp_multicast_forward_cache, pctx); | |
88 | return; | |
89 | } | |
90 | ||
91 | /* Fixed IP Address */ | |
92 | nhrp_multicast_forward_nbma(&mcast->nbma_addr, ctx->ifp, ctx->pkt); | |
93 | } | |
94 | ||
95 | static void netlink_mcast_log_handler(struct nlmsghdr *msg, struct zbuf *zb) | |
96 | { | |
97 | struct nfgenmsg *nf; | |
98 | struct rtattr *rta; | |
99 | struct zbuf rtapl, pktpl; | |
100 | struct interface *ifp; | |
101 | uint32_t *out_ndx = NULL; | |
102 | afi_t afi; | |
103 | struct mcast_ctx ctx; | |
104 | ||
105 | debugf(NHRP_DEBUG_COMMON,"Inside %s\n", __func__); | |
106 | ||
107 | nf = znl_pull(zb, sizeof(*nf)); | |
108 | if (!nf) | |
109 | return; | |
110 | ||
111 | memset(&pktpl, 0, sizeof(pktpl)); | |
112 | while ((rta = znl_rta_pull(zb, &rtapl)) != NULL) { | |
113 | switch (rta->rta_type) { | |
114 | case NFULA_IFINDEX_OUTDEV: | |
115 | out_ndx = znl_pull(&rtapl, sizeof(*out_ndx)); | |
116 | break; | |
117 | case NFULA_PAYLOAD: | |
118 | pktpl = rtapl; | |
119 | break; | |
120 | /* NFULA_HWHDR exists and is supposed to contain source | |
121 | * hardware address. However, for ip_gre it seems to be | |
122 | * the nexthop destination address if the packet matches | |
123 | * route. */ | |
124 | } | |
125 | } | |
126 | ||
127 | if (!out_ndx || !zbuf_used(&pktpl)) | |
128 | return; | |
129 | ||
130 | ifp = if_lookup_by_index(htonl(*out_ndx), VRF_DEFAULT); | |
131 | if (!ifp) | |
132 | return; | |
133 | ||
134 | debugf(NHRP_DEBUG_COMMON,"Outgoing interface = %s\n", ifp->name); | |
135 | ||
136 | ctx = (struct mcast_ctx) { | |
137 | .ifp = ifp, | |
138 | .pkt = &pktpl, | |
139 | }; | |
140 | ||
141 | for (afi = 0; afi < AFI_MAX; afi++) { | |
142 | nhrp_multicast_foreach(ifp, afi, nhrp_multicast_forward, (void *)&ctx); | |
143 | } | |
144 | } | |
145 | ||
146 | static int netlink_mcast_log_recv(struct thread *t) | |
147 | { | |
148 | uint8_t buf[65535]; /* Max OSPF Packet size */ | |
149 | int fd = THREAD_FD(t); | |
150 | struct zbuf payload, zb; | |
151 | struct nlmsghdr *n; | |
152 | ||
153 | netlink_mcast_log_thread = NULL; | |
154 | ||
155 | zbuf_init(&zb, buf, sizeof(buf), 0); | |
156 | while (zbuf_recv(&zb, fd) > 0) { | |
157 | while ((n = znl_nlmsg_pull(&zb, &payload)) != NULL) { | |
158 | debugf(NHRP_DEBUG_COMMON, | |
159 | "Netlink-mcast-log: Received msg_type %u, msg_flags %u", | |
160 | n->nlmsg_type, n->nlmsg_flags); | |
161 | switch (n->nlmsg_type) { | |
162 | case (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_PACKET: | |
163 | netlink_mcast_log_handler(n, &payload); | |
164 | break; | |
165 | } | |
166 | } | |
167 | } | |
168 | ||
169 | thread_add_read(master, netlink_mcast_log_recv, 0, netlink_mcast_log_fd, | |
170 | &netlink_mcast_log_thread); | |
171 | ||
172 | return 0; | |
173 | } | |
174 | ||
175 | static void netlink_mcast_log_register(int fd, int group) | |
176 | { | |
177 | struct nlmsghdr *n; | |
178 | struct nfgenmsg *nf; | |
179 | struct nfulnl_msg_config_cmd cmd; | |
180 | struct zbuf *zb = zbuf_alloc(512); | |
181 | ||
182 | n = znl_nlmsg_push(zb, (NFNL_SUBSYS_ULOG << 8) | NFULNL_MSG_CONFIG, | |
183 | NLM_F_REQUEST | NLM_F_ACK); | |
184 | nf = znl_push(zb, sizeof(*nf)); | |
185 | *nf = (struct nfgenmsg){ | |
186 | .nfgen_family = AF_UNSPEC, | |
187 | .version = NFNETLINK_V0, | |
188 | .res_id = htons(group), | |
189 | }; | |
190 | cmd.command = NFULNL_CFG_CMD_BIND; | |
191 | znl_rta_push(zb, NFULA_CFG_CMD, &cmd, sizeof(cmd)); | |
192 | znl_nlmsg_complete(zb, n); | |
193 | ||
194 | zbuf_send(zb, fd); | |
195 | zbuf_free(zb); | |
196 | } | |
197 | ||
198 | static int nhrp_multicast_free(struct interface *ifp, struct nhrp_multicast *mcast) | |
199 | { | |
200 | list_del(&mcast->list_entry); | |
201 | XFREE(MTYPE_NHRP_MULTICAST, mcast); | |
202 | return 0; | |
203 | } | |
204 | ||
205 | static void netlink_mcast_set_nflog_group(struct interface *ifp, int nlgroup) | |
206 | { | |
207 | if (netlink_mcast_log_fd >= 0) { | |
208 | THREAD_OFF(netlink_mcast_log_thread); | |
209 | close(netlink_mcast_log_fd); | |
210 | netlink_mcast_log_fd = -1; | |
211 | debugf(NHRP_DEBUG_COMMON, "De-register nflog group"); | |
212 | } | |
213 | netlink_mcast_nflog_group = nlgroup; | |
214 | if (nlgroup) { | |
215 | netlink_mcast_log_fd = znl_open(NETLINK_NETFILTER, 0); | |
216 | if (netlink_mcast_log_fd < 0) | |
217 | return; | |
218 | ||
219 | netlink_mcast_log_register(netlink_mcast_log_fd, nlgroup); | |
220 | thread_add_read(master, netlink_mcast_log_recv, 0, netlink_mcast_log_fd, | |
221 | &netlink_mcast_log_thread); | |
222 | debugf(NHRP_DEBUG_COMMON, "Register nflog group: %d", netlink_mcast_nflog_group); | |
223 | } | |
224 | } | |
225 | ||
226 | int nhrp_multicast_add(struct interface *ifp, afi_t afi, union sockunion *nbma_addr) | |
227 | { | |
228 | struct nhrp_interface *nifp = ifp->info; | |
229 | struct nhrp_multicast *mcast; | |
230 | char buf[SU_ADDRSTRLEN]; | |
231 | ||
232 | list_for_each_entry(mcast, &nifp->afi[afi].mcastlist_head, list_entry) | |
233 | { | |
234 | if (sockunion_same(&mcast->nbma_addr, nbma_addr)) | |
235 | return NHRP_ERR_ENTRY_EXISTS; | |
236 | } | |
237 | ||
238 | mcast = XMALLOC(MTYPE_NHRP_MULTICAST, sizeof(struct nhrp_multicast)); | |
239 | ||
240 | *mcast = (struct nhrp_multicast){ | |
241 | .afi = afi, | |
242 | .ifp = ifp, | |
243 | .nbma_addr = *nbma_addr, | |
244 | }; | |
245 | list_add_tail(&mcast->list_entry, &nifp->afi[afi].mcastlist_head); | |
246 | ||
247 | if (netlink_mcast_log_fd == -1) | |
248 | netlink_mcast_set_nflog_group(ifp, MCAST_NFLOG_GROUP); | |
249 | ||
250 | sockunion2str(nbma_addr, buf, sizeof(buf)); | |
251 | debugf(NHRP_DEBUG_COMMON, "Adding multicast entry (%s)", buf); | |
252 | ||
253 | return NHRP_OK; | |
254 | } | |
255 | ||
256 | int nhrp_multicast_del(struct interface *ifp, afi_t afi, union sockunion *nbma_addr) | |
257 | { | |
258 | struct nhrp_interface *nifp = ifp->info; | |
259 | struct nhrp_multicast *mcast, *tmp; | |
260 | char buf[SU_ADDRSTRLEN]; | |
261 | ||
262 | list_for_each_entry_safe(mcast, tmp, &nifp->afi[afi].mcastlist_head, | |
263 | list_entry) | |
264 | { | |
265 | if (!sockunion_same(&mcast->nbma_addr, nbma_addr)) | |
266 | continue; | |
267 | ||
268 | sockunion2str(nbma_addr, buf, sizeof(buf)); | |
269 | debugf(NHRP_DEBUG_COMMON, "Deleting multicast entry (%s)", buf); | |
270 | ||
271 | nhrp_multicast_free(ifp, mcast); | |
272 | ||
273 | return NHRP_OK; | |
274 | } | |
275 | ||
276 | return NHRP_ERR_ENTRY_NOT_FOUND; | |
277 | } | |
278 | ||
279 | void nhrp_multicast_interface_del(struct interface *ifp) | |
280 | { | |
281 | struct nhrp_interface *nifp = ifp->info; | |
282 | struct nhrp_multicast *mcast, *tmp; | |
283 | afi_t afi; | |
284 | ||
285 | for (afi = 0; afi < AFI_MAX; afi++) { | |
286 | debugf(NHRP_DEBUG_COMMON, "Cleaning up multicast entries (%d)", !list_empty(&nifp->afi[afi].mcastlist_head)); | |
287 | ||
288 | list_for_each_entry_safe( | |
289 | mcast, tmp, &nifp->afi[afi].mcastlist_head, | |
290 | list_entry) { | |
291 | nhrp_multicast_free(ifp, mcast); | |
292 | } | |
293 | } | |
294 | } | |
295 | ||
296 | void nhrp_multicast_foreach(struct interface *ifp, afi_t afi, | |
297 | void (*cb)(struct nhrp_multicast *, void *), | |
298 | void *ctx) | |
299 | { | |
300 | struct nhrp_interface *nifp = ifp->info; | |
301 | struct nhrp_multicast *mcast; | |
302 | ||
303 | list_for_each_entry(mcast, &nifp->afi[afi].mcastlist_head, list_entry) | |
304 | { | |
305 | cb (mcast, ctx); | |
306 | } | |
307 | } |