1 /******************************************************************************
2 *
3 * Copyright 2004-2012 Broadcom Corporation
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 /******************************************************************************
20 *
21 * This file contains the L2CAP 1.2 Flow Control and retransmissions
22 * functions
23 *
24 ******************************************************************************/
25
26 #include <bluetooth/log.h>
27 #include <stdlib.h>
28 #include <string.h>
29
30 #include "internal_include/bt_target.h"
31 #include "osi/include/allocator.h"
32 #include "stack/include/bt_hdr.h"
33 #include "stack/include/bt_types.h"
34 #include "stack/include/l2cdefs.h"
35 #include "stack/l2cap/internal/l2c_api.h"
36 #include "stack/l2cap/l2c_int.h"
37
38 /* Flag passed to retransmit_i_frames() when all packets should be retransmitted
39 */
40 #define L2C_FCR_RETX_ALL_PKTS 0xFF
41
42 using namespace bluetooth;
43
44 /* this is the minimal offset required by OBX to process incoming packets */
45 static const uint16_t OBX_BUF_MIN_OFFSET = 4;
46
47 static const char* SAR_types[] = {"Unsegmented", "Start", "End", "Continuation"};
48 static const char* SUP_types[] = {"RR", "REJ", "RNR", "SREJ"};
49
50 /* Look-up table for the CRC calculation */
51 static const uint16_t crctab[256] = {
52 0x0000, 0xc0c1, 0xc181, 0x0140, 0xc301, 0x03c0, 0x0280, 0xc241, 0xc601, 0x06c0, 0x0780,
53 0xc741, 0x0500, 0xc5c1, 0xc481, 0x0440, 0xcc01, 0x0cc0, 0x0d80, 0xcd41, 0x0f00, 0xcfc1,
54 0xce81, 0x0e40, 0x0a00, 0xcac1, 0xcb81, 0x0b40, 0xc901, 0x09c0, 0x0880, 0xc841, 0xd801,
55 0x18c0, 0x1980, 0xd941, 0x1b00, 0xdbc1, 0xda81, 0x1a40, 0x1e00, 0xdec1, 0xdf81, 0x1f40,
56 0xdd01, 0x1dc0, 0x1c80, 0xdc41, 0x1400, 0xd4c1, 0xd581, 0x1540, 0xd701, 0x17c0, 0x1680,
57 0xd641, 0xd201, 0x12c0, 0x1380, 0xd341, 0x1100, 0xd1c1, 0xd081, 0x1040, 0xf001, 0x30c0,
58 0x3180, 0xf141, 0x3300, 0xf3c1, 0xf281, 0x3240, 0x3600, 0xf6c1, 0xf781, 0x3740, 0xf501,
59 0x35c0, 0x3480, 0xf441, 0x3c00, 0xfcc1, 0xfd81, 0x3d40, 0xff01, 0x3fc0, 0x3e80, 0xfe41,
60 0xfa01, 0x3ac0, 0x3b80, 0xfb41, 0x3900, 0xf9c1, 0xf881, 0x3840, 0x2800, 0xe8c1, 0xe981,
61 0x2940, 0xeb01, 0x2bc0, 0x2a80, 0xea41, 0xee01, 0x2ec0, 0x2f80, 0xef41, 0x2d00, 0xedc1,
62 0xec81, 0x2c40, 0xe401, 0x24c0, 0x2580, 0xe541, 0x2700, 0xe7c1, 0xe681, 0x2640, 0x2200,
63 0xe2c1, 0xe381, 0x2340, 0xe101, 0x21c0, 0x2080, 0xe041, 0xa001, 0x60c0, 0x6180, 0xa141,
64 0x6300, 0xa3c1, 0xa281, 0x6240, 0x6600, 0xa6c1, 0xa781, 0x6740, 0xa501, 0x65c0, 0x6480,
65 0xa441, 0x6c00, 0xacc1, 0xad81, 0x6d40, 0xaf01, 0x6fc0, 0x6e80, 0xae41, 0xaa01, 0x6ac0,
66 0x6b80, 0xab41, 0x6900, 0xa9c1, 0xa881, 0x6840, 0x7800, 0xb8c1, 0xb981, 0x7940, 0xbb01,
67 0x7bc0, 0x7a80, 0xba41, 0xbe01, 0x7ec0, 0x7f80, 0xbf41, 0x7d00, 0xbdc1, 0xbc81, 0x7c40,
68 0xb401, 0x74c0, 0x7580, 0xb541, 0x7700, 0xb7c1, 0xb681, 0x7640, 0x7200, 0xb2c1, 0xb381,
69 0x7340, 0xb101, 0x71c0, 0x7080, 0xb041, 0x5000, 0x90c1, 0x9181, 0x5140, 0x9301, 0x53c0,
70 0x5280, 0x9241, 0x9601, 0x56c0, 0x5780, 0x9741, 0x5500, 0x95c1, 0x9481, 0x5440, 0x9c01,
71 0x5cc0, 0x5d80, 0x9d41, 0x5f00, 0x9fc1, 0x9e81, 0x5e40, 0x5a00, 0x9ac1, 0x9b81, 0x5b40,
72 0x9901, 0x59c0, 0x5880, 0x9841, 0x8801, 0x48c0, 0x4980, 0x8941, 0x4b00, 0x8bc1, 0x8a81,
73 0x4a40, 0x4e00, 0x8ec1, 0x8f81, 0x4f40, 0x8d01, 0x4dc0, 0x4c80, 0x8c41, 0x4400, 0x84c1,
74 0x8581, 0x4540, 0x8701, 0x47c0, 0x4680, 0x8641, 0x8201, 0x42c0, 0x4380, 0x8341, 0x4100,
75 0x81c1, 0x8081, 0x4040,
76 };
77
78 /*******************************************************************************
79 * Static local functions
80 */
81 static bool process_reqseq(tL2C_CCB* p_ccb, uint16_t ctrl_word);
82 static void process_s_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word);
83 static void process_i_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word, bool delay_ack);
84 static bool retransmit_i_frames(tL2C_CCB* p_ccb, uint8_t tx_seq);
85 static void prepare_I_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, bool is_retransmission);
86 static bool do_sar_reassembly(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word);
87
88 /*******************************************************************************
89 *
90 * Function l2c_fcr_updcrc
91 *
92 * Description This function computes the CRC using the look-up table.
93 *
94 * Returns CRC
95 *
96 ******************************************************************************/
l2c_fcr_updcrc(uint16_t icrc,unsigned char * icp,int icnt)97 static uint16_t l2c_fcr_updcrc(uint16_t icrc, unsigned char* icp, int icnt) {
98 uint16_t crc = icrc;
99 unsigned char* cp = icp;
100 int cnt = icnt;
101
102 while (cnt--) {
103 crc = ((crc >> 8) & 0xff) ^ crctab[(crc & 0xff) ^ *cp++];
104 }
105
106 return crc;
107 }
108
109 /*******************************************************************************
110 *
111 * Function l2c_fcr_tx_get_fcs
112 *
113 * Description This function computes the CRC for a frame to be TXed.
114 *
115 * Returns CRC
116 *
117 ******************************************************************************/
l2c_fcr_tx_get_fcs(BT_HDR * p_buf)118 static uint16_t l2c_fcr_tx_get_fcs(BT_HDR* p_buf) {
119 uint8_t* p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
120
121 return l2c_fcr_updcrc(L2CAP_FCR_INIT_CRC, p, p_buf->len);
122 }
123
124 /*******************************************************************************
125 *
126 * Function l2c_fcr_rx_get_fcs
127 *
128 * Description This function computes the CRC for a received frame.
129 *
130 * Returns CRC
131 *
132 ******************************************************************************/
l2c_fcr_rx_get_fcs(BT_HDR * p_buf)133 static uint16_t l2c_fcr_rx_get_fcs(BT_HDR* p_buf) {
134 uint8_t* p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
135
136 /* offset points past the L2CAP header, but the CRC check includes it */
137 p -= L2CAP_PKT_OVERHEAD;
138
139 return l2c_fcr_updcrc(L2CAP_FCR_INIT_CRC, p, p_buf->len + L2CAP_PKT_OVERHEAD);
140 }
141
142 /*******************************************************************************
143 *
144 * Function l2c_fcr_start_timer
145 *
146 * Description This function starts the (monitor or retransmission) timer.
147 *
148 * Returns -
149 *
150 ******************************************************************************/
l2c_fcr_start_timer(tL2C_CCB * p_ccb)151 void l2c_fcr_start_timer(tL2C_CCB* p_ccb) {
152 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
153 uint32_t tout;
154
155 /* The timers which are in milliseconds */
156 if (p_ccb->fcrb.wait_ack) {
157 tout = (uint32_t)p_ccb->our_cfg.fcr.mon_tout;
158 } else {
159 tout = (uint32_t)p_ccb->our_cfg.fcr.rtrans_tout;
160 }
161
162 /* Only start a timer that was not started */
163 if (!alarm_is_scheduled(p_ccb->fcrb.mon_retrans_timer)) {
164 alarm_set_on_mloop(p_ccb->fcrb.mon_retrans_timer, tout, l2c_ccb_timer_timeout, p_ccb);
165 }
166 }
167
168 /*******************************************************************************
169 *
170 * Function l2c_fcr_stop_timer
171 *
172 * Description This function stops the (monitor or transmission) timer.
173 *
174 * Returns -
175 *
176 ******************************************************************************/
l2c_fcr_stop_timer(tL2C_CCB * p_ccb)177 void l2c_fcr_stop_timer(tL2C_CCB* p_ccb) {
178 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
179 alarm_cancel(p_ccb->fcrb.mon_retrans_timer);
180 }
181
182 /*******************************************************************************
183 *
184 * Function l2c_fcr_cleanup
185 *
186 * Description This function cleans up the variable used for
187 * flow-control/retrans.
188 *
189 * Returns -
190 *
191 ******************************************************************************/
l2c_fcr_cleanup(tL2C_CCB * p_ccb)192 void l2c_fcr_cleanup(tL2C_CCB* p_ccb) {
193 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
194 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
195
196 alarm_free(p_fcrb->mon_retrans_timer);
197 p_fcrb->mon_retrans_timer = NULL;
198 alarm_free(p_fcrb->ack_timer);
199 p_fcrb->ack_timer = NULL;
200
201 osi_free_and_reset((void**)&p_fcrb->p_rx_sdu);
202
203 fixed_queue_free(p_fcrb->waiting_for_ack_q, osi_free);
204 p_fcrb->waiting_for_ack_q = NULL;
205
206 fixed_queue_free(p_fcrb->srej_rcv_hold_q, osi_free);
207 p_fcrb->srej_rcv_hold_q = NULL;
208
209 fixed_queue_free(p_fcrb->retrans_q, osi_free);
210 p_fcrb->retrans_q = NULL;
211
212 memset(p_fcrb, 0, sizeof(tL2C_FCRB));
213 }
214
215 /*******************************************************************************
216 *
217 * Function l2c_fcr_clone_buf
218 *
219 * Description This function allocates and copies requested part of a
220 * buffer at a new-offset.
221 *
222 * Returns pointer to new buffer
223 *
224 ******************************************************************************/
l2c_fcr_clone_buf(BT_HDR * p_buf,uint16_t new_offset,uint16_t no_of_bytes)225 BT_HDR* l2c_fcr_clone_buf(BT_HDR* p_buf, uint16_t new_offset, uint16_t no_of_bytes) {
226 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
227 /*
228 * NOTE: We allocate extra L2CAP_FCS_LEN octets, in case we need to put
229 * the FCS (Frame Check Sequence) at the end of the buffer.
230 */
231 uint16_t buf_size = no_of_bytes + sizeof(BT_HDR) + new_offset + L2CAP_FCS_LEN;
232 BT_HDR* p_buf2 = (BT_HDR*)osi_malloc(buf_size);
233
234 p_buf2->offset = new_offset;
235 p_buf2->len = no_of_bytes;
236 memcpy(((uint8_t*)(p_buf2 + 1)) + p_buf2->offset, ((uint8_t*)(p_buf + 1)) + p_buf->offset,
237 no_of_bytes);
238
239 return p_buf2;
240 }
241
242 /*******************************************************************************
243 *
244 * Function l2c_fcr_is_flow_controlled
245 *
246 * Description This function checks if the CCB is flow controlled by peer.
247 *
248 * Returns The control word
249 *
250 ******************************************************************************/
l2c_fcr_is_flow_controlled(tL2C_CCB * p_ccb)251 bool l2c_fcr_is_flow_controlled(tL2C_CCB* p_ccb) {
252 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
253 if (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE) {
254 /* Check if remote side flowed us off or the transmit window is full */
255 if ((p_ccb->fcrb.remote_busy) ||
256 (fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q) >= p_ccb->peer_cfg.fcr.tx_win_sz)) {
257 return true;
258 }
259 }
260 return false;
261 }
262
263 /*******************************************************************************
264 *
265 * Function prepare_I_frame
266 *
267 * Description This function sets the FCR variables in an I-frame that is
268 * about to be sent to HCI for transmission. This may be the
269 * first time the I-frame is sent, or a retransmission
270 *
271 * Returns -
272 *
273 ******************************************************************************/
prepare_I_frame(tL2C_CCB * p_ccb,BT_HDR * p_buf,bool is_retransmission)274 static void prepare_I_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, bool is_retransmission) {
275 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
276 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
277 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
278 uint8_t* p;
279 uint16_t fcs;
280 uint16_t ctrl_word;
281 bool set_f_bit = p_fcrb->send_f_rsp;
282
283 uint8_t fcs_len = l2cu_get_fcs_len(p_ccb);
284
285 p_fcrb->send_f_rsp = false;
286
287 if (is_retransmission) {
288 /* Get the old control word and clear out the old req_seq and F bits */
289 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset + L2CAP_PKT_OVERHEAD;
290
291 STREAM_TO_UINT16(ctrl_word, p);
292
293 ctrl_word &= ~(L2CAP_FCR_REQ_SEQ_BITS + L2CAP_FCR_F_BIT);
294 } else {
295 ctrl_word = p_buf->layer_specific & L2CAP_FCR_SEG_BITS; /* SAR bits */
296 ctrl_word |= (p_fcrb->next_tx_seq << L2CAP_FCR_TX_SEQ_BITS_SHIFT); /* Tx Seq */
297
298 p_fcrb->next_tx_seq = (p_fcrb->next_tx_seq + 1) & L2CAP_FCR_SEQ_MODULO;
299 }
300
301 /* Set the F-bit and reqseq only if using re-transmission mode */
302 if (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE) {
303 if (set_f_bit) {
304 ctrl_word |= L2CAP_FCR_F_BIT;
305 }
306
307 ctrl_word |= (p_fcrb->next_seq_expected) << L2CAP_FCR_REQ_SEQ_BITS_SHIFT;
308
309 p_fcrb->last_ack_sent = p_ccb->fcrb.next_seq_expected;
310
311 alarm_cancel(p_ccb->fcrb.ack_timer);
312 }
313
314 /* Set the control word */
315 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset + L2CAP_PKT_OVERHEAD;
316
317 UINT16_TO_STREAM(p, ctrl_word);
318
319 /* Compute the FCS and add to the end of the buffer if not bypassed */
320 /* length field in l2cap header has to include FCS length */
321 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
322 UINT16_TO_STREAM(p, p_buf->len + fcs_len - L2CAP_PKT_OVERHEAD);
323
324 if (fcs_len != 0) {
325 /* Calculate the FCS */
326 fcs = l2c_fcr_tx_get_fcs(p_buf);
327
328 /* Point to the end of the buffer and put the FCS there */
329 /*
330 * NOTE: Here we assume the allocated buffer is large enough
331 * to include extra L2CAP_FCS_LEN octets at the end.
332 */
333 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset + p_buf->len;
334
335 UINT16_TO_STREAM(p, fcs);
336
337 p_buf->len += fcs_len;
338 }
339
340 if (is_retransmission) {
341 log::verbose(
342 "L2CAP eRTM ReTx I-frame CID: 0x{:04x} Len: {} SAR: {} TxSeq: {} "
343 "ReqSeq: {} F: {}",
344 p_ccb->local_cid, p_buf->len,
345 SAR_types[(ctrl_word & L2CAP_FCR_SAR_BITS) >> L2CAP_FCR_SAR_BITS_SHIFT],
346 (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT,
347 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
348 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
349 } else {
350 log::verbose(
351 "L2CAP eRTM Tx I-frame CID: 0x{:04x} Len: {} SAR: {:<12s} TxSeq: {} "
352 " ReqSeq: {} F: {}",
353 p_ccb->local_cid, p_buf->len,
354 SAR_types[(ctrl_word & L2CAP_FCR_SAR_BITS) >> L2CAP_FCR_SAR_BITS_SHIFT],
355 (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT,
356 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
357 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
358 }
359
360 /* Start the retransmission timer if not already running */
361 if (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE) {
362 l2c_fcr_start_timer(p_ccb);
363 }
364 }
365
366 /*******************************************************************************
367 *
368 * Function l2c_fcr_send_S_frame
369 *
370 * Description This function formats and sends an S-frame for transmission.
371 *
372 * Returns -
373 *
374 ******************************************************************************/
l2c_fcr_send_S_frame(tL2C_CCB * p_ccb,uint16_t function_code,uint16_t pf_bit)375 void l2c_fcr_send_S_frame(tL2C_CCB* p_ccb, uint16_t function_code, uint16_t pf_bit) {
376 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
377 uint8_t* p;
378 uint16_t ctrl_word;
379 uint16_t fcs;
380
381 if ((!p_ccb->in_use) || (p_ccb->chnl_state != CST_OPEN)) {
382 return;
383 }
384
385 if (pf_bit == L2CAP_FCR_P_BIT) {
386 p_ccb->fcrb.wait_ack = true;
387
388 l2c_fcr_stop_timer(p_ccb); /* Restart the monitor timer */
389 l2c_fcr_start_timer(p_ccb);
390 }
391
392 /* Create the control word to use */
393 ctrl_word = (function_code << L2CAP_FCR_SUP_SHIFT) | L2CAP_FCR_S_FRAME_BIT;
394 ctrl_word |= (p_ccb->fcrb.next_seq_expected << L2CAP_FCR_REQ_SEQ_BITS_SHIFT);
395 ctrl_word |= pf_bit;
396
397 BT_HDR* p_buf = (BT_HDR*)osi_malloc(L2CAP_CMD_BUF_SIZE);
398 p_buf->offset = HCI_DATA_PREAMBLE_SIZE;
399 p_buf->len = L2CAP_PKT_OVERHEAD + L2CAP_FCR_OVERHEAD;
400
401 /* Set the pointer to the beginning of the data */
402 p = (uint8_t*)(p_buf + 1) + p_buf->offset;
403
404 uint8_t fcs_len = l2cu_get_fcs_len(p_ccb);
405
406 /* Put in the L2CAP header */
407 UINT16_TO_STREAM(p, L2CAP_FCR_OVERHEAD + fcs_len);
408 UINT16_TO_STREAM(p, p_ccb->remote_cid);
409 UINT16_TO_STREAM(p, ctrl_word);
410
411 if (fcs_len != 0) {
412 /* Compute the FCS and add to the end of the buffer if not bypassed */
413 fcs = l2c_fcr_tx_get_fcs(p_buf);
414
415 UINT16_TO_STREAM(p, fcs);
416 p_buf->len += fcs_len;
417 }
418
419 /* Now, the HCI transport header */
420 p_buf->layer_specific = L2CAP_NON_FLUSHABLE_PKT;
421 l2cu_set_acl_hci_header(p_buf, p_ccb);
422
423 if ((((ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT) == 1) ||
424 (((ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT) == 3)) {
425 log::warn(
426 "L2CAP eRTM Tx S-frame CID: 0x{:04x} ctrlword: 0x{:04x} Type: {} "
427 "ReqSeq: {} P: {} F: {}",
428 p_ccb->local_cid, ctrl_word,
429 SUP_types[(ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT],
430 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
431 (ctrl_word & L2CAP_FCR_P_BIT) >> L2CAP_FCR_P_BIT_SHIFT,
432 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
433 log::warn("Buf Len: {}", p_buf->len);
434 } else {
435 log::verbose(
436 "L2CAP eRTM Tx S-frame CID: 0x{:04x} ctrlword: 0x{:04x} Type: {} "
437 "ReqSeq: {} P: {} F: {}",
438 p_ccb->local_cid, ctrl_word,
439 SUP_types[(ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT],
440 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
441 (ctrl_word & L2CAP_FCR_P_BIT) >> L2CAP_FCR_P_BIT_SHIFT,
442 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
443 log::verbose("Buf Len: {}", p_buf->len);
444 }
445
446 l2c_link_check_send_pkts(p_ccb->p_lcb, 0, p_buf);
447
448 p_ccb->fcrb.last_ack_sent = p_ccb->fcrb.next_seq_expected;
449
450 alarm_cancel(p_ccb->fcrb.ack_timer);
451 }
452
453 /*******************************************************************************
454 *
455 * Function l2c_fcr_proc_pdu
456 *
457 * Description This function is the entry point for processing of a
458 * received PDU when in flow control and/or retransmission
459 * modes.
460 *
461 * Returns -
462 *
463 ******************************************************************************/
l2c_fcr_proc_pdu(tL2C_CCB * p_ccb,BT_HDR * p_buf)464 void l2c_fcr_proc_pdu(tL2C_CCB* p_ccb, BT_HDR* p_buf) {
465 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
466 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
467 uint8_t* p;
468 uint16_t fcs;
469 uint16_t min_pdu_len;
470 uint16_t ctrl_word;
471
472 /* Check the length */
473 uint8_t fcs_len = l2cu_get_fcs_len(p_ccb);
474
475 min_pdu_len = (uint16_t)(fcs_len + L2CAP_FCR_OVERHEAD);
476
477 if (p_buf->len < min_pdu_len) {
478 log::warn("Rx L2CAP PDU: CID: 0x{:04x} Len too short: {}", p_ccb->local_cid, p_buf->len);
479 osi_free(p_buf);
480 return;
481 }
482
483 /* Get the control word */
484 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
485 STREAM_TO_UINT16(ctrl_word, p);
486
487 if (ctrl_word & L2CAP_FCR_S_FRAME_BIT) {
488 if ((((ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT) == 1) ||
489 (((ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT) == 3)) {
490 /* REJ or SREJ */
491 log::warn(
492 "L2CAP eRTM Rx S-frame: cid: 0x{:04x} Len: {} Type: {} ReqSeq: {} "
493 " P: {} F: {}",
494 p_ccb->local_cid, p_buf->len,
495 SUP_types[(ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT],
496 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
497 (ctrl_word & L2CAP_FCR_P_BIT) >> L2CAP_FCR_P_BIT_SHIFT,
498 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
499 } else {
500 log::verbose(
501 "L2CAP eRTM Rx S-frame: cid: 0x{:04x} Len: {} Type: {} ReqSeq: {} "
502 " P: {} F: {}",
503 p_ccb->local_cid, p_buf->len,
504 SUP_types[(ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT],
505 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
506 (ctrl_word & L2CAP_FCR_P_BIT) >> L2CAP_FCR_P_BIT_SHIFT,
507 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
508 }
509 } else {
510 log::verbose(
511 "L2CAP eRTM Rx I-frame: cid: 0x{:04x} Len: {} SAR: {:<12s} TxSeq: "
512 "{} ReqSeq: {} F: {}",
513 p_ccb->local_cid, p_buf->len,
514 SAR_types[(ctrl_word & L2CAP_FCR_SAR_BITS) >> L2CAP_FCR_SAR_BITS_SHIFT],
515 (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT,
516 (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT,
517 (ctrl_word & L2CAP_FCR_F_BIT) >> L2CAP_FCR_F_BIT_SHIFT);
518 }
519
520 log::verbose(
521 "eRTM Rx Nxt_tx_seq {}, Lst_rx_ack {}, Nxt_seq_exp {}, Lst_ack_snt {}, "
522 "wt_q.cnt {}, tries {}",
523 p_ccb->fcrb.next_tx_seq, p_ccb->fcrb.last_rx_ack, p_ccb->fcrb.next_seq_expected,
524 p_ccb->fcrb.last_ack_sent, fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q),
525 p_ccb->fcrb.num_tries);
526
527 if (fcs_len != 0) {
528 /* Verify FCS if using */
529 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset + p_buf->len - fcs_len;
530
531 /* Extract and drop the FCS from the packet */
532 STREAM_TO_UINT16(fcs, p);
533 p_buf->len -= fcs_len;
534
535 if (l2c_fcr_rx_get_fcs(p_buf) != fcs) {
536 log::warn("Rx L2CAP PDU: CID: 0x{:04x} BAD FCS", p_ccb->local_cid);
537 osi_free(p_buf);
538 return;
539 }
540 }
541
542 /* Get the control word */
543 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
544
545 STREAM_TO_UINT16(ctrl_word, p);
546
547 p_buf->len -= L2CAP_FCR_OVERHEAD;
548 p_buf->offset += L2CAP_FCR_OVERHEAD;
549
550 /* If we had a poll bit outstanding, check if we got a final response */
551 if (p_ccb->fcrb.wait_ack) {
552 /* If final bit not set, ignore the frame unless it is a polled S-frame */
553 if (!(ctrl_word & L2CAP_FCR_F_BIT)) {
554 if ((ctrl_word & L2CAP_FCR_P_BIT) && (ctrl_word & L2CAP_FCR_S_FRAME_BIT)) {
555 if (p_ccb->fcrb.srej_sent) {
556 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_SREJ, L2CAP_FCR_F_BIT);
557 } else {
558 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, L2CAP_FCR_F_BIT);
559 }
560
561 /* Got a poll while in wait_ack state, so re-start our timer with
562 * 1-second */
563 /* This is a small optimization... the monitor timer is 12 secs, but we
564 * saw */
565 /* that if the other side sends us a poll when we are waiting for a
566 * final, */
567 /* then it speeds up recovery significantly if we poll it back soon
568 * after its poll. */
569 alarm_set_on_mloop(p_ccb->fcrb.mon_retrans_timer, BT_1SEC_TIMEOUT_MS, l2c_ccb_timer_timeout,
570 p_ccb);
571 }
572 osi_free(p_buf);
573 return;
574 }
575
576 p_ccb->fcrb.wait_ack = false;
577
578 /* P and F are mutually exclusive */
579 if (ctrl_word & L2CAP_FCR_S_FRAME_BIT) {
580 ctrl_word &= ~L2CAP_FCR_P_BIT;
581 }
582
583 if (fixed_queue_is_empty(p_ccb->fcrb.waiting_for_ack_q)) {
584 p_ccb->fcrb.num_tries = 0;
585 }
586
587 l2c_fcr_stop_timer(p_ccb);
588 } else {
589 /* Otherwise, ensure the final bit is ignored */
590 ctrl_word &= ~L2CAP_FCR_F_BIT;
591 }
592
593 /* Process receive sequence number */
594 if (!process_reqseq(p_ccb, ctrl_word)) {
595 osi_free(p_buf);
596 return;
597 }
598
599 /* Process based on whether it is an S-frame or an I-frame */
600 if (ctrl_word & L2CAP_FCR_S_FRAME_BIT) {
601 process_s_frame(p_ccb, p_buf, ctrl_word);
602 } else {
603 process_i_frame(p_ccb, p_buf, ctrl_word, false);
604 }
605
606 /* Return if the channel got disconnected by a bad packet or max
607 * retransmissions */
608 if ((!p_ccb->in_use) || (p_ccb->chnl_state != CST_OPEN)) {
609 return;
610 }
611
612 /* If we have some buffers held while doing SREJ, and SREJ has cleared,
613 * process them now */
614 if ((!p_ccb->fcrb.srej_sent) && (!fixed_queue_is_empty(p_ccb->fcrb.srej_rcv_hold_q))) {
615 fixed_queue_t* temp_q = p_ccb->fcrb.srej_rcv_hold_q;
616 p_ccb->fcrb.srej_rcv_hold_q = fixed_queue_new(SIZE_MAX);
617
618 while ((p_buf = (BT_HDR*)fixed_queue_try_dequeue(temp_q)) != NULL) {
619 if (p_ccb->in_use && (p_ccb->chnl_state == CST_OPEN)) {
620 /* Get the control word */
621 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset - L2CAP_FCR_OVERHEAD;
622
623 STREAM_TO_UINT16(ctrl_word, p);
624
625 log::verbose(
626 "l2c_fcr_proc_pdu() CID: 0x{:04x} Process Buffer from SREJ_Hold_Q "
627 " TxSeq: {} Expected_Seq: {}",
628 p_ccb->local_cid,
629 (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT,
630 p_ccb->fcrb.next_seq_expected);
631
632 /* Process the SREJ held I-frame, but do not send an RR for each
633 * individual frame */
634 process_i_frame(p_ccb, p_buf, ctrl_word, true);
635 } else {
636 osi_free(p_buf);
637 }
638
639 /* If more frames were lost during SREJ, send a REJ */
640 if (p_ccb->fcrb.rej_after_srej) {
641 p_ccb->fcrb.rej_after_srej = false;
642 p_ccb->fcrb.rej_sent = true;
643
644 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_REJ, 0);
645 }
646 }
647 fixed_queue_free(temp_q, NULL);
648
649 /* Now, if needed, send one RR for the whole held queue */
650 if ((!p_ccb->fcrb.rej_sent) && (!p_ccb->fcrb.srej_sent) &&
651 (p_ccb->fcrb.next_seq_expected != p_ccb->fcrb.last_ack_sent)) {
652 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, 0);
653 } else {
654 log::verbose(
655 "l2c_fcr_proc_pdu() not sending RR CID: 0x{:04x} local_busy:{} "
656 "rej_sent:{} srej_sent:{} Expected_Seq:{} Last_Ack:{}",
657 p_ccb->local_cid, 0, p_ccb->fcrb.rej_sent, p_ccb->fcrb.srej_sent,
658 p_ccb->fcrb.next_seq_expected, p_ccb->fcrb.last_ack_sent);
659 }
660 }
661
662 /* If a window has opened, check if we can send any more packets */
663 if ((!fixed_queue_is_empty(p_ccb->fcrb.retrans_q) || !fixed_queue_is_empty(p_ccb->xmit_hold_q)) &&
664 (!p_ccb->fcrb.wait_ack) && (!l2c_fcr_is_flow_controlled(p_ccb))) {
665 l2c_link_check_send_pkts(p_ccb->p_lcb, 0, NULL);
666 }
667 }
668
669 /*******************************************************************************
670 *
671 * Function l2c_lcc_proc_pdu
672 *
673 * Description This function is the entry point for processing of a
674 * received PDU when in LE Coc flow control modes.
675 *
676 * Returns -
677 *
678 ******************************************************************************/
l2c_lcc_proc_pdu(tL2C_CCB * p_ccb,BT_HDR * p_buf)679 void l2c_lcc_proc_pdu(tL2C_CCB* p_ccb, BT_HDR* p_buf) {
680 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
681 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
682 uint8_t* p = (uint8_t*)(p_buf + 1) + p_buf->offset;
683 uint16_t sdu_length;
684 BT_HDR* p_data = NULL;
685
686 /* Buffer length should not exceed local mps */
687 if (p_buf->len > p_ccb->local_conn_cfg.mps) {
688 log::error("buffer length={} exceeds local mps={}. Drop and disconnect.", p_buf->len,
689 p_ccb->local_conn_cfg.mps);
690
691 /* Discard the buffer and disconnect*/
692 osi_free(p_buf);
693 l2cu_disconnect_chnl(p_ccb);
694 return;
695 }
696
697 if (p_ccb->is_first_seg) {
698 if (p_buf->len < sizeof(sdu_length)) {
699 log::error("buffer length={} too small. Need at least 2.", p_buf->len);
700 /* Discard the buffer */
701 osi_free(p_buf);
702 return;
703 }
704 STREAM_TO_UINT16(sdu_length, p);
705
706 /* Check the SDU Length with local MTU size */
707 if (sdu_length > p_ccb->local_conn_cfg.mtu) {
708 log::error("sdu length={} exceeds local mtu={}. Drop and disconnect.", sdu_length,
709 p_ccb->local_conn_cfg.mtu);
710 /* Discard the buffer and disconnect*/
711 osi_free(p_buf);
712 l2cu_disconnect_chnl(p_ccb);
713 return;
714 }
715
716 p_buf->len -= sizeof(sdu_length);
717 p_buf->offset += sizeof(sdu_length);
718
719 if (sdu_length < p_buf->len) {
720 log::error("Invalid sdu_length: {}", sdu_length);
721 /* Discard the buffer */
722 osi_free(p_buf);
723 return;
724 }
725
726 p_data = (BT_HDR*)osi_malloc(BT_HDR_SIZE + sdu_length);
727 if (p_data == NULL) {
728 osi_free(p_buf);
729 return;
730 }
731
732 p_ccb->ble_sdu = p_data;
733 p_data->len = 0;
734 p_ccb->ble_sdu_length = sdu_length;
735 log::verbose("SDU Length = {}", sdu_length);
736 p_data->offset = 0;
737
738 } else {
739 p_data = p_ccb->ble_sdu;
740 if (p_data == NULL) {
741 osi_free(p_buf);
742 return;
743 }
744 if (p_buf->len > (p_ccb->ble_sdu_length - p_data->len)) {
745 log::error("buffer length={} too big. max={}. Dropped", p_data->len,
746 p_ccb->ble_sdu_length - p_data->len);
747 osi_free(p_buf);
748
749 /* Throw away all pending fragments and disconnects */
750 p_ccb->is_first_seg = true;
751 osi_free(p_ccb->ble_sdu);
752 p_ccb->ble_sdu = NULL;
753 p_ccb->ble_sdu_length = 0;
754 l2cu_disconnect_chnl(p_ccb);
755 return;
756 }
757 }
758
759 memcpy((uint8_t*)(p_data + 1) + p_data->offset + p_data->len,
760 (uint8_t*)(p_buf + 1) + p_buf->offset, p_buf->len);
761 p_data->len += p_buf->len;
762 p = (uint8_t*)(p_data + 1) + p_data->offset;
763 if (p_data->len == p_ccb->ble_sdu_length) {
764 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_DATA, p_data);
765 p_ccb->is_first_seg = true;
766 p_ccb->ble_sdu = NULL;
767 p_ccb->ble_sdu_length = 0;
768 } else if (p_data->len < p_ccb->ble_sdu_length) {
769 p_ccb->is_first_seg = false;
770 }
771
772 osi_free(p_buf);
773 return;
774 }
775
776 /*******************************************************************************
777 *
778 * Function l2c_fcr_proc_tout
779 *
780 * Description Handle a timeout. We should be in error recovery state.
781 *
782 * Returns -
783 *
784 ******************************************************************************/
l2c_fcr_proc_tout(tL2C_CCB * p_ccb)785 void l2c_fcr_proc_tout(tL2C_CCB* p_ccb) {
786 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
787 log::verbose(
788 "l2c_fcr_proc_tout: CID: 0x{:04x} num_tries: {} (max: {}) wait_ack: "
789 "{} ack_q_count: {}",
790 p_ccb->local_cid, p_ccb->fcrb.num_tries, p_ccb->peer_cfg.fcr.max_transmit,
791 p_ccb->fcrb.wait_ack, fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q));
792
793 if ((p_ccb->peer_cfg.fcr.max_transmit != 0) &&
794 (++p_ccb->fcrb.num_tries > p_ccb->peer_cfg.fcr.max_transmit)) {
795 l2cu_disconnect_chnl(p_ccb);
796 } else {
797 if (!p_ccb->fcrb.srej_sent && !p_ccb->fcrb.rej_sent) {
798 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, L2CAP_FCR_P_BIT);
799 }
800 }
801 }
802
803 /*******************************************************************************
804 *
805 * Function l2c_fcr_proc_ack_tout
806 *
807 * Description Send RR/RNR if we have not acked I frame
808 *
809 * Returns -
810 *
811 ******************************************************************************/
l2c_fcr_proc_ack_tout(tL2C_CCB * p_ccb)812 void l2c_fcr_proc_ack_tout(tL2C_CCB* p_ccb) {
813 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
814 log::verbose(
815 "l2c_fcr_proc_ack_tout: CID: 0x{:04x} State: {} Wack:{} Rq:{} "
816 "Acked:{}",
817 p_ccb->local_cid, p_ccb->chnl_state, p_ccb->fcrb.wait_ack, p_ccb->fcrb.next_seq_expected,
818 p_ccb->fcrb.last_ack_sent);
819
820 if ((p_ccb->chnl_state == CST_OPEN) && (!p_ccb->fcrb.wait_ack) &&
821 (p_ccb->fcrb.last_ack_sent != p_ccb->fcrb.next_seq_expected)) {
822 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, 0);
823 }
824 }
825
826 /*******************************************************************************
827 *
828 * Function process_reqseq
829 *
830 * Description Handle receive sequence number
831 *
832 * Returns -
833 *
834 ******************************************************************************/
process_reqseq(tL2C_CCB * p_ccb,uint16_t ctrl_word)835 static bool process_reqseq(tL2C_CCB* p_ccb, uint16_t ctrl_word) {
836 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
837 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
838 uint8_t req_seq, num_bufs_acked, xx;
839 uint16_t ls;
840 uint16_t full_sdus_xmitted;
841
842 /* Receive sequence number does not ack anything for SREJ with P-bit set to
843 * zero */
844 if ((ctrl_word & L2CAP_FCR_S_FRAME_BIT) &&
845 ((ctrl_word & L2CAP_FCR_SUP_BITS) == (L2CAP_FCR_SUP_SREJ << L2CAP_FCR_SUP_SHIFT)) &&
846 ((ctrl_word & L2CAP_FCR_P_BIT) == 0)) {
847 /* If anything still waiting for ack, restart the timer if it was stopped */
848 if (!fixed_queue_is_empty(p_fcrb->waiting_for_ack_q)) {
849 l2c_fcr_start_timer(p_ccb);
850 }
851
852 return true;
853 }
854
855 /* Extract the receive sequence number from the control word */
856 req_seq = (ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >> L2CAP_FCR_REQ_SEQ_BITS_SHIFT;
857
858 num_bufs_acked = (req_seq - p_fcrb->last_rx_ack) & L2CAP_FCR_SEQ_MODULO;
859
860 /* Verify the request sequence is in range before proceeding */
861 if (num_bufs_acked > fixed_queue_length(p_fcrb->waiting_for_ack_q)) {
862 /* The channel is closed if ReqSeq is not in range */
863 log::warn(
864 "L2CAP eRTM Frame BAD Req_Seq - ctrl_word: 0x{:04x} req_seq 0x{:02x} "
865 "last_rx_ack: 0x{:02x} QCount: {}",
866 ctrl_word, req_seq, p_fcrb->last_rx_ack, fixed_queue_length(p_fcrb->waiting_for_ack_q));
867
868 l2cu_disconnect_chnl(p_ccb);
869 return false;
870 }
871
872 p_fcrb->last_rx_ack = req_seq;
873
874 /* Now we can release all acknowledged frames, and restart the retransmission
875 * timer if needed */
876 if (num_bufs_acked != 0) {
877 p_fcrb->num_tries = 0;
878 full_sdus_xmitted = 0;
879
880 for (xx = 0; xx < num_bufs_acked; xx++) {
881 BT_HDR* p_tmp = (BT_HDR*)fixed_queue_try_dequeue(p_fcrb->waiting_for_ack_q);
882 ls = p_tmp->layer_specific & L2CAP_FCR_SAR_BITS;
883
884 if ((ls == L2CAP_FCR_UNSEG_SDU) || (ls == L2CAP_FCR_END_SDU)) {
885 full_sdus_xmitted++;
886 }
887
888 osi_free(p_tmp);
889 }
890
891 /* If we are still in a wait_ack state, do not mess with the timer */
892 if (!p_ccb->fcrb.wait_ack) {
893 l2c_fcr_stop_timer(p_ccb);
894 }
895
896 /* Check if we need to call the "packet_sent" callback */
897 if ((p_ccb->p_rcb) && (p_ccb->p_rcb->api.pL2CA_TxComplete_Cb) && (full_sdus_xmitted)) {
898 /* Special case for eRTM, if all packets sent, send 0xFFFF */
899 if (fixed_queue_is_empty(p_fcrb->waiting_for_ack_q) &&
900 fixed_queue_is_empty(p_ccb->xmit_hold_q)) {
901 full_sdus_xmitted = 0xFFFF;
902 }
903
904 (*p_ccb->p_rcb->api.pL2CA_TxComplete_Cb)(p_ccb->local_cid, full_sdus_xmitted);
905 }
906 }
907
908 /* If anything still waiting for ack, restart the timer if it was stopped */
909 if (!fixed_queue_is_empty(p_fcrb->waiting_for_ack_q)) {
910 l2c_fcr_start_timer(p_ccb);
911 }
912 return true;
913 }
914
915 /*******************************************************************************
916 *
917 * Function process_s_frame
918 *
919 * Description Process an S frame
920 *
921 * Returns -
922 *
923 ******************************************************************************/
process_s_frame(tL2C_CCB * p_ccb,BT_HDR * p_buf,uint16_t ctrl_word)924 static void process_s_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word) {
925 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
926 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
927
928 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
929 uint16_t s_frame_type = (ctrl_word & L2CAP_FCR_SUP_BITS) >> L2CAP_FCR_SUP_SHIFT;
930 bool remote_was_busy;
931 bool all_ok = true;
932
933 if (p_buf->len != 0) {
934 log::warn("Incorrect S-frame Length ({})", p_buf->len);
935 }
936
937 log::verbose("process_s_frame ctrl_word 0x{:04x} fcrb_remote_busy:{}", ctrl_word,
938 p_fcrb->remote_busy);
939
940 if (ctrl_word & L2CAP_FCR_P_BIT) {
941 p_fcrb->rej_sent = false; /* After checkpoint, we can send another REJ */
942 p_fcrb->send_f_rsp = true; /* Set a flag in case an I-frame is pending */
943 }
944
945 switch (s_frame_type) {
946 case L2CAP_FCR_SUP_RR:
947 remote_was_busy = p_fcrb->remote_busy;
948 p_fcrb->remote_busy = false;
949
950 if ((ctrl_word & L2CAP_FCR_F_BIT) || (remote_was_busy)) {
951 all_ok = retransmit_i_frames(p_ccb, L2C_FCR_RETX_ALL_PKTS);
952 }
953 break;
954
955 case L2CAP_FCR_SUP_REJ:
956 p_fcrb->remote_busy = false;
957 all_ok = retransmit_i_frames(p_ccb, L2C_FCR_RETX_ALL_PKTS);
958 break;
959
960 case L2CAP_FCR_SUP_RNR:
961 p_fcrb->remote_busy = true;
962 l2c_fcr_stop_timer(p_ccb);
963 break;
964
965 case L2CAP_FCR_SUP_SREJ:
966 p_fcrb->remote_busy = false;
967 all_ok = retransmit_i_frames(p_ccb, (uint8_t)((ctrl_word & L2CAP_FCR_REQ_SEQ_BITS) >>
968 L2CAP_FCR_REQ_SEQ_BITS_SHIFT));
969 break;
970 }
971
972 if (all_ok) {
973 /* If polled, we need to respond with F-bit. Note, we may have sent a
974 * I-frame with the F-bit */
975 if (p_fcrb->send_f_rsp) {
976 if (p_fcrb->srej_sent) {
977 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_SREJ, L2CAP_FCR_F_BIT);
978 } else {
979 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, L2CAP_FCR_F_BIT);
980 }
981
982 p_fcrb->send_f_rsp = false;
983 }
984 } else {
985 log::verbose("process_s_frame hit_max_retries");
986 }
987
988 osi_free(p_buf);
989 }
990
991 /*******************************************************************************
992 *
993 * Function process_i_frame
994 *
995 * Description Process an I frame
996 *
997 * Returns -
998 *
999 ******************************************************************************/
process_i_frame(tL2C_CCB * p_ccb,BT_HDR * p_buf,uint16_t ctrl_word,bool delay_ack)1000 static void process_i_frame(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word, bool delay_ack) {
1001 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1002 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
1003
1004 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
1005 uint8_t tx_seq, num_lost, num_to_ack, next_srej;
1006
1007 /* If we were doing checkpoint recovery, first retransmit all unacked I-frames
1008 */
1009 if (ctrl_word & L2CAP_FCR_F_BIT) {
1010 if (!retransmit_i_frames(p_ccb, L2C_FCR_RETX_ALL_PKTS)) {
1011 osi_free(p_buf);
1012 return;
1013 }
1014 }
1015
1016 /* Extract the sequence number */
1017 tx_seq = (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT;
1018
1019 /* Check if tx-sequence is the expected one */
1020 if (tx_seq != p_fcrb->next_seq_expected) {
1021 num_lost = (tx_seq - p_fcrb->next_seq_expected) & L2CAP_FCR_SEQ_MODULO;
1022
1023 /* Is the frame a duplicate ? If so, just drop it */
1024 if (num_lost >= p_ccb->our_cfg.fcr.tx_win_sz) {
1025 /* Duplicate - simply drop it */
1026 log::warn(
1027 "process_i_frame() Dropping Duplicate Frame tx_seq:{} ExpectedTxSeq "
1028 "{}",
1029 tx_seq, p_fcrb->next_seq_expected);
1030 osi_free(p_buf);
1031 } else {
1032 log::warn(
1033 "process_i_frame() CID: 0x{:04x} Lost: {} tx_seq:{} ExpTxSeq {} "
1034 "Rej: {} SRej: {}",
1035 p_ccb->local_cid, num_lost, tx_seq, p_fcrb->next_seq_expected, p_fcrb->rej_sent,
1036 p_fcrb->srej_sent);
1037
1038 if (p_fcrb->srej_sent) {
1039 /* If SREJ sent, save the frame for later processing as long as it is in
1040 * sequence */
1041 next_srej = (((BT_HDR*)fixed_queue_try_peek_last(p_fcrb->srej_rcv_hold_q))->layer_specific +
1042 1) &
1043 L2CAP_FCR_SEQ_MODULO;
1044
1045 if ((tx_seq == next_srej) &&
1046 (fixed_queue_length(p_fcrb->srej_rcv_hold_q) < p_ccb->our_cfg.fcr.tx_win_sz)) {
1047 log::verbose(
1048 "process_i_frame() Lost: {} tx_seq:{} ExpTxSeq {} Rej: {} "
1049 "SRej1",
1050 num_lost, tx_seq, p_fcrb->next_seq_expected, p_fcrb->rej_sent);
1051
1052 p_buf->layer_specific = tx_seq;
1053 fixed_queue_enqueue(p_fcrb->srej_rcv_hold_q, p_buf);
1054 } else {
1055 log::warn(
1056 "process_i_frame() CID: 0x{:04x} frame dropped in Srej Sent "
1057 "next_srej:{} hold_q.count:{} win_sz:{}",
1058 p_ccb->local_cid, next_srej, fixed_queue_length(p_fcrb->srej_rcv_hold_q),
1059 p_ccb->our_cfg.fcr.tx_win_sz);
1060
1061 p_fcrb->rej_after_srej = true;
1062 osi_free(p_buf);
1063 }
1064 } else if (p_fcrb->rej_sent) {
1065 log::warn(
1066 "process_i_frame() CID: 0x{:04x} Lost: {} tx_seq:{} ExpTxSeq {} "
1067 " Rej: 1 SRej: {}",
1068 p_ccb->local_cid, num_lost, tx_seq, p_fcrb->next_seq_expected, p_fcrb->srej_sent);
1069
1070 /* If REJ sent, just drop the frame */
1071 osi_free(p_buf);
1072 } else {
1073 log::verbose("process_i_frame() CID: 0x{:04x} tx_seq:{} ExpTxSeq {} Rej: {}",
1074 p_ccb->local_cid, tx_seq, p_fcrb->next_seq_expected, p_fcrb->rej_sent);
1075
1076 /* If only one lost, we will send SREJ, otherwise we will send REJ */
1077 if (num_lost > 1) {
1078 osi_free(p_buf);
1079 p_fcrb->rej_sent = true;
1080 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_REJ, 0);
1081 } else {
1082 if (!fixed_queue_is_empty(p_fcrb->srej_rcv_hold_q)) {
1083 log::error(
1084 "process_i_frame() CID: 0x{:04x} sending SREJ tx_seq:{} "
1085 "hold_q.count:{}",
1086 p_ccb->local_cid, tx_seq, fixed_queue_length(p_fcrb->srej_rcv_hold_q));
1087 }
1088 p_buf->layer_specific = tx_seq;
1089 fixed_queue_enqueue(p_fcrb->srej_rcv_hold_q, p_buf);
1090 p_fcrb->srej_sent = true;
1091 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_SREJ, 0);
1092 }
1093 alarm_cancel(p_ccb->fcrb.ack_timer);
1094 }
1095 }
1096 return;
1097 }
1098
1099 /* Seq number is the next expected. Clear possible reject exception in case it occurred */
1100 p_fcrb->rej_sent = p_fcrb->srej_sent = false;
1101
1102 /* Adjust the next_seq, so that if the upper layer sends more data in the
1103 callback
1104 context, the received frame is acked by an I-frame. */
1105 p_fcrb->next_seq_expected = (tx_seq + 1) & L2CAP_FCR_SEQ_MODULO;
1106
1107 /* If any SAR problem in eRTM mode, spec says disconnect. */
1108 if (!do_sar_reassembly(p_ccb, p_buf, ctrl_word)) {
1109 log::warn("process_i_frame() CID: 0x{:04x} reassembly failed", p_ccb->local_cid);
1110 l2cu_disconnect_chnl(p_ccb);
1111 return;
1112 }
1113
1114 /* RR optimization - if peer can still send us more, then start an ACK timer
1115 */
1116 num_to_ack = (p_fcrb->next_seq_expected - p_fcrb->last_ack_sent) & L2CAP_FCR_SEQ_MODULO;
1117
1118 if (num_to_ack < p_ccb->fcrb.max_held_acks) {
1119 delay_ack = true;
1120 }
1121
1122 /* We should neve never ack frame if we are not in OPEN state */
1123 if ((num_to_ack != 0) && p_ccb->in_use && (p_ccb->chnl_state == CST_OPEN)) {
1124 /* If no frames are awaiting transmission or are held, send an RR or RNR
1125 * S-frame for ack */
1126 if (delay_ack) {
1127 /* If it is the first I frame we did not ack, start ack timer */
1128 if (!alarm_is_scheduled(p_ccb->fcrb.ack_timer)) {
1129 alarm_set_on_mloop(p_ccb->fcrb.ack_timer, L2CAP_FCR_ACK_TIMEOUT_MS,
1130 l2c_fcrb_ack_timer_timeout, p_ccb);
1131 }
1132 } else if ((fixed_queue_is_empty(p_ccb->xmit_hold_q) || l2c_fcr_is_flow_controlled(p_ccb)) &&
1133 fixed_queue_is_empty(p_ccb->fcrb.srej_rcv_hold_q)) {
1134 l2c_fcr_send_S_frame(p_ccb, L2CAP_FCR_SUP_RR, 0);
1135 }
1136 }
1137 }
1138
1139 /*******************************************************************************
1140 *
1141 * Function do_sar_reassembly
1142 *
1143 * Description Process SAR bits and re-assemble frame
1144 *
1145 * Returns true if all OK, else false
1146 *
1147 ******************************************************************************/
do_sar_reassembly(tL2C_CCB * p_ccb,BT_HDR * p_buf,uint16_t ctrl_word)1148 static bool do_sar_reassembly(tL2C_CCB* p_ccb, BT_HDR* p_buf, uint16_t ctrl_word) {
1149 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1150 log::assert_that(p_buf != NULL, "assert failed: p_buf != NULL");
1151
1152 tL2C_FCRB* p_fcrb = &p_ccb->fcrb;
1153 uint16_t sar_type = ctrl_word & L2CAP_FCR_SEG_BITS;
1154 bool packet_ok = true;
1155 uint8_t* p;
1156
1157 /* Check if the SAR state is correct */
1158 if ((sar_type == L2CAP_FCR_UNSEG_SDU) || (sar_type == L2CAP_FCR_START_SDU)) {
1159 if (p_fcrb->p_rx_sdu != NULL) {
1160 log::warn(
1161 "SAR - got unexpected unsegmented or start SDU Expected len: {} "
1162 "Got so far: {}",
1163 p_fcrb->rx_sdu_len, p_fcrb->p_rx_sdu->len);
1164
1165 packet_ok = false;
1166 }
1167 /* Check the length of the packet */
1168 if ((sar_type == L2CAP_FCR_START_SDU) && (p_buf->len < L2CAP_SDU_LEN_OVERHEAD)) {
1169 log::warn("SAR start packet too short: {}", p_buf->len);
1170 packet_ok = false;
1171 }
1172 } else {
1173 if (p_fcrb->p_rx_sdu == NULL) {
1174 log::warn("SAR - got unexpected cont or end SDU");
1175 packet_ok = false;
1176 }
1177 }
1178
1179 if ((packet_ok) && (sar_type != L2CAP_FCR_UNSEG_SDU)) {
1180 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset;
1181
1182 /* For start SDU packet, extract the SDU length */
1183 if (sar_type == L2CAP_FCR_START_SDU) {
1184 /* Get the SDU length */
1185 STREAM_TO_UINT16(p_fcrb->rx_sdu_len, p);
1186 p_buf->offset += 2;
1187 p_buf->len -= 2;
1188
1189 if (p_fcrb->rx_sdu_len > p_ccb->max_rx_mtu) {
1190 log::warn("SAR - SDU len: {} larger than MTU: {}", p_fcrb->rx_sdu_len, p_ccb->max_rx_mtu);
1191 packet_ok = false;
1192 } else {
1193 p_fcrb->p_rx_sdu =
1194 (BT_HDR*)osi_malloc(BT_HDR_SIZE + OBX_BUF_MIN_OFFSET + p_fcrb->rx_sdu_len);
1195 p_fcrb->p_rx_sdu->offset = OBX_BUF_MIN_OFFSET;
1196 p_fcrb->p_rx_sdu->len = 0;
1197 }
1198 }
1199
1200 if (packet_ok) {
1201 if ((p_fcrb->p_rx_sdu->len + p_buf->len) > p_fcrb->rx_sdu_len) {
1202 log::error("SAR - SDU len exceeded Type: {} Lengths: {} {} {}", sar_type,
1203 p_fcrb->p_rx_sdu->len, p_buf->len, p_fcrb->rx_sdu_len);
1204 packet_ok = false;
1205 } else if ((sar_type == L2CAP_FCR_END_SDU) &&
1206 ((p_fcrb->p_rx_sdu->len + p_buf->len) != p_fcrb->rx_sdu_len)) {
1207 log::warn("SAR - SDU end rcvd but SDU incomplete: {} {} {}", p_fcrb->p_rx_sdu->len,
1208 p_buf->len, p_fcrb->rx_sdu_len);
1209 packet_ok = false;
1210 } else {
1211 memcpy(((uint8_t*)(p_fcrb->p_rx_sdu + 1)) + p_fcrb->p_rx_sdu->offset +
1212 p_fcrb->p_rx_sdu->len,
1213 p, p_buf->len);
1214
1215 p_fcrb->p_rx_sdu->len += p_buf->len;
1216
1217 osi_free(p_buf);
1218 p_buf = NULL;
1219
1220 if (sar_type == L2CAP_FCR_END_SDU) {
1221 p_buf = p_fcrb->p_rx_sdu;
1222 p_fcrb->p_rx_sdu = NULL;
1223 }
1224 }
1225 }
1226 }
1227
1228 if (!packet_ok) {
1229 osi_free(p_buf);
1230 } else if (p_buf != NULL) {
1231 if (p_ccb->local_cid < L2CAP_BASE_APPL_CID &&
1232 (p_ccb->local_cid >= L2CAP_FIRST_FIXED_CHNL && p_ccb->local_cid <= L2CAP_LAST_FIXED_CHNL)) {
1233 if (l2cb.fixed_reg[p_ccb->local_cid - L2CAP_FIRST_FIXED_CHNL].pL2CA_FixedData_Cb) {
1234 l2cu_fixed_channel_data_cb(p_ccb->p_lcb, p_ccb->local_cid, p_buf);
1235 }
1236 } else {
1237 l2c_csm_execute(p_ccb, L2CEVT_L2CAP_DATA, p_buf);
1238 }
1239 }
1240
1241 return packet_ok;
1242 }
1243
1244 /*******************************************************************************
1245 *
1246 * Function retransmit_i_frames
1247 *
1248 * Description This function retransmits i-frames awaiting acks.
1249 *
1250 * Returns bool - true if retransmitted
1251 *
1252 ******************************************************************************/
retransmit_i_frames(tL2C_CCB * p_ccb,uint8_t tx_seq)1253 static bool retransmit_i_frames(tL2C_CCB* p_ccb, uint8_t tx_seq) {
1254 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1255
1256 BT_HDR* p_buf = NULL;
1257 uint8_t* p;
1258 uint8_t buf_seq;
1259 uint16_t ctrl_word;
1260
1261 if ((!fixed_queue_is_empty(p_ccb->fcrb.waiting_for_ack_q)) &&
1262 (p_ccb->peer_cfg.fcr.max_transmit != 0) &&
1263 (p_ccb->fcrb.num_tries >= p_ccb->peer_cfg.fcr.max_transmit)) {
1264 log::verbose(
1265 "Max Tries Exceeded: (last_acq: {} CID: 0x{:04x} num_tries: {} "
1266 "(max: {}) ack_q_count: {}",
1267 p_ccb->fcrb.last_rx_ack, p_ccb->local_cid, p_ccb->fcrb.num_tries,
1268 p_ccb->peer_cfg.fcr.max_transmit, fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q));
1269
1270 l2cu_disconnect_chnl(p_ccb);
1271 return false;
1272 }
1273
1274 /* tx_seq indicates whether to retransmit a specific sequence or all (if ==
1275 * L2C_FCR_RETX_ALL_PKTS) */
1276 list_t* list_ack = NULL;
1277 const list_node_t* node_ack = NULL;
1278 if (!fixed_queue_is_empty(p_ccb->fcrb.waiting_for_ack_q)) {
1279 list_ack = fixed_queue_get_list(p_ccb->fcrb.waiting_for_ack_q);
1280 node_ack = list_begin(list_ack);
1281 }
1282 if (tx_seq != L2C_FCR_RETX_ALL_PKTS) {
1283 /* If sending only one, the sequence number tells us which one. Look for it.
1284 */
1285 if (list_ack != NULL) {
1286 for (; node_ack != list_end(list_ack); node_ack = list_next(node_ack)) {
1287 p_buf = (BT_HDR*)list_node(node_ack);
1288 /* Get the old control word */
1289 p = ((uint8_t*)(p_buf + 1)) + p_buf->offset + L2CAP_PKT_OVERHEAD;
1290
1291 STREAM_TO_UINT16(ctrl_word, p);
1292
1293 buf_seq = (ctrl_word & L2CAP_FCR_TX_SEQ_BITS) >> L2CAP_FCR_TX_SEQ_BITS_SHIFT;
1294
1295 log::verbose("retransmit_i_frames() cur seq: {} looking for: {}", buf_seq, tx_seq);
1296
1297 if (tx_seq == buf_seq) {
1298 break;
1299 }
1300 }
1301 }
1302
1303 if (!p_buf) {
1304 log::error("retransmit_i_frames() UNKNOWN seq: {} q_count: {}", tx_seq,
1305 fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q));
1306 return true;
1307 }
1308 } else {
1309 // Iterate though list and flush the amount requested from
1310 // the transmit data queue that satisfy the layer and event conditions.
1311 for (list_node_t* node_tmp = list_begin(p_ccb->p_lcb->link_xmit_data_q);
1312 node_tmp != list_end(p_ccb->p_lcb->link_xmit_data_q);) {
1313 BT_HDR* p_tmp = (BT_HDR*)list_node(node_tmp);
1314 node_tmp = list_next(node_tmp);
1315
1316 /* Do not flush other CIDs or partial segments */
1317 if ((p_tmp->layer_specific == 0) && (p_tmp->event == p_ccb->local_cid)) {
1318 list_remove(p_ccb->p_lcb->link_xmit_data_q, p_tmp);
1319 osi_free(p_tmp);
1320 }
1321 }
1322
1323 /* Also flush our retransmission queue */
1324 while (!fixed_queue_is_empty(p_ccb->fcrb.retrans_q)) {
1325 osi_free(fixed_queue_try_dequeue(p_ccb->fcrb.retrans_q));
1326 }
1327
1328 if (list_ack != NULL) {
1329 node_ack = list_begin(list_ack);
1330 }
1331 }
1332
1333 if (list_ack != NULL) {
1334 while (node_ack != list_end(list_ack)) {
1335 p_buf = (BT_HDR*)list_node(node_ack);
1336 node_ack = list_next(node_ack);
1337
1338 BT_HDR* p_buf2 = l2c_fcr_clone_buf(p_buf, p_buf->offset, p_buf->len);
1339 if (p_buf2) {
1340 p_buf2->layer_specific = p_buf->layer_specific;
1341
1342 fixed_queue_enqueue(p_ccb->fcrb.retrans_q, p_buf2);
1343 }
1344
1345 if ((tx_seq != L2C_FCR_RETX_ALL_PKTS) || (p_buf2 == NULL)) {
1346 break;
1347 }
1348 }
1349 }
1350
1351 l2c_link_check_send_pkts(p_ccb->p_lcb, 0, NULL);
1352
1353 if (fixed_queue_length(p_ccb->fcrb.waiting_for_ack_q)) {
1354 p_ccb->fcrb.num_tries++;
1355 l2c_fcr_start_timer(p_ccb);
1356 }
1357
1358 return true;
1359 }
1360
1361 /*******************************************************************************
1362 *
1363 * Function l2c_fcr_get_next_xmit_sdu_seg
1364 *
1365 * Description Get the next SDU segment to transmit.
1366 *
1367 * Returns pointer to buffer with segment or NULL
1368 *
1369 ******************************************************************************/
l2c_fcr_get_next_xmit_sdu_seg(tL2C_CCB * p_ccb,uint16_t max_packet_length)1370 BT_HDR* l2c_fcr_get_next_xmit_sdu_seg(tL2C_CCB* p_ccb, uint16_t max_packet_length) {
1371 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1372
1373 bool first_seg = false, /* The segment is the first part of data */
1374 mid_seg = false, /* The segment is the middle part of data */
1375 last_seg = false; /* The segment is the last part of data */
1376 uint16_t sdu_len = 0;
1377 BT_HDR *p_buf, *p_xmit;
1378 uint8_t* p;
1379 uint16_t max_pdu = p_ccb->tx_mps /* Needed? - L2CAP_MAX_HEADER_FCS*/;
1380
1381 /* If there is anything in the retransmit queue, that goes first
1382 */
1383 p_buf = (BT_HDR*)fixed_queue_try_dequeue(p_ccb->fcrb.retrans_q);
1384 if (p_buf != NULL) {
1385 /* Update Rx Seq and FCS if we acked some packets while this one was queued
1386 */
1387 prepare_I_frame(p_ccb, p_buf, true);
1388
1389 p_buf->event = p_ccb->local_cid;
1390
1391 return p_buf;
1392 }
1393
1394 /* For BD/EDR controller, max_packet_length is set to 0 */
1395 /* For AMP controller, max_packet_length is set by available blocks */
1396 if ((max_packet_length > L2CAP_MAX_HEADER_FCS) &&
1397 (max_pdu + L2CAP_MAX_HEADER_FCS > max_packet_length)) {
1398 max_pdu = max_packet_length - L2CAP_MAX_HEADER_FCS;
1399 }
1400
1401 p_buf = (BT_HDR*)fixed_queue_try_peek_first(p_ccb->xmit_hold_q);
1402
1403 /* If there is more data than the MPS, it requires segmentation */
1404 if (p_buf->len > max_pdu) {
1405 /* We are using the "event" field to tell is if we already started
1406 * segmentation */
1407 if (p_buf->event == 0) {
1408 first_seg = true;
1409 sdu_len = p_buf->len;
1410 } else {
1411 mid_seg = true;
1412 }
1413
1414 /* Get a new buffer and copy the data that can be sent in a PDU */
1415 p_xmit = l2c_fcr_clone_buf(p_buf, L2CAP_MIN_OFFSET + L2CAP_SDU_LEN_OFFSET, max_pdu);
1416
1417 if (p_xmit != NULL) {
1418 p_buf->event = p_ccb->local_cid;
1419 p_xmit->event = p_ccb->local_cid;
1420
1421 p_buf->len -= max_pdu;
1422 p_buf->offset += max_pdu;
1423
1424 /* copy PBF setting */
1425 p_xmit->layer_specific = p_buf->layer_specific;
1426 } else {
1427 /* Should never happen if the application has configured buffers correctly */
1428 log::error("L2CAP - cannot get buffer for segmentation, max_pdu: {}", max_pdu);
1429 return NULL;
1430 }
1431 } else {
1432 /* Use the original buffer if no segmentation, or the last segment */
1433 p_xmit = (BT_HDR*)fixed_queue_try_dequeue(p_ccb->xmit_hold_q);
1434
1435 if (p_xmit->event != 0) {
1436 last_seg = true;
1437 }
1438
1439 p_xmit->event = p_ccb->local_cid;
1440 }
1441
1442 /* Step back to add the L2CAP headers */
1443 p_xmit->offset -= (L2CAP_PKT_OVERHEAD + L2CAP_FCR_OVERHEAD);
1444 p_xmit->len += L2CAP_PKT_OVERHEAD + L2CAP_FCR_OVERHEAD;
1445
1446 if (first_seg) {
1447 p_xmit->offset -= L2CAP_SDU_LEN_OVERHEAD;
1448 p_xmit->len += L2CAP_SDU_LEN_OVERHEAD;
1449 }
1450
1451 /* Set the pointer to the beginning of the data */
1452 p = (uint8_t*)(p_xmit + 1) + p_xmit->offset;
1453
1454 /* Now the L2CAP header */
1455
1456 /* Note: if FCS has to be included then the length is recalculated later */
1457 UINT16_TO_STREAM(p, p_xmit->len - L2CAP_PKT_OVERHEAD);
1458
1459 UINT16_TO_STREAM(p, p_ccb->remote_cid);
1460
1461 if (first_seg) {
1462 /* Skip control word and add SDU length */
1463 p += 2;
1464 UINT16_TO_STREAM(p, sdu_len);
1465
1466 /* We will store the SAR type in layer-specific */
1467 /* layer_specific is shared with flushable flag(bits 0-1), don't clear it */
1468 p_xmit->layer_specific |= L2CAP_FCR_START_SDU;
1469
1470 first_seg = false;
1471 } else if (mid_seg) {
1472 p_xmit->layer_specific |= L2CAP_FCR_CONT_SDU;
1473 } else if (last_seg) {
1474 p_xmit->layer_specific |= L2CAP_FCR_END_SDU;
1475 } else {
1476 p_xmit->layer_specific |= L2CAP_FCR_UNSEG_SDU;
1477 }
1478
1479 prepare_I_frame(p_ccb, p_xmit, false);
1480 uint8_t fcs_len = l2cu_get_fcs_len(p_ccb);
1481
1482 if (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE) {
1483 BT_HDR* p_wack = l2c_fcr_clone_buf(p_xmit, HCI_DATA_PREAMBLE_SIZE, p_xmit->len);
1484
1485 if (!p_wack) {
1486 log::error("L2CAP - no buffer for xmit cloning, CID: 0x{:04x} Length: {}", p_ccb->local_cid,
1487 p_xmit->len);
1488
1489 /* We will not save the FCS in case we reconfigure and change options */
1490 p_xmit->len -= fcs_len;
1491
1492 /* Pretend we sent it and it got lost */
1493 fixed_queue_enqueue(p_ccb->fcrb.waiting_for_ack_q, p_xmit);
1494 return NULL;
1495 } else {
1496 /* We will not save the FCS in case we reconfigure and change options */
1497 p_wack->len -= fcs_len;
1498
1499 p_wack->layer_specific = p_xmit->layer_specific;
1500 fixed_queue_enqueue(p_ccb->fcrb.waiting_for_ack_q, p_wack);
1501 }
1502 }
1503
1504 return p_xmit;
1505 }
1506
1507 /** Get the next PDU to transmit for LE connection oriented channel. Returns
1508 * pointer to buffer with PDU. |last_piece_of_sdu| will be set to true, if
1509 * returned PDU is last piece from this SDU.*/
l2c_lcc_get_next_xmit_sdu_seg(tL2C_CCB * p_ccb,bool * last_piece_of_sdu)1510 BT_HDR* l2c_lcc_get_next_xmit_sdu_seg(tL2C_CCB* p_ccb, bool* last_piece_of_sdu) {
1511 uint16_t max_pdu = p_ccb->peer_conn_cfg.mps - 4 /* Length and CID */;
1512
1513 BT_HDR* p_buf = (BT_HDR*)fixed_queue_try_peek_first(p_ccb->xmit_hold_q);
1514 bool first_pdu = (p_buf->event == 0) ? true : false;
1515
1516 uint16_t no_of_bytes_to_send =
1517 std::min(p_buf->len, (uint16_t)(first_pdu ? (max_pdu - L2CAP_LCC_SDU_LENGTH) : max_pdu));
1518 bool last_pdu = (no_of_bytes_to_send == p_buf->len);
1519
1520 /* Get a new buffer and copy the data that can be sent in a PDU */
1521 BT_HDR* p_xmit = l2c_fcr_clone_buf(p_buf, first_pdu ? L2CAP_LCC_OFFSET : L2CAP_MIN_OFFSET,
1522 no_of_bytes_to_send);
1523
1524 p_buf->event = p_ccb->local_cid;
1525 p_xmit->event = p_ccb->local_cid;
1526
1527 if (first_pdu) {
1528 p_xmit->offset -= L2CAP_LCC_SDU_LENGTH; /* for writing the SDU length. */
1529 uint8_t* p = (uint8_t*)(p_xmit + 1) + p_xmit->offset;
1530 UINT16_TO_STREAM(p, p_buf->len);
1531 p_xmit->len += L2CAP_LCC_SDU_LENGTH;
1532 }
1533
1534 p_buf->len -= no_of_bytes_to_send;
1535 p_buf->offset += no_of_bytes_to_send;
1536
1537 /* copy PBF setting */
1538 p_xmit->layer_specific = p_buf->layer_specific;
1539
1540 if (last_piece_of_sdu) {
1541 *last_piece_of_sdu = last_pdu;
1542 }
1543
1544 if (last_pdu) {
1545 p_buf = (BT_HDR*)fixed_queue_try_dequeue(p_ccb->xmit_hold_q);
1546 osi_free(p_buf);
1547 }
1548
1549 /* Step back to add the L2CAP headers */
1550 p_xmit->offset -= L2CAP_PKT_OVERHEAD;
1551 p_xmit->len += L2CAP_PKT_OVERHEAD;
1552
1553 /* Set the pointer to the beginning of the data */
1554 uint8_t* p = (uint8_t*)(p_xmit + 1) + p_xmit->offset;
1555
1556 /* Note: if FCS has to be included then the length is recalculated later */
1557 UINT16_TO_STREAM(p, p_xmit->len - L2CAP_PKT_OVERHEAD);
1558 UINT16_TO_STREAM(p, p_ccb->remote_cid);
1559 return p_xmit;
1560 }
1561
1562 /*******************************************************************************
1563 * Configuration negotiation functions
1564 *
1565 * The following functions are used in negotiating channel modes during
1566 * configuration
1567 ******************************************************************************/
1568
1569 /*******************************************************************************
1570 *
1571 * Function l2c_fcr_chk_chan_modes
1572 *
1573 * Description Validates and adjusts if necessary, the FCR options
1574 * based on remote EXT features.
1575 *
1576 * Note: This assumes peer EXT Features have been received.
1577 * Basic mode is used if FCR Options have not been received
1578 *
1579 * Returns uint8_t - nonzero if can continue, '0' if no compatible
1580 * channels
1581 *
1582 ******************************************************************************/
l2c_fcr_chk_chan_modes(tL2C_CCB * p_ccb)1583 uint8_t l2c_fcr_chk_chan_modes(tL2C_CCB* p_ccb) {
1584 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1585
1586 /* Remove nonbasic options that the peer does not support */
1587 if (!(p_ccb->p_lcb->peer_ext_fea & L2CAP_EXTFEA_ENH_RETRANS) &&
1588 p_ccb->p_rcb->ertm_info.preferred_mode == L2CAP_FCR_ERTM_MODE) {
1589 log::warn("L2CAP - Peer does not support our desired channel types");
1590 p_ccb->p_rcb->ertm_info.preferred_mode = 0;
1591 return false;
1592 }
1593 return true;
1594 }
1595
1596 /*******************************************************************************
1597 *
1598 * Function l2c_fcr_adj_monitor_retran_timeout
1599 *
1600 * Description Overrides monitor/retrans timer value based on controller
1601 *
1602 * Returns None
1603 *
1604 ******************************************************************************/
l2c_fcr_adj_monitor_retran_timeout(tL2C_CCB * p_ccb)1605 void l2c_fcr_adj_monitor_retran_timeout(tL2C_CCB* p_ccb) {
1606 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1607
1608 /* adjust our monitor/retran timeout */
1609 if (p_ccb->out_cfg_fcr_present) {
1610 /*
1611 ** if we requestd ERTM or accepted ERTM
1612 ** We may accept ERTM even if we didn't request ERTM, in case of requesting
1613 *STREAM
1614 */
1615 if ((p_ccb->our_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE) ||
1616 (p_ccb->peer_cfg.fcr.mode == L2CAP_FCR_ERTM_MODE)) {
1617 /* upper layer setting is ignored */
1618 p_ccb->our_cfg.fcr.mon_tout = L2CAP_MIN_MONITOR_TOUT;
1619 p_ccb->our_cfg.fcr.rtrans_tout = L2CAP_MIN_RETRANS_TOUT;
1620 } else {
1621 p_ccb->our_cfg.fcr.mon_tout = 0;
1622 p_ccb->our_cfg.fcr.rtrans_tout = 0;
1623 }
1624
1625 log::verbose("l2c_fcr_adj_monitor_retran_timeout: mon_tout:{}, rtrans_tout:{}",
1626 p_ccb->our_cfg.fcr.mon_tout, p_ccb->our_cfg.fcr.rtrans_tout);
1627 }
1628 }
1629 /*******************************************************************************
1630 *
1631 * Function l2c_fcr_adj_our_rsp_options
1632 *
1633 * Description Overrides any neccesary FCR options passed in from
1634 * L2CA_ConfigRsp based on our FCR options.
1635 * Only makes adjustments if channel is in ERTM mode.
1636 *
1637 * Returns None
1638 *
1639 ******************************************************************************/
l2c_fcr_adj_our_rsp_options(tL2C_CCB * p_ccb,tL2CAP_CFG_INFO * p_cfg)1640 void l2c_fcr_adj_our_rsp_options(tL2C_CCB* p_ccb, tL2CAP_CFG_INFO* p_cfg) {
1641 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1642 log::assert_that(p_cfg != NULL, "assert failed: p_cfg != NULL");
1643
1644 /* adjust our monitor/retran timeout */
1645 l2c_fcr_adj_monitor_retran_timeout(p_ccb);
1646
1647 p_cfg->fcr_present = p_ccb->out_cfg_fcr_present;
1648
1649 if (p_cfg->fcr_present) {
1650 /* Temporary - until a better algorithm is implemented */
1651 /* If peer's tx_wnd_sz requires too many buffers for us to support, then
1652 * adjust it. For now, respond with our own tx_wnd_sz. */
1653 /* Note: peer is not guaranteed to obey our adjustment */
1654 if (p_ccb->peer_cfg.fcr.tx_win_sz > p_ccb->our_cfg.fcr.tx_win_sz) {
1655 log::verbose("adjusting requested tx_win_sz from {} to {}", p_ccb->peer_cfg.fcr.tx_win_sz,
1656 p_ccb->our_cfg.fcr.tx_win_sz);
1657 p_ccb->peer_cfg.fcr.tx_win_sz = p_ccb->our_cfg.fcr.tx_win_sz;
1658 }
1659
1660 p_cfg->fcr.mode = p_ccb->peer_cfg.fcr.mode;
1661 p_cfg->fcr.tx_win_sz = p_ccb->peer_cfg.fcr.tx_win_sz;
1662 p_cfg->fcr.max_transmit = p_ccb->peer_cfg.fcr.max_transmit;
1663 p_cfg->fcr.mps = p_ccb->peer_cfg.fcr.mps;
1664 p_cfg->fcr.rtrans_tout = p_ccb->our_cfg.fcr.rtrans_tout;
1665 p_cfg->fcr.mon_tout = p_ccb->our_cfg.fcr.mon_tout;
1666 }
1667 }
1668
1669 /*******************************************************************************
1670 *
1671 * Function l2c_fcr_renegotiate_chan
1672 *
1673 * Description Called upon unsuccessful peer response to config request.
1674 * If the error is because of the channel mode, it will try
1675 * to resend using another supported optional channel.
1676 *
1677 * Returns true if resent configuration, False if channel matches or
1678 * cannot match.
1679 *
1680 ******************************************************************************/
l2c_fcr_renegotiate_chan(tL2C_CCB * p_ccb,tL2CAP_CFG_INFO * p_cfg)1681 bool l2c_fcr_renegotiate_chan(tL2C_CCB* p_ccb, tL2CAP_CFG_INFO* p_cfg) {
1682 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1683 log::assert_that(p_cfg != NULL, "assert failed: p_cfg != NULL");
1684
1685 uint8_t peer_mode = p_ccb->our_cfg.fcr.mode;
1686 bool can_renegotiate;
1687
1688 /* Skip if this is a reconfiguration from OPEN STATE or if FCR is not returned
1689 */
1690 if (!p_cfg->fcr_present || (p_ccb->config_done & RECONFIG_FLAG)) {
1691 return false;
1692 }
1693
1694 /* Only retry if there are more channel options to try */
1695 if (p_cfg->result == tL2CAP_CFG_RESULT::L2CAP_CFG_UNACCEPTABLE_PARAMS) {
1696 peer_mode = (p_cfg->fcr_present) ? p_cfg->fcr.mode : L2CAP_FCR_BASIC_MODE;
1697
1698 if (p_ccb->our_cfg.fcr.mode != peer_mode) {
1699 if ((--p_ccb->fcr_cfg_tries) == 0) {
1700 p_cfg->result = tL2CAP_CFG_RESULT::L2CAP_CFG_FAILED_NO_REASON;
1701 log::warn("l2c_fcr_renegotiate_chan (Max retries exceeded)");
1702 }
1703
1704 can_renegotiate = false;
1705
1706 /* Try another supported mode if available based on our last attempted
1707 * channel */
1708 switch (p_ccb->our_cfg.fcr.mode) {
1709 case L2CAP_FCR_ERTM_MODE:
1710 /* We can try basic for any other peer mode because it's always
1711 * supported */
1712 log::verbose("(Trying Basic)");
1713 can_renegotiate = true;
1714 p_ccb->our_cfg.fcr.mode = L2CAP_FCR_BASIC_MODE;
1715 break;
1716
1717 default:
1718 /* All other scenarios cannot be renegotiated */
1719 break;
1720 }
1721
1722 if (can_renegotiate) {
1723 p_ccb->our_cfg.fcr_present = true;
1724
1725 if (p_ccb->our_cfg.fcr.mode == L2CAP_FCR_BASIC_MODE) {
1726 p_ccb->our_cfg.fcs_present = false;
1727 p_ccb->our_cfg.ext_flow_spec_present = false;
1728
1729 /* Basic Mode uses ACL Data Pool, make sure the MTU fits */
1730 if ((p_cfg->mtu_present) && (p_cfg->mtu > L2CAP_MTU_SIZE)) {
1731 log::warn("L2CAP - adjust MTU: {} too large", p_cfg->mtu);
1732 p_cfg->mtu = L2CAP_MTU_SIZE;
1733 }
1734 }
1735
1736 l2cu_process_our_cfg_req(p_ccb, &p_ccb->our_cfg);
1737 l2cu_send_peer_config_req(p_ccb, &p_ccb->our_cfg);
1738 alarm_set_on_mloop(p_ccb->l2c_ccb_timer, L2CAP_CHNL_CFG_TIMEOUT_MS, l2c_ccb_timer_timeout,
1739 p_ccb);
1740 return true;
1741 }
1742 }
1743 }
1744
1745 /* Disconnect if the channels do not match */
1746 if (p_ccb->our_cfg.fcr.mode != peer_mode) {
1747 log::warn("L2C CFG: Channels incompatible (local {}, peer {})", p_ccb->our_cfg.fcr.mode,
1748 peer_mode);
1749 l2cu_disconnect_chnl(p_ccb);
1750 }
1751
1752 return false;
1753 }
1754
1755 /*******************************************************************************
1756 *
1757 * Function l2c_fcr_process_peer_cfg_req
1758 *
1759 * Description This function is called to process the FCR options passed
1760 * in the peer's configuration request.
1761 *
1762 * Returns uint8_t - L2CAP_PEER_CFG_OK, L2CAP_PEER_CFG_UNACCEPTABLE,
1763 * or L2CAP_PEER_CFG_DISCONNECT.
1764 *
1765 ******************************************************************************/
l2c_fcr_process_peer_cfg_req(tL2C_CCB * p_ccb,tL2CAP_CFG_INFO * p_cfg)1766 uint8_t l2c_fcr_process_peer_cfg_req(tL2C_CCB* p_ccb, tL2CAP_CFG_INFO* p_cfg) {
1767 log::assert_that(p_ccb != NULL, "assert failed: p_ccb != NULL");
1768 log::assert_that(p_cfg != NULL, "assert failed: p_cfg != NULL");
1769
1770 uint16_t max_retrans_size;
1771 uint8_t fcr_ok = L2CAP_PEER_CFG_OK;
1772
1773 p_ccb->p_lcb->w4_info_rsp = false; /* Handles T61x SonyEricsson Bug in Info Request */
1774
1775 log::verbose(
1776 "l2c_fcr_process_peer_cfg_req() CFG fcr_present:{} fcr.mode:{} CCB FCR "
1777 "mode:{} preferred: {}",
1778 p_cfg->fcr_present, p_cfg->fcr.mode, p_ccb->our_cfg.fcr.mode,
1779 p_ccb->p_rcb->ertm_info.preferred_mode);
1780
1781 /* Need to negotiate if our modes are not the same */
1782 if (p_cfg->fcr.mode != p_ccb->p_rcb->ertm_info.preferred_mode) {
1783 /* If peer wants a mode that we don't support then retry our mode (ex.
1784 *rtx/flc), OR
1785 ** If we want ERTM and they want non-basic mode, retry our mode.
1786 ** Note: If we have already determined they support our mode previously
1787 ** from their EXF mask.
1788 */
1789 if ((((1 << p_cfg->fcr.mode) & L2CAP_FCR_CHAN_OPT_ALL_MASK) == 0) ||
1790 ((p_ccb->p_rcb->ertm_info.preferred_mode == L2CAP_FCR_ERTM_MODE) &&
1791 (p_cfg->fcr.mode != L2CAP_FCR_BASIC_MODE))) {
1792 p_cfg->fcr.mode = p_ccb->our_cfg.fcr.mode;
1793 p_cfg->fcr.tx_win_sz = p_ccb->our_cfg.fcr.tx_win_sz;
1794 p_cfg->fcr.max_transmit = p_ccb->our_cfg.fcr.max_transmit;
1795 fcr_ok = L2CAP_PEER_CFG_UNACCEPTABLE;
1796 } else if (p_ccb->p_rcb->ertm_info.preferred_mode == L2CAP_FCR_BASIC_MODE) {
1797 /* If we wanted basic, then try to renegotiate it */
1798 p_cfg->fcr.mode = L2CAP_FCR_BASIC_MODE;
1799 p_cfg->fcr.max_transmit = p_cfg->fcr.tx_win_sz = 0;
1800 p_cfg->fcr.rtrans_tout = p_cfg->fcr.mon_tout = p_cfg->fcr.mps = 0;
1801 p_ccb->our_cfg.fcr.rtrans_tout = p_ccb->our_cfg.fcr.mon_tout = p_ccb->our_cfg.fcr.mps = 0;
1802 fcr_ok = L2CAP_PEER_CFG_UNACCEPTABLE;
1803 }
1804 }
1805
1806 uint8_t fcs_len = l2cu_get_fcs_len(p_ccb);
1807
1808 /* Configuration for FCR channels so make any adjustments and fwd to upper
1809 * layer */
1810 if (fcr_ok == L2CAP_PEER_CFG_OK) {
1811 /* by default don't need to send params in the response */
1812 p_ccb->out_cfg_fcr_present = false;
1813
1814 /* Make any needed adjustments for the response to the peer */
1815 if (p_cfg->fcr_present && p_cfg->fcr.mode != L2CAP_FCR_BASIC_MODE) {
1816 /* Peer desires to bypass FCS check, and streaming or ERTM mode */
1817 if (p_cfg->fcs_present) {
1818 p_ccb->peer_cfg.fcs = p_cfg->fcs;
1819 }
1820
1821 max_retrans_size = BT_DEFAULT_BUFFER_SIZE - sizeof(BT_HDR) - L2CAP_MIN_OFFSET -
1822 L2CAP_SDU_LEN_OFFSET - fcs_len;
1823
1824 /* Ensure the MPS is not bigger than the MTU */
1825 if ((p_cfg->fcr.mps == 0) || (p_cfg->fcr.mps > p_ccb->peer_cfg.mtu)) {
1826 p_cfg->fcr.mps = p_ccb->peer_cfg.mtu;
1827 p_ccb->out_cfg_fcr_present = true;
1828 }
1829
1830 /* Ensure the MPS is not bigger than our retransmission buffer */
1831 if (p_cfg->fcr.mps > max_retrans_size) {
1832 log::verbose("CFG: Overriding MPS to {} (orig {})", max_retrans_size, p_cfg->fcr.mps);
1833
1834 p_cfg->fcr.mps = max_retrans_size;
1835 p_ccb->out_cfg_fcr_present = true;
1836 }
1837
1838 if (p_cfg->fcr.mode == L2CAP_FCR_ERTM_MODE) {
1839 /* Always respond with FCR ERTM parameters */
1840 p_ccb->out_cfg_fcr_present = true;
1841 }
1842 }
1843
1844 /* Everything ok, so save the peer's adjusted fcr options */
1845 p_ccb->peer_cfg.fcr = p_cfg->fcr;
1846
1847 } else if (fcr_ok == L2CAP_PEER_CFG_UNACCEPTABLE) {
1848 /* Allow peer only one retry for mode */
1849 if (p_ccb->peer_cfg_already_rejected) {
1850 fcr_ok = L2CAP_PEER_CFG_DISCONNECT;
1851 } else {
1852 p_ccb->peer_cfg_already_rejected = true;
1853 }
1854 }
1855
1856 return fcr_ok;
1857 }
1858