1 /*-
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2010-2011 Monthadar Al Jaberi, TerraNet AB
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer,
12 * without modification.
13 * 2. Redistributions in binary form must reproduce at minimum a disclaimer
14 * similar to the "NO WARRANTY" disclaimer below ("Disclaimer") and any
15 * redistribution must be conditioned upon including a substantially
16 * similar Disclaimer requirement for further binary redistribution.
17 *
18 * NO WARRANTY
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21 * LIMITED TO, THE IMPLIED WARRANTIES OF NONINFRINGEMENT, MERCHANTIBILITY
22 * AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL
23 * THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE LIABLE FOR SPECIAL, EXEMPLARY,
24 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
27 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF
29 * THE POSSIBILITY OF SUCH DAMAGES.
30 */
31
32 #include <sys/param.h>
33 #include <sys/jail.h>
34
35 #include <sys/socket.h>
36
37 #include <net/if.h>
38 #include <netinet/in.h>
39
40 #include "visibility.h"
41
42 /* Function prototypes */
43 static d_ioctl_t vis_ioctl;
44
45 static struct cdevsw vis_cdevsw = {
46 .d_version = D_VERSION,
47 .d_flags = 0,
48 .d_ioctl = vis_ioctl,
49 .d_name = "visctl",
50 };
51
52 void
visibility_init(struct wtap_plugin * plugin)53 visibility_init(struct wtap_plugin *plugin)
54 {
55 struct visibility_plugin *vis_plugin;
56
57 vis_plugin = (struct visibility_plugin *) plugin;
58 plugin->wp_sdev = make_dev(&vis_cdevsw,0,UID_ROOT,GID_WHEEL,0600,
59 (const char *)"visctl");
60 plugin->wp_sdev->si_drv1 = vis_plugin;
61 mtx_init(&vis_plugin->pl_mtx, "visibility_plugin mtx",
62 NULL, MTX_DEF | MTX_RECURSE);
63 printf("Using visibility wtap plugin...\n");
64 }
65
66 void
visibility_deinit(struct wtap_plugin * plugin)67 visibility_deinit(struct wtap_plugin *plugin)
68 {
69 struct visibility_plugin *vis_plugin;
70
71 vis_plugin = (struct visibility_plugin *) plugin;
72 destroy_dev(plugin->wp_sdev);
73 mtx_destroy(&vis_plugin->pl_mtx);
74 free(vis_plugin, M_WTAP_PLUGIN);
75 printf("Removing visibility wtap plugin...\n");
76 }
77
78 /* We need to use a mutex lock when we read out a visibility map
79 * and when we change visibility map from user space through IOCTL
80 */
81 void
visibility_work(struct wtap_plugin * plugin,struct packet * p)82 visibility_work(struct wtap_plugin *plugin, struct packet *p)
83 {
84 struct visibility_plugin *vis_plugin =
85 (struct visibility_plugin *) plugin;
86 struct wtap_hal *hal = (struct wtap_hal *)vis_plugin->base.wp_hal;
87 struct vis_map *map;
88
89 KASSERT(mtod(p->m, const char *) != (const char *) 0xdeadc0de ||
90 mtod(p->m, const char *) != NULL,
91 ("[%s] got a corrupt packet from master queue, p->m=%p, p->id=%d\n",
92 __func__, p->m, p->id));
93 DWTAP_PRINTF("[%d] BROADCASTING m=%p\n", p->id, p->m);
94 mtx_lock(&vis_plugin->pl_mtx);
95 map = &vis_plugin->pl_node[p->id];
96 mtx_unlock(&vis_plugin->pl_mtx);
97
98 /* This is O(n*n) which is not optimal for large
99 * number of nodes. Another way of doing it is
100 * creating groups of nodes that hear each other.
101 * Atleast for this simple static node plugin.
102 */
103 for(int i=0; i<ARRAY_SIZE; ++i){
104 uint32_t index = map->map[i];
105 for(int j=0; j<32; ++j){
106 int vis = index & 0x01;
107 if(vis){
108 int k = i*ARRAY_SIZE + j;
109 if(hal->hal_devs[k] != NULL
110 && hal->hal_devs[k]->up == 1){
111 struct wtap_softc *sc =
112 hal->hal_devs[k];
113 struct mbuf *m =
114 m_dup(p->m, M_NOWAIT);
115 DWTAP_PRINTF("[%d] duplicated old_m=%p"
116 "to new_m=%p\n", p->id, p->m, m);
117 #if 0
118 printf("[%d] sending to %d\n",
119 p->id, k);
120 #endif
121 wtap_inject(sc, m);
122 }
123 }
124 index = index >> 1;
125 }
126 }
127 }
128
129 static void
add_link(struct visibility_plugin * vis_plugin,struct link * l)130 add_link(struct visibility_plugin *vis_plugin, struct link *l)
131 {
132
133 mtx_lock(&vis_plugin->pl_mtx);
134 struct vis_map *map = &vis_plugin->pl_node[l->id1];
135 int index = l->id2/ARRAY_SIZE;
136 int bit = l->id2 % ARRAY_SIZE;
137 uint32_t value = 1 << bit;
138 map->map[index] = map->map[index] | value;
139 mtx_unlock(&vis_plugin->pl_mtx);
140 #if 0
141 printf("l->id1=%d, l->id2=%d, map->map[%d] = %u, bit=%d\n",
142 l->id1, l->id2, index, map->map[index], bit);
143 #endif
144 }
145
146 static void
del_link(struct visibility_plugin * vis_plugin,struct link * l)147 del_link(struct visibility_plugin *vis_plugin, struct link *l)
148 {
149
150 mtx_lock(&vis_plugin->pl_mtx);
151 struct vis_map *map = &vis_plugin->pl_node[l->id1];
152 int index = l->id2/ARRAY_SIZE;
153 int bit = l->id2 % ARRAY_SIZE;
154 uint32_t value = 1 << bit;
155 map->map[index] = map->map[index] & ~value;
156 mtx_unlock(&vis_plugin->pl_mtx);
157 #if 0
158 printf("map->map[index] = %u\n", map->map[index]);
159 #endif
160 }
161
162 int
vis_ioctl(struct cdev * sdev,u_long cmd,caddr_t data,int fflag,struct thread * td)163 vis_ioctl(struct cdev *sdev, u_long cmd, caddr_t data,
164 int fflag, struct thread *td)
165 {
166 struct visibility_plugin *vis_plugin =
167 (struct visibility_plugin *) sdev->si_drv1;
168 struct wtap_hal *hal = vis_plugin->base.wp_hal;
169 struct link l;
170 int op;
171 int error = 0;
172
173 CURVNET_SET(CRED_TO_VNET(curthread->td_ucred));
174 switch(cmd) {
175 case VISIOCTLOPEN:
176 op = *(int *)data;
177 if(op == 0)
178 medium_close(hal->hal_md);
179 else
180 medium_open(hal->hal_md);
181 break;
182 case VISIOCTLLINK:
183 l = *(struct link *)data;
184 if(l.op == 0)
185 del_link(vis_plugin, &l);
186 else
187 add_link(vis_plugin, &l);
188 #if 0
189 printf("op=%d, id1=%d, id2=%d\n", l.op, l.id1, l.id2);
190 #endif
191 break;
192 default:
193 DWTAP_PRINTF("Unknown WTAP IOCTL\n");
194 error = EINVAL;
195 }
196
197 CURVNET_RESTORE();
198 return error;
199 }
200