[337] | 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;
|
---|
[374] | 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);
|
---|
[337] | 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;
|
---|
[374] | 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);
|
---|
[337] | 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;
|
---|
[374] | 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);
|
---|
[337] | 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;
|
---|
[374] | 135 | little_endian_store_16(event, 2, channel->l2cap_cid);
|
---|
[337] | 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);
|
---|
[374] | 466 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
[337] | 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 */
|
---|
[374] | 477 | network_protocol_type = big_endian_read_16(packet, pos + 2);
|
---|
[337] | 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) {
|
---|
[374] | 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);
|
---|
[337] | 812 |
|
---|
[374] | 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)) {
|
---|
[337] | 816 | log_error("BNEP_CONNECTION_REQUEST: Invalid destination service UUID: %04x", channel->uuid_dest);
|
---|
| 817 | channel->uuid_dest = 0;
|
---|
| 818 | }
|
---|
[374] | 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)) {
|
---|
[337] | 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
|
---|
[374] | 831 | if ((channel->uuid_source != BLUETOOTH_SERVICE_CLASS_PANU) && (channel->uuid_dest != BLUETOOTH_SERVICE_CLASS_PANU)) {
|
---|
[337] | 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 |
|
---|
[374] | 859 | response_code = big_endian_read_16(packet, 1);
|
---|
[337] | 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 |
|
---|
[374] | 884 | list_length = big_endian_read_16(packet, 1);
|
---|
[337] | 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 ++) {
|
---|
[374] | 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);
|
---|
[337] | 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 |
|
---|
[374] | 947 | response_code = big_endian_read_16(packet, 1);
|
---|
[337] | 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 |
|
---|
[374] | 968 | list_length = big_endian_read_16(packet, 1);
|
---|
[337] | 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 |
|
---|
[374] | 1034 | response_code = big_endian_read_16(packet, 1);
|
---|
[337] | 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) */
|
---|
[374] | 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);
|
---|
[337] | 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 */
|
---|
[374] | 1213 | if (little_endian_read_16(packet, 11) != PSM_BNEP) {
|
---|
[337] | 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 */
|
---|
[374] | 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);
|
---|
[337] | 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);
|
---|
[374] | 1254 | channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17));
|
---|
[337] | 1255 | bnep_run();
|
---|
| 1256 | break;
|
---|
| 1257 | case BNEP_CHANNEL_STATE_WAIT_FOR_CONNECTION_REQUEST:
|
---|
| 1258 | /* New information: channel mtu */
|
---|
[374] | 1259 | channel->max_frame_size = bnep_max_frame_size_for_l2cap_mtu(little_endian_read_16(packet, 17));
|
---|
[337] | 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)
|
---|
[374] | 1274 | l2cap_cid = little_endian_read_16(packet, 2);
|
---|
[337] | 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);
|
---|
[374] | 1333 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
[337] | 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);
|
---|
[374] | 1339 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
[337] | 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);
|
---|
[374] | 1346 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
[337] | 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);
|
---|
[374] | 1353 | network_protocol_type = big_endian_read_16(packet, pos);
|
---|
[337] | 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 |
|
---|
[374] | 1565 | channel->uuid_source = BLUETOOTH_SERVICE_CLASS_PANU;
|
---|
[337] | 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 */
|
---|
[374] | 1599 | if ((service_uuid != BLUETOOTH_SERVICE_CLASS_PANU) &&
|
---|
| 1600 | (service_uuid != BLUETOOTH_SERVICE_CLASS_NAP) &&
|
---|
| 1601 | (service_uuid != BLUETOOTH_SERVICE_CLASS_GN)) {
|
---|
[337] | 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 |
|
---|