1 1.12 joerg /* $NetBSD: bnep.c,v 1.12 2016/10/04 21:40:31 joerg Exp $ */ 2 1.1 plunky 3 1.1 plunky /*- 4 1.1 plunky * Copyright (c) 2008 Iain Hibbert 5 1.1 plunky * All rights reserved. 6 1.1 plunky * 7 1.1 plunky * Redistribution and use in source and binary forms, with or without 8 1.1 plunky * modification, are permitted provided that the following conditions 9 1.1 plunky * are met: 10 1.1 plunky * 1. Redistributions of source code must retain the above copyright 11 1.1 plunky * notice, this list of conditions and the following disclaimer. 12 1.1 plunky * 2. Redistributions in binary form must reproduce the above copyright 13 1.1 plunky * notice, this list of conditions and the following disclaimer in the 14 1.1 plunky * documentation and/or other materials provided with the distribution. 15 1.1 plunky * 16 1.1 plunky * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 17 1.1 plunky * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 18 1.1 plunky * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 19 1.1 plunky * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 20 1.1 plunky * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 21 1.1 plunky * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 22 1.1 plunky * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 23 1.1 plunky * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 24 1.1 plunky * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 25 1.1 plunky * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 1.1 plunky */ 27 1.1 plunky 28 1.1 plunky #include <sys/cdefs.h> 29 1.12 joerg __RCSID("$NetBSD: bnep.c,v 1.12 2016/10/04 21:40:31 joerg Exp $"); 30 1.1 plunky 31 1.1 plunky #include <bluetooth.h> 32 1.1 plunky #include <sdp.h> 33 1.1 plunky #include <stdarg.h> 34 1.1 plunky #include <string.h> 35 1.1 plunky 36 1.1 plunky #include "btpand.h" 37 1.1 plunky #include "bnep.h" 38 1.1 plunky 39 1.1 plunky static bool bnep_recv_extension(packet_t *); 40 1.1 plunky static size_t bnep_recv_control(channel_t *, uint8_t *, size_t, bool); 41 1.1 plunky static size_t bnep_recv_control_command_not_understood(channel_t *, uint8_t *, size_t); 42 1.1 plunky static size_t bnep_recv_setup_connection_req(channel_t *, uint8_t *, size_t); 43 1.1 plunky static size_t bnep_recv_setup_connection_rsp(channel_t *, uint8_t *, size_t); 44 1.1 plunky static size_t bnep_recv_filter_net_type_set(channel_t *, uint8_t *, size_t); 45 1.1 plunky static size_t bnep_recv_filter_net_type_rsp(channel_t *, uint8_t *, size_t); 46 1.1 plunky static size_t bnep_recv_filter_multi_addr_set(channel_t *, uint8_t *, size_t); 47 1.1 plunky static size_t bnep_recv_filter_multi_addr_rsp(channel_t *, uint8_t *, size_t); 48 1.1 plunky 49 1.1 plunky static bool bnep_pfilter(channel_t *, packet_t *); 50 1.1 plunky static bool bnep_mfilter(channel_t *, packet_t *); 51 1.1 plunky 52 1.11 joerg static const uint8_t NAP_UUID[] = { 53 1.1 plunky 0x00, 0x00, 0x11, 0x16, 54 1.1 plunky 0x00, 0x00, 55 1.1 plunky 0x10, 0x00, 56 1.1 plunky 0x80, 0x00, 57 1.1 plunky 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 58 1.1 plunky }; 59 1.1 plunky 60 1.11 joerg static const uint8_t GN_UUID[] = { 61 1.1 plunky 0x00, 0x00, 0x11, 0x17, 62 1.1 plunky 0x00, 0x00, 63 1.1 plunky 0x10, 0x00, 64 1.1 plunky 0x80, 0x00, 65 1.1 plunky 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb, 66 1.1 plunky }; 67 1.1 plunky 68 1.11 joerg static const uint8_t PANU_UUID[] = { 69 1.1 plunky 0x00, 0x00, 0x11, 0x15, 70 1.1 plunky 0x00, 0x00, 71 1.1 plunky 0x10, 0x00, 72 1.1 plunky 0x80, 0x00, 73 1.1 plunky 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb 74 1.1 plunky }; 75 1.1 plunky 76 1.1 plunky /* 77 1.1 plunky * receive BNEP packet 78 1.1 plunky * return true if packet is to be forwarded 79 1.1 plunky */ 80 1.1 plunky bool 81 1.1 plunky bnep_recv(packet_t *pkt) 82 1.1 plunky { 83 1.1 plunky size_t len; 84 1.1 plunky uint8_t type; 85 1.1 plunky 86 1.1 plunky if (pkt->len < 1) 87 1.1 plunky return false; 88 1.1 plunky 89 1.1 plunky type = pkt->ptr[0]; 90 1.1 plunky packet_adj(pkt, 1); 91 1.1 plunky 92 1.1 plunky switch (BNEP_TYPE(type)) { 93 1.1 plunky case BNEP_GENERAL_ETHERNET: 94 1.1 plunky if (pkt->len < (ETHER_ADDR_LEN * 2) + ETHER_TYPE_LEN) { 95 1.1 plunky log_debug("dropped short packet (type 0x%2.2x)", type); 96 1.1 plunky return false; 97 1.1 plunky } 98 1.1 plunky 99 1.1 plunky pkt->dst = pkt->ptr; 100 1.1 plunky packet_adj(pkt, ETHER_ADDR_LEN); 101 1.1 plunky pkt->src = pkt->ptr; 102 1.1 plunky packet_adj(pkt, ETHER_ADDR_LEN); 103 1.1 plunky pkt->type = pkt->ptr; 104 1.1 plunky packet_adj(pkt, ETHER_TYPE_LEN); 105 1.1 plunky break; 106 1.1 plunky 107 1.1 plunky case BNEP_CONTROL: 108 1.1 plunky len = bnep_recv_control(pkt->chan, pkt->ptr, pkt->len, false); 109 1.1 plunky if (len == 0) 110 1.1 plunky return false; 111 1.1 plunky 112 1.1 plunky packet_adj(pkt, len); 113 1.1 plunky break; 114 1.1 plunky 115 1.1 plunky case BNEP_COMPRESSED_ETHERNET: 116 1.1 plunky if (pkt->len < ETHER_TYPE_LEN) { 117 1.1 plunky log_debug("dropped short packet (type 0x%2.2x)", type); 118 1.1 plunky return false; 119 1.1 plunky } 120 1.1 plunky 121 1.1 plunky pkt->dst = pkt->chan->laddr; 122 1.1 plunky pkt->src = pkt->chan->raddr; 123 1.1 plunky pkt->type = pkt->ptr; 124 1.1 plunky packet_adj(pkt, ETHER_TYPE_LEN); 125 1.1 plunky break; 126 1.1 plunky 127 1.1 plunky case BNEP_COMPRESSED_ETHERNET_SRC_ONLY: 128 1.1 plunky if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 129 1.1 plunky log_debug("dropped short packet (type 0x%2.2x)", type); 130 1.1 plunky return false; 131 1.1 plunky } 132 1.1 plunky 133 1.1 plunky pkt->dst = pkt->chan->laddr; 134 1.1 plunky pkt->src = pkt->ptr; 135 1.1 plunky packet_adj(pkt, ETHER_ADDR_LEN); 136 1.1 plunky pkt->type = pkt->ptr; 137 1.1 plunky packet_adj(pkt, ETHER_TYPE_LEN); 138 1.1 plunky break; 139 1.1 plunky 140 1.1 plunky case BNEP_COMPRESSED_ETHERNET_DST_ONLY: 141 1.1 plunky if (pkt->len < ETHER_ADDR_LEN + ETHER_TYPE_LEN) { 142 1.1 plunky log_debug("dropped short packet (type 0x%2.2x)", type); 143 1.1 plunky return false; 144 1.1 plunky } 145 1.1 plunky 146 1.1 plunky pkt->dst = pkt->ptr; 147 1.1 plunky packet_adj(pkt, ETHER_ADDR_LEN); 148 1.1 plunky pkt->src = pkt->chan->raddr; 149 1.1 plunky pkt->type = pkt->ptr; 150 1.1 plunky packet_adj(pkt, ETHER_TYPE_LEN); 151 1.1 plunky break; 152 1.1 plunky 153 1.1 plunky default: 154 1.1 plunky /* 155 1.1 plunky * Any packet containing a reserved BNEP 156 1.1 plunky * header packet type SHALL be dropped. 157 1.1 plunky */ 158 1.1 plunky 159 1.1 plunky log_debug("dropped packet with reserved type 0x%2.2x", type); 160 1.1 plunky return false; 161 1.1 plunky } 162 1.1 plunky 163 1.1 plunky if (BNEP_TYPE_EXT(type) 164 1.1 plunky && !bnep_recv_extension(pkt)) 165 1.1 plunky return false; /* invalid extensions */ 166 1.1 plunky 167 1.1 plunky if (BNEP_TYPE(type) == BNEP_CONTROL 168 1.1 plunky || pkt->chan->state != CHANNEL_OPEN) 169 1.1 plunky return false; /* no forwarding */ 170 1.1 plunky 171 1.5 plunky if (pkt->len > ETHER_MAX_LEN) 172 1.5 plunky log_debug("received long packet " 173 1.6 kefren "(type=0x%2.2x, proto=0x%4.4x, len=%zu)", 174 1.5 plunky type, be16dec(pkt->type), pkt->len); 175 1.5 plunky 176 1.1 plunky return true; 177 1.1 plunky } 178 1.1 plunky 179 1.1 plunky static bool 180 1.1 plunky bnep_recv_extension(packet_t *pkt) 181 1.1 plunky { 182 1.1 plunky exthdr_t *eh; 183 1.3 plunky size_t len, size; 184 1.3 plunky uint8_t type; 185 1.1 plunky 186 1.1 plunky do { 187 1.1 plunky if (pkt->len < 2) 188 1.1 plunky return false; 189 1.1 plunky 190 1.1 plunky type = pkt->ptr[0]; 191 1.1 plunky size = pkt->ptr[1]; 192 1.1 plunky 193 1.1 plunky if (pkt->len < size + 2) 194 1.1 plunky return false; 195 1.1 plunky 196 1.1 plunky switch (type) { 197 1.1 plunky case BNEP_EXTENSION_CONTROL: 198 1.1 plunky len = bnep_recv_control(pkt->chan, pkt->ptr + 2, size, true); 199 1.1 plunky if (len != size) 200 1.1 plunky log_err("ignored spurious data in exthdr"); 201 1.1 plunky 202 1.1 plunky break; 203 1.1 plunky 204 1.1 plunky default: 205 1.1 plunky /* Unknown extension headers in data packets */ 206 1.1 plunky /* SHALL be forwarded irrespective of any */ 207 1.1 plunky /* network protocol or multicast filter settings */ 208 1.1 plunky /* and any local filtering policy. */ 209 1.1 plunky 210 1.1 plunky eh = malloc(sizeof(exthdr_t)); 211 1.1 plunky if (eh == NULL) { 212 1.1 plunky log_err("exthdr malloc() failed: %m"); 213 1.1 plunky break; 214 1.1 plunky } 215 1.1 plunky 216 1.1 plunky eh->ptr = pkt->ptr; 217 1.1 plunky eh->len = size; 218 1.1 plunky STAILQ_INSERT_TAIL(&pkt->extlist, eh, next); 219 1.1 plunky break; 220 1.1 plunky } 221 1.1 plunky 222 1.1 plunky packet_adj(pkt, size + 2); 223 1.1 plunky } while (BNEP_TYPE_EXT(type)); 224 1.1 plunky 225 1.1 plunky return true; 226 1.1 plunky } 227 1.1 plunky 228 1.1 plunky static size_t 229 1.1 plunky bnep_recv_control(channel_t *chan, uint8_t *ptr, size_t size, bool isext) 230 1.1 plunky { 231 1.1 plunky uint8_t type; 232 1.1 plunky size_t len; 233 1.1 plunky 234 1.1 plunky if (size-- < 1) 235 1.1 plunky return 0; 236 1.1 plunky 237 1.1 plunky type = *ptr++; 238 1.1 plunky 239 1.1 plunky switch (type) { 240 1.1 plunky case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 241 1.1 plunky len = bnep_recv_control_command_not_understood(chan, ptr, size); 242 1.1 plunky break; 243 1.1 plunky 244 1.1 plunky case BNEP_SETUP_CONNECTION_REQUEST: 245 1.1 plunky if (isext) 246 1.1 plunky return 0; /* not allowed in extension headers */ 247 1.1 plunky 248 1.1 plunky len = bnep_recv_setup_connection_req(chan, ptr, size); 249 1.1 plunky break; 250 1.1 plunky 251 1.1 plunky case BNEP_SETUP_CONNECTION_RESPONSE: 252 1.1 plunky if (isext) 253 1.1 plunky return 0; /* not allowed in extension headers */ 254 1.1 plunky 255 1.1 plunky len = bnep_recv_setup_connection_rsp(chan, ptr, size); 256 1.1 plunky break; 257 1.1 plunky 258 1.1 plunky case BNEP_FILTER_NET_TYPE_SET: 259 1.1 plunky len = bnep_recv_filter_net_type_set(chan, ptr, size); 260 1.1 plunky break; 261 1.1 plunky 262 1.1 plunky case BNEP_FILTER_NET_TYPE_RESPONSE: 263 1.1 plunky len = bnep_recv_filter_net_type_rsp(chan, ptr, size); 264 1.1 plunky break; 265 1.1 plunky 266 1.1 plunky case BNEP_FILTER_MULTI_ADDR_SET: 267 1.1 plunky len = bnep_recv_filter_multi_addr_set(chan, ptr, size); 268 1.1 plunky break; 269 1.1 plunky 270 1.1 plunky case BNEP_FILTER_MULTI_ADDR_RESPONSE: 271 1.1 plunky len = bnep_recv_filter_multi_addr_rsp(chan, ptr, size); 272 1.1 plunky break; 273 1.1 plunky 274 1.1 plunky default: 275 1.1 plunky len = 0; 276 1.1 plunky break; 277 1.1 plunky } 278 1.1 plunky 279 1.1 plunky if (len == 0) 280 1.1 plunky bnep_send_control(chan, BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD, type); 281 1.1 plunky 282 1.1 plunky return len; 283 1.1 plunky } 284 1.1 plunky 285 1.1 plunky static size_t 286 1.1 plunky bnep_recv_control_command_not_understood(channel_t *chan, uint8_t *ptr, size_t size) 287 1.1 plunky { 288 1.1 plunky uint8_t type; 289 1.1 plunky 290 1.1 plunky if (size < 1) 291 1.1 plunky return 0; 292 1.1 plunky 293 1.1 plunky type = *ptr++; 294 1.1 plunky log_err("received Control Command Not Understood (0x%2.2x)", type); 295 1.1 plunky 296 1.8 plunky /* we didn't send any reserved commands, just shut them down */ 297 1.8 plunky chan->down(chan); 298 1.1 plunky 299 1.1 plunky return 1; 300 1.1 plunky } 301 1.1 plunky 302 1.1 plunky static size_t 303 1.1 plunky bnep_recv_setup_connection_req(channel_t *chan, uint8_t *ptr, size_t size) 304 1.1 plunky { 305 1.3 plunky size_t len; 306 1.3 plunky uint8_t off; 307 1.1 plunky int src, dst, rsp; 308 1.1 plunky 309 1.1 plunky if (size < 1) 310 1.1 plunky return 0; 311 1.1 plunky 312 1.1 plunky len = *ptr++; 313 1.1 plunky if (size < (len * 2 + 1)) 314 1.1 plunky return 0; 315 1.1 plunky 316 1.1 plunky if (chan->state != CHANNEL_WAIT_CONNECT_REQ 317 1.1 plunky && chan->state != CHANNEL_OPEN) { 318 1.1 plunky log_debug("ignored"); 319 1.1 plunky return (len * 2 + 1); 320 1.1 plunky } 321 1.1 plunky 322 1.1 plunky if (len == 2) 323 1.1 plunky off = 2; 324 1.1 plunky else if (len == 4) 325 1.1 plunky off = 0; 326 1.1 plunky else if (len == 16) 327 1.1 plunky off = 0; 328 1.1 plunky else { 329 1.1 plunky rsp = BNEP_SETUP_INVALID_UUID_SIZE; 330 1.1 plunky goto done; 331 1.1 plunky } 332 1.1 plunky 333 1.1 plunky if (memcmp(ptr, NAP_UUID + off, len) == 0) 334 1.1 plunky dst = SDP_SERVICE_CLASS_NAP; 335 1.1 plunky else if (memcmp(ptr, GN_UUID + off, len) == 0) 336 1.1 plunky dst = SDP_SERVICE_CLASS_GN; 337 1.1 plunky else if (memcmp(ptr, PANU_UUID + off, len) == 0) 338 1.1 plunky dst = SDP_SERVICE_CLASS_PANU; 339 1.1 plunky else 340 1.1 plunky dst = 0; 341 1.1 plunky 342 1.1 plunky if (dst != service_class) { 343 1.1 plunky rsp = BNEP_SETUP_INVALID_DST_UUID; 344 1.1 plunky goto done; 345 1.1 plunky } 346 1.1 plunky 347 1.1 plunky ptr += len; 348 1.1 plunky 349 1.2 plunky if (memcmp(ptr, NAP_UUID + off, len) == 0) 350 1.1 plunky src = SDP_SERVICE_CLASS_NAP; 351 1.1 plunky else if (memcmp(ptr, GN_UUID + off, len) == 0) 352 1.1 plunky src = SDP_SERVICE_CLASS_GN; 353 1.2 plunky else if (memcmp(ptr, PANU_UUID + off, len) == 0) 354 1.1 plunky src = SDP_SERVICE_CLASS_PANU; 355 1.1 plunky else 356 1.1 plunky src = 0; 357 1.1 plunky 358 1.1 plunky if ((dst != SDP_SERVICE_CLASS_PANU && src != SDP_SERVICE_CLASS_PANU) 359 1.1 plunky || src == 0) { 360 1.1 plunky rsp = BNEP_SETUP_INVALID_SRC_UUID; 361 1.1 plunky goto done; 362 1.1 plunky } 363 1.1 plunky 364 1.1 plunky rsp = BNEP_SETUP_SUCCESS; 365 1.1 plunky chan->state = CHANNEL_OPEN; 366 1.1 plunky channel_timeout(chan, 0); 367 1.1 plunky 368 1.1 plunky done: 369 1.1 plunky log_debug("addr %s response 0x%2.2x", 370 1.1 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 371 1.1 plunky 372 1.1 plunky bnep_send_control(chan, BNEP_SETUP_CONNECTION_RESPONSE, rsp); 373 1.9 plunky if (rsp == BNEP_SETUP_SUCCESS) { 374 1.9 plunky bnep_send_control(chan, BNEP_FILTER_NET_TYPE_SET); 375 1.9 plunky bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_SET); 376 1.9 plunky } 377 1.1 plunky return (len * 2 + 1); 378 1.1 plunky } 379 1.1 plunky 380 1.1 plunky static size_t 381 1.1 plunky bnep_recv_setup_connection_rsp(channel_t *chan, uint8_t *ptr, size_t size) 382 1.1 plunky { 383 1.1 plunky int rsp; 384 1.1 plunky 385 1.1 plunky if (size < 2) 386 1.1 plunky return 0; 387 1.1 plunky 388 1.1 plunky rsp = be16dec(ptr); 389 1.1 plunky 390 1.1 plunky if (chan->state != CHANNEL_WAIT_CONNECT_RSP) { 391 1.1 plunky log_debug("ignored"); 392 1.1 plunky return 2; 393 1.1 plunky } 394 1.1 plunky 395 1.1 plunky log_debug("addr %s response 0x%2.2x", 396 1.1 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 397 1.1 plunky 398 1.1 plunky if (rsp == BNEP_SETUP_SUCCESS) { 399 1.1 plunky chan->state = CHANNEL_OPEN; 400 1.1 plunky channel_timeout(chan, 0); 401 1.9 plunky bnep_send_control(chan, BNEP_FILTER_NET_TYPE_SET); 402 1.9 plunky bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_SET); 403 1.1 plunky } else { 404 1.8 plunky chan->down(chan); 405 1.1 plunky } 406 1.1 plunky 407 1.1 plunky return 2; 408 1.1 plunky } 409 1.1 plunky 410 1.1 plunky static size_t 411 1.1 plunky bnep_recv_filter_net_type_set(channel_t *chan, uint8_t *ptr, size_t size) 412 1.1 plunky { 413 1.1 plunky pfilter_t *pf; 414 1.3 plunky int i, nf, rsp; 415 1.3 plunky size_t len; 416 1.1 plunky 417 1.1 plunky if (size < 2) 418 1.1 plunky return 0; 419 1.1 plunky 420 1.1 plunky len = be16dec(ptr); 421 1.1 plunky ptr += 2; 422 1.1 plunky 423 1.1 plunky if (size < (len + 2)) 424 1.1 plunky return 0; 425 1.1 plunky 426 1.1 plunky if (chan->state != CHANNEL_OPEN) { 427 1.1 plunky log_debug("ignored"); 428 1.1 plunky return (len + 2); 429 1.1 plunky } 430 1.1 plunky 431 1.1 plunky nf = len / 4; 432 1.10 plunky if (nf > BNEP_MAX_NET_TYPE_FILTERS) { 433 1.10 plunky rsp = BNEP_FILTER_TOO_MANY_FILTERS; 434 1.10 plunky goto done; 435 1.10 plunky } 436 1.1 plunky pf = malloc(nf * sizeof(pfilter_t)); 437 1.1 plunky if (pf == NULL) { 438 1.1 plunky rsp = BNEP_FILTER_TOO_MANY_FILTERS; 439 1.1 plunky goto done; 440 1.1 plunky } 441 1.1 plunky 442 1.1 plunky log_debug("nf = %d", nf); 443 1.1 plunky 444 1.1 plunky for (i = 0; i < nf; i++) { 445 1.1 plunky pf[i].start = be16dec(ptr); 446 1.1 plunky ptr += 2; 447 1.1 plunky pf[i].end = be16dec(ptr); 448 1.1 plunky ptr += 2; 449 1.1 plunky 450 1.1 plunky if (pf[i].start > pf[i].end) { 451 1.1 plunky free(pf); 452 1.1 plunky rsp = BNEP_FILTER_INVALID_RANGE; 453 1.1 plunky goto done; 454 1.1 plunky } 455 1.1 plunky 456 1.1 plunky log_debug("pf[%d] = %#4.4x, %#4.4x", i, pf[i].start, pf[i].end); 457 1.1 plunky } 458 1.1 plunky 459 1.1 plunky if (chan->pfilter) 460 1.1 plunky free(chan->pfilter); 461 1.1 plunky 462 1.1 plunky chan->pfilter = pf; 463 1.1 plunky chan->npfilter = nf; 464 1.1 plunky 465 1.1 plunky rsp = BNEP_FILTER_SUCCESS; 466 1.1 plunky 467 1.1 plunky done: 468 1.1 plunky log_debug("addr %s response 0x%2.2x", 469 1.1 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 470 1.1 plunky 471 1.1 plunky bnep_send_control(chan, BNEP_FILTER_NET_TYPE_RESPONSE, rsp); 472 1.1 plunky return (len + 2); 473 1.1 plunky } 474 1.1 plunky 475 1.1 plunky static size_t 476 1.1 plunky bnep_recv_filter_net_type_rsp(channel_t *chan, uint8_t *ptr, size_t size) 477 1.1 plunky { 478 1.1 plunky int rsp; 479 1.1 plunky 480 1.1 plunky if (size < 2) 481 1.1 plunky return 0; 482 1.1 plunky 483 1.1 plunky if (chan->state != CHANNEL_OPEN) { 484 1.1 plunky log_debug("ignored"); 485 1.1 plunky return 2; 486 1.1 plunky } 487 1.1 plunky 488 1.1 plunky rsp = be16dec(ptr); 489 1.7 plunky if (rsp != BNEP_FILTER_SUCCESS) 490 1.7 plunky log_err("filter_net_type: addr %s response 0x%2.2x", 491 1.7 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 492 1.1 plunky 493 1.1 plunky return 2; 494 1.1 plunky } 495 1.1 plunky 496 1.1 plunky static size_t 497 1.1 plunky bnep_recv_filter_multi_addr_set(channel_t *chan, uint8_t *ptr, size_t size) 498 1.1 plunky { 499 1.1 plunky mfilter_t *mf; 500 1.3 plunky int i, nf, rsp; 501 1.3 plunky size_t len; 502 1.1 plunky 503 1.1 plunky if (size < 2) 504 1.1 plunky return 0; 505 1.1 plunky 506 1.1 plunky len = be16dec(ptr); 507 1.1 plunky ptr += 2; 508 1.1 plunky 509 1.1 plunky if (size < (len + 2)) 510 1.1 plunky return 0; 511 1.1 plunky 512 1.1 plunky if (chan->state != CHANNEL_OPEN) { 513 1.1 plunky log_debug("ignored"); 514 1.1 plunky return (len + 2); 515 1.1 plunky } 516 1.1 plunky 517 1.1 plunky nf = len / (ETHER_ADDR_LEN * 2); 518 1.10 plunky if (nf > BNEP_MAX_MULTI_ADDR_FILTERS) { 519 1.10 plunky rsp = BNEP_FILTER_TOO_MANY_FILTERS; 520 1.10 plunky goto done; 521 1.10 plunky } 522 1.1 plunky mf = malloc(nf * sizeof(mfilter_t)); 523 1.1 plunky if (mf == NULL) { 524 1.1 plunky rsp = BNEP_FILTER_TOO_MANY_FILTERS; 525 1.1 plunky goto done; 526 1.1 plunky } 527 1.1 plunky 528 1.1 plunky log_debug("nf = %d", nf); 529 1.1 plunky 530 1.1 plunky for (i = 0; i < nf; i++) { 531 1.1 plunky memcpy(mf[i].start, ptr, ETHER_ADDR_LEN); 532 1.1 plunky ptr += ETHER_ADDR_LEN; 533 1.1 plunky 534 1.1 plunky memcpy(mf[i].end, ptr, ETHER_ADDR_LEN); 535 1.1 plunky ptr += ETHER_ADDR_LEN; 536 1.1 plunky 537 1.1 plunky if (memcmp(mf[i].start, mf[i].end, ETHER_ADDR_LEN) > 0) { 538 1.1 plunky free(mf); 539 1.1 plunky rsp = BNEP_FILTER_INVALID_RANGE; 540 1.1 plunky goto done; 541 1.1 plunky } 542 1.1 plunky 543 1.1 plunky log_debug("pf[%d] = " 544 1.1 plunky "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x, " 545 1.1 plunky "%2.2x:%2.2x:%2.2x:%2.2x:%2.2x:%2.2x", i, 546 1.1 plunky mf[i].start[0], mf[i].start[1], mf[i].start[2], 547 1.1 plunky mf[i].start[3], mf[i].start[4], mf[i].start[5], 548 1.1 plunky mf[i].end[0], mf[i].end[1], mf[i].end[2], 549 1.1 plunky mf[i].end[3], mf[i].end[4], mf[i].end[5]); 550 1.1 plunky } 551 1.1 plunky 552 1.1 plunky if (chan->mfilter) 553 1.1 plunky free(chan->mfilter); 554 1.1 plunky 555 1.1 plunky chan->mfilter = mf; 556 1.1 plunky chan->nmfilter = nf; 557 1.1 plunky 558 1.1 plunky rsp = BNEP_FILTER_SUCCESS; 559 1.1 plunky 560 1.1 plunky done: 561 1.1 plunky log_debug("addr %s response 0x%2.2x", 562 1.1 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 563 1.1 plunky 564 1.1 plunky bnep_send_control(chan, BNEP_FILTER_MULTI_ADDR_RESPONSE, rsp); 565 1.1 plunky return (len + 2); 566 1.1 plunky } 567 1.1 plunky 568 1.1 plunky static size_t 569 1.1 plunky bnep_recv_filter_multi_addr_rsp(channel_t *chan, uint8_t *ptr, size_t size) 570 1.1 plunky { 571 1.1 plunky int rsp; 572 1.1 plunky 573 1.1 plunky if (size < 2) 574 1.1 plunky return false; 575 1.1 plunky 576 1.1 plunky if (chan->state != CHANNEL_OPEN) { 577 1.1 plunky log_debug("ignored"); 578 1.1 plunky return 2; 579 1.1 plunky } 580 1.1 plunky 581 1.1 plunky rsp = be16dec(ptr); 582 1.7 plunky if (rsp != BNEP_FILTER_SUCCESS) 583 1.7 plunky log_err("filter_multi_addr: addr %s response 0x%2.2x", 584 1.7 plunky ether_ntoa((struct ether_addr *)chan->raddr), rsp); 585 1.1 plunky 586 1.1 plunky return 2; 587 1.1 plunky } 588 1.1 plunky 589 1.1 plunky void 590 1.12 joerg bnep_send_control(channel_t *chan, int type, ...) 591 1.1 plunky { 592 1.1 plunky packet_t *pkt; 593 1.1 plunky uint8_t *p; 594 1.1 plunky va_list ap; 595 1.1 plunky 596 1.4 plunky assert(chan->state != CHANNEL_CLOSED); 597 1.1 plunky 598 1.1 plunky pkt = packet_alloc(chan); 599 1.1 plunky if (pkt == NULL) 600 1.1 plunky return; 601 1.1 plunky 602 1.1 plunky p = pkt->ptr; 603 1.1 plunky va_start(ap, type); 604 1.1 plunky 605 1.1 plunky *p++ = BNEP_CONTROL; 606 1.1 plunky *p++ = type; 607 1.1 plunky 608 1.1 plunky switch(type) { 609 1.1 plunky case BNEP_CONTROL_COMMAND_NOT_UNDERSTOOD: 610 1.1 plunky *p++ = va_arg(ap, int); 611 1.1 plunky break; 612 1.1 plunky 613 1.1 plunky case BNEP_SETUP_CONNECTION_REQUEST: 614 1.1 plunky *p++ = va_arg(ap, int); 615 1.1 plunky be16enc(p, va_arg(ap, int)); 616 1.1 plunky p += 2; 617 1.1 plunky be16enc(p, va_arg(ap, int)); 618 1.1 plunky p += 2; 619 1.1 plunky break; 620 1.1 plunky 621 1.1 plunky case BNEP_SETUP_CONNECTION_RESPONSE: 622 1.1 plunky case BNEP_FILTER_NET_TYPE_RESPONSE: 623 1.1 plunky case BNEP_FILTER_MULTI_ADDR_RESPONSE: 624 1.1 plunky be16enc(p, va_arg(ap, int)); 625 1.1 plunky p += 2; 626 1.1 plunky break; 627 1.1 plunky 628 1.9 plunky case BNEP_FILTER_NET_TYPE_SET: 629 1.9 plunky case BNEP_FILTER_MULTI_ADDR_SET: 630 1.9 plunky be16enc(p, 0); /* just clear filters for now */ 631 1.9 plunky p += 2; 632 1.9 plunky break; 633 1.9 plunky 634 1.1 plunky default: 635 1.1 plunky log_err("Can't send control type 0x%2.2x", type); 636 1.1 plunky break; 637 1.1 plunky } 638 1.1 plunky 639 1.1 plunky va_end(ap); 640 1.1 plunky pkt->len = p - pkt->ptr; 641 1.1 plunky 642 1.1 plunky channel_put(chan, pkt); 643 1.1 plunky packet_free(pkt); 644 1.1 plunky } 645 1.1 plunky 646 1.1 plunky /* 647 1.1 plunky * BNEP send packet routine 648 1.1 plunky * return true if packet can be removed from queue 649 1.1 plunky */ 650 1.1 plunky bool 651 1.1 plunky bnep_send(channel_t *chan, packet_t *pkt) 652 1.1 plunky { 653 1.1 plunky struct iovec iov[2]; 654 1.1 plunky uint8_t *p, *type, *proto; 655 1.1 plunky exthdr_t *eh; 656 1.1 plunky bool src, dst; 657 1.1 plunky size_t nw; 658 1.1 plunky 659 1.1 plunky if (pkt->type == NULL) { 660 1.1 plunky iov[0].iov_base = pkt->ptr; 661 1.1 plunky iov[0].iov_len = pkt->len; 662 1.1 plunky iov[1].iov_base = NULL; 663 1.1 plunky iov[1].iov_len = 0; 664 1.1 plunky } else { 665 1.1 plunky p = chan->sendbuf; 666 1.1 plunky 667 1.1 plunky dst = (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) != 0); 668 1.1 plunky src = (memcmp(pkt->src, chan->laddr, ETHER_ADDR_LEN) != 0); 669 1.1 plunky 670 1.1 plunky type = p; 671 1.1 plunky p += 1; 672 1.1 plunky 673 1.1 plunky if (dst && src) 674 1.1 plunky *type = BNEP_GENERAL_ETHERNET; 675 1.1 plunky else if (dst && !src) 676 1.1 plunky *type = BNEP_COMPRESSED_ETHERNET_DST_ONLY; 677 1.1 plunky else if (!dst && src) 678 1.1 plunky *type = BNEP_COMPRESSED_ETHERNET_SRC_ONLY; 679 1.1 plunky else /* (!dst && !src) */ 680 1.1 plunky *type = BNEP_COMPRESSED_ETHERNET; 681 1.1 plunky 682 1.1 plunky if (dst) { 683 1.1 plunky memcpy(p, pkt->dst, ETHER_ADDR_LEN); 684 1.1 plunky p += ETHER_ADDR_LEN; 685 1.1 plunky } 686 1.1 plunky 687 1.1 plunky if (src) { 688 1.1 plunky memcpy(p, pkt->src, ETHER_ADDR_LEN); 689 1.1 plunky p += ETHER_ADDR_LEN; 690 1.1 plunky } 691 1.1 plunky 692 1.1 plunky proto = p; 693 1.1 plunky memcpy(p, pkt->type, ETHER_TYPE_LEN); 694 1.1 plunky p += ETHER_TYPE_LEN; 695 1.1 plunky 696 1.1 plunky STAILQ_FOREACH(eh, &pkt->extlist, next) { 697 1.1 plunky if (p + eh->len > chan->sendbuf + chan->mtu) 698 1.1 plunky break; 699 1.1 plunky 700 1.1 plunky *type |= BNEP_EXT; 701 1.1 plunky type = p; 702 1.1 plunky 703 1.1 plunky memcpy(p, eh->ptr, eh->len); 704 1.1 plunky p += eh->len; 705 1.1 plunky } 706 1.1 plunky 707 1.1 plunky *type &= ~BNEP_EXT; 708 1.1 plunky 709 1.1 plunky iov[0].iov_base = chan->sendbuf; 710 1.1 plunky iov[0].iov_len = (p - chan->sendbuf); 711 1.1 plunky 712 1.1 plunky if ((chan->npfilter == 0 || bnep_pfilter(chan, pkt)) 713 1.1 plunky && (chan->nmfilter == 0 || bnep_mfilter(chan, pkt))) { 714 1.1 plunky iov[1].iov_base = pkt->ptr; 715 1.1 plunky iov[1].iov_len = pkt->len; 716 1.1 plunky } else if (be16dec(proto) == ETHERTYPE_VLAN 717 1.1 plunky && pkt->len >= ETHER_VLAN_ENCAP_LEN) { 718 1.1 plunky iov[1].iov_base = pkt->ptr; 719 1.1 plunky iov[1].iov_len = ETHER_VLAN_ENCAP_LEN; 720 1.1 plunky } else { 721 1.1 plunky iov[1].iov_base = NULL; 722 1.1 plunky iov[1].iov_len = 0; 723 1.1 plunky memset(proto, 0, ETHER_TYPE_LEN); 724 1.1 plunky } 725 1.1 plunky } 726 1.1 plunky 727 1.1 plunky if (iov[0].iov_len + iov[1].iov_len > chan->mtu) { 728 1.1 plunky log_err("packet exceeded MTU (dropped)"); 729 1.1 plunky return false; 730 1.1 plunky } 731 1.1 plunky 732 1.1 plunky nw = writev(chan->fd, iov, __arraycount(iov)); 733 1.1 plunky return (nw > 0); 734 1.1 plunky } 735 1.1 plunky 736 1.1 plunky static bool 737 1.1 plunky bnep_pfilter(channel_t *chan, packet_t *pkt) 738 1.1 plunky { 739 1.1 plunky int proto, i; 740 1.1 plunky 741 1.1 plunky proto = be16dec(pkt->type); 742 1.1 plunky if (proto == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */ 743 1.1 plunky if (pkt->len < 4) 744 1.1 plunky return false; 745 1.1 plunky 746 1.1 plunky proto = be16dec(pkt->ptr + 2); 747 1.1 plunky } 748 1.1 plunky 749 1.1 plunky for (i = 0; i < chan->npfilter; i++) { 750 1.1 plunky if (chan->pfilter[i].start <= proto 751 1.1 plunky && chan->pfilter[i].end >=proto) 752 1.1 plunky return true; 753 1.1 plunky } 754 1.1 plunky 755 1.1 plunky return false; 756 1.1 plunky } 757 1.1 plunky 758 1.1 plunky static bool 759 1.1 plunky bnep_mfilter(channel_t *chan, packet_t *pkt) 760 1.1 plunky { 761 1.1 plunky int i; 762 1.1 plunky 763 1.1 plunky if (!ETHER_IS_MULTICAST(pkt->dst)) 764 1.1 plunky return true; 765 1.1 plunky 766 1.1 plunky for (i = 0; i < chan->nmfilter; i++) { 767 1.1 plunky if (memcmp(pkt->dst, chan->mfilter[i].start, ETHER_ADDR_LEN) >= 0 768 1.1 plunky && memcmp(pkt->dst, chan->mfilter[i].end, ETHER_ADDR_LEN) <= 0) 769 1.1 plunky return true; 770 1.1 plunky } 771 1.1 plunky 772 1.1 plunky return false; 773 1.1 plunky } 774