l2cap.c (18ef5dfc3c950410b449df7c7ebb6238d64bd058) l2cap.c (7d5c2f20da25e56e90bc87f1f8b57caba59706da)
1/*
2 * Copyright (C) 2014 BlueKitchen GmbH
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

--- 1111 unchanged lines hidden (view full) ---

1120
1121// used for Classic Channels + LE Data Channels. local_cid >= 0x40
1122#ifdef L2CAP_USES_CHANNELS
1123static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
1124 if (local_cid < 0x40u) return NULL;
1125 return (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid);
1126}
1127
1/*
2 * Copyright (C) 2014 BlueKitchen GmbH
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

--- 1111 unchanged lines hidden (view full) ---

1120
1121// used for Classic Channels + LE Data Channels. local_cid >= 0x40
1122#ifdef L2CAP_USES_CHANNELS
1123static l2cap_channel_t * l2cap_get_channel_for_local_cid(uint16_t local_cid){
1124 if (local_cid < 0x40u) return NULL;
1125 return (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid);
1126}
1127
1128static l2cap_channel_t * l2cap_get_channel_for_local_cid_and_handle(uint16_t local_cid, hci_con_handle_t con_handle){
1129 if (local_cid < 0x40u) return NULL;
1130 l2cap_channel_t * l2cap_channel = (l2cap_channel_t*) l2cap_channel_item_by_cid(local_cid);
1131 if (l2cap_channel == NULL) return NULL;
1132 if (l2cap_channel->con_handle != con_handle) return NULL;
1133 return l2cap_channel;
1134}
1135
1128void l2cap_request_can_send_now_event(uint16_t local_cid){
1129 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid);
1130 if (!channel) return;
1131 channel->waiting_for_can_send_now = 1;
1132#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
1133 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
1134 l2cap_ertm_notify_channel_can_send(channel);
1135 return;

--- 2138 unchanged lines hidden (view full) ---

3274 break;
3275
3276 case LE_FLOW_CONTROL_CREDIT:
3277 // check size
3278 if (len < 4u) return 0u;
3279
3280 // find channel
3281 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0);
1136void l2cap_request_can_send_now_event(uint16_t local_cid){
1137 l2cap_channel_t *channel = l2cap_get_channel_for_local_cid(local_cid);
1138 if (!channel) return;
1139 channel->waiting_for_can_send_now = 1;
1140#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
1141 if (channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
1142 l2cap_ertm_notify_channel_can_send(channel);
1143 return;

--- 2138 unchanged lines hidden (view full) ---

3282 break;
3283
3284 case LE_FLOW_CONTROL_CREDIT:
3285 // check size
3286 if (len < 4u) return 0u;
3287
3288 // find channel
3289 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0);
3282 channel = l2cap_get_channel_for_local_cid(local_cid);
3290 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle);
3283 if (!channel) {
3284 log_error("l2cap: no channel for cid 0x%02x", local_cid);
3285 break;
3286 }
3287 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2);
3288 credits_before = channel->credits_outgoing;
3289 channel->credits_outgoing += new_credits;
3290 // check for credit overrun

--- 7 unchanged lines hidden (view full) ---

3298
3299 case DISCONNECTION_REQUEST:
3300
3301 // check size
3302 if (len < 4u) return 0u;
3303
3304 // find channel
3305 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0);
3291 if (!channel) {
3292 log_error("l2cap: no channel for cid 0x%02x", local_cid);
3293 break;
3294 }
3295 new_credits = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 2);
3296 credits_before = channel->credits_outgoing;
3297 channel->credits_outgoing += new_credits;
3298 // check for credit overrun

--- 7 unchanged lines hidden (view full) ---

3306
3307 case DISCONNECTION_REQUEST:
3308
3309 // check size
3310 if (len < 4u) return 0u;
3311
3312 // find channel
3313 local_cid = little_endian_read_16(command, L2CAP_SIGNALING_COMMAND_DATA_OFFSET + 0);
3306 channel = l2cap_get_channel_for_local_cid(local_cid);
3314 channel = l2cap_get_channel_for_local_cid_and_handle(local_cid, handle);
3307 if (!channel) {
3308 log_error("l2cap: no channel for cid 0x%02x", local_cid);
3309 break;
3310 }
3311 channel->remote_sig_id = sig_id;
3312 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
3313 break;
3314

--- 214 unchanged lines hidden (view full) ---

3529}
3530#endif
3531
3532static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){
3533#ifdef ENABLE_CLASSIC
3534 l2cap_channel_t * l2cap_channel;
3535 l2cap_fixed_channel_t * l2cap_fixed_channel;
3536
3315 if (!channel) {
3316 log_error("l2cap: no channel for cid 0x%02x", local_cid);
3317 break;
3318 }
3319 channel->remote_sig_id = sig_id;
3320 channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_RESPONSE;
3321 break;
3322

--- 214 unchanged lines hidden (view full) ---

3537}
3538#endif
3539
3540static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){
3541#ifdef ENABLE_CLASSIC
3542 l2cap_channel_t * l2cap_channel;
3543 l2cap_fixed_channel_t * l2cap_fixed_channel;
3544
3537 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
3545 uint16_t channel_id = READ_L2CAP_CHANNEL_ID(packet);
3546 uint8_t broadcast_flag = READ_ACL_FLAGS(packet) >> 2;
3538 switch (channel_id) {
3539
3540 case L2CAP_CID_SIGNALING: {
3547 switch (channel_id) {
3548
3549 case L2CAP_CID_SIGNALING: {
3550 if (broadcast_flag != 0) break;
3541 uint32_t command_offset = 8;
3542 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) {
3543 // assert signaling command is fully inside packet
3544 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
3545 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len;
3546 if (next_command_offset > size){
3547 log_error("l2cap signaling command len invalid -> drop");
3548 break;
3549 }
3550 // handle signaling command
3551 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
3552 // go to next command
3553 command_offset = next_command_offset;
3554 }
3555 break;
3556 }
3557 case L2CAP_CID_CONNECTIONLESS_CHANNEL:
3551 uint32_t command_offset = 8;
3552 while ((command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET) < size) {
3553 // assert signaling command is fully inside packet
3554 uint16_t data_len = little_endian_read_16(packet, command_offset + L2CAP_SIGNALING_COMMAND_LENGTH_OFFSET);
3555 uint32_t next_command_offset = command_offset + L2CAP_SIGNALING_COMMAND_DATA_OFFSET + data_len;
3556 if (next_command_offset > size){
3557 log_error("l2cap signaling command len invalid -> drop");
3558 break;
3559 }
3560 // handle signaling command
3561 l2cap_signaling_handler_dispatch(handle, &packet[command_offset]);
3562 // go to next command
3563 command_offset = next_command_offset;
3564 }
3565 break;
3566 }
3567 case L2CAP_CID_CONNECTIONLESS_CHANNEL:
3568 if (broadcast_flag == 0) break;
3558 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL);
3559 if (!l2cap_fixed_channel) break;
3560 if (!l2cap_fixed_channel->packet_handler) break;
3561 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
3562 break;
3563
3569 l2cap_fixed_channel = l2cap_fixed_channel_for_channel_id(L2CAP_CID_CONNECTIONLESS_CHANNEL);
3570 if (!l2cap_fixed_channel) break;
3571 if (!l2cap_fixed_channel->packet_handler) break;
3572 (*l2cap_fixed_channel->packet_handler)(UCD_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
3573 break;
3574
3564 default:
3575 default:
3576 if (broadcast_flag != 0) break;
3565 // Find channel for this channel_id and connection handle
3577 // Find channel for this channel_id and connection handle
3566 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id);
3567 if (l2cap_channel) {
3578 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle);
3579 if (l2cap_channel != NULL){
3568 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size);
3569 }
3570 break;
3571 }
3572#else
3573 UNUSED(handle); // ok: no code
3574 UNUSED(packet); // ok: no code
3575 UNUSED(size); // ok: no code

--- 34 unchanged lines hidden (view full) ---

3610 if (!l2cap_fixed_channel) break;
3611 if (!l2cap_fixed_channel->packet_handler) break;
3612 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
3613 break;
3614
3615 default:
3616
3617#ifdef ENABLE_LE_DATA_CHANNELS
3580 l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size);
3581 }
3582 break;
3583 }
3584#else
3585 UNUSED(handle); // ok: no code
3586 UNUSED(packet); // ok: no code
3587 UNUSED(size); // ok: no code

--- 34 unchanged lines hidden (view full) ---

3622 if (!l2cap_fixed_channel) break;
3623 if (!l2cap_fixed_channel->packet_handler) break;
3624 (*l2cap_fixed_channel->packet_handler)(SM_DATA_PACKET, handle, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
3625 break;
3626
3627 default:
3628
3629#ifdef ENABLE_LE_DATA_CHANNELS
3618 l2cap_channel = l2cap_get_channel_for_local_cid(channel_id);
3619 if (l2cap_channel) {
3630 l2cap_channel = l2cap_get_channel_for_local_cid_and_handle(channel_id, handle);
3631 if (l2cap_channel != NULL) {
3620 // credit counting
3621 if (l2cap_channel->credits_incoming == 0u){
3632 // credit counting
3633 if (l2cap_channel->credits_incoming == 0u){
3622 log_error("LE Data Channel packet received but no incoming credits");
3634 log_info("LE Data Channel packet received but no incoming credits");
3623 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
3624 break;
3625 }
3626 l2cap_channel->credits_incoming--;
3627
3628 // automatic credits
3629 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){
3630 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT;

--- 17 unchanged lines hidden (view full) ---

3648 fragment_size);
3649 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER;
3650 // done?
3651 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len);
3652 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){
3653 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len);
3654 l2cap_channel->receive_sdu_len = 0;
3655 }
3635 l2cap_channel->state = L2CAP_STATE_WILL_SEND_DISCONNECT_REQUEST;
3636 break;
3637 }
3638 l2cap_channel->credits_incoming--;
3639
3640 // automatic credits
3641 if ((l2cap_channel->credits_incoming < L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_WATERMARK) && l2cap_channel->automatic_credits){
3642 l2cap_channel->new_credits_incoming = L2CAP_LE_DATA_CHANNELS_AUTOMATIC_CREDITS_INCREMENT;

--- 17 unchanged lines hidden (view full) ---

3660 fragment_size);
3661 l2cap_channel->receive_sdu_pos += size - COMPLETE_L2CAP_HEADER;
3662 // done?
3663 log_debug("le packet pos %u, len %u", l2cap_channel->receive_sdu_pos, l2cap_channel->receive_sdu_len);
3664 if (l2cap_channel->receive_sdu_pos >= l2cap_channel->receive_sdu_len){
3665 l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, l2cap_channel->receive_sdu_buffer, l2cap_channel->receive_sdu_len);
3666 l2cap_channel->receive_sdu_len = 0;
3667 }
3656 } else {
3657 log_error("LE Data Channel packet received but no channel found for cid 0x%02x", channel_id);
3658 }
3659#endif
3660 break;
3661 }
3662#else
3663 UNUSED(handle); // ok: no code
3664 UNUSED(packet); // ok: no code
3665 UNUSED(size); // ok: no code

--- 529 unchanged lines hidden --- 		3668 }
3669#endif
3670 break;
3671 }
3672#else
3673 UNUSED(handle); // ok: no code
3674 UNUSED(packet); // ok: no code
3675 UNUSED(size); // ok: no code

--- 529 unchanged lines hidden ---