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