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 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 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 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 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 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 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