mirror of
https://github.com/bluekitchen/btstack.git
synced 2025-01-26 12:35:25 +00:00
l2cap: extract l2cap_acl_classic_handler_for_channel
This commit is contained in:
Matthias Ringwald
parent
a3b9c6207b
commit
0b667d4199
397
src/l2cap.c
397
src/l2cap.c
@ -3277,6 +3277,207 @@ static int l2cap_le_signaling_handler_dispatch(hci_con_handle_t handle, uint8_t
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef ENABLE_CLASSIC
|
||||
static void l2cap_acl_classic_handler_for_channel(l2cap_channel_t * l2cap_channel, uint8_t * packet, uint16_t size){
|
||||
#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
|
||||
if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
|
||||
|
||||
int fcs_size = l2cap_channel->fcs_option ? 2 : 0;
|
||||
|
||||
// assert control + FCS fields are inside
|
||||
if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) return;
|
||||
|
||||
if (l2cap_channel->fcs_option){
|
||||
// verify FCS (required if one side requested it)
|
||||
uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2));
|
||||
uint16_t fcs_packet = little_endian_read_16(packet, size-2);
|
||||
|
||||
#ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL
|
||||
// simulate fcs error
|
||||
static int counter = 0;
|
||||
if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) {
|
||||
log_info("Simulate fcs error");
|
||||
fcs_calculated++;
|
||||
counter = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (fcs_calculated == fcs_packet){
|
||||
log_info("Packet FCS 0x%04x verified", fcs_packet);
|
||||
} else {
|
||||
log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated);
|
||||
// ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS'
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// switch on packet type
|
||||
uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER);
|
||||
uint8_t req_seq = (control >> 8) & 0x3f;
|
||||
int final = (control >> 7) & 0x01;
|
||||
if (control & 1){
|
||||
// S-Frame
|
||||
int poll = (control >> 4) & 0x01;
|
||||
l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03);
|
||||
log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq);
|
||||
l2cap_ertm_tx_packet_state_t * tx_state;
|
||||
switch (s){
|
||||
case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY");
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
if (poll && final){
|
||||
// S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time.
|
||||
log_error("P=F=1 in S-Frame");
|
||||
break;
|
||||
}
|
||||
if (poll){
|
||||
// check if we did request selective retransmission before <==> we have stored SDU segments
|
||||
int i;
|
||||
int num_stored_out_of_order_packets = 0;
|
||||
for (i=0;i<l2cap_channel->num_rx_buffers;i++){
|
||||
int index = l2cap_channel->rx_store_index + i;
|
||||
if (index >= l2cap_channel->num_rx_buffers){
|
||||
index -= l2cap_channel->num_rx_buffers;
|
||||
}
|
||||
l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index];
|
||||
if (!rx_state->valid) continue;
|
||||
num_stored_out_of_order_packets++;
|
||||
}
|
||||
if (num_stored_out_of_order_packets){
|
||||
l2cap_channel->send_supervisor_frame_selective_reject = 1;
|
||||
} else {
|
||||
l2cap_channel->send_supervisor_frame_receiver_ready = 1;
|
||||
}
|
||||
l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1;
|
||||
}
|
||||
if (final){
|
||||
// Stop-MonitorTimer
|
||||
l2cap_ertm_stop_monitor_timer(l2cap_channel);
|
||||
// If UnackedFrames > 0 then Start-RetransTimer
|
||||
if (l2cap_channel->unacked_frames){
|
||||
l2cap_ertm_start_retransmission_timer(l2cap_channel);
|
||||
}
|
||||
// final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
}
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT");
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
// restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq)
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY:
|
||||
log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY");
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT");
|
||||
if (poll){
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
}
|
||||
// find requested i-frame
|
||||
tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq);
|
||||
if (tx_state){
|
||||
log_info("Retransmission for tx_seq %u requested", req_seq);
|
||||
l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll;
|
||||
tx_state->retransmission_requested = 1;
|
||||
l2cap_channel->srej_active = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// I-Frame
|
||||
// get control
|
||||
l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14);
|
||||
uint8_t tx_seq = (control >> 1) & 0x3f;
|
||||
log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq);
|
||||
log_info("SAR: pos %u", l2cap_channel->reassembly_pos);
|
||||
log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq);
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
if (final){
|
||||
// final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
}
|
||||
|
||||
// get SDU
|
||||
const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2];
|
||||
uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size);
|
||||
|
||||
// assert SDU size is smaller or equal to our buffers
|
||||
uint16_t max_payload_size = 0;
|
||||
switch (sar){
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU:
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU:
|
||||
// SDU Length + MPS
|
||||
max_payload_size = l2cap_channel->local_mps + 2;
|
||||
break;
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU:
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU:
|
||||
max_payload_size = l2cap_channel->local_mps;
|
||||
break;
|
||||
}
|
||||
if (payload_len > max_payload_size){
|
||||
log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size);
|
||||
return;
|
||||
}
|
||||
|
||||
// check ordering
|
||||
if (l2cap_channel->expected_tx_seq == tx_seq){
|
||||
log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq);
|
||||
l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->req_seq = l2cap_channel->expected_tx_seq;
|
||||
|
||||
// process SDU
|
||||
l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len);
|
||||
|
||||
// process stored segments
|
||||
while (true){
|
||||
int index = l2cap_channel->rx_store_index;
|
||||
l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index];
|
||||
if (!rx_state->valid) break;
|
||||
|
||||
log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->req_seq = l2cap_channel->expected_tx_seq;
|
||||
|
||||
rx_state->valid = 0;
|
||||
l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len);
|
||||
|
||||
// update rx store index
|
||||
index++;
|
||||
if (index >= l2cap_channel->num_rx_buffers){
|
||||
index = 0;
|
||||
}
|
||||
l2cap_channel->rx_store_index = index;
|
||||
}
|
||||
|
||||
//
|
||||
l2cap_channel->send_supervisor_frame_receiver_ready = 1;
|
||||
|
||||
} else {
|
||||
int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f;
|
||||
if (delta < 2){
|
||||
// store segment
|
||||
l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len);
|
||||
|
||||
log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->send_supervisor_frame_selective_reject = 1;
|
||||
} else {
|
||||
log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->send_supervisor_frame_reject = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet, uint16_t size){
|
||||
#ifdef ENABLE_CLASSIC
|
||||
l2cap_channel_t * l2cap_channel;
|
||||
@ -3313,201 +3514,7 @@ static void l2cap_acl_classic_handler(hci_con_handle_t handle, uint8_t *packet,
|
||||
// Find channel for this channel_id and connection handle
|
||||
l2cap_channel = l2cap_get_channel_for_local_cid(channel_id);
|
||||
if (l2cap_channel) {
|
||||
#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
|
||||
if (l2cap_channel->mode == L2CAP_CHANNEL_MODE_ENHANCED_RETRANSMISSION){
|
||||
|
||||
int fcs_size = l2cap_channel->fcs_option ? 2 : 0;
|
||||
|
||||
// assert control + FCS fields are inside
|
||||
if (size < COMPLETE_L2CAP_HEADER+2+fcs_size) break;
|
||||
|
||||
if (l2cap_channel->fcs_option){
|
||||
// verify FCS (required if one side requested it)
|
||||
uint16_t fcs_calculated = crc16_calc(&packet[4], size - (4+2));
|
||||
uint16_t fcs_packet = little_endian_read_16(packet, size-2);
|
||||
|
||||
#ifdef L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL
|
||||
// simulate fcs error
|
||||
static int counter = 0;
|
||||
if (++counter == L2CAP_ERTM_SIMULATE_FCS_ERROR_INTERVAL) {
|
||||
log_info("Simulate fcs error");
|
||||
fcs_calculated++;
|
||||
counter = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (fcs_calculated == fcs_packet){
|
||||
log_info("Packet FCS 0x%04x verified", fcs_packet);
|
||||
} else {
|
||||
log_error("FCS mismatch! Packet 0x%04x, calculated 0x%04x", fcs_packet, fcs_calculated);
|
||||
// ERTM State Machine in Bluetooth Spec does not handle 'I-Frame with invalid FCS'
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// switch on packet type
|
||||
uint16_t control = little_endian_read_16(packet, COMPLETE_L2CAP_HEADER);
|
||||
uint8_t req_seq = (control >> 8) & 0x3f;
|
||||
int final = (control >> 7) & 0x01;
|
||||
if (control & 1){
|
||||
// S-Frame
|
||||
int poll = (control >> 4) & 0x01;
|
||||
l2cap_supervisory_function_t s = (l2cap_supervisory_function_t) ((control >> 2) & 0x03);
|
||||
log_info("Control: 0x%04x => Supervisory function %u, ReqSeq %02u", control, (int) s, req_seq);
|
||||
l2cap_ertm_tx_packet_state_t * tx_state;
|
||||
switch (s){
|
||||
case L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_RR_RECEIVER_READY");
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
if (poll && final){
|
||||
// S-frames shall not be transmitted with both the F-bit and the P-bit set to 1 at the same time.
|
||||
log_error("P=F=1 in S-Frame");
|
||||
break;
|
||||
}
|
||||
if (poll){
|
||||
// check if we did request selective retransmission before <==> we have stored SDU segments
|
||||
int i;
|
||||
int num_stored_out_of_order_packets = 0;
|
||||
for (i=0;i<l2cap_channel->num_rx_buffers;i++){
|
||||
int index = l2cap_channel->rx_store_index + i;
|
||||
if (index >= l2cap_channel->num_rx_buffers){
|
||||
index -= l2cap_channel->num_rx_buffers;
|
||||
}
|
||||
l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index];
|
||||
if (!rx_state->valid) continue;
|
||||
num_stored_out_of_order_packets++;
|
||||
}
|
||||
if (num_stored_out_of_order_packets){
|
||||
l2cap_channel->send_supervisor_frame_selective_reject = 1;
|
||||
} else {
|
||||
l2cap_channel->send_supervisor_frame_receiver_ready = 1;
|
||||
}
|
||||
l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = 1;
|
||||
}
|
||||
if (final){
|
||||
// Stop-MonitorTimer
|
||||
l2cap_ertm_stop_monitor_timer(l2cap_channel);
|
||||
// If UnackedFrames > 0 then Start-RetransTimer
|
||||
if (l2cap_channel->unacked_frames){
|
||||
l2cap_ertm_start_retransmission_timer(l2cap_channel);
|
||||
}
|
||||
// final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
}
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_REJ_REJECT");
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
// restart transmittion from last unacknowledted packet (earlier packets already freed in l2cap_ertm_process_req_seq)
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY:
|
||||
log_error("L2CAP_SUPERVISORY_FUNCTION_RNR_RECEIVER_NOT_READY");
|
||||
break;
|
||||
case L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT:
|
||||
log_info("L2CAP_SUPERVISORY_FUNCTION_SREJ_SELECTIVE_REJECT");
|
||||
if (poll){
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
}
|
||||
// find requested i-frame
|
||||
tx_state = l2cap_ertm_get_tx_state(l2cap_channel, req_seq);
|
||||
if (tx_state){
|
||||
log_info("Retransmission for tx_seq %u requested", req_seq);
|
||||
l2cap_channel->set_final_bit_after_packet_with_poll_bit_set = poll;
|
||||
tx_state->retransmission_requested = 1;
|
||||
l2cap_channel->srej_active = 1;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// I-Frame
|
||||
// get control
|
||||
l2cap_segmentation_and_reassembly_t sar = (l2cap_segmentation_and_reassembly_t) (control >> 14);
|
||||
uint8_t tx_seq = (control >> 1) & 0x3f;
|
||||
log_info("Control: 0x%04x => SAR %u, ReqSeq %02u, R?, TxSeq %02u", control, (int) sar, req_seq, tx_seq);
|
||||
log_info("SAR: pos %u", l2cap_channel->reassembly_pos);
|
||||
log_info("State: expected_tx_seq %02u, req_seq %02u", l2cap_channel->expected_tx_seq, l2cap_channel->req_seq);
|
||||
l2cap_ertm_process_req_seq(l2cap_channel, req_seq);
|
||||
if (final){
|
||||
// final bit set <- response to RR with poll bit set. All not acknowledged packets need to be retransmitted
|
||||
l2cap_ertm_retransmit_unacknowleded_frames(l2cap_channel);
|
||||
}
|
||||
|
||||
// get SDU
|
||||
const uint8_t * payload_data = &packet[COMPLETE_L2CAP_HEADER+2];
|
||||
uint16_t payload_len = size-(COMPLETE_L2CAP_HEADER+2+fcs_size);
|
||||
|
||||
// assert SDU size is smaller or equal to our buffers
|
||||
uint16_t max_payload_size = 0;
|
||||
switch (sar){
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_UNSEGMENTED_L2CAP_SDU:
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU:
|
||||
// SDU Length + MPS
|
||||
max_payload_size = l2cap_channel->local_mps + 2;
|
||||
break;
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_CONTINUATION_OF_L2CAP_SDU:
|
||||
case L2CAP_SEGMENTATION_AND_REASSEMBLY_END_OF_L2CAP_SDU:
|
||||
max_payload_size = l2cap_channel->local_mps;
|
||||
break;
|
||||
}
|
||||
if (payload_len > max_payload_size){
|
||||
log_info("payload len %u > max payload %u -> drop packet", payload_len, max_payload_size);
|
||||
break;
|
||||
}
|
||||
|
||||
// check ordering
|
||||
if (l2cap_channel->expected_tx_seq == tx_seq){
|
||||
log_info("Received expected frame with TxSeq == ExpectedTxSeq == %02u", tx_seq);
|
||||
l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->req_seq = l2cap_channel->expected_tx_seq;
|
||||
|
||||
// process SDU
|
||||
l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, sar, payload_data, payload_len);
|
||||
|
||||
// process stored segments
|
||||
while (true){
|
||||
int index = l2cap_channel->rx_store_index;
|
||||
l2cap_ertm_rx_packet_state_t * rx_state = &l2cap_channel->rx_packets_state[index];
|
||||
if (!rx_state->valid) break;
|
||||
|
||||
log_info("Processing stored frame with TxSeq == ExpectedTxSeq == %02u", l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->expected_tx_seq = l2cap_next_ertm_seq_nr(l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->req_seq = l2cap_channel->expected_tx_seq;
|
||||
|
||||
rx_state->valid = 0;
|
||||
l2cap_ertm_handle_in_sequence_sdu(l2cap_channel, rx_state->sar, &l2cap_channel->rx_packets_data[index], rx_state->len);
|
||||
|
||||
// update rx store index
|
||||
index++;
|
||||
if (index >= l2cap_channel->num_rx_buffers){
|
||||
index = 0;
|
||||
}
|
||||
l2cap_channel->rx_store_index = index;
|
||||
}
|
||||
|
||||
//
|
||||
l2cap_channel->send_supervisor_frame_receiver_ready = 1;
|
||||
|
||||
} else {
|
||||
int delta = (tx_seq - l2cap_channel->expected_tx_seq) & 0x3f;
|
||||
if (delta < 2){
|
||||
// store segment
|
||||
l2cap_ertm_handle_out_of_sequence_sdu(l2cap_channel, sar, delta, payload_data, payload_len);
|
||||
|
||||
log_info("Received unexpected frame TxSeq %u but expected %u -> send S-SREJ", tx_seq, l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->send_supervisor_frame_selective_reject = 1;
|
||||
} else {
|
||||
log_info("Received unexpected frame TxSeq %u but expected %u -> send S-REJ", tx_seq, l2cap_channel->expected_tx_seq);
|
||||
l2cap_channel->send_supervisor_frame_reject = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
l2cap_dispatch_to_channel(l2cap_channel, L2CAP_DATA_PACKET, &packet[COMPLETE_L2CAP_HEADER], size-COMPLETE_L2CAP_HEADER);
|
||||
l2cap_acl_classic_handler_for_channel(l2cap_channel, packet, size);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user