1 | /*
|
---|
2 | * Copyright (C) 2014 by Ole Reinhardt <ole.reinhardt@kernelconcepts.de>
|
---|
3 | *
|
---|
4 | * Redistribution and use in source and binary forms, with or without
|
---|
5 | * modification, are permitted provided that the following conditions
|
---|
6 | * are met:
|
---|
7 | *
|
---|
8 | * 1. Redistributions of source code must retain the above copyright
|
---|
9 | * notice, this list of conditions and the following disclaimer.
|
---|
10 | * 2. Redistributions in binary form must reproduce the above copyright
|
---|
11 | * notice, this list of conditions and the following disclaimer in the
|
---|
12 | * documentation and/or other materials provided with the distribution.
|
---|
13 | * 3. Neither the name of the copyright holders nor the names of
|
---|
14 | * contributors may be used to endorse or promote products derived
|
---|
15 | * from this software without specific prior written permission.
|
---|
16 | * 4. Any redistribution, use, or modification is done solely for
|
---|
17 | * personal benefit and not for any commercial purpose or for
|
---|
18 | * monetary gain.
|
---|
19 | *
|
---|
20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
---|
21 | * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
---|
22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
---|
23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
---|
24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
---|
25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
---|
26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
---|
27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
---|
28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
|
---|
29 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
|
---|
30 | * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
---|
31 | * SUCH DAMAGE.
|
---|
32 | *
|
---|
33 | * Please inquire about commercial licensing options at btstack@ringwald.ch
|
---|
34 | *
|
---|
35 | */
|
---|
36 |
|
---|
37 | /*
|
---|
38 | * bnep.c
|
---|
39 | */
|
---|
40 |
|
---|
41 | #include <stdio.h>
|
---|
42 | #include <stdlib.h>
|
---|
43 | #include <string.h> // memcpy
|
---|
44 | #include <stdint.h>
|
---|
45 |
|
---|
46 | #include <btstack/btstack.h>
|
---|
47 | #include <btstack/hci_cmds.h>
|
---|
48 | #include <btstack/utils.h>
|
---|
49 |
|
---|
50 | #include <btstack/utils.h>
|
---|
51 | #include "btstack_memory.h"
|
---|
52 | #include "hci.h"
|
---|
53 | #include "hci_dump.h"
|
---|
54 | #include "debug.h"
|
---|
55 | #include "bnep.h"
|
---|
56 |
|
---|
57 | #include "l2cap.h"
|
---|
58 |
|
---|
59 | #define BNEP_CONNECTION_TIMEOUT_MS 10000
|
---|
60 | #define BNEP_CONNECTION_MAX_RETRIES 1
|
---|
61 |
|
---|
62 | /*static*/ linked_list_t bnep_services = NULL;
|
---|
63 | /*static*/ linked_list_t bnep_channels = NULL;
|
---|
64 |
|
---|
65 | /*static*/ gap_security_level_t bnep_security_level;
|
---|
66 |
|
---|
67 | static void (*app_packet_handler)(void * connection, uint8_t packet_type,
|
---|
68 | uint16_t channel, uint8_t *packet, uint16_t size);
|
---|
69 |
|
---|
70 |
|
---|
71 | static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid);
|
---|
72 | static void bnep_channel_finalize(bnep_channel_t *channel);
|
---|
73 | static void bnep_run(void);
|
---|
74 | static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout);
|
---|
75 | /*inline*/ static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event);
|
---|
76 |
|
---|
77 | static void bnep_emit_open_channel_complete(bnep_channel_t *channel, uint8_t status)
|
---|
78 | {
|
---|
79 | uint8_t event[3 + sizeof(bd_addr_t) + 3 * sizeof(uint16_t)];
|
---|
80 | log_info("BNEP_EVENT_OPEN_CHANNEL_COMPLETE status 0x%02x bd_addr: %s, handler %p", status, bd_addr_to_str(channel->remote_addr), channel->packet_handler);
|
---|
81 | if (!app_packet_handler) return;
|
---|
82 |
|
---|
83 | event[0] = BNEP_EVENT_OPEN_CHANNEL_COMPLETE;
|
---|
84 | event[1] = sizeof(event) - 2;
|
---|
85 | event[2] = status;
|
---|
86 | little_endian_store_16(event, 3, channel->l2cap_cid);
|
---|
87 | little_endian_store_16(event, 5, channel->uuid_source);
|
---|
88 | little_endian_store_16(event, 7, channel->uuid_dest);
|
---|
89 | little_endian_store_16(event, 9, channel->max_frame_size);
|
---|
90 | BD_ADDR_COPY(&event[11], channel->remote_addr);
|
---|
91 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
92 | (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
|
---|
93 | }
|
---|
94 |
|
---|
95 | static void bnep_emit_channel_timeout(bnep_channel_t *channel)
|
---|
96 | {
|
---|
97 | uint8_t event[2 + sizeof(bd_addr_t) + 2 * sizeof(uint16_t) + sizeof(uint8_t)];
|
---|
98 | log_info("BNEP_EVENT_CHANNEL_TIMEOUT bd_addr: %s, handler %p", bd_addr_to_str(channel->remote_addr), channel->packet_handler);
|
---|
99 | if (!app_packet_handler) return;
|
---|
100 |
|
---|
101 | event[0] = BNEP_EVENT_CHANNEL_TIMEOUT;
|
---|
102 | event[1] = sizeof(event) - 2;
|
---|
103 | little_endian_store_16(event, 2, channel->l2cap_cid);
|
---|
104 | little_endian_store_16(event, 4, channel->uuid_source);
|
---|
105 | little_endian_store_16(event, 6, channel->uuid_dest);
|
---|
106 | BD_ADDR_COPY(&event[8], channel->remote_addr);
|
---|
107 | event[14] = channel->state;
|
---|
108 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
109 | (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
|
---|
110 | }
|
---|
111 |
|
---|
112 | static void bnep_emit_channel_closed(bnep_channel_t *channel)
|
---|
113 | {
|
---|
114 | uint8_t event[2 + sizeof(bd_addr_t) + 3 * sizeof(uint16_t)];
|
---|
115 | log_info("BNEP_EVENT_CHANNEL_CLOSED bd_addr: %s, handler %p", bd_addr_to_str(channel->remote_addr));
|
---|
116 | if (!app_packet_handler) return;
|
---|
117 |
|
---|
118 | event[0] = BNEP_EVENT_CHANNEL_CLOSED;
|
---|
119 | event[1] = sizeof(event) - 2;
|
---|
120 | little_endian_store_16(event, 2, channel->l2cap_cid);
|
---|
121 | little_endian_store_16(event, 4, channel->uuid_source);
|
---|
122 | little_endian_store_16(event, 6, channel->uuid_dest);
|
---|
123 | BD_ADDR_COPY(&event[8], channel->remote_addr);
|
---|
124 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
125 | (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, channel->l2cap_cid, (uint8_t *) event, sizeof(event));
|
---|
126 | }
|
---|
127 |
|
---|
128 | static void bnep_emit_ready_to_send(bnep_channel_t *channel)
|
---|
129 | {
|
---|
130 | if (!app_packet_handler) return;
|
---|
131 |
|
---|
132 | uint8_t event[4];
|
---|
133 | event[0] = BNEP_EVENT_READY_TO_SEND;
|
---|
134 | event[1] = sizeof(event) - 2;
|
---|
135 | little_endian_store_16(event, 2, channel->l2cap_cid);
|
---|
136 | hci_dump_packet( HCI_EVENT_PACKET, 0, event, sizeof(event));
|
---|
137 | (*app_packet_handler)(channel->connection, HCI_EVENT_PACKET, 0, (uint8_t *) event, sizeof(event));
|
---|
138 | }
|
---|
139 |
|
---|
140 | /* Send BNEP connection request */
|
---|
141 | static int bnep_send_command_not_understood(bnep_channel_t *channel, uint8_t control_type)
|
---|
142 | {
|
---|
143 | uint8_t *bnep_out_buffer = NULL;
|
---|
144 | uint16_t pos = 0;
|
---|
145 | int err = 0;
|
---|
146 |
|
---|
147 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
148 | return -1; // TODO
|
---|
149 | }
|
---|
150 |
|
---|
151 | l2cap_reserve_packet_buffer();
|
---|
152 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
153 |
|
---|
154 | /* Setup control packet type */
|
---|
155 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
156 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD;
|
---|
157 |
|
---|
158 | /* Add not understood control type */
|
---|
159 | bnep_out_buffer[pos++] = control_type;
|
---|
160 |
|
---|
161 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
162 |
|
---|
163 | if (err) {
|
---|
164 | // TODO: Log error
|
---|
165 | }
|
---|
166 | return err;
|
---|
167 | }
|
---|
168 |
|
---|
169 |
|
---|
170 | /* Send BNEP connection request */
|
---|
171 | static int bnep_send_connection_request(bnep_channel_t *channel, uint16_t uuid_source, uint16_t uuid_dest)
|
---|
172 | {
|
---|
173 | uint8_t *bnep_out_buffer = NULL;
|
---|
174 | uint16_t pos = 0;
|
---|
175 | int err = 0;
|
---|
176 |
|
---|
177 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
178 | return -1; // TODO
|
---|
179 | }
|
---|
180 |
|
---|
181 | l2cap_reserve_packet_buffer();
|
---|
182 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
183 |
|
---|
184 | /* Setup control packet type */
|
---|
185 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
186 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST;
|
---|
187 |
|
---|
188 | /* Add UUID Size */
|
---|
189 | bnep_out_buffer[pos++] = 2;
|
---|
190 |
|
---|
191 | /* Add dest and source UUID */
|
---|
192 | net_store_16(bnep_out_buffer, pos, uuid_dest);
|
---|
193 | pos += 2;
|
---|
194 |
|
---|
195 | net_store_16(bnep_out_buffer, pos, uuid_source);
|
---|
196 | pos += 2;
|
---|
197 |
|
---|
198 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
199 |
|
---|
200 | if (err) {
|
---|
201 | // TODO: Log error
|
---|
202 | }
|
---|
203 | return err;
|
---|
204 | }
|
---|
205 |
|
---|
206 | /* Send BNEP connection response */
|
---|
207 | static int bnep_send_connection_response(bnep_channel_t *channel, uint16_t response_code)
|
---|
208 | {
|
---|
209 | uint8_t *bnep_out_buffer = NULL;
|
---|
210 | uint16_t pos = 0;
|
---|
211 | int err = 0;
|
---|
212 |
|
---|
213 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
214 | return -1; // TODO
|
---|
215 | }
|
---|
216 |
|
---|
217 | l2cap_reserve_packet_buffer();
|
---|
218 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
219 |
|
---|
220 | /* Setup control packet type */
|
---|
221 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
222 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE;
|
---|
223 |
|
---|
224 | /* Add response code */
|
---|
225 | net_store_16(bnep_out_buffer, pos, response_code);
|
---|
226 | pos += 2;
|
---|
227 |
|
---|
228 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
229 |
|
---|
230 | if (err) {
|
---|
231 | // TODO: Log error
|
---|
232 | }
|
---|
233 | return err;
|
---|
234 | }
|
---|
235 |
|
---|
236 | /* Send BNEP filter net type set message */
|
---|
237 | static int bnep_send_filter_net_type_set(bnep_channel_t *channel, bnep_net_filter_t *filter, uint16_t len)
|
---|
238 | {
|
---|
239 | uint8_t *bnep_out_buffer = NULL;
|
---|
240 | uint16_t pos = 0;
|
---|
241 | int err = 0;
|
---|
242 | int i;
|
---|
243 |
|
---|
244 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
245 | return -1;
|
---|
246 | }
|
---|
247 |
|
---|
248 | l2cap_reserve_packet_buffer();
|
---|
249 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
250 |
|
---|
251 | /* Setup control packet type */
|
---|
252 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
253 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET;
|
---|
254 |
|
---|
255 | net_store_16(bnep_out_buffer, pos, len * 2 * 2);
|
---|
256 | pos += 2;
|
---|
257 |
|
---|
258 | for (i = 0; i < len; i ++) {
|
---|
259 | net_store_16(bnep_out_buffer, pos, filter[i].range_start);
|
---|
260 | pos += 2;
|
---|
261 | net_store_16(bnep_out_buffer, pos, filter[i].range_end);
|
---|
262 | pos += 2;
|
---|
263 | }
|
---|
264 |
|
---|
265 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
266 |
|
---|
267 | if (err) {
|
---|
268 | // TODO: Log error
|
---|
269 | }
|
---|
270 | return err;
|
---|
271 | }
|
---|
272 |
|
---|
273 | /* Send BNEP filter net type response message */
|
---|
274 | static int bnep_send_filter_net_type_response(bnep_channel_t *channel, uint16_t response_code)
|
---|
275 | {
|
---|
276 | uint8_t *bnep_out_buffer = NULL;
|
---|
277 | uint16_t pos = 0;
|
---|
278 | int err = 0;
|
---|
279 |
|
---|
280 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
281 | return -1;
|
---|
282 | }
|
---|
283 |
|
---|
284 | l2cap_reserve_packet_buffer();
|
---|
285 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
286 |
|
---|
287 | /* Setup control packet type */
|
---|
288 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
289 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE;
|
---|
290 |
|
---|
291 | /* Add response code */
|
---|
292 | net_store_16(bnep_out_buffer, pos, response_code);
|
---|
293 | pos += 2;
|
---|
294 |
|
---|
295 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
296 |
|
---|
297 | if (err) {
|
---|
298 | // TODO: Log error
|
---|
299 | }
|
---|
300 | return err;
|
---|
301 | }
|
---|
302 |
|
---|
303 | /* Send BNEP filter multicast address set message */
|
---|
304 |
|
---|
305 | static int bnep_send_filter_multi_addr_set(bnep_channel_t *channel, bnep_multi_filter_t *filter, uint16_t len)
|
---|
306 | {
|
---|
307 | uint8_t *bnep_out_buffer = NULL;
|
---|
308 | uint16_t pos = 0;
|
---|
309 | int err = 0;
|
---|
310 | int i;
|
---|
311 |
|
---|
312 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
313 | return -1;
|
---|
314 | }
|
---|
315 |
|
---|
316 | l2cap_reserve_packet_buffer();
|
---|
317 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
318 |
|
---|
319 | /* Setup control packet type */
|
---|
320 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
321 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET;
|
---|
322 |
|
---|
323 | net_store_16(bnep_out_buffer, pos, len * 2 * ETHER_ADDR_LEN);
|
---|
324 | pos += 2;
|
---|
325 |
|
---|
326 | for (i = 0; i < len; i ++) {
|
---|
327 | BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_start);
|
---|
328 | pos += ETHER_ADDR_LEN;
|
---|
329 | BD_ADDR_COPY(bnep_out_buffer + pos, filter[i].addr_end);
|
---|
330 | pos += ETHER_ADDR_LEN;
|
---|
331 | }
|
---|
332 |
|
---|
333 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
334 |
|
---|
335 | if (err) {
|
---|
336 | // TODO: Log error
|
---|
337 | }
|
---|
338 | return err;
|
---|
339 | }
|
---|
340 |
|
---|
341 | /* Send BNEP filter multicast address response message */
|
---|
342 | static int bnep_send_filter_multi_addr_response(bnep_channel_t *channel, uint16_t response_code)
|
---|
343 | {
|
---|
344 | uint8_t *bnep_out_buffer = NULL;
|
---|
345 | uint16_t pos = 0;
|
---|
346 | int err = 0;
|
---|
347 |
|
---|
348 | if (channel->state == BNEP_CHANNEL_STATE_CLOSED) {
|
---|
349 | return -1;
|
---|
350 | }
|
---|
351 |
|
---|
352 | l2cap_reserve_packet_buffer();
|
---|
353 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
354 |
|
---|
355 | /* Setup control packet type */
|
---|
356 | bnep_out_buffer[pos++] = BNEP_PKT_TYPE_CONTROL;
|
---|
357 | bnep_out_buffer[pos++] = BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE;
|
---|
358 |
|
---|
359 | /* Add response code */
|
---|
360 | net_store_16(bnep_out_buffer, pos, response_code);
|
---|
361 | pos += 2;
|
---|
362 |
|
---|
363 | err = l2cap_send_prepared(channel->l2cap_cid, pos);
|
---|
364 |
|
---|
365 | if (err) {
|
---|
366 | // TODO: Log error
|
---|
367 | }
|
---|
368 | return err;
|
---|
369 | }
|
---|
370 |
|
---|
371 | int bnep_can_send_packet_now(uint16_t bnep_cid)
|
---|
372 | {
|
---|
373 | bnep_channel_t *channel = bnep_channel_for_l2cap_cid(bnep_cid);
|
---|
374 |
|
---|
375 | if (!channel){
|
---|
376 | log_error("bnep_can_send_packet_now cid 0x%02x doesn't exist!", bnep_cid);
|
---|
377 | return 0;
|
---|
378 | }
|
---|
379 |
|
---|
380 | return l2cap_can_send_packet_now(channel->l2cap_cid);
|
---|
381 | }
|
---|
382 |
|
---|
383 |
|
---|
384 | static int bnep_filter_protocol(bnep_channel_t *channel, uint16_t network_protocol_type)
|
---|
385 | {
|
---|
386 | int i;
|
---|
387 |
|
---|
388 | if (channel->net_filter_count == 0) {
|
---|
389 | /* No filter set */
|
---|
390 | return 1;
|
---|
391 | }
|
---|
392 |
|
---|
393 | for (i = 0; i < channel->net_filter_count; i ++) {
|
---|
394 | if ((network_protocol_type >= channel->net_filter[i].range_start) &&
|
---|
395 | (network_protocol_type <= channel->net_filter[i].range_end)) {
|
---|
396 | return 1;
|
---|
397 | }
|
---|
398 | }
|
---|
399 |
|
---|
400 | return 0;
|
---|
401 | }
|
---|
402 |
|
---|
403 | static int bnep_filter_multicast(bnep_channel_t *channel, bd_addr_t addr_dest)
|
---|
404 | {
|
---|
405 | int i;
|
---|
406 |
|
---|
407 | /* Check if the multicast flag is set int the destination address */
|
---|
408 | if ((addr_dest[0] & 0x01) == 0x00) {
|
---|
409 | /* Not a multicast frame, do not apply filtering and send it in any case */
|
---|
410 | return 1;
|
---|
411 | }
|
---|
412 |
|
---|
413 | if (channel->multicast_filter_count == 0) {
|
---|
414 | /* No filter set */
|
---|
415 | return 1;
|
---|
416 | }
|
---|
417 |
|
---|
418 | for (i = 0; i < channel->multicast_filter_count; i ++) {
|
---|
419 | if ((memcmp(addr_dest, channel->multicast_filter[i].addr_start, sizeof(bd_addr_t)) >= 0) &&
|
---|
420 | (memcmp(addr_dest, channel->multicast_filter[i].addr_end, sizeof(bd_addr_t)) <= 0)) {
|
---|
421 | return 1;
|
---|
422 | }
|
---|
423 | }
|
---|
424 |
|
---|
425 | return 0;
|
---|
426 | }
|
---|
427 |
|
---|
428 |
|
---|
429 | /* Send BNEP ethernet packet */
|
---|
430 | int bnep_send(uint16_t bnep_cid, uint8_t *packet, uint16_t len)
|
---|
431 | {
|
---|
432 | bnep_channel_t *channel;
|
---|
433 | uint8_t *bnep_out_buffer = NULL;
|
---|
434 | uint16_t pos = 0;
|
---|
435 | uint16_t pos_out = 0;
|
---|
436 | uint16_t payload_len;
|
---|
437 | int err = 0;
|
---|
438 | int has_source;
|
---|
439 | int has_dest;
|
---|
440 |
|
---|
441 | bd_addr_t addr_dest;
|
---|
442 | bd_addr_t addr_source;
|
---|
443 | uint16_t network_protocol_type;
|
---|
444 |
|
---|
445 | channel = bnep_channel_for_l2cap_cid(bnep_cid);
|
---|
446 | if (channel == NULL) {
|
---|
447 | log_error("bnep_send cid 0x%02x doesn't exist!", bnep_cid);
|
---|
448 | return 1;
|
---|
449 | }
|
---|
450 |
|
---|
451 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
452 | return BNEP_CHANNEL_NOT_CONNECTED;
|
---|
453 | }
|
---|
454 |
|
---|
455 | /* Check for free ACL buffers */
|
---|
456 | if (!l2cap_can_send_packet_now(channel->l2cap_cid)) {
|
---|
457 | return BTSTACK_ACL_BUFFERS_FULL;
|
---|
458 | }
|
---|
459 |
|
---|
460 | /* Extract destination and source address from the ethernet packet */
|
---|
461 | pos = 0;
|
---|
462 | BD_ADDR_COPY(addr_dest, &packet[pos]);
|
---|
463 | pos += sizeof(bd_addr_t);
|
---|
464 | BD_ADDR_COPY(addr_source, &packet[pos]);
|
---|
465 | pos += sizeof(bd_addr_t);
|
---|
466 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
467 | pos += sizeof(uint16_t);
|
---|
468 |
|
---|
469 | payload_len = len - pos;
|
---|
470 |
|
---|
471 | if (network_protocol_type == ETHERTYPE_VLAN) { /* IEEE 802.1Q tag header */
|
---|
472 | if (payload_len < 4) {
|
---|
473 | /* Omit this packet */
|
---|
474 | return 0;
|
---|
475 | }
|
---|
476 | /* The "real" network protocol type is 4 bytes ahead in a VLAN packet */
|
---|
477 | network_protocol_type = big_endian_read_16(packet, pos + 2);
|
---|
478 | }
|
---|
479 |
|
---|
480 | /* Check network protocol and multicast filters before sending */
|
---|
481 | if (!bnep_filter_protocol(channel, network_protocol_type) ||
|
---|
482 | !bnep_filter_multicast(channel, addr_dest)) {
|
---|
483 | /* Packet did not pass filter... */
|
---|
484 | if ((network_protocol_type == ETHERTYPE_VLAN) &&
|
---|
485 | (payload_len >= 4)) {
|
---|
486 | /* The packet has been tagged as a with IEE 802.1Q tag and has been filtered out.
|
---|
487 | According to the spec the IEE802.1Q tag header shall be sended without ethernet payload.
|
---|
488 | So limit the payload_len to 4.
|
---|
489 | */
|
---|
490 | payload_len = 4;
|
---|
491 | } else {
|
---|
492 | /* Packet is not tagged with IEE802.1Q header and was filtered out. Omit this packet */
|
---|
493 | return 0;
|
---|
494 | }
|
---|
495 | }
|
---|
496 |
|
---|
497 | /* Reserve l2cap packet buffer */
|
---|
498 | l2cap_reserve_packet_buffer();
|
---|
499 | bnep_out_buffer = l2cap_get_outgoing_buffer();
|
---|
500 |
|
---|
501 | /* Check if source address is the same as our local address and if the
|
---|
502 | destination address is the same as the remote addr. Maybe we can use
|
---|
503 | the compressed data format
|
---|
504 | */
|
---|
505 | has_source = (memcmp(addr_source, channel->local_addr, ETHER_ADDR_LEN) != 0);
|
---|
506 | has_dest = (memcmp(addr_dest, channel->remote_addr, ETHER_ADDR_LEN) != 0);
|
---|
507 |
|
---|
508 | /* Check for MTU limits */
|
---|
509 | if (payload_len > channel->max_frame_size) {
|
---|
510 | log_error("bnep_send: Max frame size (%d) exceeded: %d", channel->max_frame_size, payload_len);
|
---|
511 | return BNEP_DATA_LEN_EXCEEDS_MTU;
|
---|
512 | }
|
---|
513 |
|
---|
514 | /* Fill in the package type depending on the given source and destination address */
|
---|
515 | if (has_source && has_dest) {
|
---|
516 | bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_GENERAL_ETHERNET;
|
---|
517 | } else
|
---|
518 | if (has_source && !has_dest) {
|
---|
519 | bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY;
|
---|
520 | } else
|
---|
521 | if (!has_source && has_dest) {
|
---|
522 | bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY;
|
---|
523 | } else {
|
---|
524 | bnep_out_buffer[pos_out++] = BNEP_PKT_TYPE_COMPRESSED_ETHERNET;
|
---|
525 | }
|
---|
526 |
|
---|
527 | /* Add the destination address if needed */
|
---|
528 | if (has_dest) {
|
---|
529 | BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_dest);
|
---|
530 | pos_out += sizeof(bd_addr_t);
|
---|
531 | }
|
---|
532 |
|
---|
533 | /* Add the source address if needed */
|
---|
534 | if (has_source) {
|
---|
535 | BD_ADDR_COPY(bnep_out_buffer + pos_out, addr_source);
|
---|
536 | pos_out += sizeof(bd_addr_t);
|
---|
537 | }
|
---|
538 |
|
---|
539 | /* Add protocol type */
|
---|
540 | net_store_16(bnep_out_buffer, pos_out, network_protocol_type);
|
---|
541 | pos_out += 2;
|
---|
542 |
|
---|
543 | /* TODO: Add extension headers, if we may support them at a later stage */
|
---|
544 | /* Add the payload and then send out the package */
|
---|
545 | memcpy(bnep_out_buffer + pos_out, packet + pos, payload_len);
|
---|
546 | pos_out += payload_len;
|
---|
547 |
|
---|
548 | err = l2cap_send_prepared(channel->l2cap_cid, pos_out);
|
---|
549 |
|
---|
550 | if (err) {
|
---|
551 | log_error("bnep_send: error %d", err);
|
---|
552 | }
|
---|
553 | return err;
|
---|
554 | }
|
---|
555 |
|
---|
556 |
|
---|
557 | /* Set BNEP network protocol type filter */
|
---|
558 | int bnep_set_net_type_filter(uint16_t bnep_cid, bnep_net_filter_t *filter, uint16_t len)
|
---|
559 | {
|
---|
560 | bnep_channel_t *channel;
|
---|
561 |
|
---|
562 | if (filter == NULL) {
|
---|
563 | return -1;
|
---|
564 | }
|
---|
565 |
|
---|
566 | channel = bnep_channel_for_l2cap_cid(bnep_cid);
|
---|
567 | if (channel == NULL) {
|
---|
568 | log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid);
|
---|
569 | return 1;
|
---|
570 | }
|
---|
571 |
|
---|
572 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
573 | return BNEP_CHANNEL_NOT_CONNECTED;
|
---|
574 | }
|
---|
575 |
|
---|
576 | if (len > MAX_BNEP_NETFILTER_OUT) {
|
---|
577 | return BNEP_DATA_LEN_EXCEEDS_MTU;
|
---|
578 | }
|
---|
579 |
|
---|
580 | channel->net_filter_out = filter;
|
---|
581 | channel->net_filter_out_count = len;
|
---|
582 |
|
---|
583 | /* Set flag to send out the network protocol type filter set reqeuest on next statemachine cycle */
|
---|
584 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET);
|
---|
585 | bnep_run();
|
---|
586 |
|
---|
587 | return 0;
|
---|
588 | }
|
---|
589 |
|
---|
590 | /* Set BNEP network protocol type filter */
|
---|
591 | int bnep_set_multicast_filter(uint16_t bnep_cid, bnep_multi_filter_t *filter, uint16_t len)
|
---|
592 | {
|
---|
593 | bnep_channel_t *channel;
|
---|
594 |
|
---|
595 | if (filter == NULL) {
|
---|
596 | return -1;
|
---|
597 | }
|
---|
598 |
|
---|
599 | channel = bnep_channel_for_l2cap_cid(bnep_cid);
|
---|
600 | if (channel == NULL) {
|
---|
601 | log_error("bnep_set_net_type_filter cid 0x%02x doesn't exist!", bnep_cid);
|
---|
602 | return 1;
|
---|
603 | }
|
---|
604 |
|
---|
605 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
606 | return BNEP_CHANNEL_NOT_CONNECTED;
|
---|
607 | }
|
---|
608 |
|
---|
609 | if (len > MAX_BNEP_MULTICAST_FULTER_OUT) {
|
---|
610 | return BNEP_DATA_LEN_EXCEEDS_MTU;
|
---|
611 | }
|
---|
612 |
|
---|
613 | channel->multicast_filter_out = filter;
|
---|
614 | channel->multicast_filter_out_count = len;
|
---|
615 |
|
---|
616 | /* Set flag to send out the multicast filter set reqeuest on next statemachine cycle */
|
---|
617 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET);
|
---|
618 | bnep_run();
|
---|
619 |
|
---|
620 | return 0;
|
---|
621 | }
|
---|
622 |
|
---|
623 | /* BNEP timeout timer helper function */
|
---|
624 | static void bnep_channel_timer_handler(timer_source_t *timer)
|
---|
625 | {
|
---|
626 | bnep_channel_t *channel = (bnep_channel_t *)linked_item_get_user((linked_item_t *) timer);
|
---|
627 | // retry send setup connection at least one time
|
---|
628 | if (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE){
|
---|
629 | if (channel->retry_count < BNEP_CONNECTION_MAX_RETRIES){
|
---|
630 | channel->retry_count++;
|
---|
631 | bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS);
|
---|
632 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST);
|
---|
633 | bnep_run();
|
---|
634 | return;
|
---|
635 | }
|
---|
636 | }
|
---|
637 |
|
---|
638 | log_info( "bnep_channel_timeout_handler callback: shutting down connection!");
|
---|
639 | bnep_emit_channel_timeout(channel);
|
---|
640 | bnep_channel_finalize(channel);
|
---|
641 | }
|
---|
642 |
|
---|
643 |
|
---|
644 | static void bnep_channel_stop_timer(bnep_channel_t *channel)
|
---|
645 | {
|
---|
646 | if (channel->timer_active) {
|
---|
647 | run_loop_remove_timer(&channel->timer);
|
---|
648 | channel->timer_active = 0;
|
---|
649 | }
|
---|
650 | }
|
---|
651 |
|
---|
652 | static void bnep_channel_start_timer(bnep_channel_t *channel, int timeout)
|
---|
653 | {
|
---|
654 | /* Stop any eventually running timeout timer */
|
---|
655 | bnep_channel_stop_timer(channel);
|
---|
656 |
|
---|
657 | /* Start bnep channel timeout check timer */
|
---|
658 | run_loop_set_timer(&channel->timer, timeout);
|
---|
659 | channel->timer.process = bnep_channel_timer_handler;
|
---|
660 | linked_item_set_user((linked_item_t*) &channel->timer, channel);
|
---|
661 | run_loop_add_timer(&channel->timer);
|
---|
662 | channel->timer_active = 1;
|
---|
663 | }
|
---|
664 |
|
---|
665 | /* BNEP statemachine functions */
|
---|
666 |
|
---|
667 | /*inline*/ static void bnep_channel_state_add(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){
|
---|
668 | channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var | event);
|
---|
669 | }
|
---|
670 | /*inline*/ static void bnep_channel_state_remove(bnep_channel_t *channel, BNEP_CHANNEL_STATE_VAR event){
|
---|
671 | channel->state_var = (BNEP_CHANNEL_STATE_VAR) (channel->state_var & ~event);
|
---|
672 | }
|
---|
673 |
|
---|
674 | static uint16_t bnep_max_frame_size_for_l2cap_mtu(uint16_t l2cap_mtu){
|
---|
675 |
|
---|
676 | /* Assume a standard BNEP header, containing BNEP Type (1 Byte), dest and
|
---|
677 | source address (6 bytes each) and networking protocol type (2 bytes)
|
---|
678 | */
|
---|
679 | uint16_t max_frame_size = l2cap_mtu - 15; // 15 bytes BNEP header
|
---|
680 |
|
---|
681 | log_info("bnep_max_frame_size_for_l2cap_mtu: %u -> %u", l2cap_mtu, max_frame_size);
|
---|
682 | return max_frame_size;
|
---|
683 | }
|
---|
684 |
|
---|
685 | static bnep_channel_t * bnep_channel_create_for_addr(bd_addr_t *addr)
|
---|
686 | {
|
---|
687 | /* Allocate new channel structure */
|
---|
688 | bnep_channel_t *channel = btstack_memory_bnep_channel_get();
|
---|
689 | if (!channel) {
|
---|
690 | return NULL;
|
---|
691 | }
|
---|
692 |
|
---|
693 | /* Initialize the channel struct */
|
---|
694 | memset(channel, 0, sizeof(bnep_channel_t));
|
---|
695 |
|
---|
696 | channel->state = BNEP_CHANNEL_STATE_CLOSED;
|
---|
697 | channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(l2cap_max_mtu());
|
---|
698 | BD_ADDR_COPY(&channel->remote_addr, addr);
|
---|
699 | BD_ADDR_COPY(&channel->local_addr, hci_local_bd_addr());
|
---|
700 |
|
---|
701 | channel->net_filter_count = 0;
|
---|
702 | channel->multicast_filter_count = 0;
|
---|
703 | channel->retry_count = 0;
|
---|
704 |
|
---|
705 | /* Finally add it to the channel list */
|
---|
706 | linked_list_add(&bnep_channels, (linked_item_t *) channel);
|
---|
707 |
|
---|
708 | return channel;
|
---|
709 | }
|
---|
710 |
|
---|
711 | static bnep_channel_t* bnep_channel_for_addr(bd_addr_t *addr)
|
---|
712 | {
|
---|
713 | linked_item_t *it;
|
---|
714 | for (it = (linked_item_t *) bnep_channels; it ; it = it->next){
|
---|
715 | bnep_channel_t *channel = ((bnep_channel_t *) it);
|
---|
716 | if (BD_ADDR_CMP(addr, channel->remote_addr) == 0) {
|
---|
717 | return channel;
|
---|
718 | }
|
---|
719 | }
|
---|
720 | return NULL;
|
---|
721 | }
|
---|
722 |
|
---|
723 | static bnep_channel_t * bnep_channel_for_l2cap_cid(uint16_t l2cap_cid)
|
---|
724 | {
|
---|
725 | linked_item_t *it;
|
---|
726 | for (it = (linked_item_t *) bnep_channels; it ; it = it->next){
|
---|
727 | bnep_channel_t *channel = ((bnep_channel_t *) it);
|
---|
728 | if (channel->l2cap_cid == l2cap_cid) {
|
---|
729 | return channel;
|
---|
730 | }
|
---|
731 | }
|
---|
732 | return NULL;
|
---|
733 | }
|
---|
734 |
|
---|
735 | static bnep_service_t * bnep_service_for_uuid(uint16_t uuid)
|
---|
736 | {
|
---|
737 | linked_item_t *it;
|
---|
738 | for (it = (linked_item_t *) bnep_services; it ; it = it->next){
|
---|
739 | bnep_service_t * service = ((bnep_service_t *) it);
|
---|
740 | if ( service->service_uuid == uuid){
|
---|
741 | return service;
|
---|
742 | }
|
---|
743 | }
|
---|
744 | return NULL;
|
---|
745 | }
|
---|
746 |
|
---|
747 | static void bnep_channel_free(bnep_channel_t *channel)
|
---|
748 | {
|
---|
749 | linked_list_remove( &bnep_channels, (linked_item_t *) channel);
|
---|
750 | btstack_memory_bnep_channel_free(channel);
|
---|
751 | }
|
---|
752 |
|
---|
753 | static void bnep_channel_finalize(bnep_channel_t *channel)
|
---|
754 | {
|
---|
755 | uint16_t l2cap_cid;
|
---|
756 |
|
---|
757 | /* Inform application about closed channel */
|
---|
758 | if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
759 | bnep_emit_channel_closed(channel);
|
---|
760 | }
|
---|
761 |
|
---|
762 | l2cap_cid = channel->l2cap_cid;
|
---|
763 |
|
---|
764 | /* Stop any eventually running timer */
|
---|
765 | bnep_channel_stop_timer(channel);
|
---|
766 |
|
---|
767 | /* Free ressources and then close the l2cap channel */
|
---|
768 | bnep_channel_free(channel);
|
---|
769 | l2cap_disconnect_internal(l2cap_cid, 0x13);
|
---|
770 | }
|
---|
771 |
|
---|
772 | static int bnep_handle_connection_request(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
773 | {
|
---|
774 | uint16_t uuid_size;
|
---|
775 | uint16_t uuid_offset;
|
---|
776 | uint16_t response_code;
|
---|
777 | bnep_service_t * service;
|
---|
778 | uuid_size = packet[1];
|
---|
779 | response_code = BNEP_RESP_SETUP_SUCCESS;
|
---|
780 |
|
---|
781 | /* Sanity check packet size */
|
---|
782 | if (size < 1 + 1 + 2 * uuid_size) {
|
---|
783 | return 0;
|
---|
784 | }
|
---|
785 |
|
---|
786 | if ((channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST) &&
|
---|
787 | (channel->state != BNEP_CHANNEL_STATE_CONNECTED)) {
|
---|
788 | /* Ignore a connection request if not waiting for or still connected */
|
---|
789 | log_error("BNEP_CONNECTION_REQUEST: ignored in state %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid);
|
---|
790 | return 0;
|
---|
791 | }
|
---|
792 |
|
---|
793 | /* Extract source and destination UUID and convert them to UUID16 format */
|
---|
794 | switch (uuid_size) {
|
---|
795 | case 2: /* UUID16 */
|
---|
796 | uuid_offset = 0;
|
---|
797 | break;
|
---|
798 | case 4: /* UUID32 */
|
---|
799 | case 16: /* UUID128 */
|
---|
800 | uuid_offset = 2;
|
---|
801 | break;
|
---|
802 | default:
|
---|
803 | log_error("BNEP_CONNECTION_REQUEST: Invalid UUID size %d, l2cap_cid: %d!", channel->state, channel->l2cap_cid);
|
---|
804 | response_code = BNEP_RESP_SETUP_INVALID_SERVICE_UUID_SIZE;
|
---|
805 | break;
|
---|
806 | }
|
---|
807 |
|
---|
808 | /* Check source and destination UUIDs for valid combinations */
|
---|
809 | if (response_code == BNEP_RESP_SETUP_SUCCESS) {
|
---|
810 | channel->uuid_dest = big_endian_read_16(packet, 2 + uuid_offset);
|
---|
811 | channel->uuid_source = big_endian_read_16(packet, 2 + uuid_offset + uuid_size);
|
---|
812 |
|
---|
813 | if ((channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_PANU) &&
|
---|
814 | (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_NAP) &&
|
---|
815 | (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_GN)) {
|
---|
816 | log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest);
|
---|
817 | channel->uuid_dest = 0;
|
---|
818 | }
|
---|
819 | if ((channel->uuid_source != BLUETOOTH_SERVICE_CLASS_PANU) &&
|
---|
820 | (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_NAP) &&
|
---|
821 | (channel->uuid_source != BLUETOOTH_SERVICE_CLASS_GN)) {
|
---|
822 | log_error("BNEP_CONNECTION_REQUEST: Invalid source service UUID: %04x", channel->uuid_source);
|
---|
823 | channel->uuid_source = 0;
|
---|
824 | }
|
---|
825 |
|
---|
826 | /* Check if we have registered a service for the requested destination UUID */
|
---|
827 | service = bnep_service_for_uuid(channel->uuid_dest);
|
---|
828 | if (service == NULL) {
|
---|
829 | response_code = BNEP_RESP_SETUP_INVALID_DEST_UUID;
|
---|
830 | } else
|
---|
831 | if ((channel->uuid_source != BLUETOOTH_SERVICE_CLASS_PANU) && (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_PANU)) {
|
---|
832 | response_code = BNEP_RESP_SETUP_INVALID_SOURCE_UUID;
|
---|
833 | }
|
---|
834 | }
|
---|
835 |
|
---|
836 | /* Set flag to send out the connection response on next statemachine cycle */
|
---|
837 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE);
|
---|
838 | channel->response_code = response_code;
|
---|
839 |
|
---|
840 | /* Return the number of processed package bytes = BNEP Type, BNEP Control Type, UUID-Size + 2 * UUID */
|
---|
841 | return 1 + 1 + 2 * uuid_size;
|
---|
842 | }
|
---|
843 |
|
---|
844 | static int bnep_handle_connection_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
845 | {
|
---|
846 | uint16_t response_code;
|
---|
847 |
|
---|
848 | /* Sanity check packet size */
|
---|
849 | if (size < 1 + 2) {
|
---|
850 | return 0;
|
---|
851 | }
|
---|
852 |
|
---|
853 | if (channel->state != BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE) {
|
---|
854 | /* Ignore a connection response in any state but WAIT_FOR_CONNECTION_RESPONSE */
|
---|
855 | log_error("BNEP_CONNECTION_RESPONSE: Ignored in channel state %d", channel->state);
|
---|
856 | return 1 + 2;
|
---|
857 | }
|
---|
858 |
|
---|
859 | response_code = big_endian_read_16(packet, 1);
|
---|
860 |
|
---|
861 | if (response_code == BNEP_RESP_SETUP_SUCCESS) {
|
---|
862 | log_info("BNEP_CONNECTION_RESPONSE: Channel established to %s", bd_addr_to_str(channel->remote_addr));
|
---|
863 | channel->state = BNEP_CHANNEL_STATE_CONNECTED;
|
---|
864 | /* Stop timeout timer! */
|
---|
865 | bnep_channel_stop_timer(channel);
|
---|
866 | bnep_emit_open_channel_complete(channel, 0);
|
---|
867 | } else {
|
---|
868 | log_error("BNEP_CONNECTION_RESPONSE: Connection to %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code);
|
---|
869 | bnep_channel_finalize(channel);
|
---|
870 | }
|
---|
871 | return 1 + 2;
|
---|
872 | }
|
---|
873 |
|
---|
874 | static int bnep_handle_filter_net_type_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
875 | {
|
---|
876 | uint16_t list_length;
|
---|
877 | uint16_t response_code = BNEP_RESP_FILTER_SUCCESS;
|
---|
878 |
|
---|
879 | /* Sanity check packet size */
|
---|
880 | if (size < 3) {
|
---|
881 | return 0;
|
---|
882 | }
|
---|
883 |
|
---|
884 | list_length = big_endian_read_16(packet, 1);
|
---|
885 | /* Sanity check packet size again with known package size */
|
---|
886 | if (size < 3 + list_length) {
|
---|
887 | return 0;
|
---|
888 | }
|
---|
889 |
|
---|
890 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
891 | /* Ignore filter net type set in any state but CONNECTED */
|
---|
892 | log_error("BNEP_FILTER_NET_TYPE_SET: Ignored in channel state %d", channel->state);
|
---|
893 | return 3 + list_length;
|
---|
894 | }
|
---|
895 |
|
---|
896 | /* Check if we have enough space for more filters */
|
---|
897 | if ((list_length / (2*2)) > MAX_BNEP_NETFILTER) {
|
---|
898 | log_info("BNEP_FILTER_NET_TYPE_SET: Too many filter");
|
---|
899 | response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS;
|
---|
900 | } else {
|
---|
901 | int i;
|
---|
902 | channel->net_filter_count = 0;
|
---|
903 | /* There is still enough space, copy the filters to our filter list */
|
---|
904 | for (i = 0; i < list_length / (2 * 2); i ++) {
|
---|
905 | channel->net_filter[channel->net_filter_count].range_start = big_endian_read_16(packet, 1 + 2 + i * 4);
|
---|
906 | channel->net_filter[channel->net_filter_count].range_end = big_endian_read_16(packet, 1 + 2 + i * 4 + 2);
|
---|
907 | if (channel->net_filter[channel->net_filter_count].range_start > channel->net_filter[channel->net_filter_count].range_end) {
|
---|
908 | /* Invalid filter range, ignore this filter rule */
|
---|
909 | log_error("BNEP_FILTER_NET_TYPE_SET: Invalid filter: start: %d, end: %d",
|
---|
910 | channel->net_filter[channel->net_filter_count].range_start,
|
---|
911 | channel->net_filter[channel->net_filter_count].range_end);
|
---|
912 | response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE;
|
---|
913 | } else {
|
---|
914 | /* Valid filter, increase the filter count */
|
---|
915 | log_info("BNEP_FILTER_NET_TYPE_SET: Add filter: start: %d, end: %d",
|
---|
916 | channel->net_filter[channel->net_filter_count].range_start,
|
---|
917 | channel->net_filter[channel->net_filter_count].range_end);
|
---|
918 | channel->net_filter_count ++;
|
---|
919 | }
|
---|
920 | }
|
---|
921 | }
|
---|
922 |
|
---|
923 | /* Set flag to send out the set net filter response on next statemachine cycle */
|
---|
924 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE);
|
---|
925 | channel->response_code = response_code;
|
---|
926 |
|
---|
927 | return 3 + list_length;
|
---|
928 | }
|
---|
929 |
|
---|
930 | static int bnep_handle_filter_net_type_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
931 | {
|
---|
932 | uint16_t response_code;
|
---|
933 |
|
---|
934 | // TODO: Currently we do not support setting a network filter.
|
---|
935 |
|
---|
936 | /* Sanity check packet size */
|
---|
937 | if (size < 1 + 2) {
|
---|
938 | return 0;
|
---|
939 | }
|
---|
940 |
|
---|
941 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
942 | /* Ignore a filter net type response in any state but CONNECTED */
|
---|
943 | log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Ignored in channel state %d", channel->state);
|
---|
944 | return 1 + 2;
|
---|
945 | }
|
---|
946 |
|
---|
947 | response_code = big_endian_read_16(packet, 1);
|
---|
948 |
|
---|
949 | if (response_code == BNEP_RESP_FILTER_SUCCESS) {
|
---|
950 | log_info("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter set successfully for %s", bd_addr_to_str(channel->remote_addr));
|
---|
951 | } else {
|
---|
952 | log_error("BNEP_FILTER_NET_TYPE_RESPONSE: Net filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code);
|
---|
953 | }
|
---|
954 |
|
---|
955 | return 1 + 2;
|
---|
956 | }
|
---|
957 |
|
---|
958 | static int bnep_handle_multi_addr_set(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
959 | {
|
---|
960 | uint16_t list_length;
|
---|
961 | uint16_t response_code = BNEP_RESP_FILTER_SUCCESS;
|
---|
962 |
|
---|
963 | /* Sanity check packet size */
|
---|
964 | if (size < 3) {
|
---|
965 | return 0;
|
---|
966 | }
|
---|
967 |
|
---|
968 | list_length = big_endian_read_16(packet, 1);
|
---|
969 | /* Sanity check packet size again with known package size */
|
---|
970 | if (size < 3 + list_length) {
|
---|
971 | return 0;
|
---|
972 | }
|
---|
973 |
|
---|
974 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
975 | /* Ignore multicast filter address set in any state but CONNECTED */
|
---|
976 | log_error("BNEP_MULTI_ADDR_SET: Ignored in channel state %d", channel->state);
|
---|
977 | return 3 + list_length;
|
---|
978 | }
|
---|
979 |
|
---|
980 | /* Check if we have enough space for more filters */
|
---|
981 | if ((list_length / (2 * ETHER_ADDR_LEN)) > MAX_BNEP_MULTICAST_FILTER) {
|
---|
982 | log_info("BNEP_MULTI_ADDR_SET: Too many filter");
|
---|
983 | response_code = BNEP_RESP_FILTER_ERR_TOO_MANY_FILTERS;
|
---|
984 | } else {
|
---|
985 | unsigned int i;
|
---|
986 | channel->multicast_filter_count = 0;
|
---|
987 | /* There is enough space, copy the filters to our filter list */
|
---|
988 | for (i = 0; i < list_length / (2 * ETHER_ADDR_LEN); i ++) {
|
---|
989 | BD_ADDR_COPY(channel->multicast_filter[channel->multicast_filter_count].addr_start, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2);
|
---|
990 | BD_ADDR_COPY(channel->multicast_filter[channel->multicast_filter_count].addr_end, packet + 1 + 2 + i * ETHER_ADDR_LEN * 2 + ETHER_ADDR_LEN);
|
---|
991 |
|
---|
992 | if (memcmp(channel->multicast_filter[channel->multicast_filter_count].addr_start,
|
---|
993 | channel->multicast_filter[channel->multicast_filter_count].addr_end, ETHER_ADDR_LEN) > 0) {
|
---|
994 | /* Invalid filter range, ignore this filter rule */
|
---|
995 | log_error("BNEP_MULTI_ADDR_SET: Invalid filter: start: %s",
|
---|
996 | bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start));
|
---|
997 | log_error("BNEP_MULTI_ADDR_SET: Invalid filter: end: %s",
|
---|
998 | bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end));
|
---|
999 | response_code = BNEP_RESP_FILTER_ERR_INVALID_RANGE;
|
---|
1000 | } else {
|
---|
1001 | /* Valid filter, increase the filter count */
|
---|
1002 | log_info("BNEP_MULTI_ADDR_SET: Add filter: start: %s",
|
---|
1003 | bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_start));
|
---|
1004 | log_info("BNEP_MULTI_ADDR_SET: Add filter: end: %s",
|
---|
1005 | bd_addr_to_str(channel->multicast_filter[channel->multicast_filter_count].addr_end));
|
---|
1006 | channel->multicast_filter_count ++;
|
---|
1007 | }
|
---|
1008 | }
|
---|
1009 | }
|
---|
1010 | /* Set flag to send out the set multi addr response on next statemachine cycle */
|
---|
1011 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE);
|
---|
1012 | channel->response_code = response_code;
|
---|
1013 |
|
---|
1014 | return 3 + list_length;
|
---|
1015 | }
|
---|
1016 |
|
---|
1017 | static int bnep_handle_multi_addr_response(bnep_channel_t *channel, uint8_t *packet, uint16_t size)
|
---|
1018 | {
|
---|
1019 | uint16_t response_code;
|
---|
1020 |
|
---|
1021 | // TODO: Currently we do not support setting multicast address filter.
|
---|
1022 |
|
---|
1023 | /* Sanity check packet size */
|
---|
1024 | if (size < 1 + 2) {
|
---|
1025 | return 0;
|
---|
1026 | }
|
---|
1027 |
|
---|
1028 | if (channel->state != BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
1029 | /* Ignore multicast filter set response in any state but CONNECTED */
|
---|
1030 | log_error("BNEP_MULTI_ADDR_RESPONSE: Ignored in channel state %d", channel->state);
|
---|
1031 | return 1 + 2;
|
---|
1032 | }
|
---|
1033 |
|
---|
1034 | response_code = big_endian_read_16(packet, 1);
|
---|
1035 |
|
---|
1036 | if (response_code == BNEP_RESP_FILTER_SUCCESS) {
|
---|
1037 | log_info("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter set successfully for %s", bd_addr_to_str(channel->remote_addr));
|
---|
1038 | } else {
|
---|
1039 | log_error("BNEP_MULTI_ADDR_RESPONSE: Multicast address filter setting for %s failed. Err: %d", bd_addr_to_str(channel->remote_addr), response_code);
|
---|
1040 | }
|
---|
1041 |
|
---|
1042 | return 1 + 2;
|
---|
1043 | }
|
---|
1044 |
|
---|
1045 | static int bnep_handle_ethernet_packet(bnep_channel_t *channel, bd_addr_t addr_dest, bd_addr_t addr_source, uint16_t network_protocol_type, uint8_t *payload, uint16_t size)
|
---|
1046 | {
|
---|
1047 | uint16_t pos = 0;
|
---|
1048 |
|
---|
1049 | #if (HCI_INCOMING_PRE_BUFFER_SIZE) && (HCI_INCOMING_PRE_BUFFER_SIZE >= 14 - 8) // 2 * sizeof(bd_addr_t) + sizeof(uint16_t) - L2CAP Header (4) - ACL Header (4)
|
---|
1050 | /* In-place modify the package and add the ethernet header in front of the payload.
|
---|
1051 | * WARNING: This modifies the data in front of the payload and may overwrite 14 bytes there!
|
---|
1052 | */
|
---|
1053 | uint8_t *ethernet_packet = payload - 2 * sizeof(bd_addr_t) - sizeof(uint16_t);
|
---|
1054 | /* Restore the ethernet packet header */
|
---|
1055 | BD_ADDR_COPY(ethernet_packet + pos, addr_dest);
|
---|
1056 | pos += sizeof(bd_addr_t);
|
---|
1057 | BD_ADDR_COPY(ethernet_packet + pos, addr_source);
|
---|
1058 | pos += sizeof(bd_addr_t);
|
---|
1059 | net_store_16(ethernet_packet, pos, network_protocol_type);
|
---|
1060 | /* Payload is just in place... */
|
---|
1061 | #else
|
---|
1062 | /* Copy ethernet frame to statically allocated buffer. This solution is more
|
---|
1063 | * save, but needs an extra copy and more stack!
|
---|
1064 | */
|
---|
1065 | uint8_t ethernet_packet[BNEP_MTU_MIN];
|
---|
1066 |
|
---|
1067 | /* Restore the ethernet packet header */
|
---|
1068 | BD_ADDR_COPY(ethernet_packet + pos, addr_dest);
|
---|
1069 | pos += sizeof(bd_addr_t);
|
---|
1070 | BD_ADDR_COPY(ethernet_packet + pos, addr_source);
|
---|
1071 | pos += sizeof(bd_addr_t);
|
---|
1072 | net_store_16(ethernet_packet, pos, network_protocol_type);
|
---|
1073 | pos += 2;
|
---|
1074 | memcpy(ethernet_packet + pos, payload, size);
|
---|
1075 | #endif
|
---|
1076 |
|
---|
1077 | /* Notify application layer and deliver the ethernet packet */
|
---|
1078 | if (app_packet_handler){
|
---|
1079 | (*app_packet_handler)(channel->connection, BNEP_DATA_PACKET, channel->l2cap_cid,
|
---|
1080 | ethernet_packet, size + sizeof(uint16_t) + 2 * sizeof(bd_addr_t));
|
---|
1081 | }
|
---|
1082 |
|
---|
1083 | return size;
|
---|
1084 | }
|
---|
1085 |
|
---|
1086 | static int bnep_handle_control_packet(bnep_channel_t *channel, uint8_t *packet, uint16_t size, int is_extension)
|
---|
1087 | {
|
---|
1088 | uint16_t len = 0;
|
---|
1089 | uint8_t bnep_control_type;
|
---|
1090 |
|
---|
1091 | bnep_control_type = packet[0];
|
---|
1092 | /* Save last control type. Needed by statemachin in case of unknown control code */
|
---|
1093 |
|
---|
1094 | channel->last_control_type = bnep_control_type;
|
---|
1095 | log_info("BNEP_CONTROL: Type: %d, size: %d, is_extension: %d", bnep_control_type, size, is_extension);
|
---|
1096 | switch (bnep_control_type) {
|
---|
1097 | case BNEP_CONTROL_TYPE_COMMAND_NOT_UNDERSTOOD:
|
---|
1098 | /* The last command we send was not understood. We should close the connection */
|
---|
1099 | log_error("BNEP_CONTROL: Received COMMAND_NOT_UNDERSTOOD: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, packet[3]);
|
---|
1100 | bnep_channel_finalize(channel);
|
---|
1101 | len = 2; // Length of command not understood packet - bnep-type field
|
---|
1102 | break;
|
---|
1103 | case BNEP_CONTROL_TYPE_SETUP_CONNECTION_REQUEST:
|
---|
1104 | if (is_extension) {
|
---|
1105 | /* Connection requests are not allowed to be send in an extension header
|
---|
1106 | * ignore, do not set "COMMAND_NOT_UNDERSTOOD"
|
---|
1107 | */
|
---|
1108 | log_error("BNEP_CONTROL: Received SETUP_CONNECTION_REQUEST in extension header: l2cap_cid: %d", channel->l2cap_cid);
|
---|
1109 | return 0;
|
---|
1110 | } else {
|
---|
1111 | len = bnep_handle_connection_request(channel, packet, size);
|
---|
1112 | }
|
---|
1113 | break;
|
---|
1114 | case BNEP_CONTROL_TYPE_SETUP_CONNECTION_RESPONSE:
|
---|
1115 | if (is_extension) {
|
---|
1116 | /* Connection requests are not allowed to be send in an
|
---|
1117 | * extension header, ignore, do not set "COMMAND_NOT_UNDERSTOOD"
|
---|
1118 | */
|
---|
1119 | log_error("BNEP_CONTROL: Received SETUP_CONNECTION_RESPONSE in extension header: l2cap_cid: %d", channel->l2cap_cid);
|
---|
1120 | return 0;
|
---|
1121 | } else {
|
---|
1122 | len = bnep_handle_connection_response(channel, packet, size);
|
---|
1123 | }
|
---|
1124 | break;
|
---|
1125 | case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_SET:
|
---|
1126 | len = bnep_handle_filter_net_type_set(channel, packet, size);
|
---|
1127 | break;
|
---|
1128 | case BNEP_CONTROL_TYPE_FILTER_NET_TYPE_RESPONSE:
|
---|
1129 | len = bnep_handle_filter_net_type_response(channel, packet, size);
|
---|
1130 | break;
|
---|
1131 | case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_SET:
|
---|
1132 | len = bnep_handle_multi_addr_set(channel, packet, size);
|
---|
1133 | break;
|
---|
1134 | case BNEP_CONTROL_TYPE_FILTER_MULTI_ADDR_RESPONSE:
|
---|
1135 | len = bnep_handle_multi_addr_response(channel, packet, size);
|
---|
1136 | break;
|
---|
1137 | default:
|
---|
1138 | log_error("BNEP_CONTROL: Invalid bnep control type: l2cap_cid: %d, cmd: %d", channel->l2cap_cid, bnep_control_type);
|
---|
1139 | len = 0;
|
---|
1140 | break;
|
---|
1141 | }
|
---|
1142 |
|
---|
1143 | if (len == 0) {
|
---|
1144 | /* In case the command could not be handled, send a
|
---|
1145 | COMMAND_NOT_UNDERSTOOD message.
|
---|
1146 | Set flag to process the request in the next statemachine loop
|
---|
1147 | */
|
---|
1148 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD);
|
---|
1149 | }
|
---|
1150 |
|
---|
1151 | return len;
|
---|
1152 | }
|
---|
1153 |
|
---|
1154 | /**
|
---|
1155 | * @return handled packet
|
---|
1156 | */
|
---|
1157 | static int bnep_hci_event_handler(uint8_t *packet, uint16_t size)
|
---|
1158 | {
|
---|
1159 | bd_addr_t event_addr;
|
---|
1160 | uint16_t psm;
|
---|
1161 | uint16_t l2cap_cid;
|
---|
1162 | hci_con_handle_t con_handle;
|
---|
1163 | bnep_channel_t *channel = NULL;
|
---|
1164 | uint8_t status;
|
---|
1165 |
|
---|
1166 | switch (packet[0]) {
|
---|
1167 |
|
---|
1168 | /* Accept an incoming L2CAP connection on PSM_BNEP */
|
---|
1169 | case L2CAP_EVENT_INCOMING_CONNECTION:
|
---|
1170 | /* L2CAP event data: event(8), len(8), address(48), handle (16), psm (16), source cid(16) dest cid(16) */
|
---|
1171 | reverse_bd_addr(&packet[2], event_addr);
|
---|
1172 | con_handle = little_endian_read_16(packet, 8);
|
---|
1173 | psm = little_endian_read_16(packet, 10);
|
---|
1174 | l2cap_cid = little_endian_read_16(packet, 12);
|
---|
1175 |
|
---|
1176 | if (psm != PSM_BNEP) break;
|
---|
1177 |
|
---|
1178 | channel = bnep_channel_for_addr(&event_addr);
|
---|
1179 |
|
---|
1180 | if (channel) {
|
---|
1181 | log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - channel already exists", l2cap_cid);
|
---|
1182 | l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available
|
---|
1183 | return 1;
|
---|
1184 | }
|
---|
1185 |
|
---|
1186 | /* Create a new BNEP channel instance (incoming) */
|
---|
1187 | channel = bnep_channel_create_for_addr(&event_addr);
|
---|
1188 |
|
---|
1189 | if (!channel) {
|
---|
1190 | log_error("INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => decline - no memory left", l2cap_cid);
|
---|
1191 | l2cap_decline_connection_internal(l2cap_cid, 0x04); // no resources available
|
---|
1192 | return 1;
|
---|
1193 | }
|
---|
1194 |
|
---|
1195 | /* Assign connection handle and l2cap cid */
|
---|
1196 | channel->con_handle = con_handle;
|
---|
1197 | channel->l2cap_cid = l2cap_cid;
|
---|
1198 |
|
---|
1199 | /* Set channel into accept state */
|
---|
1200 | channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST;
|
---|
1201 |
|
---|
1202 | /* Start connection timeout timer */
|
---|
1203 | bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS);
|
---|
1204 |
|
---|
1205 | log_info("L2CAP_EVENT_INCOMING_CONNECTION (l2cap_cid 0x%02x) for PSM_BNEP => accept", l2cap_cid);
|
---|
1206 | l2cap_accept_connection_internal(l2cap_cid);
|
---|
1207 | return 1;
|
---|
1208 | break;
|
---|
1209 |
|
---|
1210 | /* Outgoing L2CAP connection has been opened -> store l2cap_cid, remote_addr */
|
---|
1211 | case L2CAP_EVENT_CHANNEL_OPENED:
|
---|
1212 | /* Check if the l2cap channel has been opened for PSM_BNEP */
|
---|
1213 | if (little_endian_read_16(packet, 11) != PSM_BNEP) {
|
---|
1214 | break;
|
---|
1215 | }
|
---|
1216 |
|
---|
1217 | status = packet[2];
|
---|
1218 | log_info("L2CAP_EVENT_CHANNEL_OPENED for PSM_BNEP, status %u", status);
|
---|
1219 |
|
---|
1220 | /* Get the bnep channel fpr remote address */
|
---|
1221 | con_handle = little_endian_read_16(packet, 9);
|
---|
1222 | l2cap_cid = little_endian_read_16(packet, 13);
|
---|
1223 | reverse_bd_addr(&packet[3], event_addr);
|
---|
1224 | channel = bnep_channel_for_addr(&event_addr);
|
---|
1225 | if (!channel) {
|
---|
1226 | log_error("L2CAP_EVENT_CHANNEL_OPENED but no BNEP channel prepared");
|
---|
1227 | return 1;
|
---|
1228 | }
|
---|
1229 |
|
---|
1230 | /* On L2CAP open error discard everything */
|
---|
1231 | if (status) {
|
---|
1232 | /* Emit bnep_open_channel_complete with status and free channel */
|
---|
1233 | bnep_emit_open_channel_complete(channel, status);
|
---|
1234 |
|
---|
1235 | /* Free BNEP channel mempory */
|
---|
1236 | bnep_channel_free(channel);
|
---|
1237 | return 1;
|
---|
1238 | }
|
---|
1239 |
|
---|
1240 | switch (channel->state){
|
---|
1241 | case BNEP_CHANNEL_STATE_CLOSED:
|
---|
1242 |
|
---|
1243 | log_info("L2CAP_EVENT_CHANNEL_OPENED: outgoing connection");
|
---|
1244 |
|
---|
1245 | bnep_channel_start_timer(channel, BNEP_CONNECTION_TIMEOUT_MS);
|
---|
1246 |
|
---|
1247 | /* Assign connection handle and l2cap cid */
|
---|
1248 | channel->l2cap_cid = l2cap_cid;
|
---|
1249 | channel->con_handle = con_handle;
|
---|
1250 |
|
---|
1251 | /* Initiate the connection request */
|
---|
1252 | channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE;
|
---|
1253 | bnep_channel_state_add(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST);
|
---|
1254 | channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17));
|
---|
1255 | bnep_run();
|
---|
1256 | break;
|
---|
1257 | case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST:
|
---|
1258 | /* New information: channel mtu */
|
---|
1259 | channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17));
|
---|
1260 | break;
|
---|
1261 | default:
|
---|
1262 | log_error("L2CAP_EVENT_CHANNEL_OPENED: Invalid state: %d", channel->state);
|
---|
1263 | break;
|
---|
1264 | }
|
---|
1265 | return 1;
|
---|
1266 | break;
|
---|
1267 |
|
---|
1268 | case DAEMON_EVENT_HCI_PACKET_SENT:
|
---|
1269 | bnep_run();
|
---|
1270 | break;
|
---|
1271 |
|
---|
1272 | case L2CAP_EVENT_CHANNEL_CLOSED:
|
---|
1273 | // data: event (8), len(8), channel (16)
|
---|
1274 | l2cap_cid = little_endian_read_16(packet, 2);
|
---|
1275 | channel = bnep_channel_for_l2cap_cid(l2cap_cid);
|
---|
1276 | log_info("L2CAP_EVENT_CHANNEL_CLOSED cid 0x%0x, channel %p", l2cap_cid, channel);
|
---|
1277 |
|
---|
1278 | if (!channel) {
|
---|
1279 | break;
|
---|
1280 | }
|
---|
1281 |
|
---|
1282 | log_info("L2CAP_EVENT_CHANNEL_CLOSED state %u", channel->state);
|
---|
1283 | switch (channel->state) {
|
---|
1284 | case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST:
|
---|
1285 | case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE:
|
---|
1286 | case BNEP_CHANNEL_STATE_CONNECTED:
|
---|
1287 | bnep_channel_finalize(channel);
|
---|
1288 | return 1;
|
---|
1289 | default:
|
---|
1290 | break;
|
---|
1291 | }
|
---|
1292 | break;
|
---|
1293 | default:
|
---|
1294 | bnep_run();
|
---|
1295 | break;
|
---|
1296 | }
|
---|
1297 | return 0;
|
---|
1298 | }
|
---|
1299 |
|
---|
1300 | static int bnep_l2cap_packet_handler(uint16_t l2cap_cid, uint8_t *packet, uint16_t size)
|
---|
1301 | {
|
---|
1302 | int rc = 0;
|
---|
1303 | uint8_t bnep_type;
|
---|
1304 | uint8_t bnep_header_has_ext;
|
---|
1305 | uint8_t extension_type;
|
---|
1306 | uint16_t pos = 0;
|
---|
1307 | bd_addr_t addr_source;
|
---|
1308 | bd_addr_t addr_dest;
|
---|
1309 | uint16_t network_protocol_type = 0xffff;
|
---|
1310 | bnep_channel_t *channel = NULL;
|
---|
1311 |
|
---|
1312 | /* Get the bnep channel for this package */
|
---|
1313 | channel = bnep_channel_for_l2cap_cid(l2cap_cid);
|
---|
1314 | if (!channel) {
|
---|
1315 | return rc;
|
---|
1316 | }
|
---|
1317 |
|
---|
1318 | /* Sort out short packages */
|
---|
1319 | if (size < 2) {
|
---|
1320 | return rc;
|
---|
1321 | }
|
---|
1322 |
|
---|
1323 | bnep_type = BNEP_TYPE(packet[pos]);
|
---|
1324 | bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]);
|
---|
1325 | pos ++;
|
---|
1326 |
|
---|
1327 | switch(bnep_type) {
|
---|
1328 | case BNEP_PKT_TYPE_GENERAL_ETHERNET:
|
---|
1329 | BD_ADDR_COPY(addr_dest, &packet[pos]);
|
---|
1330 | pos += sizeof(bd_addr_t);
|
---|
1331 | BD_ADDR_COPY(addr_source, &packet[pos]);
|
---|
1332 | pos += sizeof(bd_addr_t);
|
---|
1333 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
1334 | pos += 2;
|
---|
1335 | break;
|
---|
1336 | case BNEP_PKT_TYPE_COMPRESSED_ETHERNET:
|
---|
1337 | BD_ADDR_COPY(addr_dest, channel->local_addr);
|
---|
1338 | BD_ADDR_COPY(addr_source, channel->remote_addr);
|
---|
1339 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
1340 | pos += 2;
|
---|
1341 | break;
|
---|
1342 | case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_SOURCE_ONLY:
|
---|
1343 | BD_ADDR_COPY(addr_dest, channel->local_addr);
|
---|
1344 | BD_ADDR_COPY(addr_source, &packet[pos]);
|
---|
1345 | pos += sizeof(bd_addr_t);
|
---|
1346 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
1347 | pos += 2;
|
---|
1348 | break;
|
---|
1349 | case BNEP_PKT_TYPE_COMPRESSED_ETHERNET_DEST_ONLY:
|
---|
1350 | BD_ADDR_COPY(addr_dest, &packet[pos]);
|
---|
1351 | pos += sizeof(bd_addr_t);
|
---|
1352 | BD_ADDR_COPY(addr_source, channel->remote_addr);
|
---|
1353 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
1354 | pos += 2;
|
---|
1355 | break;
|
---|
1356 | case BNEP_PKT_TYPE_CONTROL:
|
---|
1357 | rc = bnep_handle_control_packet(channel, packet + pos, size - pos, 0);
|
---|
1358 | pos += rc;
|
---|
1359 | break;
|
---|
1360 | default:
|
---|
1361 | break;
|
---|
1362 | }
|
---|
1363 |
|
---|
1364 | if (bnep_header_has_ext) {
|
---|
1365 | do {
|
---|
1366 | uint8_t ext_len;
|
---|
1367 |
|
---|
1368 | /* Read extension type and check for further extensions */
|
---|
1369 | extension_type = BNEP_TYPE(packet[pos]);
|
---|
1370 | bnep_header_has_ext = BNEP_HEADER_HAS_EXT(packet[pos]);
|
---|
1371 | pos ++;
|
---|
1372 |
|
---|
1373 | /* Read extension header length */
|
---|
1374 | ext_len = packet[pos];
|
---|
1375 | pos ++;
|
---|
1376 |
|
---|
1377 | if (size - pos < ext_len) {
|
---|
1378 | log_error("BNEP pkt handler: Invalid extension length! Packet ignored");
|
---|
1379 | /* Invalid packet size! */
|
---|
1380 | return 0;
|
---|
1381 | }
|
---|
1382 |
|
---|
1383 | switch (extension_type) {
|
---|
1384 | case BNEP_EXT_HEADER_TYPE_EXTENSION_CONTROL:
|
---|
1385 | if (ext_len != bnep_handle_control_packet(channel, packet + pos, ext_len, 1)) {
|
---|
1386 | log_error("BNEP pkt handler: Ignore invalid control packet in extension header");
|
---|
1387 | }
|
---|
1388 |
|
---|
1389 | pos += ext_len;
|
---|
1390 | break;
|
---|
1391 |
|
---|
1392 | default:
|
---|
1393 | /* Extension header type unknown. Unknown extension SHALL be
|
---|
1394 | * SHALL be forwarded in any way. But who shall handle these
|
---|
1395 | * extension packets?
|
---|
1396 | * For now: We ignore them and just drop them!
|
---|
1397 | */
|
---|
1398 | log_error("BNEP pkt handler: Unknown extension type ignored, data dropped!");
|
---|
1399 | pos += ext_len;
|
---|
1400 | break;
|
---|
1401 | }
|
---|
1402 |
|
---|
1403 | } while (bnep_header_has_ext);
|
---|
1404 | }
|
---|
1405 |
|
---|
1406 | if (bnep_type != BNEP_PKT_TYPE_CONTROL) {
|
---|
1407 | if (channel->state == BNEP_CHANNEL_STATE_CONNECTED) {
|
---|
1408 | rc = bnep_handle_ethernet_packet(channel, addr_dest, addr_source, network_protocol_type, packet + pos, size - pos);
|
---|
1409 | } else {
|
---|
1410 | rc = 0;
|
---|
1411 | }
|
---|
1412 | }
|
---|
1413 |
|
---|
1414 | return rc;
|
---|
1415 |
|
---|
1416 | }
|
---|
1417 |
|
---|
1418 | void bnep_packet_handler(uint8_t packet_type, uint16_t l2cap_cid, uint8_t *packet, uint16_t size)
|
---|
1419 | {
|
---|
1420 | int handled = 0;
|
---|
1421 | switch (packet_type) {
|
---|
1422 | case HCI_EVENT_PACKET:
|
---|
1423 | handled = bnep_hci_event_handler(packet, size);
|
---|
1424 | break;
|
---|
1425 | case L2CAP_DATA_PACKET:
|
---|
1426 | handled = bnep_l2cap_packet_handler(l2cap_cid, packet, size);
|
---|
1427 | break;
|
---|
1428 | default:
|
---|
1429 | break;
|
---|
1430 | }
|
---|
1431 |
|
---|
1432 | if (handled) {
|
---|
1433 | bnep_run();
|
---|
1434 | return;
|
---|
1435 | }
|
---|
1436 |
|
---|
1437 | /* Forward non l2cap packages to application handler */
|
---|
1438 | if (packet_type != L2CAP_DATA_PACKET) {
|
---|
1439 | (*app_packet_handler)(NULL, packet_type, l2cap_cid, packet, size);
|
---|
1440 | return;
|
---|
1441 | }
|
---|
1442 |
|
---|
1443 | bnep_run();
|
---|
1444 | }
|
---|
1445 |
|
---|
1446 | static void bnep_channel_state_machine(bnep_channel_t* channel, bnep_channel_event_t *event)
|
---|
1447 | {
|
---|
1448 | log_info("bnep_state_machine: state %u, state var: %02x, event %u", channel->state, channel->state_var, event->type);
|
---|
1449 |
|
---|
1450 | if (event->type == BNEP_CH_EVT_READY_TO_SEND) {
|
---|
1451 | /* Send outstanding packets. */
|
---|
1452 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD) {
|
---|
1453 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_NOT_UNDERSTOOD);
|
---|
1454 | bnep_send_command_not_understood(channel, channel->last_control_type);
|
---|
1455 | return;
|
---|
1456 | }
|
---|
1457 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST) {
|
---|
1458 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_REQUEST);
|
---|
1459 | channel->state = BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_RESPONSE;
|
---|
1460 | bnep_send_connection_request(channel, channel->uuid_source, channel->uuid_dest);
|
---|
1461 | }
|
---|
1462 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE) {
|
---|
1463 | int emit_connected = 0;
|
---|
1464 | if ((channel->state == BNEP_CHANNEL_STATE_CLOSED) ||
|
---|
1465 | (channel->state == BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST)) {
|
---|
1466 | /* Set channel state to STATE_CONNECTED */
|
---|
1467 | channel->state = BNEP_CHANNEL_STATE_CONNECTED;
|
---|
1468 | /* Stop timeout timer! */
|
---|
1469 | bnep_channel_stop_timer(channel);
|
---|
1470 | emit_connected = 1;
|
---|
1471 | }
|
---|
1472 |
|
---|
1473 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_CONNECTION_RESPONSE);
|
---|
1474 | bnep_send_connection_response(channel, channel->response_code);
|
---|
1475 | if (emit_connected){
|
---|
1476 | bnep_emit_open_channel_complete(channel, 0);
|
---|
1477 | }
|
---|
1478 | return;
|
---|
1479 | }
|
---|
1480 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET) {
|
---|
1481 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_SET);
|
---|
1482 | if ((channel->net_filter_out_count > 0) && (channel->net_filter_out != NULL)) {
|
---|
1483 | bnep_send_filter_net_type_set(channel, channel->net_filter_out, channel->net_filter_out_count);
|
---|
1484 | channel->net_filter_out_count = 0;
|
---|
1485 | channel->net_filter_out = NULL;
|
---|
1486 | }
|
---|
1487 | return;
|
---|
1488 | }
|
---|
1489 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE) {
|
---|
1490 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_NET_TYPE_RESPONSE);
|
---|
1491 | bnep_send_filter_net_type_response(channel, channel->response_code);
|
---|
1492 | return;
|
---|
1493 | }
|
---|
1494 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET) {
|
---|
1495 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_SET);
|
---|
1496 | if ((channel->multicast_filter_out_count > 0) && (channel->multicast_filter_out != NULL)) {
|
---|
1497 | bnep_send_filter_multi_addr_set(channel, channel->multicast_filter_out, channel->multicast_filter_out_count);
|
---|
1498 | channel->multicast_filter_out_count = 0;
|
---|
1499 | channel->multicast_filter_out = NULL;
|
---|
1500 | }
|
---|
1501 | return;
|
---|
1502 | }
|
---|
1503 | if (channel->state_var & BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE) {
|
---|
1504 | bnep_channel_state_remove(channel, BNEP_CHANNEL_STATE_VAR_SND_FILTER_MULTI_ADDR_RESPONSE);
|
---|
1505 | bnep_send_filter_multi_addr_response(channel, channel->response_code);
|
---|
1506 | return;
|
---|
1507 | }
|
---|
1508 |
|
---|
1509 |
|
---|
1510 | /* If the event was not yet handled, notify the application layer */
|
---|
1511 | bnep_emit_ready_to_send(channel);
|
---|
1512 | }
|
---|
1513 | }
|
---|
1514 |
|
---|
1515 |
|
---|
1516 | /* Process oustanding signaling tasks */
|
---|
1517 | static void bnep_run(void)
|
---|
1518 | {
|
---|
1519 | linked_item_t *it;
|
---|
1520 | linked_item_t *next;
|
---|
1521 |
|
---|
1522 | for (it = (linked_item_t *) bnep_channels; it ; it = next){
|
---|
1523 | bnep_channel_t * channel;
|
---|
1524 | bnep_channel_event_t channel_event = { BNEP_CH_EVT_READY_TO_SEND };
|
---|
1525 |
|
---|
1526 | next = it->next; // be prepared for removal of channel in state machine
|
---|
1527 |
|
---|
1528 | channel = ((bnep_channel_t *) it);
|
---|
1529 |
|
---|
1530 | if (!l2cap_can_send_packet_now(channel->l2cap_cid)) {
|
---|
1531 | continue;
|
---|
1532 | }
|
---|
1533 |
|
---|
1534 | bnep_channel_state_machine(channel, &channel_event);
|
---|
1535 | }
|
---|
1536 | }
|
---|
1537 |
|
---|
1538 | /* BNEP BTStack API */
|
---|
1539 | void bnep_init(void)
|
---|
1540 | {
|
---|
1541 | bnep_security_level = LEVEL_0;
|
---|
1542 | }
|
---|
1543 |
|
---|
1544 | void bnep_set_required_security_level(gap_security_level_t security_level)
|
---|
1545 | {
|
---|
1546 | bnep_security_level = security_level;
|
---|
1547 | }
|
---|
1548 |
|
---|
1549 | /* Register application packet handler */
|
---|
1550 | void bnep_register_packet_handler(void (*handler)(void * connection, uint8_t packet_type,
|
---|
1551 | uint16_t channel, uint8_t *packet, uint16_t size)){
|
---|
1552 | app_packet_handler = handler;
|
---|
1553 | }
|
---|
1554 |
|
---|
1555 | int bnep_connect(void * connection, bd_addr_t *addr, uint16_t l2cap_psm, uint16_t uuid_dest)
|
---|
1556 | {
|
---|
1557 | bnep_channel_t *channel;
|
---|
1558 | log_info("BNEP_CONNECT addr %s", bd_addr_to_str(*addr));
|
---|
1559 |
|
---|
1560 | channel = bnep_channel_create_for_addr(addr);
|
---|
1561 | if (channel == NULL) {
|
---|
1562 | return -1;
|
---|
1563 | }
|
---|
1564 |
|
---|
1565 | channel->uuid_source = BLUETOOTH_SERVICE_CLASS_PANU;
|
---|
1566 | channel->uuid_dest = uuid_dest;
|
---|
1567 |
|
---|
1568 | l2cap_create_channel_internal(connection, bnep_packet_handler, *addr, l2cap_psm, l2cap_max_mtu());
|
---|
1569 |
|
---|
1570 | return 0;
|
---|
1571 | }
|
---|
1572 |
|
---|
1573 | void bnep_disconnect(bd_addr_t *addr)
|
---|
1574 | {
|
---|
1575 | bnep_channel_t *channel;
|
---|
1576 | log_info("BNEP_DISCONNECT");
|
---|
1577 |
|
---|
1578 | channel = bnep_channel_for_addr(addr);
|
---|
1579 |
|
---|
1580 | bnep_channel_finalize(channel);
|
---|
1581 |
|
---|
1582 | bnep_run();
|
---|
1583 | }
|
---|
1584 |
|
---|
1585 |
|
---|
1586 | int bnep_register_service(void * connection, uint16_t service_uuid, uint16_t max_frame_size)
|
---|
1587 | {
|
---|
1588 | bnep_service_t * service;
|
---|
1589 |
|
---|
1590 | log_info("BNEP_REGISTER_SERVICE mtu %d", max_frame_size);
|
---|
1591 |
|
---|
1592 | /* Check if we already registered a service */
|
---|
1593 | service = bnep_service_for_uuid(service_uuid);
|
---|
1594 | if (service) {
|
---|
1595 | return BNEP_SERVICE_ALREADY_REGISTERED;
|
---|
1596 | }
|
---|
1597 |
|
---|
1598 | /* Only alow one the three service types: PANU, NAP, GN */
|
---|
1599 | if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) &&
|
---|
1600 | (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) &&
|
---|
1601 | (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) {
|
---|
1602 | log_info("BNEP_REGISTER_SERVICE: Invalid service UUID: %04x", service_uuid);
|
---|
1603 | return BNEP_CHANNEL_NOT_CONNECTED;
|
---|
1604 | }
|
---|
1605 |
|
---|
1606 | /* Allocate service memory */
|
---|
1607 | service = (bnep_service_t*) btstack_memory_bnep_service_get();
|
---|
1608 | if (!service) {
|
---|
1609 | return BNEP_SERVICE_ALREADY_REGISTERED;
|
---|
1610 | }
|
---|
1611 | memset(service, 0, sizeof(bnep_service_t));
|
---|
1612 |
|
---|
1613 | /* register with l2cap if not registered before, max MTU */
|
---|
1614 | l2cap_register_service_internal(NULL, bnep_packet_handler, PSM_BNEP, 0xffff, bnep_security_level);
|
---|
1615 |
|
---|
1616 | /* Setup the service struct */
|
---|
1617 | service->connection = connection;
|
---|
1618 | service->max_frame_size = max_frame_size;
|
---|
1619 | service->service_uuid = service_uuid;
|
---|
1620 |
|
---|
1621 | /* Add to services list */
|
---|
1622 | linked_list_add(&bnep_services, (linked_item_t *) service);
|
---|
1623 |
|
---|
1624 | return 0;
|
---|
1625 | }
|
---|
1626 |
|
---|
1627 | void bnep_unregister_service(uint16_t service_uuid)
|
---|
1628 | {
|
---|
1629 | bnep_service_t *service;
|
---|
1630 | log_info("BNEP_UNREGISTER_SERVICE #%04x", service_uuid);
|
---|
1631 |
|
---|
1632 | service = bnep_service_for_uuid(service_uuid);
|
---|
1633 | if (!service) {
|
---|
1634 | return;
|
---|
1635 | }
|
---|
1636 |
|
---|
1637 | linked_list_remove(&bnep_services, (linked_item_t *) service);
|
---|
1638 | btstack_memory_bnep_service_free(service);
|
---|
1639 | service = NULL;
|
---|
1640 |
|
---|
1641 | l2cap_unregister_service_internal(NULL, PSM_BNEP);
|
---|
1642 | }
|
---|
1643 |
|
---|