l2cap.c (3b81f0d3adff30ebdac0ecdbed83e8e161072187) l2cap.c (a8409e807e64501eed511af3a39f148e0658f912)
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

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

188 return (req_seq << 8) | (final << 7) | (poll << 4) | (((int) supervisory_function) << 2) | 1;
189}
190
191static int l2cap_next_ertm_seq_nr(int seq_nr){
192 return (seq_nr + 1) & 0x3f;
193}
194
195static int l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){
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

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

188 return (req_seq << 8) | (final << 7) | (poll << 4) | (((int) supervisory_function) << 2) | 1;
189}
190
191static int l2cap_next_ertm_seq_nr(int seq_nr){
192 return (seq_nr + 1) & 0x3f;
193}
194
195static int l2cap_ertm_can_store_packet_now(l2cap_channel_t * channel){
196 // get num free tx buffers
197 int num_tx_buffers_used = channel->tx_write_index - channel->tx_read_index;
198 if (num_tx_buffers_used < 0){
199 num_tx_buffers_used += channel->num_tx_buffers;
200 }
201 int num_free_tx_buffers = channel->num_tx_buffers - num_tx_buffers_used;
196 // get num free tx buffers
197 int num_free_tx_buffers = channel->num_tx_buffers - channel->num_stored_tx_frames;
202 // calculate num tx buffers for remote MTU
203 int num_tx_buffers_for_max_remote_mtu;
204 if (channel->remote_mtu <= channel->remote_mps){
205 // MTU fits into single packet
206 num_tx_buffers_for_max_remote_mtu = 1;
207 } else {
208 // include SDU Length
209 num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (channel->remote_mps - 1)) / channel->remote_mps;
210 }
198 // calculate num tx buffers for remote MTU
199 int num_tx_buffers_for_max_remote_mtu;
200 if (channel->remote_mtu <= channel->remote_mps){
201 // MTU fits into single packet
202 num_tx_buffers_for_max_remote_mtu = 1;
203 } else {
204 // include SDU Length
205 num_tx_buffers_for_max_remote_mtu = (channel->remote_mtu + 2 + (channel->remote_mps - 1)) / channel->remote_mps;
206 }
207 log_debug("num_free_tx_buffers %u, num_tx_buffers_for_max_remote_mtu %u", num_free_tx_buffers, num_tx_buffers_for_max_remote_mtu);
211 return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers;
212}
213
214static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){
215 channel->tx_write_index++;
216 if (channel->tx_write_index < channel->num_tx_buffers) return;
217 channel->tx_write_index = 0;
218}

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

308
309 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index];
310 tx_state->tx_seq = channel->next_tx_seq;
311 tx_state->len = len;
312 tx_state->sar = sar;
313 tx_state->retry_count = 0;
314
315 uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mps];
208 return num_tx_buffers_for_max_remote_mtu <= num_free_tx_buffers;
209}
210
211static void l2cap_ertm_next_tx_write_index(l2cap_channel_t * channel){
212 channel->tx_write_index++;
213 if (channel->tx_write_index < channel->num_tx_buffers) return;
214 channel->tx_write_index = 0;
215}

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

305
306 l2cap_ertm_tx_packet_state_t * tx_state = &channel->tx_packets_state[index];
307 tx_state->tx_seq = channel->next_tx_seq;
308 tx_state->len = len;
309 tx_state->sar = sar;
310 tx_state->retry_count = 0;
311
312 uint8_t * tx_packet = &channel->tx_packets_data[index * channel->local_mps];
316 log_info("index %u, mtu %u, packet tx %p", index, channel->local_mtu, tx_packet);
313 log_debug("index %u, mtu %u, packet tx %p", index, channel->local_mtu, tx_packet);
317 int pos = 0;
318 if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){
319 little_endian_store_16(tx_packet, 0, sdu_length);
320 pos += 2;
321 }
322 memcpy(&tx_packet[pos], data, len);
323
324 // update
314 int pos = 0;
315 if (sar == L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU){
316 little_endian_store_16(tx_packet, 0, sdu_length);
317 pos += 2;
318 }
319 memcpy(&tx_packet[pos], data, len);
320
321 // update
322 channel->num_stored_tx_frames++;
325 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq);
326 l2cap_ertm_next_tx_write_index(channel);
327
323 channel->next_tx_seq = l2cap_next_ertm_seq_nr(channel->next_tx_seq);
324 l2cap_ertm_next_tx_write_index(channel);
325
328 log_info("l2cap_ertm_store_fragment: after store, tx_read_index %u, tx_write_index %u", channel->tx_read_index, channel->tx_write_index);
326 log_info("l2cap_ertm_store_fragment: tx_read_index %u, tx_write_index %u, num stored %u", channel->tx_read_index, channel->tx_write_index, channel->num_stored_tx_frames);
329
330}
331
332static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){
333 if (len > channel->remote_mtu){
327
328}
329
330static int l2cap_ertm_send(l2cap_channel_t * channel, uint8_t * data, uint16_t len){
331 if (len > channel->remote_mtu){
334 log_error("l2cap_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid);
332 log_error("l2cap_ertm_send cid 0x%02x, data length exceeds remote MTU.", channel->local_cid);
335 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
336 }
337
333 return L2CAP_DATA_LEN_EXCEEDS_REMOTE_MTU;
334 }
335
336 if (!l2cap_ertm_can_store_packet_now(channel)){
337 log_error("l2cap_ertm_send cid 0x%02x, fragment store full", channel->local_cid);
338 return BTSTACK_ACL_BUFFERS_FULL;
339 }
340
338 // check if it needs to get fragmented
339 if (len > channel->remote_mps){
340 // fragmentation needed.
341 l2cap_segmentation_and_reassembly_t sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU;
342 int chunk_len;
343 while (len){
344 switch (sar){
345 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU:

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

625
626 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index];
627 // calc delta
628 int delta = (req_seq - tx_state->tx_seq) & 0x03f;
629 if (delta == 0) break; // all packets acknowledged
630 if (delta > l2cap_channel->remote_tx_window_size) break;
631
632 num_buffers_acked++;
341 // check if it needs to get fragmented
342 if (len > channel->remote_mps){
343 // fragmentation needed.
344 l2cap_segmentation_and_reassembly_t sar = L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU;
345 int chunk_len;
346 while (len){
347 switch (sar){
348 case L2CAP_SEGMENTATION_AND_REASSEMBLY_START_OF_L2CAP_SDU:

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

628
629 tx_state = &l2cap_channel->tx_packets_state[l2cap_channel->tx_read_index];
630 // calc delta
631 int delta = (req_seq - tx_state->tx_seq) & 0x03f;
632 if (delta == 0) break; // all packets acknowledged
633 if (delta > l2cap_channel->remote_tx_window_size) break;
634
635 num_buffers_acked++;
636 l2cap_channel->num_stored_tx_frames--;
633 l2cap_channel->unacked_frames--;
634 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq);
635
636 l2cap_channel->tx_read_index++;
637 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){
638 l2cap_channel->tx_read_index = 0;
639 }
640 }
637 l2cap_channel->unacked_frames--;
638 log_info("RR seq %u => packet with tx_seq %u done", req_seq, tx_state->tx_seq);
639
640 l2cap_channel->tx_read_index++;
641 if (l2cap_channel->tx_read_index >= l2cap_channel->num_rx_buffers){
642 l2cap_channel->tx_read_index = 0;
643 }
644 }
641
642 if (num_buffers_acked){
645 if (num_buffers_acked){
646 log_info("num_buffers_acked %u", num_buffers_acked);
643 l2cap_ertm_notify_channel_can_send(l2cap_channel);
644 }
645}
646
647static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){
648 int i;
649 for (i=0;i<l2cap_channel->num_tx_buffers;i++){
650 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i];

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

1936 if (!channel->waiting_for_can_send_now) continue;
1937 int can_send = 0;
1938 if (l2cap_is_le_channel_type(channel->channel_type)){
1939#ifdef ENABLE_BLE
1940 can_send = hci_can_send_acl_le_packet_now();
1941#endif
1942 } else {
1943#ifdef ENABLE_CLASSIC
647 l2cap_ertm_notify_channel_can_send(l2cap_channel);
648 }
649}
650
651static l2cap_ertm_tx_packet_state_t * l2cap_ertm_get_tx_state(l2cap_channel_t * l2cap_channel, uint8_t tx_seq){
652 int i;
653 for (i=0;i<l2cap_channel->num_tx_buffers;i++){
654 l2cap_ertm_tx_packet_state_t * tx_state = &l2cap_channel->tx_packets_state[i];

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

1940 if (!channel->waiting_for_can_send_now) continue;
1941 int can_send = 0;
1942 if (l2cap_is_le_channel_type(channel->channel_type)){
1943#ifdef ENABLE_BLE
1944 can_send = hci_can_send_acl_le_packet_now();
1945#endif
1946 } else {
1947#ifdef ENABLE_CLASSIC
1948#ifdef ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE
1949 // skip ertm channels as they only depend on free buffers in storage
1950 if (channel->mode == L2CAP_CHANNEL_MODE_BASIC){
1951 can_send = hci_can_send_acl_classic_packet_now();
1952 }
1953#else
1944 can_send = hci_can_send_acl_classic_packet_now();
1954 can_send = hci_can_send_acl_classic_packet_now();
1945#endif
1955#endif /* ENABLE_L2CAP_ENHANCED_RETRANSMISSION_MODE */
1956#endif /* ENABLE_CLASSIC */
1946 }
1947 if (!can_send) continue;
1948 // requeue for fairness
1949 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel);
1950 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel);
1951 // emit can send
1952 channel->waiting_for_can_send_now = 0;
1953 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid);

--- 1851 unchanged lines hidden --- 		1957 }
1958 if (!can_send) continue;
1959 // requeue for fairness
1960 btstack_linked_list_remove(&l2cap_channels, (btstack_linked_item_t *) channel);
1961 btstack_linked_list_add_tail(&l2cap_channels, (btstack_linked_item_t *) channel);
1962 // emit can send
1963 channel->waiting_for_can_send_now = 0;
1964 l2cap_emit_can_send_now(channel->packet_handler, channel->local_cid);

--- 1851 unchanged lines hidden ---