Sat May 2 20:07:52 2009 UTC ()
use assert() instead of _DIAGASSERT()

suggested by emax@freebsd


(plunky)
diff -r1.3 -r1.4 src/usr.sbin/btpand/bnep.c
diff -r1.1 -r1.2 src/usr.sbin/btpand/channel.c
diff -r1.1 -r1.2 src/usr.sbin/btpand/packet.c
diff -r1.2 -r1.3 src/usr.sbin/btpand/tap.c
cvs diff -r1.3 -r1.4 src/usr.sbin/btpand/bnep.c (switch to unified diff)

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

--- src/usr.sbin/btpand/channel.c 2008/08/17 13:20:57 1.1
+++ src/usr.sbin/btpand/channel.c 2009/05/02 20:07:51 1.2
@@ -1,326 +1,326 @@ @@ -1,326 +1,326 @@
1/* $NetBSD: channel.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */ 1/* $NetBSD: channel.c,v 1.2 2009/05/02 20:07:51 plunky Exp $ */
2 2
3/*- 3/*-
4 * Copyright (c) 2008 Iain Hibbert 4 * Copyright (c) 2008 Iain Hibbert
5 * All rights reserved. 5 * All rights reserved.
6 * 6 *
7 * Redistribution and use in source and binary forms, with or without 7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions 8 * modification, are permitted provided that the following conditions
9 * are met: 9 * are met:
10 * 1. Redistributions of source code must retain the above copyright 10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer. 11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright 12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the 13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution. 14 * documentation and/or other materials provided with the distribution.
15 * 15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28#include <sys/cdefs.h> 28#include <sys/cdefs.h>
29__RCSID("$NetBSD: channel.c,v 1.1 2008/08/17 13:20:57 plunky Exp $"); 29__RCSID("$NetBSD: channel.c,v 1.2 2009/05/02 20:07:51 plunky Exp $");
30 30
31#include <sys/ioctl.h> 31#include <sys/ioctl.h>
32 32
33#include <unistd.h> 33#include <unistd.h>
34 34
35#include "btpand.h" 35#include "btpand.h"
36 36
37static struct chlist channel_list; 37static struct chlist channel_list;
38static int channel_count; 38static int channel_count;
39static int channel_tick; 39static int channel_tick;
40 40
41static void channel_start(int, short, void *); 41static void channel_start(int, short, void *);
42static void channel_read(int, short, void *); 42static void channel_read(int, short, void *);
43static void channel_dispatch(packet_t *); 43static void channel_dispatch(packet_t *);
44static void channel_watchdog(int, short, void *); 44static void channel_watchdog(int, short, void *);
45 45
46void 46void
47channel_init(void) 47channel_init(void)
48{ 48{
49 49
50 LIST_INIT(&channel_list); 50 LIST_INIT(&channel_list);
51} 51}
52 52
53channel_t * 53channel_t *
54channel_alloc(void) 54channel_alloc(void)
55{ 55{
56 channel_t *chan; 56 channel_t *chan;
57 57
58 chan = malloc(sizeof(channel_t)); 58 chan = malloc(sizeof(channel_t));
59 if (chan == NULL) { 59 if (chan == NULL) {
60 log_err("%s() failed: %m", __func__); 60 log_err("%s() failed: %m", __func__);
61 return NULL; 61 return NULL;
62 } 62 }
63 63
64 memset(chan, 0, sizeof(channel_t)); 64 memset(chan, 0, sizeof(channel_t));
65 STAILQ_INIT(&chan->pktlist); 65 STAILQ_INIT(&chan->pktlist);
66 chan->state = CHANNEL_CLOSED; 66 chan->state = CHANNEL_CLOSED;
67 LIST_INSERT_HEAD(&channel_list, chan, next); 67 LIST_INSERT_HEAD(&channel_list, chan, next);
68 68
69 server_update(++channel_count); 69 server_update(++channel_count);
70 70
71 return chan; 71 return chan;
72} 72}
73 73
74bool 74bool
75channel_open(channel_t *chan, int fd) 75channel_open(channel_t *chan, int fd)
76{ 76{
77 int n; 77 int n;
78 78
79 _DIAGASSERT(chan->refcnt == 0); 79 assert(chan->refcnt == 0);
80 _DIAGASSERT(chan->state != CHANNEL_CLOSED); 80 assert(chan->state != CHANNEL_CLOSED);
81 81
82 if (chan->mtu > 0) { 82 if (chan->mtu > 0) {
83 chan->sendbuf = malloc(chan->mtu); 83 chan->sendbuf = malloc(chan->mtu);
84 if (chan->sendbuf == NULL) { 84 if (chan->sendbuf == NULL) {
85 log_err("Could not malloc channel sendbuf: %m"); 85 log_err("Could not malloc channel sendbuf: %m");
86 return false; 86 return false;
87 } 87 }
88 } 88 }
89 89
90 n = 1; 90 n = 1;
91 if (ioctl(fd, FIONBIO, &n) == -1) { 91 if (ioctl(fd, FIONBIO, &n) == -1) {
92 log_err("Could not set non-blocking IO: %m"); 92 log_err("Could not set non-blocking IO: %m");
93 return false; 93 return false;
94 } 94 }
95 95
96 event_set(&chan->rd_ev, fd, EV_READ | EV_PERSIST, channel_read, chan); 96 event_set(&chan->rd_ev, fd, EV_READ | EV_PERSIST, channel_read, chan);
97 if (event_add(&chan->rd_ev, NULL) == -1) { 97 if (event_add(&chan->rd_ev, NULL) == -1) {
98 log_err("Could not add channel read event: %m"); 98 log_err("Could not add channel read event: %m");
99 return false; 99 return false;
100 } 100 }
101 101
102 event_set(&chan->wr_ev, fd, EV_WRITE, channel_start, chan); 102 event_set(&chan->wr_ev, fd, EV_WRITE, channel_start, chan);
103 103
104 chan->refcnt++; 104 chan->refcnt++;
105 chan->fd = fd; 105 chan->fd = fd;
106 106
107 log_debug("(fd#%d)", chan->fd); 107 log_debug("(fd#%d)", chan->fd);
108 108
109 return true; 109 return true;
110} 110}
111 111
112void 112void
113channel_close(channel_t *chan) 113channel_close(channel_t *chan)
114{ 114{
115 pkthdr_t *ph; 115 pkthdr_t *ph;
116 116
117 _DIAGASSERT(chan->state != CHANNEL_CLOSED); 117 assert(chan->state != CHANNEL_CLOSED);
118 118
119 log_debug("(fd#%d)", chan->fd); 119 log_debug("(fd#%d)", chan->fd);
120 120
121 chan->state = CHANNEL_CLOSED; 121 chan->state = CHANNEL_CLOSED;
122 event_del(&chan->rd_ev); 122 event_del(&chan->rd_ev);
123 event_del(&chan->wr_ev); 123 event_del(&chan->wr_ev);
124 close(chan->fd); 124 close(chan->fd);
125 chan->refcnt--; 125 chan->refcnt--;
126 chan->tick = 0; 126 chan->tick = 0;
127 127
128 while ((ph = STAILQ_FIRST(&chan->pktlist)) != NULL) { 128 while ((ph = STAILQ_FIRST(&chan->pktlist)) != NULL) {
129 STAILQ_REMOVE_HEAD(&chan->pktlist, next); 129 STAILQ_REMOVE_HEAD(&chan->pktlist, next);
130 pkthdr_free(ph); 130 pkthdr_free(ph);
131 chan->qlen--; 131 chan->qlen--;
132 } 132 }
133 133
134 if (chan->refcnt == 0) 134 if (chan->refcnt == 0)
135 channel_free(chan); 135 channel_free(chan);
136} 136}
137 137
138void 138void
139channel_free(channel_t *chan) 139channel_free(channel_t *chan)
140{ 140{
141 141
142 _DIAGASSERT(chan->refcnt == 0); 142 assert(chan->refcnt == 0);
143 _DIAGASSERT(chan->state == CHANNEL_CLOSED); 143 assert(chan->state == CHANNEL_CLOSED);
144 _DIAGASSERT(chan->qlen == 0); 144 assert(chan->qlen == 0);
145 _DIAGASSERT(STAILQ_EMPTY(&chan->pktlist)); 145 assert(STAILQ_EMPTY(&chan->pktlist));
146 146
147 LIST_REMOVE(chan, next); 147 LIST_REMOVE(chan, next);
148 free(chan->pfilter); 148 free(chan->pfilter);
149 free(chan->mfilter); 149 free(chan->mfilter);
150 free(chan->sendbuf); 150 free(chan->sendbuf);
151 free(chan); 151 free(chan);
152 152
153 server_update(--channel_count); 153 server_update(--channel_count);
154 154
155 if (server_limit == 0) { 155 if (server_limit == 0) {
156 log_info("connection closed, exiting"); 156 log_info("connection closed, exiting");
157 exit(EXIT_SUCCESS); 157 exit(EXIT_SUCCESS);
158 } 158 }
159} 159}
160 160
161static void 161static void
162channel_start(int fd, short ev, void *arg) 162channel_start(int fd, short ev, void *arg)
163{ 163{
164 channel_t *chan = arg; 164 channel_t *chan = arg;
165 pkthdr_t *ph; 165 pkthdr_t *ph;
166 166
167 chan->oactive = true; 167 chan->oactive = true;
168 168
169 while (chan->qlen > 0) { 169 while (chan->qlen > 0) {
170 ph = STAILQ_FIRST(&chan->pktlist); 170 ph = STAILQ_FIRST(&chan->pktlist);
171 171
172 channel_timeout(chan, 10); 172 channel_timeout(chan, 10);
173 if (chan->send(chan, ph->data) == false) { 173 if (chan->send(chan, ph->data) == false) {
174 if (event_add(&chan->wr_ev, NULL) == -1) { 174 if (event_add(&chan->wr_ev, NULL) == -1) {
175 log_err("Could not add channel write event: %m"); 175 log_err("Could not add channel write event: %m");
176 channel_close(chan); 176 channel_close(chan);
177 } 177 }
178 return; 178 return;
179 } 179 }
180 180
181 STAILQ_REMOVE_HEAD(&chan->pktlist, next); 181 STAILQ_REMOVE_HEAD(&chan->pktlist, next);
182 pkthdr_free(ph); 182 pkthdr_free(ph);
183 chan->qlen--; 183 chan->qlen--;
184 } 184 }
185 185
186 channel_timeout(chan, 0); 186 channel_timeout(chan, 0);
187 chan->oactive = false; 187 chan->oactive = false;
188} 188}
189 189
190static void 190static void
191channel_read(int fd, short ev, void *arg) 191channel_read(int fd, short ev, void *arg)
192{ 192{
193 channel_t *chan = arg; 193 channel_t *chan = arg;
194 packet_t *pkt; 194 packet_t *pkt;
195 ssize_t nr; 195 ssize_t nr;
196 196
197 pkt = packet_alloc(chan); 197 pkt = packet_alloc(chan);
198 if (pkt == NULL) { 198 if (pkt == NULL) {
199 channel_close(chan); 199 channel_close(chan);
200 return; 200 return;
201 } 201 }
202 202
203 nr = read(fd, pkt->buf, chan->mru); 203 nr = read(fd, pkt->buf, chan->mru);
204 if (nr == -1) { 204 if (nr == -1) {
205 log_err("channel read error: %m"); 205 log_err("channel read error: %m");
206 packet_free(pkt); 206 packet_free(pkt);
207 channel_close(chan); 207 channel_close(chan);
208 return; 208 return;
209 } 209 }
210 if (nr == 0) { /* EOF */ 210 if (nr == 0) { /* EOF */
211 log_debug("(fd#%d) EOF", fd); 211 log_debug("(fd#%d) EOF", fd);
212 packet_free(pkt); 212 packet_free(pkt);
213 channel_close(chan); 213 channel_close(chan);
214 return; 214 return;
215 } 215 }
216 pkt->len = nr; 216 pkt->len = nr;
217 217
218 if (chan->recv(pkt) == true) 218 if (chan->recv(pkt) == true)
219 channel_dispatch(pkt); 219 channel_dispatch(pkt);
220 220
221 packet_free(pkt); 221 packet_free(pkt);
222} 222}
223 223
224static void 224static void
225channel_dispatch(packet_t *pkt) 225channel_dispatch(packet_t *pkt)
226{ 226{
227 channel_t *chan; 227 channel_t *chan;
228 228
229 /* 229 /*
230 * This is simple routing. I'm not sure if its allowed by 230 * This is simple routing. I'm not sure if its allowed by
231 * the PAN or BNEP specifications, but it seems logical 231 * the PAN or BNEP specifications, but it seems logical
232 * to send unicast packets to connected destinations where 232 * to send unicast packets to connected destinations where
233 * possible. 233 * possible.
234 */ 234 */
235 if (!ETHER_IS_MULTICAST(pkt->dst)) { 235 if (!ETHER_IS_MULTICAST(pkt->dst)) {
236 LIST_FOREACH(chan, &channel_list, next) { 236 LIST_FOREACH(chan, &channel_list, next) {
237 if (chan == pkt->chan 237 if (chan == pkt->chan
238 || chan->state != CHANNEL_OPEN) 238 || chan->state != CHANNEL_OPEN)
239 continue; 239 continue;
240 240
241 if (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) == 0) { 241 if (memcmp(pkt->dst, chan->raddr, ETHER_ADDR_LEN) == 0) {
242 if (chan->qlen > CHANNEL_MAXQLEN) 242 if (chan->qlen > CHANNEL_MAXQLEN)
243 log_notice("Queue overflow"); 243 log_notice("Queue overflow");
244 else 244 else
245 channel_put(chan, pkt); 245 channel_put(chan, pkt);
246 246
247 return; 247 return;
248 } 248 }
249 } 249 }
250 } 250 }
251 251
252 LIST_FOREACH(chan, &channel_list, next) { 252 LIST_FOREACH(chan, &channel_list, next) {
253 if (chan == pkt->chan 253 if (chan == pkt->chan
254 || chan->state != CHANNEL_OPEN) 254 || chan->state != CHANNEL_OPEN)
255 continue; 255 continue;
256 256
257 if (chan->qlen > CHANNEL_MAXQLEN) { 257 if (chan->qlen > CHANNEL_MAXQLEN) {
258 log_notice("Queue overflow"); 258 log_notice("Queue overflow");
259 continue; 259 continue;
260 } 260 }
261 261
262 channel_put(chan, pkt); 262 channel_put(chan, pkt);
263 } 263 }
264} 264}
265 265
266void 266void
267channel_put(channel_t *chan, packet_t *pkt) 267channel_put(channel_t *chan, packet_t *pkt)
268{ 268{
269 pkthdr_t *ph; 269 pkthdr_t *ph;
270 270
271 ph = pkthdr_alloc(pkt); 271 ph = pkthdr_alloc(pkt);
272 if (ph == NULL) 272 if (ph == NULL)
273 return; 273 return;
274 274
275 chan->qlen++; 275 chan->qlen++;
276 STAILQ_INSERT_TAIL(&chan->pktlist, ph, next); 276 STAILQ_INSERT_TAIL(&chan->pktlist, ph, next);
277 277
278 if (!chan->oactive) 278 if (!chan->oactive)
279 channel_start(chan->fd, EV_WRITE, chan); 279 channel_start(chan->fd, EV_WRITE, chan);
280} 280}
281 281
282/* 282/*
283 * Simple watchdog timer, only ticks when it is required and 283 * Simple watchdog timer, only ticks when it is required and
284 * closes the channel down if it times out. 284 * closes the channel down if it times out.
285 */ 285 */
286void 286void
287channel_timeout(channel_t *chan, int to) 287channel_timeout(channel_t *chan, int to)
288{ 288{
289 static struct event ev; 289 static struct event ev;
290 290
291 if (to == 0) 291 if (to == 0)
292 chan->tick = 0; 292 chan->tick = 0;
293 else 293 else
294 chan->tick = (channel_tick + to) % 60; 294 chan->tick = (channel_tick + to) % 60;
295 295
296 if (channel_tick == 0) { 296 if (channel_tick == 0) {
297 evtimer_set(&ev, channel_watchdog, &ev); 297 evtimer_set(&ev, channel_watchdog, &ev);
298 channel_watchdog(0, 0, &ev); 298 channel_watchdog(0, 0, &ev);
299 } 299 }
300} 300}
301 301
302static void 302static void
303channel_watchdog(int fd, short ev, void *arg) 303channel_watchdog(int fd, short ev, void *arg)
304{ 304{
305 static struct timeval tv = { .tv_sec = 1 }; 305 static struct timeval tv = { .tv_sec = 1 };
306 channel_t *chan, *next; 306 channel_t *chan, *next;
307 int tick; 307 int tick;
308 308
309 tick = (channel_tick % 60) + 1; 309 tick = (channel_tick % 60) + 1;
310 channel_tick = 0; 310 channel_tick = 0;
311 311
312 next = LIST_FIRST(&channel_list); 312 next = LIST_FIRST(&channel_list);
313 while ((chan = next) != NULL) { 313 while ((chan = next) != NULL) {
314 next = LIST_NEXT(chan, next); 314 next = LIST_NEXT(chan, next);
315 315
316 if (chan->tick == tick) 316 if (chan->tick == tick)
317 channel_close(chan); 317 channel_close(chan);
318 else if (chan->tick != 0) 318 else if (chan->tick != 0)
319 channel_tick = tick; 319 channel_tick = tick;
320 } 320 }
321 321
322 if (channel_tick != 0 && evtimer_add(arg, &tv) < 0) { 322 if (channel_tick != 0 && evtimer_add(arg, &tv) < 0) {
323 log_err("Could not add watchdog event: %m"); 323 log_err("Could not add watchdog event: %m");
324 exit(EXIT_FAILURE); 324 exit(EXIT_FAILURE);
325 } 325 }
326} 326}
cvs diff -r1.1 -r1.2 src/usr.sbin/btpand/packet.c (switch to unified diff)

--- src/usr.sbin/btpand/packet.c 2008/08/17 13:20:57 1.1
+++ src/usr.sbin/btpand/packet.c 2009/05/02 20:07:51 1.2
@@ -1,108 +1,108 @@ @@ -1,108 +1,108 @@
1/* $NetBSD: packet.c,v 1.1 2008/08/17 13:20:57 plunky Exp $ */ 1/* $NetBSD: packet.c,v 1.2 2009/05/02 20:07:51 plunky Exp $ */
2 2
3/*- 3/*-
4 * Copyright (c) 2008 Iain Hibbert 4 * Copyright (c) 2008 Iain Hibbert
5 * All rights reserved. 5 * All rights reserved.
6 * 6 *
7 * Redistribution and use in source and binary forms, with or without 7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions 8 * modification, are permitted provided that the following conditions
9 * are met: 9 * are met:
10 * 1. Redistributions of source code must retain the above copyright 10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer. 11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright 12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the 13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution. 14 * documentation and/or other materials provided with the distribution.
15 * 15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28#include <sys/cdefs.h> 28#include <sys/cdefs.h>
29__RCSID("$NetBSD: packet.c,v 1.1 2008/08/17 13:20:57 plunky Exp $"); 29__RCSID("$NetBSD: packet.c,v 1.2 2009/05/02 20:07:51 plunky Exp $");
30 30
31#include "btpand.h" 31#include "btpand.h"
32 32
33packet_t * 33packet_t *
34packet_alloc(channel_t *chan) 34packet_alloc(channel_t *chan)
35{ 35{
36 packet_t *pkt; 36 packet_t *pkt;
37 37
38 pkt = malloc(sizeof(packet_t) + chan->mru); 38 pkt = malloc(sizeof(packet_t) + chan->mru);
39 if (pkt == NULL) { 39 if (pkt == NULL) {
40 log_err("%s() failed: %m", __func__); 40 log_err("%s() failed: %m", __func__);
41 return NULL; 41 return NULL;
42 } 42 }
43 43
44 memset(pkt, 0, sizeof(packet_t)); 44 memset(pkt, 0, sizeof(packet_t));
45 STAILQ_INIT(&pkt->extlist); 45 STAILQ_INIT(&pkt->extlist);
46 pkt->ptr = pkt->buf; 46 pkt->ptr = pkt->buf;
47 47
48 pkt->chan = chan; 48 pkt->chan = chan;
49 chan->refcnt++; 49 chan->refcnt++;
50 50
51 return pkt; 51 return pkt;
52} 52}
53 53
54void 54void
55packet_free(packet_t *pkt) 55packet_free(packet_t *pkt)
56{ 56{
57 exthdr_t *eh; 57 exthdr_t *eh;
58 58
59 if (pkt->refcnt-- > 0) 59 if (pkt->refcnt-- > 0)
60 return; 60 return;
61 61
62 while ((eh = STAILQ_FIRST(&pkt->extlist)) != NULL) { 62 while ((eh = STAILQ_FIRST(&pkt->extlist)) != NULL) {
63 STAILQ_REMOVE_HEAD(&pkt->extlist, next); 63 STAILQ_REMOVE_HEAD(&pkt->extlist, next);
64 free(eh); 64 free(eh);
65 } 65 }
66 66
67 pkt->chan->refcnt--; 67 pkt->chan->refcnt--;
68 if (pkt->chan->refcnt == 0) 68 if (pkt->chan->refcnt == 0)
69 channel_free(pkt->chan); 69 channel_free(pkt->chan);
70 70
71 free(pkt); 71 free(pkt);
72} 72}
73 73
74void 74void
75packet_adj(packet_t *pkt, size_t size) 75packet_adj(packet_t *pkt, size_t size)
76{ 76{
77 77
78 _DIAGASSERT(pkt->refcnt == 0); 78 assert(pkt->refcnt == 0);
79 _DIAGASSERT(pkt->len >= size); 79 assert(pkt->len >= size);
80 80
81 pkt->ptr += size; 81 pkt->ptr += size;
82 pkt->len -= size; 82 pkt->len -= size;
83} 83}
84 84
85pkthdr_t * 85pkthdr_t *
86pkthdr_alloc(packet_t *pkt) 86pkthdr_alloc(packet_t *pkt)
87{ 87{
88 pkthdr_t *ph; 88 pkthdr_t *ph;
89 89
90 ph = malloc(sizeof(pkthdr_t)); 90 ph = malloc(sizeof(pkthdr_t));
91 if (ph == NULL) { 91 if (ph == NULL) {
92 log_err("%s() failed: %m", __func__); 92 log_err("%s() failed: %m", __func__);
93 return NULL; 93 return NULL;
94 } 94 }
95 95
96 ph->data = pkt; 96 ph->data = pkt;
97 pkt->refcnt++; 97 pkt->refcnt++;
98 98
99 return ph; 99 return ph;
100} 100}
101 101
102void 102void
103pkthdr_free(pkthdr_t *ph) 103pkthdr_free(pkthdr_t *ph)
104{ 104{
105 105
106 packet_free(ph->data); 106 packet_free(ph->data);
107 free(ph); 107 free(ph);
108} 108}
cvs diff -r1.2 -r1.3 src/usr.sbin/btpand/tap.c (switch to unified diff)

--- src/usr.sbin/btpand/tap.c 2009/03/10 22:12:17 1.2
+++ src/usr.sbin/btpand/tap.c 2009/05/02 20:07:51 1.3
@@ -1,161 +1,161 @@ @@ -1,161 +1,161 @@
1/* $NetBSD: tap.c,v 1.2 2009/03/10 22:12:17 plunky Exp $ */ 1/* $NetBSD: tap.c,v 1.3 2009/05/02 20:07:51 plunky Exp $ */
2 2
3/*- 3/*-
4 * Copyright (c) 2008 Iain Hibbert 4 * Copyright (c) 2008 Iain Hibbert
5 * All rights reserved. 5 * All rights reserved.
6 * 6 *
7 * Redistribution and use in source and binary forms, with or without 7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions 8 * modification, are permitted provided that the following conditions
9 * are met: 9 * are met:
10 * 1. Redistributions of source code must retain the above copyright 10 * 1. Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer. 11 * notice, this list of conditions and the following disclaimer.
12 * 2. Redistributions in binary form must reproduce the above copyright 12 * 2. Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the 13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution. 14 * documentation and/or other materials provided with the distribution.
15 * 15 *
16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR 16 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 17 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. 18 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, 19 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT 20 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 21 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 22 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28#include <sys/cdefs.h> 28#include <sys/cdefs.h>
29__RCSID("$NetBSD: tap.c,v 1.2 2009/03/10 22:12:17 plunky Exp $"); 29__RCSID("$NetBSD: tap.c,v 1.3 2009/05/02 20:07:51 plunky Exp $");
30 30
31#include <sys/ioctl.h> 31#include <sys/ioctl.h>
32#include <sys/uio.h> 32#include <sys/uio.h>
33 33
34#include <net/if_dl.h> 34#include <net/if_dl.h>
35#include <net/if_tap.h> 35#include <net/if_tap.h>
36 36
37#include <fcntl.h> 37#include <fcntl.h>
38#include <unistd.h> 38#include <unistd.h>
39#include <util.h> 39#include <util.h>
40 40
41#include "btpand.h" 41#include "btpand.h"
42 42
43static bool tap_send(channel_t *, packet_t *); 43static bool tap_send(channel_t *, packet_t *);
44static bool tap_recv(packet_t *); 44static bool tap_recv(packet_t *);
45 45
46void 46void
47tap_init(void) 47tap_init(void)
48{ 48{
49 channel_t *chan; 49 channel_t *chan;
50 struct sockaddr_dl *sdl; 50 struct sockaddr_dl *sdl;
51 struct if_laddrreq iflr; 51 struct if_laddrreq iflr;
52 struct ifreq ifr; 52 struct ifreq ifr;
53 int fd, s; 53 int fd, s;
54 54
55 fd = open(interface_name, O_RDWR); 55 fd = open(interface_name, O_RDWR);
56 if (fd == -1) { 56 if (fd == -1) {
57 log_err("Could not open \"%s\": %m", interface_name); 57 log_err("Could not open \"%s\": %m", interface_name);
58 exit(EXIT_FAILURE); 58 exit(EXIT_FAILURE);
59 } 59 }
60 60
61 memset(&ifr, 0, sizeof(ifr)); 61 memset(&ifr, 0, sizeof(ifr));
62 if (ioctl(fd, TAPGIFNAME, &ifr) == -1) { 62 if (ioctl(fd, TAPGIFNAME, &ifr) == -1) {
63 log_err("Could not get interface name: %m"); 63 log_err("Could not get interface name: %m");
64 exit(EXIT_FAILURE); 64 exit(EXIT_FAILURE);
65 } 65 }
66 66
67 s = socket(PF_LINK, SOCK_DGRAM, 0); 67 s = socket(PF_LINK, SOCK_DGRAM, 0);
68 if (s == -1) { 68 if (s == -1) {
69 log_err("Could not open PF_LINK socket: %m"); 69 log_err("Could not open PF_LINK socket: %m");
70 exit(EXIT_FAILURE); 70 exit(EXIT_FAILURE);
71 } 71 }
72 72
73 memset(&iflr, 0, sizeof(iflr)); 73 memset(&iflr, 0, sizeof(iflr));
74 memcpy(iflr.iflr_name, ifr.ifr_name, IFNAMSIZ); 74 memcpy(iflr.iflr_name, ifr.ifr_name, IFNAMSIZ);
75 iflr.flags = IFLR_ACTIVE; 75 iflr.flags = IFLR_ACTIVE;
76 76
77 sdl = satosdl(sstosa(&iflr.addr)); 77 sdl = satosdl(sstosa(&iflr.addr));
78 sdl->sdl_family = AF_LINK; 78 sdl->sdl_family = AF_LINK;
79 sdl->sdl_len = sizeof(struct sockaddr_dl); 79 sdl->sdl_len = sizeof(struct sockaddr_dl);
80 sdl->sdl_alen = ETHER_ADDR_LEN; 80 sdl->sdl_alen = ETHER_ADDR_LEN;
81 b2eaddr(LLADDR(sdl), &local_bdaddr); 81 b2eaddr(LLADDR(sdl), &local_bdaddr);
82 82
83 if (ioctl(s, SIOCALIFADDR, &iflr) == -1) { 83 if (ioctl(s, SIOCALIFADDR, &iflr) == -1) {
84 log_err("Could not add %s link address: %m", iflr.iflr_name); 84 log_err("Could not add %s link address: %m", iflr.iflr_name);
85 exit(EXIT_FAILURE); 85 exit(EXIT_FAILURE);
86 } 86 }
87 87
88 if (ioctl(s, SIOCGIFFLAGS, &ifr) == -1) { 88 if (ioctl(s, SIOCGIFFLAGS, &ifr) == -1) {
89 log_err("Could not get interface flags: %m"); 89 log_err("Could not get interface flags: %m");
90 exit(EXIT_FAILURE); 90 exit(EXIT_FAILURE);
91 } 91 }
92 92
93 if ((ifr.ifr_flags & IFF_UP) == 0) { 93 if ((ifr.ifr_flags & IFF_UP) == 0) {
94 ifr.ifr_flags |= IFF_UP; 94 ifr.ifr_flags |= IFF_UP;
95 95
96 if (ioctl(s, SIOCSIFFLAGS, &ifr) == -1) { 96 if (ioctl(s, SIOCSIFFLAGS, &ifr) == -1) {
97 log_err("Could not set IFF_UP: %m"); 97 log_err("Could not set IFF_UP: %m");
98 exit(EXIT_FAILURE); 98 exit(EXIT_FAILURE);
99 } 99 }
100 } 100 }
101 101
102 close(s); 102 close(s);
103 103
104 log_info("Using interface %s with addr %s", 104 log_info("Using interface %s with addr %s",
105 ifr.ifr_name, ether_ntoa((struct ether_addr *)LLADDR(sdl))); 105 ifr.ifr_name, ether_ntoa((struct ether_addr *)LLADDR(sdl)));
106 106
107 chan = channel_alloc(); 107 chan = channel_alloc();
108 if (chan == NULL) 108 if (chan == NULL)
109 exit(EXIT_FAILURE); 109 exit(EXIT_FAILURE);
110 110
111 chan->send = tap_send; 111 chan->send = tap_send;
112 chan->recv = tap_recv; 112 chan->recv = tap_recv;
113 chan->mru = ETHER_HDR_LEN + ETHER_MAX_LEN; 113 chan->mru = ETHER_HDR_LEN + ETHER_MAX_LEN;
114 memcpy(chan->raddr, LLADDR(sdl), ETHER_ADDR_LEN); 114 memcpy(chan->raddr, LLADDR(sdl), ETHER_ADDR_LEN);
115 memcpy(chan->laddr, LLADDR(sdl), ETHER_ADDR_LEN); 115 memcpy(chan->laddr, LLADDR(sdl), ETHER_ADDR_LEN);
116 chan->state = CHANNEL_OPEN; 116 chan->state = CHANNEL_OPEN;
117 if (!channel_open(chan, fd)) 117 if (!channel_open(chan, fd))
118 exit(EXIT_FAILURE); 118 exit(EXIT_FAILURE);
119 119
120 if (pidfile(ifr.ifr_name) == -1) 120 if (pidfile(ifr.ifr_name) == -1)
121 log_err("pidfile not made"); 121 log_err("pidfile not made");
122} 122}
123 123
124static bool 124static bool
125tap_send(channel_t *chan, packet_t *pkt) 125tap_send(channel_t *chan, packet_t *pkt)
126{ 126{
127 struct iovec iov[4]; 127 struct iovec iov[4];
128 ssize_t nw; 128 ssize_t nw;
129 129
130 iov[0].iov_base = pkt->dst; 130 iov[0].iov_base = pkt->dst;
131 iov[0].iov_len = ETHER_ADDR_LEN; 131 iov[0].iov_len = ETHER_ADDR_LEN;
132 iov[1].iov_base = pkt->src; 132 iov[1].iov_base = pkt->src;
133 iov[1].iov_len = ETHER_ADDR_LEN; 133 iov[1].iov_len = ETHER_ADDR_LEN;
134 iov[2].iov_base = pkt->type; 134 iov[2].iov_base = pkt->type;
135 iov[2].iov_len = ETHER_TYPE_LEN; 135 iov[2].iov_len = ETHER_TYPE_LEN;
136 iov[3].iov_base = pkt->ptr; 136 iov[3].iov_base = pkt->ptr;
137 iov[3].iov_len = pkt->len; 137 iov[3].iov_len = pkt->len;
138 138
139 /* tap device write never fails */ 139 /* tap device write never fails */
140 nw = writev(chan->fd, iov, __arraycount(iov)); 140 nw = writev(chan->fd, iov, __arraycount(iov));
141 _DIAGASSERT(nw > 0); 141 assert(nw > 0);
142 142
143 return true; 143 return true;
144} 144}
145 145
146static bool 146static bool
147tap_recv(packet_t *pkt) 147tap_recv(packet_t *pkt)
148{ 148{
149 149
150 if (pkt->len < ETHER_HDR_LEN) 150 if (pkt->len < ETHER_HDR_LEN)
151 return false; 151 return false;
152 152
153 pkt->dst = pkt->ptr; 153 pkt->dst = pkt->ptr;
154 packet_adj(pkt, ETHER_ADDR_LEN); 154 packet_adj(pkt, ETHER_ADDR_LEN);
155 pkt->src = pkt->ptr; 155 pkt->src = pkt->ptr;
156 packet_adj(pkt, ETHER_ADDR_LEN); 156 packet_adj(pkt, ETHER_ADDR_LEN);
157 pkt->type = pkt->ptr; 157 pkt->type = pkt->ptr;
158 packet_adj(pkt, ETHER_TYPE_LEN); 158 packet_adj(pkt, ETHER_TYPE_LEN);
159 159
160 return true; 160 return true;
161} 161}