1 /*
2 * Copyright (C) 2021 Huawei Device Co., Ltd.
3 * Licensed under the Apache License, Version 2.0 (the "License");
4 * you may not use this file except in compliance with the License.
5 * You may obtain a copy of the License at
6 *
7 * http://www.apache.org/licenses/LICENSE-2.0
8 *
9 * Unless required by applicable law or agreed to in writing, software
10 * distributed under the License is distributed on an "AS IS" BASIS,
11 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 * See the License for the specific language governing permissions and
13 * limitations under the License.
14 */
15
16 #include "l2cap_core.h"
17
18 #include <stdio.h>
19 #include <stdlib.h>
20 #include <string.h>
21
22 #include "btm.h"
23 #include "log.h"
24
25 #include "l2cap_cmn.h"
26 #include "l2cap_crc.h"
27
L2capGetNewIdentifier(L2capConnection * conn)28 static uint8_t L2capGetNewIdentifier(L2capConnection *conn)
29 {
30 uint8_t ident;
31
32 ident = conn->nextIdentifier;
33 if (ident == L2CAP_MAX_IDENTIFIER) {
34 conn->nextIdentifier = L2CAP_MIN_IDENTIFIER;
35 } else {
36 conn->nextIdentifier += 1;
37 }
38
39 return ident;
40 }
41
L2capDisconnectAbnormal(const L2capChannel * chan,uint8_t reason)42 static void L2capDisconnectAbnormal(const L2capChannel *chan, uint8_t reason)
43 {
44 L2capPsm *psm = NULL;
45
46 psm = L2capGetPsm(chan->lpsm);
47 if (psm != NULL) {
48 LOG_DEBUG(
49 "L2capCallback disconnectAbnormal:%{public}d begin, cid = 0x%04X, reason = %hhu", __LINE__, chan->lcid, reason);
50 psm->service.disconnectAbnormal(chan->lcid, reason, psm->ctx);
51 LOG_DEBUG("L2capCallback disconnectAbnormal:%{public}d end", __LINE__);
52 }
53
54 return;
55 }
56
L2capResponseTimeout(const void * parameter)57 static void L2capResponseTimeout(const void *parameter)
58 {
59 L2capInstance *inst = L2capGetInstance();
60 L2capConnection *conn = NULL;
61 L2capPendingRequest *req = NULL;
62 ListNode *node = NULL;
63
64 if (inst->connList == NULL) {
65 return;
66 }
67
68 node = ListGetFirstNode(inst->connList);
69 while (node != NULL) {
70 conn = ListGetNodeData(node);
71 req = L2capGetPendingRequest2(conn->pendingList, parameter);
72 if (req != NULL) {
73 ListRemoveNode(conn->pendingList, req);
74 break;
75 }
76
77 node = ListGetNextNode(node);
78 }
79
80 if (req == NULL) {
81 return;
82 }
83
84 if (req->code == L2CAP_INFORMATION_REQUEST) {
85 L2capDisconnect(conn->aclHandle, 0x13);
86 } else if (req->lcid > 0) {
87 L2capChannel *chan = NULL;
88
89 chan = L2capGetChannel(conn, req->lcid);
90 if (chan != NULL) {
91 L2capDisconnectAbnormal(chan, 0);
92 L2capDeleteChannel(conn, chan, 1);
93 }
94 }
95
96 AlarmDelete(req->timer);
97 L2capFree(req);
98 return;
99 }
100
L2capResponseTimeoutCallback(void * parameter)101 static void L2capResponseTimeoutCallback(void *parameter)
102 {
103 L2capAsynchronousProcess(L2capResponseTimeout, NULL, parameter);
104 return;
105 }
106
L2capSendEchoReq(L2capConnection * conn,const uint8_t * data,uint16_t dataLen)107 int L2capSendEchoReq(L2capConnection *conn, const uint8_t *data, uint16_t dataLen)
108 {
109 Packet *pkt = NULL;
110 L2capSignalHeader signal = {0};
111
112 signal.code = L2CAP_ECHO_REQUEST;
113 signal.identifier = L2capGetNewIdentifier(conn);
114 signal.length = dataLen;
115
116 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, data);
117
118 L2capCreatePendingRequest(conn->pendingList, 0, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
119 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
120 }
121
L2capSendEchoRsp(L2capConnection * conn,uint8_t ident,const uint8_t * data,uint16_t dataLen)122 int L2capSendEchoRsp(L2capConnection *conn, uint8_t ident, const uint8_t *data, uint16_t dataLen)
123 {
124 Packet *pkt = NULL;
125 L2capSignalHeader signal = {0};
126
127 signal.code = L2CAP_ECHO_RESPONSE;
128 signal.identifier = ident;
129 signal.length = dataLen;
130
131 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, data);
132 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
133 }
134
L2capSendConnectionReq(L2capConnection * conn,L2capChannel * chan)135 int L2capSendConnectionReq(L2capConnection *conn, L2capChannel *chan)
136 {
137 Packet *pkt = NULL;
138 uint8_t buff[4] = {0};
139 L2capSignalHeader signal = {0};
140
141 BTM_ExitSniffMode(&(conn->addr));
142
143 L2capCpuToLe16(buff + 0, chan->rpsm);
144 L2capCpuToLe16(buff + L2CAP_OFFSET_2, chan->lcid);
145
146 signal.code = L2CAP_CONNECTION_REQUEST;
147 signal.identifier = L2capGetNewIdentifier(conn);
148 signal.length = sizeof(buff);
149 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
150
151 chan->state = L2CAP_CHANNEL_CONNECT_OUT_REQ;
152
153 L2capCreatePendingRequest(conn->pendingList, chan->lcid, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
154 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
155 }
156
L2capSendConnectionRsp(L2capConnection * conn,L2capChannel * chan,uint8_t ident,uint16_t result,uint16_t status)157 int L2capSendConnectionRsp(L2capConnection *conn, L2capChannel *chan, uint8_t ident, uint16_t result, uint16_t status)
158 {
159 Packet *pkt = NULL;
160 uint8_t buff[8] = {0};
161 L2capSignalHeader signal = {0};
162
163 L2capCpuToLe16(buff + 0, chan->lcid);
164 L2capCpuToLe16(buff + L2CAP_OFFSET_2, chan->rcid);
165 L2capCpuToLe16(buff + L2CAP_OFFSET_4, result);
166 L2capCpuToLe16(buff + L2CAP_OFFSET_6, status);
167
168 signal.code = L2CAP_CONNECTION_RESPONSE;
169 signal.identifier = ident;
170 signal.length = sizeof(buff);
171
172 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
173
174 if (result == L2CAP_CONNECTION_SUCCESSFUL) {
175 chan->state = L2CAP_CHANNEL_CONFIGING;
176 }
177
178 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
179 }
180
L2capSendConfigurationReq(L2capConnection * conn,const L2capChannel * chan)181 int L2capSendConfigurationReq(L2capConnection *conn, const L2capChannel *chan)
182 {
183 Packet *pkt = NULL;
184 uint8_t buff[48] = {0};
185 L2capSignalHeader signal = {0};
186
187 L2capCpuToLe16(buff + 0, chan->rcid);
188 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 0);
189
190 buff[L2CAP_OFFSET_4] = L2CAP_OPTION_MAXIMUM_TRANSMISSION_UNIT;
191 buff[L2CAP_OFFSET_5] = L2CAP_SIZE_2;
192 L2capCpuToLe16(buff + L2CAP_OFFSET_6, chan->lcfg.mtu);
193
194 signal.length = L2CAP_SIZE_8;
195
196 if (chan->lcfg.flushTimeout != 0xFFFF) {
197 buff[signal.length + 0] = L2CAP_OPTION_FLUSH_TIMEOUT;
198 buff[signal.length + L2CAP_OFFSET_1] = L2CAP_SIZE_2;
199 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_2, chan->lcfg.flushTimeout);
200 signal.length += L2CAP_SIZE_4;
201 }
202
203 if (chan->lcfg.rfc.mode != L2CAP_BASIC_MODE) {
204 buff[signal.length + 0] = L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL;
205 buff[signal.length + L2CAP_OFFSET_1] = L2CAP_SIZE_9;
206 buff[signal.length + L2CAP_OFFSET_2] = chan->lcfg.rfc.mode;
207 buff[signal.length + L2CAP_OFFSET_3] = chan->lcfg.rfc.rxWindowSize;
208 buff[signal.length + L2CAP_OFFSET_4] = chan->lcfg.rfc.maxTransmit;
209 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_5, 0);
210 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_7, 0);
211 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_9, chan->lcfg.rfc.mps);
212 signal.length += L2CAP_SIZE_11;
213 }
214
215 if (chan->lcfg.fcs != 0x01) {
216 buff[signal.length + 0] = L2CAP_OPTION_FRAME_CHECK_SEQUENCE;
217 buff[signal.length + L2CAP_OFFSET_1] = L2CAP_SIZE_1;
218 buff[signal.length + L2CAP_OFFSET_2] = 0;
219 signal.length += L2CAP_SIZE_3;
220 }
221
222 signal.code = L2CAP_CONFIGURATION_REQUEST;
223 signal.identifier = L2capGetNewIdentifier(conn);
224
225 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
226 L2capCreatePendingRequest(conn->pendingList, chan->lcid, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
227 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
228 }
229
L2capChannelConnected(L2capChannel * chan)230 static void L2capChannelConnected(L2capChannel *chan)
231 {
232 if ((chan->cfgState & L2CAP_CONFIG_STATE_OUT_DONE) && (chan->cfgState & L2CAP_CONFIG_STATE_IN_DONE)) {
233 chan->state = L2CAP_CHANNEL_CONNECTED;
234 if ((chan->lcfg.fcs == 0x01) || (chan->rcfg.fcs == 0x01)) {
235 chan->lcfg.fcs = 0x01;
236 chan->rcfg.fcs = 0x01;
237 }
238 }
239
240 return;
241 }
242
L2capSendConfigurationRspRfcProcess(L2capChannel * chan,L2capSignalHeader * signal,uint8_t * buff,uint16_t result,const L2capConfigInfo * cfg)243 static void L2capSendConfigurationRspRfcProcess(
244 L2capChannel *chan, L2capSignalHeader *signal, uint8_t *buff, uint16_t result, const L2capConfigInfo *cfg)
245 {
246 if (chan->rcfg.rfc.mode != cfg->rfc.mode) {
247 if (result == L2CAP_UNACCEPTABLE_PARAMETERS) {
248 chan->rcfg.rfc.mode = cfg->rfc.mode;
249 }
250
251 buff[signal->length + 0] = L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL;
252 buff[signal->length + L2CAP_OFFSET_1] = L2CAP_SIZE_9;
253 buff[signal->length + L2CAP_OFFSET_2] = cfg->rfc.mode;
254 buff[signal->length + L2CAP_OFFSET_3] = cfg->rfc.txWindowSize;
255 buff[signal->length + L2CAP_OFFSET_4] = cfg->rfc.maxTransmit;
256 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_5, cfg->rfc.retransmissionTimeout);
257 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_7, cfg->rfc.monitorTimeout);
258 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_9, cfg->rfc.mps);
259 signal->length += L2CAP_SIZE_11;
260 } else {
261 if (cfg->rfc.mode == L2CAP_ENHANCED_RETRANSMISSION_MODE) {
262 buff[signal->length + 0] = L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL;
263 buff[signal->length + L2CAP_OFFSET_1] = L2CAP_SIZE_9;
264 buff[signal->length + L2CAP_OFFSET_2] = cfg->rfc.mode;
265 buff[signal->length + L2CAP_OFFSET_3] = chan->lcfg.rfc.txWindowSize;
266 buff[signal->length + L2CAP_OFFSET_4] = 0;
267 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_5, chan->lcfg.rfc.retransmissionTimeout);
268 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_7, chan->lcfg.rfc.monitorTimeout);
269 L2capCpuToLe16(buff + signal->length + L2CAP_OFFSET_9, chan->rcfg.rfc.mps);
270 signal->length += L2CAP_SIZE_11;
271 }
272 }
273
274 return;
275 }
276
L2capSendConfigurationRsp(const L2capConnection * conn,L2capChannel * chan,uint8_t ident,uint16_t result,const L2capConfigInfo * cfg)277 int L2capSendConfigurationRsp(
278 const L2capConnection *conn, L2capChannel *chan, uint8_t ident, uint16_t result, const L2capConfigInfo *cfg)
279 {
280 Packet *pkt = NULL;
281 uint8_t buff[48] = {0};
282 L2capSignalHeader signal = {0};
283
284 L2capCpuToLe16(buff + 0, chan->rcid);
285 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 0);
286 L2capCpuToLe16(buff + L2CAP_OFFSET_4, result);
287 signal.length = L2CAP_SIZE_6;
288
289 if (chan->rcfg.mtu != cfg->mtu) {
290 if (result == L2CAP_UNACCEPTABLE_PARAMETERS) {
291 chan->rcfg.mtu = cfg->mtu;
292 }
293
294 buff[signal.length + 0] = L2CAP_OPTION_MAXIMUM_TRANSMISSION_UNIT;
295 buff[signal.length + L2CAP_OFFSET_1] = L2CAP_SIZE_2;
296 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_2, cfg->mtu);
297 signal.length += L2CAP_SIZE_4;
298 }
299
300 if (chan->rcfg.flushTimeout != cfg->flushTimeout) {
301 if (result == L2CAP_UNACCEPTABLE_PARAMETERS) {
302 chan->rcfg.flushTimeout = cfg->flushTimeout;
303 }
304
305 buff[signal.length + 0] = L2CAP_OPTION_FLUSH_TIMEOUT;
306 buff[signal.length + L2CAP_OFFSET_1] = L2CAP_SIZE_2;
307 L2capCpuToLe16(buff + signal.length + L2CAP_OFFSET_2, cfg->flushTimeout);
308 signal.length += L2CAP_SIZE_4;
309 }
310
311 L2capSendConfigurationRspRfcProcess(chan, &signal, buff, result, cfg);
312
313 signal.code = L2CAP_CONFIGURATION_RESPONSE;
314 signal.identifier = ident;
315
316 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
317
318 if (result == L2CAP_SUCCESS) {
319 chan->cfgState |= L2CAP_CONFIG_STATE_IN_DONE;
320 L2capChannelConnected(chan);
321 }
322
323 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
324 }
325
L2capSendDisconnectionReq(L2capConnection * conn,L2capChannel * chan)326 int L2capSendDisconnectionReq(L2capConnection *conn, L2capChannel *chan)
327 {
328 Packet *pkt = NULL;
329 uint8_t buff[4] = {0};
330 L2capSignalHeader signal = {0};
331
332 BTM_ExitSniffMode(&(conn->addr));
333
334 L2capCpuToLe16(buff + 0, chan->rcid);
335 L2capCpuToLe16(buff + L2CAP_OFFSET_2, chan->lcid);
336
337 signal.code = L2CAP_DISCONNECTION_REQUEST;
338 signal.identifier = L2capGetNewIdentifier(conn);
339 signal.length = sizeof(buff);
340
341 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
342
343 chan->state = L2CAP_CHANNEL_DISCONNECT_OUT_REQ;
344 L2capCreatePendingRequest(conn->pendingList, chan->lcid, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
345 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
346 }
347
L2capSendDisconnectionRsp(const L2capConnection * conn,const L2capChannel * chan,uint8_t ident)348 int L2capSendDisconnectionRsp(const L2capConnection *conn, const L2capChannel *chan, uint8_t ident)
349 {
350 Packet *pkt = NULL;
351 uint8_t buff[4] = {0};
352 L2capSignalHeader signal = {0};
353
354 L2capCpuToLe16(buff + 0, chan->lcid);
355 L2capCpuToLe16(buff + L2CAP_OFFSET_2, chan->rcid);
356
357 signal.code = L2CAP_DISCONNECTION_RESPONSE;
358 signal.identifier = ident;
359 signal.length = sizeof(buff);
360
361 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
362
363 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
364 }
365
L2capSendInformationReq(L2capConnection * conn,uint16_t type)366 int L2capSendInformationReq(L2capConnection *conn, uint16_t type)
367 {
368 Packet *pkt = NULL;
369 uint8_t buff[2] = {0};
370 L2capSignalHeader signal = {0};
371
372 L2capCpuToLe16(buff + 0, type);
373
374 signal.code = L2CAP_INFORMATION_REQUEST;
375 signal.identifier = L2capGetNewIdentifier(conn);
376 signal.length = sizeof(buff);
377
378 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
379
380 conn->info.state = L2CAP_INFO_STATE_PROCESSING;
381 L2capCreatePendingRequest(conn->pendingList, 0, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
382 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
383 }
384
L2capSendInformationRsp(const L2capConnection * conn,uint8_t ident,uint16_t infoType)385 int L2capSendInformationRsp(const L2capConnection *conn, uint8_t ident, uint16_t infoType)
386 {
387 Packet *pkt = NULL;
388 uint8_t buff[L2CAP_OFFSET_12] = {0};
389 L2capSignalHeader signal = {0};
390
391 (void)memset_s(buff, sizeof(buff), 0, sizeof(buff));
392
393 L2capCpuToLe16(buff + 0, infoType);
394
395 if (infoType == L2CAP_INFORMATION_TYPE_EXTENDED_FEATURE) {
396 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 0); // Success
397
398 buff[L2CAP_OFFSET_4] = L2CAP_FEATURE_ENHANCED_RETRANSMISSION_MODE | L2CAP_FEATURE_STREAMING_MODE |
399 L2CAP_FEATURE_FCS_OPTION | L2CAP_FEATURE_FIXED_CHANNELS;
400 signal.length = L2CAP_SIZE_8;
401 } else if (infoType == L2CAP_INFORMATION_TYPE_FIXED_CHANNEL) {
402 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 0); // Success
403
404 buff[L2CAP_OFFSET_4] = L2CAP_FEATURE_SIGNALING_CHANNEl;
405 signal.length = L2CAP_SIZE_12;
406 } else {
407 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 1); // Not supported
408 signal.length = L2CAP_SIZE_4;
409 }
410
411 signal.code = L2CAP_INFORMATION_RESPONSE;
412 signal.identifier = ident;
413
414 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
415 return L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
416 }
417
L2capGetNextSeq(uint16_t seq)418 static uint16_t L2capGetNextSeq(uint16_t seq)
419 {
420 return ((seq + 1) % L2CAP_MAX_TX_WINDOW);
421 }
422
L2capGetSeqWindow(uint16_t endSeq,uint16_t startSeq)423 static uint16_t L2capGetSeqWindow(uint16_t endSeq, uint16_t startSeq)
424 {
425 uint16_t seqWindow;
426
427 if (startSeq > endSeq) {
428 seqWindow = endSeq + L2CAP_MAX_TX_WINDOW - startSeq;
429 } else {
430 seqWindow = endSeq - startSeq;
431 }
432
433 return seqWindow;
434 }
435
L2capGetTxWindow(const L2capErfc * erfc)436 static uint16_t L2capGetTxWindow(const L2capErfc *erfc)
437 {
438 return L2capGetSeqWindow(erfc->nextTxSeq, erfc->expectedAckSeq);
439 }
440
L2capGetRxWindow(const L2capErfc * erfc)441 static uint16_t L2capGetRxWindow(const L2capErfc *erfc)
442 {
443 return L2capGetSeqWindow(erfc->expectedTxSeq, erfc->bufferSeq);
444 }
445
L2capAddCrc(Packet * pkt)446 static void L2capAddCrc(Packet *pkt)
447 {
448 uint16_t crc;
449 uint8_t *tail = NULL;
450
451 crc = PacketCalCrc16(pkt, CalCrc16WithPrev);
452 tail = BufferPtr(PacketTail(pkt));
453 L2capCpuToLe16(tail, crc);
454
455 return;
456 }
457
L2capCheckCrc(Packet * pkt)458 static int L2capCheckCrc(Packet *pkt)
459 {
460 uint8_t tail[2] = {0};
461 uint8_t *tailPtr = NULL;
462 uint16_t fcs;
463 uint16_t fcsCalc;
464
465 PacketExtractTail(pkt, tail, sizeof(tail));
466
467 tailPtr = tail;
468 fcs = L2capLe16ToCpu(tailPtr);
469
470 fcsCalc = PacketCalCrc16(pkt, CalCrc16WithPrev);
471 if (fcs != fcsCalc) {
472 LOG_ERROR("L2cap CRC Error, %{public}s:%{public}d", __FUNCTION__, __LINE__);
473 return BT_BAD_PARAM;
474 }
475
476 return BT_SUCCESS;
477 }
478
L2capBuildSFrame(const L2capChannel * chan,const L2capErfcSControl * sCtrl)479 static Packet *L2capBuildSFrame(const L2capChannel *chan, const L2capErfcSControl *sCtrl)
480 {
481 Packet *spkt = NULL;
482 uint8_t *header = NULL;
483 uint8_t tailLength = 0;
484
485 if (chan->lcfg.fcs == 0x01) {
486 tailLength = L2CAP_SIZE_2;
487 }
488
489 spkt = PacketMalloc(L2CAP_HEADER_LENGTH + L2CAP_SIZE_2, tailLength, 0);
490 header = BufferPtr(PacketHead(spkt));
491
492 L2capCpuToLe16(header + 0, tailLength + L2CAP_SIZE_2);
493 L2capCpuToLe16(header + L2CAP_OFFSET_2, chan->rcid);
494 if (memcpy_s(header + L2CAP_HEADER_LENGTH, sizeof(L2capErfcSControl), sCtrl, sizeof(L2capErfcSControl)) != EOK) {
495 LOG_ERROR("[%{public}s][%{public}d] memcpy_s fail.", __FUNCTION__, __LINE__);
496 PacketFree(spkt);
497 return NULL;
498 }
499
500 if (chan->lcfg.fcs == 0x01) {
501 L2capAddCrc(spkt);
502 }
503
504 return spkt;
505 }
506
L2capSendSFrame(const L2capConnection * conn,L2capChannel * chan,uint8_t pBit,uint8_t fBit,uint8_t sBit)507 int L2capSendSFrame(const L2capConnection *conn, L2capChannel *chan, uint8_t pBit, uint8_t fBit, uint8_t sBit)
508 {
509 L2capErfc *erfc = NULL;
510 L2capErfcSControl sCtrl = {0};
511 Packet *spkt = NULL;
512
513 erfc = &(chan->erfc);
514
515 sCtrl.type = L2CAP_SFRAME;
516 sCtrl.reqSeq = erfc->expectedTxSeq;
517 sCtrl.fBit = fBit;
518
519 sCtrl.pBit = pBit;
520 sCtrl.sBit = sBit;
521
522 erfc->bufferSeq = erfc->expectedTxSeq;
523 spkt = L2capBuildSFrame(chan, &sCtrl);
524
525 L2capSendPacket(conn->aclHandle, chan->lcfg.flushTimeout, spkt);
526 return BT_SUCCESS;
527 }
528
L2capProcessRxReqSeq(L2capChannel * chan,uint16_t reqSeq)529 static void L2capProcessRxReqSeq(L2capChannel *chan, uint16_t reqSeq)
530 {
531 L2capErfc *erfc = NULL;
532 ListNode *node = NULL;
533 L2capErfcTxPacket *tx = NULL;
534 uint16_t finishedPacketNum = 0;
535
536 erfc = &(chan->erfc);
537 if (erfc->nextTxSeq == reqSeq) {
538 finishedPacketNum = L2capGetTxWindow(erfc);
539 } else {
540 uint8_t *header = NULL;
541 L2capErfcIControl *ctrl = NULL;
542
543 node = ListGetFirstNode(erfc->txList);
544 while (node != NULL) {
545 tx = ListGetNodeData(node);
546 header = BufferPtr(PacketHead(tx->pkt));
547
548 ctrl = (L2capErfcIControl *)(header + L2CAP_HEADER_LENGTH);
549 if (ctrl->txSeq == erfc->nextTxSeq) {
550 return; // wrong ReqSeq, ignore
551 }
552
553 if (ctrl->txSeq == reqSeq) {
554 break;
555 }
556
557 finishedPacketNum += 1;
558 node = ListGetNextNode(node);
559 }
560 }
561
562 erfc->expectedAckSeq = reqSeq;
563
564 if (finishedPacketNum > 0) {
565 AlarmCancel(chan->erfc.retransmissionTimer);
566
567 if (L2capGetTxWindow(erfc) > 0) {
568 L2capErfcStartRetransmissionTimer(chan);
569 }
570 }
571
572 while (finishedPacketNum) {
573 node = ListGetFirstNode(erfc->txList);
574 tx = ListGetNodeData(node);
575
576 ListRemoveNode(erfc->txList, tx);
577 PacketFree(tx->pkt);
578 L2capFree(tx);
579
580 finishedPacketNum -= 1;
581 }
582
583 return;
584 }
585
L2capErfcTxOneFrame(L2capConnection * conn,L2capChannel * chan,uint8_t txSeq)586 static void L2capErfcTxOneFrame(L2capConnection *conn, L2capChannel *chan, uint8_t txSeq)
587 {
588 ListNode *node = NULL;
589 L2capErfcTxPacket *tx = NULL;
590 uint8_t *pktHeader = NULL;
591 L2capErfcIControl *ctrl = NULL;
592
593 L2capErfc *erfc = &(chan->erfc);
594
595 if (erfc->txList == NULL) {
596 return;
597 }
598
599 node = ListGetFirstNode(erfc->txList);
600 while (node != NULL) {
601 tx = ListGetNodeData(node);
602 pktHeader = BufferPtr(PacketHead(tx->pkt));
603
604 ctrl = (L2capErfcIControl *)(pktHeader + L2CAP_HEADER_LENGTH);
605 if (ctrl->txSeq == erfc->nextTxSeq) {
606 break;
607 }
608
609 if (ctrl->txSeq == txSeq) {
610 if (tx->retryCount == chan->lcfg.rfc.maxTransmit) {
611 L2capSendDisconnectionReq(conn, chan);
612 return;
613 }
614
615 ctrl->reqSeq = erfc->expectedTxSeq;
616 if (chan->lcfg.fcs == 0x01) {
617 L2capAddCrc(tx->pkt);
618 }
619
620 L2capSendPacketNoFree(conn->aclHandle, chan->lcfg.flushTimeout, tx->pkt);
621 break;
622 }
623
624 node = ListGetNextNode(node);
625 }
626
627 return;
628 }
629
L2capErfcRemoteBusyProcess(L2capChannel * chan,uint8_t isBusy)630 static void L2capErfcRemoteBusyProcess(L2capChannel *chan, uint8_t isBusy)
631 {
632 L2capErfc *erfc = NULL;
633 L2capPsm *psm = NULL;
634
635 erfc = &(chan->erfc);
636
637 if (isBusy) {
638 if (erfc->busyState & L2CAP_BUSY_REMOTE_BUSY) {
639 return;
640 }
641
642 erfc->busyState |= L2CAP_BUSY_REMOTE_BUSY;
643 } else {
644 if (!(erfc->busyState & L2CAP_BUSY_REMOTE_BUSY)) {
645 return;
646 }
647
648 erfc->busyState &= (~L2CAP_BUSY_REMOTE_BUSY);
649 }
650
651 psm = L2capGetPsm(chan->lpsm);
652 if (psm != NULL) {
653 LOG_DEBUG("L2capCallback remoteBusy:%{public}d begin, cid = 0x%04X, busyState = %{public}d", __LINE__, chan->lcid, isBusy);
654 psm->service.remoteBusy(chan->lcid, isBusy, psm->ctx);
655 LOG_DEBUG("L2capCallback remoteBusy:%{public}d end", __LINE__);
656 }
657
658 return;
659 }
660
L2capErfcMonitorTimeout(const void * parameter)661 static void L2capErfcMonitorTimeout(const void *parameter)
662 {
663 L2capInstance *inst = L2capGetInstance();
664 L2capChannel *chan = NULL;
665 L2capConnection *conn = NULL;
666
667 if (inst->connList == NULL) {
668 return;
669 }
670
671 chan = (L2capChannel *)parameter;
672 conn = L2capGetConnection3(chan);
673 if (conn == NULL) {
674 return;
675 }
676
677 if (chan->erfc.retryCount == chan->lcfg.rfc.maxTransmit) {
678 L2capSendDisconnectionReq(conn, chan);
679 return;
680 }
681
682 if (chan->erfc.busyState & L2CAP_BUSY_LOCAL_BUSY) {
683 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_ON, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RNR);
684 } else {
685 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_ON, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RR);
686 }
687
688 chan->erfc.retryCount += 1;
689 L2capErfcStartMonitorTimer(chan);
690 return;
691 }
692
L2capErfcMonitorTimeoutCallback(void * parameter)693 static void L2capErfcMonitorTimeoutCallback(void *parameter)
694 {
695 L2capAsynchronousProcess(L2capErfcMonitorTimeout, NULL, parameter);
696 return;
697 }
698
L2capErfcStartMonitorTimer(L2capChannel * chan)699 void L2capErfcStartMonitorTimer(L2capChannel *chan)
700 {
701 if (chan->erfc.monitorTimer == NULL) {
702 chan->erfc.monitorTimer = AlarmCreate("", false);
703 } else {
704 AlarmCancel(chan->erfc.monitorTimer);
705 }
706
707 AlarmSet(chan->erfc.monitorTimer, chan->lcfg.rfc.monitorTimeout, L2capErfcMonitorTimeoutCallback, chan);
708 return;
709 }
710
L2capErfcRetransmissionTimeout(const void * parameter)711 static void L2capErfcRetransmissionTimeout(const void *parameter)
712 {
713 L2capInstance *inst = L2capGetInstance();
714 L2capConnection *conn = NULL;
715 L2capChannel *chan = NULL;
716
717 if (inst->connList == NULL) {
718 return;
719 }
720
721 chan = (L2capChannel *)parameter;
722 conn = L2capGetConnection3(chan);
723 if (conn == NULL) {
724 return;
725 }
726
727 if (chan->erfc.busyState & L2CAP_BUSY_LOCAL_BUSY) {
728 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_ON, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RNR);
729 } else {
730 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_ON, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RR);
731 }
732
733 chan->erfc.retryCount = 1;
734 L2capErfcStartMonitorTimer(chan);
735
736 chan->erfc.busyState |= L2CAP_BUSY_WAIT_F;
737 L2capErfcRemoteBusyProcess(chan, 1);
738 return;
739 }
740
L2capErfcRetransmissionTimeoutCallback(void * parameter)741 static void L2capErfcRetransmissionTimeoutCallback(void *parameter)
742 {
743 L2capAsynchronousProcess(L2capErfcRetransmissionTimeout, NULL, parameter);
744 return;
745 }
746
L2capErfcStartRetransmissionTimer(L2capChannel * chan)747 void L2capErfcStartRetransmissionTimer(L2capChannel *chan)
748 {
749 if (chan->erfc.retransmissionTimer == NULL) {
750 chan->erfc.retransmissionTimer = AlarmCreate("", false);
751 } else {
752 AlarmCancel(chan->erfc.retransmissionTimer);
753 }
754
755 AlarmSet(chan->erfc.retransmissionTimer,
756 chan->lcfg.rfc.retransmissionTimeout,
757 L2capErfcRetransmissionTimeoutCallback,
758 chan);
759 return;
760 }
761
L2capErfcDoTx(L2capConnection * conn,L2capChannel * chan,uint16_t txWindow,const ListNode * node)762 static void L2capErfcDoTx(L2capConnection *conn, L2capChannel *chan, uint16_t txWindow, const ListNode *node)
763 {
764 uint8_t *header = NULL;
765 L2capErfcTxPacket *tx = NULL;
766 L2capErfcIControl *ctrl = NULL;
767
768 L2capErfc *erfc = &(chan->erfc);
769
770 while ((node != NULL) && txWindow) {
771 tx = ListGetNodeData(node);
772 if (tx->retryCount == chan->lcfg.rfc.maxTransmit) {
773 L2capSendDisconnectionReq(conn, chan);
774 return;
775 }
776
777 header = BufferPtr(PacketHead(tx->pkt));
778
779 ctrl = (L2capErfcIControl *)(header + L2CAP_HEADER_LENGTH);
780 ctrl->reqSeq = erfc->expectedTxSeq;
781
782 if (chan->lcfg.fcs == 0x01) {
783 L2capAddCrc(tx->pkt);
784 }
785
786 tx->retryCount += 1;
787 L2capSendPacketNoFree(conn->aclHandle, chan->lcfg.flushTimeout, tx->pkt);
788
789 if (txWindow == chan->lcfg.rfc.txWindowSize) {
790 L2capErfcStartRetransmissionTimer(chan);
791 }
792
793 erfc->bufferSeq = erfc->expectedTxSeq;
794 erfc->nextTxSeq = L2capGetNextSeq(erfc->nextTxSeq);
795
796 txWindow -= 1;
797 node = ListGetNextNode(node);
798 }
799
800 if (txWindow == 0) {
801 L2capErfcRemoteBusyProcess(chan, 1);
802 } else {
803 L2capErfcRemoteBusyProcess(chan, 0);
804 }
805
806 return;
807 }
808
L2capErfcTx(L2capConnection * conn,L2capChannel * chan)809 static void L2capErfcTx(L2capConnection *conn, L2capChannel *chan)
810 {
811 uint16_t txWindow;
812 ListNode *node = NULL;
813 L2capErfcTxPacket *tx = NULL;
814 uint8_t *header = NULL;
815 L2capErfcIControl *ctrl = NULL;
816
817 L2capErfc *erfc = &(chan->erfc);
818
819 if (erfc->txList == NULL) {
820 return;
821 }
822
823 if ((erfc->busyState & L2CAP_BUSY_REMOTE_RNR) || (erfc->busyState & L2CAP_BUSY_WAIT_F)) {
824 return;
825 }
826
827 txWindow = chan->lcfg.rfc.txWindowSize - L2capGetTxWindow(erfc);
828 if (txWindow == 0) {
829 return;
830 }
831
832 node = ListGetFirstNode(erfc->txList);
833 while (node != NULL) {
834 tx = ListGetNodeData(node);
835 header = BufferPtr(PacketHead(tx->pkt));
836
837 ctrl = (L2capErfcIControl *)(header + L2CAP_HEADER_LENGTH);
838 if (ctrl->txSeq == erfc->nextTxSeq) {
839 break;
840 }
841
842 node = ListGetNextNode(node);
843 }
844
845 L2capErfcDoTx(conn, chan, txWindow, node);
846 return;
847 }
848
L2capStreamTx(const L2capConnection * conn,L2capChannel * chan)849 static void L2capStreamTx(const L2capConnection *conn, L2capChannel *chan)
850 {
851 ListNode *node = NULL;
852 Packet *pkt = NULL;
853
854 L2capErfc *erfc = &(chan->erfc);
855
856 if (erfc->txList == NULL) {
857 return;
858 }
859
860 while (1) {
861 node = ListGetFirstNode(erfc->txList);
862 if (node == NULL) {
863 break;
864 }
865
866 pkt = ListGetNodeData(node);
867 ListRemoveNode(erfc->txList, pkt);
868
869 if (chan->lcfg.fcs == 0x01) {
870 L2capAddCrc(pkt);
871 }
872
873 L2capSendPacket(conn->aclHandle, chan->lcfg.flushTimeout, pkt);
874 }
875
876 return;
877 }
878
L2capBuildIFrame(L2capChannel * chan,const Packet * pkt,uint8_t sar,uint16_t sduLength)879 static Packet *L2capBuildIFrame(L2capChannel *chan, const Packet *pkt, uint8_t sar, uint16_t sduLength)
880 {
881 L2capErfc *erfc = NULL;
882 L2capErfcIControl *iCtrl = NULL;
883 Packet *ipkt = NULL;
884 uint8_t *header = NULL;
885 uint16_t headerLength;
886 uint16_t tailLength = 0;
887
888 erfc = &(chan->erfc);
889 headerLength = L2CAP_HEADER_LENGTH + L2CAP_SIZE_2;
890 if (sar == L2CAP_ERFC_START_SDU) {
891 headerLength = L2CAP_HEADER_LENGTH + L2CAP_SIZE_4;
892 }
893
894 if (chan->lcfg.fcs == 0x01) {
895 tailLength = L2CAP_SIZE_2;
896 }
897
898 ipkt = PacketInheritMalloc(pkt, headerLength, tailLength);
899 header = BufferPtr(PacketHead(ipkt));
900
901 L2capCpuToLe16(header + 0, PacketSize(ipkt) - L2CAP_HEADER_LENGTH);
902 L2capCpuToLe16(header + L2CAP_OFFSET_2, chan->rcid);
903 iCtrl = (L2capErfcIControl *)(header + L2CAP_HEADER_LENGTH);
904
905 iCtrl->type = L2CAP_IFRAME;
906 iCtrl->txSeq = erfc->txSeq;
907 iCtrl->reqSeq = 0;
908 iCtrl->fBit = L2CAP_ERFC_FBIT_OFF;
909 iCtrl->sar = sar;
910
911 if (sar == L2CAP_ERFC_START_SDU) {
912 L2capCpuToLe16(header + L2CAP_OFFSET_6, sduLength);
913 }
914
915 return ipkt;
916 }
917
L2capAddNewPacket(L2capChannel * chan,Packet * pkt)918 static void L2capAddNewPacket(L2capChannel *chan, Packet *pkt)
919 {
920 if (chan->lcfg.rfc.mode == L2CAP_ENHANCED_RETRANSMISSION_MODE) {
921 L2capErfcTxPacket *tx = NULL;
922
923 tx = L2capAlloc(sizeof(L2capErfcTxPacket));
924 if (tx == NULL) {
925 return;
926 }
927 tx->pkt = pkt;
928 tx->retryCount = 0;
929
930 ListAddLast(chan->erfc.txList, tx);
931 } else if (chan->lcfg.rfc.mode == L2CAP_STREAM_MODE) {
932 ListAddLast(chan->erfc.txList, pkt);
933 }
934
935 return;
936 }
937
L2capSendIFrame(L2capConnection * conn,L2capChannel * chan,Packet * pkt)938 int L2capSendIFrame(L2capConnection *conn, L2capChannel *chan, Packet *pkt)
939 {
940 L2capErfc *erfc = NULL;
941 Packet *ipkt = NULL;
942 uint16_t length;
943 uint8_t sar;
944
945 erfc = &(chan->erfc);
946 if (erfc->txList == NULL) {
947 erfc->txList = ListCreate(NULL);
948 }
949
950 length = PacketSize(pkt);
951 if (length > chan->rcfg.rfc.mps) {
952 Packet *frag = NULL;
953 sar = L2CAP_ERFC_START_SDU;
954 while (1) {
955 frag = PacketMalloc(0, 0, 0);
956 int remainLength = PacketFragment(pkt, frag, chan->rcfg.rfc.mps);
957
958 ipkt = L2capBuildIFrame(chan, frag, sar, length);
959 PacketFree(frag);
960
961 L2capAddNewPacket(chan, ipkt);
962 erfc->txSeq = L2capGetNextSeq(erfc->txSeq);
963
964 if (remainLength == 0) {
965 break;
966 }
967
968 sar = L2CAP_ERFC_END_SDU;
969 if (remainLength > chan->rcfg.rfc.mps) {
970 sar = L2CAP_ERFC_CONTINUATION_SDU;
971 }
972 }
973 } else {
974 sar = L2CAP_ERFC_UNSEGMENTED_SDU;
975 ipkt = L2capBuildIFrame(chan, pkt, sar, 0);
976
977 L2capAddNewPacket(chan, ipkt);
978 erfc->txSeq = L2capGetNextSeq(erfc->txSeq);
979 }
980
981 if (chan->lcfg.rfc.mode == L2CAP_ENHANCED_RETRANSMISSION_MODE) {
982 L2capErfcTx(conn, chan);
983 } else if (chan->lcfg.rfc.mode == L2CAP_STREAM_MODE) {
984 L2capStreamTx(conn, chan);
985 }
986
987 return BT_SUCCESS;
988 }
989
L2capErfcProcessFBit(L2capChannel * chan,uint8_t fBit,uint8_t reqSeq)990 static void L2capErfcProcessFBit(L2capChannel *chan, uint8_t fBit, uint8_t reqSeq)
991 {
992 if (chan->erfc.busyState & L2CAP_BUSY_WAIT_F) {
993 if (fBit == L2CAP_ERFC_FBIT_ON) {
994 AlarmCancel(chan->erfc.monitorTimer);
995 chan->erfc.busyState &= (~L2CAP_BUSY_WAIT_F);
996 chan->erfc.retryCount = 0;
997 chan->erfc.nextTxSeq = reqSeq;
998 }
999 }
1000
1001 return;
1002 }
1003
L2capErfcProcessSFrame(L2capConnection * conn,L2capChannel * chan,uint8_t ctrl[2])1004 static void L2capErfcProcessSFrame(L2capConnection *conn, L2capChannel *chan, uint8_t ctrl[2])
1005 {
1006 L2capErfc *erfc = NULL;
1007 L2capErfcSControl *sCtrl = NULL;
1008
1009 erfc = &(chan->erfc);
1010 sCtrl = (L2capErfcSControl *)ctrl;
1011
1012 if ((sCtrl->sBit != L2CAP_ERFC_SREJ) || (sCtrl->pBit != L2CAP_ERFC_PBIT_OFF)) {
1013 L2capProcessRxReqSeq(chan, sCtrl->reqSeq);
1014 }
1015
1016 L2capErfcProcessFBit(chan, sCtrl->fBit, sCtrl->reqSeq);
1017
1018 if (sCtrl->pBit == L2CAP_ERFC_PBIT_ON) {
1019 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_OFF, L2CAP_ERFC_FBIT_ON, L2CAP_ERFC_RR);
1020 }
1021
1022 if (sCtrl->sBit == L2CAP_ERFC_RR) {
1023 if (erfc->busyState & L2CAP_BUSY_REMOTE_RNR) {
1024 if (sCtrl->pBit == L2CAP_ERFC_PBIT_ON) {
1025 erfc->busyState &= (~L2CAP_BUSY_REMOTE_RNR);
1026 L2capErfcTx(conn, chan);
1027 }
1028 } else {
1029 L2capErfcTx(conn, chan);
1030 }
1031 } else if (sCtrl->sBit == L2CAP_ERFC_REJ) {
1032 erfc->nextTxSeq = sCtrl->reqSeq;
1033 L2capErfcTx(conn, chan);
1034 } else if (sCtrl->sBit == L2CAP_ERFC_RNR) {
1035 erfc->busyState |= L2CAP_BUSY_REMOTE_RNR;
1036 L2capErfcRemoteBusyProcess(chan, 1);
1037 } else if (sCtrl->sBit == L2CAP_ERFC_SREJ) {
1038 L2capErfcTxOneFrame(conn, chan, sCtrl->reqSeq);
1039 }
1040
1041 return;
1042 }
1043
L2capReassembleIFrame(L2capErfc * erfc,const Packet * pkt,L2capErfcIControl * iCtrl)1044 static Packet *L2capReassembleIFrame(L2capErfc *erfc, const Packet *pkt, L2capErfcIControl *iCtrl)
1045 {
1046 Packet *ipkt = NULL;
1047
1048 if (iCtrl->sar == L2CAP_ERFC_START_SDU) {
1049 if (erfc->rxSarPacket != NULL) {
1050 PacketFree(erfc->rxSarPacket);
1051 }
1052
1053 erfc->rxSarPacket = PacketRefMalloc(pkt);
1054 } else if (iCtrl->sar == L2CAP_ERFC_CONTINUATION_SDU) {
1055 if (erfc->rxSarPacket == NULL) {
1056 return NULL;
1057 }
1058
1059 PacketAssemble(erfc->rxSarPacket, pkt);
1060 } else if (iCtrl->sar == L2CAP_ERFC_END_SDU) {
1061 uint8_t header[2] = {0};
1062 uint8_t *headerPtr = NULL;
1063 uint16_t sduLength;
1064
1065 if (erfc->rxSarPacket == NULL) {
1066 return NULL;
1067 }
1068
1069 PacketAssemble(erfc->rxSarPacket, pkt);
1070
1071 PacketExtractHead(erfc->rxSarPacket, header, sizeof(header));
1072
1073 headerPtr = header;
1074 sduLength = L2capLe16ToCpu(headerPtr);
1075 if (sduLength != PacketSize(erfc->rxSarPacket)) {
1076 PacketFree(erfc->rxSarPacket);
1077 erfc->rxSarPacket = NULL;
1078 return NULL;
1079 }
1080
1081 ipkt = erfc->rxSarPacket;
1082 erfc->rxSarPacket = NULL;
1083 } else if (iCtrl->sar == L2CAP_ERFC_UNSEGMENTED_SDU) {
1084 ipkt = PacketRefMalloc(pkt);
1085 }
1086
1087 return ipkt;
1088 }
1089
L2capErfcProcessIFrame(L2capConnection * conn,L2capChannel * chan,const Packet * pkt,uint8_t ctrl[2])1090 static Packet *L2capErfcProcessIFrame(L2capConnection *conn, L2capChannel *chan, const Packet *pkt, uint8_t ctrl[2])
1091 {
1092 L2capErfc *erfc = NULL;
1093 L2capErfcIControl *iCtrl = NULL;
1094 Packet *ipkt = NULL;
1095
1096 erfc = &(chan->erfc);
1097 iCtrl = (L2capErfcIControl *)ctrl;
1098
1099 L2capProcessRxReqSeq(chan, iCtrl->reqSeq);
1100
1101 L2capErfcProcessFBit(chan, iCtrl->fBit, iCtrl->reqSeq);
1102
1103 if (erfc->expectedTxSeq != iCtrl->txSeq) {
1104 if (erfc->rejState == 0) {
1105 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_OFF, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_REJ);
1106 erfc->rejState = 1;
1107 }
1108
1109 return NULL;
1110 }
1111
1112 erfc->rejState = 0;
1113 erfc->expectedTxSeq = L2capGetNextSeq(erfc->expectedTxSeq);
1114
1115 ipkt = L2capReassembleIFrame(erfc, pkt, iCtrl);
1116
1117 if (L2capGetRxWindow(erfc) == (chan->rcfg.rfc.txWindowSize - 1)) {
1118 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_OFF, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RR);
1119 } else {
1120 if (ipkt != NULL) { // full packet received
1121 if (erfc->nextTxSeq == erfc->txSeq) { // no pending packet in list
1122 L2capSendSFrame(conn, chan, L2CAP_ERFC_PBIT_OFF, L2CAP_ERFC_FBIT_OFF, L2CAP_ERFC_RR);
1123 }
1124 }
1125 }
1126
1127 L2capErfcTx(conn, chan);
1128 return ipkt;
1129 }
1130
L2capStreamProcessIFrame(L2capChannel * chan,const Packet * pkt,L2capErfcIControl * iCtrl)1131 static Packet *L2capStreamProcessIFrame(L2capChannel *chan, const Packet *pkt, L2capErfcIControl *iCtrl)
1132 {
1133 L2capErfc *erfc = NULL;
1134 Packet *ipkt = NULL;
1135
1136 erfc = &(chan->erfc);
1137 if (erfc->expectedTxSeq != iCtrl->txSeq) {
1138 if (erfc->rxSarPacket != NULL) {
1139 PacketFree(erfc->rxSarPacket);
1140 erfc->rxSarPacket = NULL;
1141 }
1142 }
1143
1144 erfc->expectedTxSeq = L2capGetNextSeq(iCtrl->txSeq);
1145 ipkt = L2capReassembleIFrame(erfc, pkt, iCtrl);
1146
1147 return ipkt;
1148 }
1149
L2capCheckConfigurationOptionLength(uint8_t optType,uint8_t optLength)1150 static int L2capCheckConfigurationOptionLength(uint8_t optType, uint8_t optLength)
1151 {
1152 switch (optType) {
1153 case L2CAP_OPTION_MAXIMUM_TRANSMISSION_UNIT:
1154 if (optLength != L2CAP_SIZE_2) {
1155 return BT_BAD_PARAM;
1156 }
1157
1158 break;
1159 case L2CAP_OPTION_FLUSH_TIMEOUT:
1160 if (optLength != L2CAP_SIZE_2) {
1161 return BT_BAD_PARAM;
1162 }
1163
1164 break;
1165 case L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL:
1166 if (optLength != L2CAP_SIZE_9) {
1167 return BT_BAD_PARAM;
1168 }
1169
1170 break;
1171 case L2CAP_OPTION_FRAME_CHECK_SEQUENCE:
1172 if (optLength != L2CAP_SIZE_1) {
1173 return BT_BAD_PARAM;
1174 }
1175
1176 break;
1177 case L2CAP_OPTION_QUALITY_OF_SERVICE:
1178 if (optLength != L2CAP_SIZE_22) {
1179 return BT_BAD_PARAM;
1180 }
1181
1182 break;
1183 default:
1184 break;
1185 }
1186
1187 return BT_SUCCESS;
1188 }
1189
L2capProcessErfcOption(const uint8_t * data,uint16_t offset,L2capConfigInfo * cfg)1190 static void L2capProcessErfcOption(const uint8_t *data, uint16_t offset, L2capConfigInfo *cfg)
1191 {
1192 cfg->rfc.mode = data[offset + L2CAP_OFFSET_2];
1193 cfg->rfc.txWindowSize = data[offset + L2CAP_OFFSET_3];
1194 cfg->rfc.maxTransmit = data[offset + L2CAP_OFFSET_4];
1195
1196 cfg->rfc.retransmissionTimeout = L2capLe16ToCpu(data + offset + L2CAP_OFFSET_5);
1197 cfg->rfc.monitorTimeout = L2capLe16ToCpu(data + offset + L2CAP_OFFSET_7);
1198 cfg->rfc.mps = L2capLe16ToCpu(data + offset + L2CAP_OFFSET_9);
1199
1200 return;
1201 }
1202
L2capParseConfiguration(const uint8_t * data,uint16_t length,L2capConfigInfo * cfg,L2capOptions * unknown)1203 static int L2capParseConfiguration(const uint8_t *data, uint16_t length, L2capConfigInfo *cfg, L2capOptions *unknown)
1204 {
1205 uint16_t offset = 0;
1206
1207 while (length > (offset + L2CAP_SIZE_2)) {
1208 uint8_t optType = data[offset + 0];
1209 uint8_t optLength = data[offset + L2CAP_OFFSET_1];
1210
1211 if (length < (offset + optLength + L2CAP_SIZE_2)) {
1212 return BT_BAD_PARAM;
1213 }
1214
1215 if (L2capCheckConfigurationOptionLength(optType, optLength) != BT_SUCCESS) {
1216 return BT_BAD_PARAM;
1217 }
1218
1219 switch (optType) {
1220 case L2CAP_OPTION_MAXIMUM_TRANSMISSION_UNIT:
1221 cfg->mtu = L2capLe16ToCpu(data + offset + L2CAP_OFFSET_2);
1222 break;
1223 case L2CAP_OPTION_FLUSH_TIMEOUT:
1224 cfg->flushTimeout = L2capLe16ToCpu(data + offset + L2CAP_OFFSET_2);
1225 break;
1226 case L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL:
1227 L2capProcessErfcOption(data, offset, cfg);
1228 break;
1229 case L2CAP_OPTION_FRAME_CHECK_SEQUENCE:
1230 cfg->fcs = data[offset + L2CAP_OFFSET_2];
1231 break;
1232 case L2CAP_OPTION_QUALITY_OF_SERVICE:
1233 if ((data[offset + L2CAP_OFFSET_3] != L2CAP_QOS_SERVICE_TYPE_BEST_EFFORT) &&
1234 (data[offset + L2CAP_OFFSET_3] != L2CAP_QOS_SERVICE_TYPE_GUARANTEED)) {
1235 return BT_BAD_PARAM;
1236 }
1237 break;
1238 case L2CAP_OPTION_EXTENDED_FLOW_SPECIFICATION: // dummy
1239 case L2CAP_OPTION_EXTENDED_WINDOW_SIZE:
1240 return BT_BAD_PARAM;
1241 default:
1242 if ((optType & L2CAP_OPTION_HINT) || (unknown == NULL)) {
1243 break;
1244 }
1245
1246 if (unknown->options == NULL) {
1247 unknown->options = L2capAlloc(length);
1248 }
1249
1250 (void)memcpy_s(unknown->options + unknown->length,
1251 optLength + L2CAP_SIZE_2,
1252 data + offset,
1253 optLength + L2CAP_SIZE_2);
1254 unknown->length += optLength + L2CAP_SIZE_2;
1255 break;
1256 }
1257
1258 offset += (optLength + L2CAP_SIZE_2);
1259 }
1260
1261 return BT_SUCCESS;
1262 }
1263
L2capConfigurationRspReject(const L2capConnection * conn,uint8_t ident,uint16_t rcid)1264 static void L2capConfigurationRspReject(const L2capConnection *conn, uint8_t ident, uint16_t rcid)
1265 {
1266 Packet *pkt = NULL;
1267 uint8_t buff[6] = {0};
1268 L2capSignalHeader signal = {0};
1269
1270 L2capCpuToLe16(buff + 0, rcid);
1271 L2capCpuToLe16(buff + L2CAP_OFFSET_2, (uint16_t)0); // continuation flag
1272 L2capCpuToLe16(buff + L2CAP_OFFSET_4, (uint16_t)L2CAP_REJECTED);
1273
1274 signal.length = L2CAP_SIZE_6;
1275 signal.code = L2CAP_CONFIGURATION_RESPONSE;
1276 signal.identifier = ident;
1277
1278 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
1279 L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
1280 return;
1281 }
1282
L2capConfigurationRspUnknownOptions(const L2capConnection * conn,uint8_t ident,uint16_t rcid,L2capOptions * unknown)1283 static void L2capConfigurationRspUnknownOptions(
1284 const L2capConnection *conn, uint8_t ident, uint16_t rcid, L2capOptions *unknown)
1285 {
1286 Packet *pkt = NULL;
1287 uint8_t buff[L2CAP_SIGNAL_MTU] = {0};
1288 L2capSignalHeader signal = {0};
1289
1290 L2capCpuToLe16(buff + 0, rcid);
1291 L2capCpuToLe16(buff + L2CAP_OFFSET_2, (uint16_t)0); // continuation flag
1292 L2capCpuToLe16(buff + L2CAP_OFFSET_4, (uint16_t)L2CAP_UNKNOWN_OPTIONS);
1293
1294 if (memcpy_s(buff + L2CAP_OFFSET_6, L2CAP_SIGNAL_MTU - L2CAP_OFFSET_6, unknown->options, unknown->length) != EOK) {
1295 LOG_ERROR("[%{public}s][%{public}d] memcpy_s fail.", __FUNCTION__, __LINE__);
1296 return;
1297 }
1298
1299 signal.length = L2CAP_SIZE_6 + unknown->length;
1300 signal.code = L2CAP_CONFIGURATION_RESPONSE;
1301 signal.identifier = ident;
1302
1303 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
1304 L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
1305 return;
1306 }
1307
L2capConfigurationRspContinuation(const L2capConnection * conn,uint8_t ident,uint16_t rcid)1308 static void L2capConfigurationRspContinuation(const L2capConnection *conn, uint8_t ident, uint16_t rcid)
1309 {
1310 Packet *pkt = NULL;
1311 uint8_t buff[6] = {0};
1312 L2capSignalHeader signal = {0};
1313
1314 L2capCpuToLe16(buff + 0, rcid);
1315 L2capCpuToLe16(buff + L2CAP_OFFSET_2, (uint16_t)1); // continuation flag
1316 L2capCpuToLe16(buff + L2CAP_OFFSET_4, (uint16_t)L2CAP_SUCCESS);
1317
1318 signal.length = L2CAP_SIZE_6;
1319 signal.code = L2CAP_CONFIGURATION_RESPONSE;
1320 signal.identifier = ident;
1321
1322 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
1323 L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
1324 return;
1325 }
1326
L2capDestroyPartOption(L2capChannel * chan)1327 static void L2capDestroyPartOption(L2capChannel *chan)
1328 {
1329 if (chan->part.options != NULL) {
1330 L2capFree(chan->part.options);
1331 chan->part.options = NULL;
1332 chan->part.length = 0;
1333 }
1334
1335 return;
1336 }
1337
L2capProcessConfigurationReqWithContinue(L2capConnection * conn,L2capChannel * chan,L2capSignalHeader * signal,const uint8_t * data,uint16_t optLength)1338 static void L2capProcessConfigurationReqWithContinue(
1339 L2capConnection *conn, L2capChannel *chan, L2capSignalHeader *signal, const uint8_t *data, uint16_t optLength)
1340 {
1341 if (optLength > 0) {
1342 if (chan->part.options == NULL) {
1343 chan->part.options = L2capAlloc(L2CAP_SIGNAL_MTU);
1344 if (chan->part.options == NULL) {
1345 LOG_WARN("malloc failed");
1346 return;
1347 }
1348 chan->part.length = 0;
1349 }
1350
1351 if ((chan->part.length + optLength) > L2CAP_SIGNAL_MTU) {
1352 L2capSendCommandReject(
1353 conn->aclHandle, L2CAP_SIGNALING_CHANNEL, signal->identifier, L2CAP_COMMAND_NOT_UNDERSTOOD, NULL);
1354
1355 L2capDestroyPartOption(chan);
1356 return;
1357 }
1358
1359 if (memcpy_s(chan->part.options + chan->part.length, optLength, data, optLength) != EOK) {
1360 LOG_ERROR("L2capProcessConfigurationReqWithContinue memcpy_s failed");
1361 return;
1362 }
1363 chan->part.length += optLength;
1364 }
1365
1366 L2capConfigurationRspContinuation(conn, signal->identifier, chan->rcid);
1367 return;
1368 }
1369
L2capProcessConfigurationReqContinueOption(L2capConnection * conn,L2capChannel * chan,L2capSignalHeader * signal,const uint8_t * data,uint16_t optLength)1370 static int L2capProcessConfigurationReqContinueOption(
1371 L2capConnection *conn, L2capChannel *chan, L2capSignalHeader *signal, const uint8_t *data, uint16_t optLength)
1372 {
1373 if ((chan->part.length + optLength) > L2CAP_SIGNAL_MTU) {
1374 L2capSendCommandReject(
1375 conn->aclHandle, L2CAP_SIGNALING_CHANNEL, signal->identifier, L2CAP_COMMAND_NOT_UNDERSTOOD, NULL);
1376
1377 L2capDestroyPartOption(chan);
1378 return BT_BAD_PARAM;
1379 }
1380
1381 if (optLength > 0) {
1382 if (memcpy_s(chan->part.options + chan->part.length, optLength, data, optLength) != EOK) {
1383 LOG_ERROR("[%{public}s][%{public}d] memcpy_s fail.", __FUNCTION__, __LINE__);
1384 return BT_NO_MEMORY;
1385 }
1386 chan->part.length += optLength;
1387 }
1388
1389 return BT_SUCCESS;
1390 }
1391
L2capProcessConfigurationReqParse(L2capConnection * conn,L2capChannel * chan,L2capSignalHeader * signal,const uint8_t * data,uint16_t optLength)1392 NO_SANITIZE("cfi") static void L2capProcessConfigurationReqParse(
1393 L2capConnection *conn, L2capChannel *chan, L2capSignalHeader *signal, const uint8_t *data, uint16_t optLength)
1394 {
1395 L2capConfigInfo cfg = {0};
1396 L2capPsm *psm = NULL;
1397
1398 (void)memcpy_s(&cfg, sizeof(L2capConfigInfo), &(chan->rcfg), sizeof(L2capConfigInfo));
1399 if (optLength > 0) {
1400 L2capOptions unknown = {0};
1401
1402 if (L2capParseConfiguration(data, optLength, &cfg, &unknown) != BT_SUCCESS) {
1403 if (unknown.options != NULL) {
1404 L2capFree(unknown.options);
1405 }
1406
1407 L2capConfigurationRspReject(conn, signal->identifier, chan->rcid);
1408 L2capDestroyPartOption(chan);
1409 return;
1410 }
1411
1412 if (unknown.options != NULL) {
1413 L2capConfigurationRspUnknownOptions(conn, signal->identifier, chan->rcid, &unknown);
1414 L2capFree(unknown.options);
1415 L2capDestroyPartOption(chan);
1416 return;
1417 }
1418
1419 chan->rcfg.mtu = cfg.mtu;
1420 chan->rcfg.fcs = cfg.fcs;
1421 chan->rcfg.flushTimeout = cfg.flushTimeout;
1422 chan->rcfg.rfc.mode = cfg.rfc.mode;
1423 chan->rcfg.rfc.maxTransmit = cfg.rfc.maxTransmit;
1424 chan->rcfg.rfc.rxWindowSize = cfg.rfc.txWindowSize;
1425 chan->rcfg.rfc.mps = cfg.rfc.mps;
1426 }
1427
1428 L2capDestroyPartOption(chan);
1429
1430 psm = L2capGetPsm(chan->lpsm);
1431 if (psm != NULL) {
1432 LOG_DEBUG(
1433 "L2capCallback recvConfigReq:%{public}d begin, cid = 0x%04X, id = %hhu", __LINE__, chan->lcid, signal->identifier);
1434 psm->service.recvConfigReq(chan->lcid, signal->identifier, &cfg, psm->ctx);
1435 LOG_DEBUG("L2capCallback recvConfigReq:%{public}d end", __LINE__);
1436 }
1437
1438 return;
1439 }
1440
L2capProcessConfigurationReq(uint16_t aclHandle,L2capSignalHeader * signal,const uint8_t * data)1441 static void L2capProcessConfigurationReq(uint16_t aclHandle, L2capSignalHeader *signal, const uint8_t *data)
1442 {
1443 L2capConnection *conn = NULL;
1444 L2capChannel *chan = NULL;
1445 uint16_t cid;
1446 uint16_t flag;
1447 uint16_t optLength;
1448
1449 conn = L2capGetConnection(aclHandle);
1450 if (conn == NULL) {
1451 return;
1452 }
1453
1454 cid = L2capLe16ToCpu(data + 0);
1455 flag = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1456
1457 optLength = signal->length - L2CAP_SIZE_4;
1458 data += L2CAP_SIZE_4;
1459
1460 chan = L2capGetChannel(conn, cid);
1461 if (chan == NULL) {
1462 uint16_t rejCid[2] = {0, cid};
1463
1464 L2capSendCommandReject(
1465 aclHandle, L2CAP_SIGNALING_CHANNEL, signal->identifier, L2CAP_INVALID_CID_IN_REQUEST, rejCid);
1466 return;
1467 }
1468
1469 if (flag & 0x0001) { // continuation flag is set
1470 L2capProcessConfigurationReqWithContinue(conn, chan, signal, data, optLength);
1471 return;
1472 }
1473
1474 if (chan->part.options != NULL) {
1475 if (L2capProcessConfigurationReqContinueOption(conn, chan, signal, data, optLength) != BT_SUCCESS) {
1476 return;
1477 }
1478
1479 data = chan->part.options;
1480 optLength = chan->part.length;
1481 }
1482
1483 L2capProcessConfigurationReqParse(conn, chan, signal, data, optLength);
1484 return;
1485 }
1486
L2capConfigurationReqContinuation(L2capConnection * conn,const L2capChannel * chan)1487 static void L2capConfigurationReqContinuation(L2capConnection *conn, const L2capChannel *chan)
1488 {
1489 Packet *pkt = NULL;
1490 uint8_t buff[4] = {0};
1491 L2capSignalHeader signal = {0};
1492
1493 L2capCpuToLe16(buff + 0, chan->rcid);
1494 L2capCpuToLe16(buff + L2CAP_OFFSET_2, 0);
1495 signal.length = sizeof(buff);
1496
1497 signal.code = L2CAP_CONFIGURATION_REQUEST;
1498 signal.identifier = L2capGetNewIdentifier(conn);
1499
1500 pkt = L2capBuildSignalPacket(L2CAP_SIGNALING_CHANNEL, &signal, buff);
1501
1502 L2capCreatePendingRequest(conn->pendingList, chan->lcid, &signal, L2CAP_DEFAULT_RTX, L2capResponseTimeoutCallback);
1503 L2capSendPacket(conn->aclHandle, L2CAP_NONE_FLUSH_PACKET, pkt);
1504 return;
1505 }
1506
1507 NO_SANITIZE("cfi")
L2capProcessConfigurationRsp(uint16_t aclHandle,L2capSignalHeader * signal,uint8_t * data)1508 static void L2capProcessConfigurationRsp(uint16_t aclHandle, L2capSignalHeader *signal, uint8_t *data)
1509 {
1510 L2capConnection *conn = NULL;
1511 L2capChannel *chan = NULL;
1512 uint16_t cid;
1513 uint16_t flag;
1514 uint16_t result;
1515 L2capConfigInfo cfg = {0};
1516 L2capPsm *psm = NULL;
1517
1518 conn = L2capGetConnection(aclHandle);
1519 if (conn == NULL) {
1520 return;
1521 }
1522
1523 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1524
1525 cid = L2capLe16ToCpu(data + 0);
1526 flag = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1527 result = L2capLe16ToCpu(data + L2CAP_OFFSET_4);
1528
1529 chan = L2capGetChannel(conn, cid);
1530 if (chan == NULL) {
1531 return;
1532 }
1533
1534 (void)memcpy_s(&cfg, sizeof(L2capConfigInfo), &(chan->lcfg), sizeof(L2capConfigInfo));
1535 if (signal->length > L2CAP_SIZE_6) {
1536 L2capParseConfiguration(data + L2CAP_OFFSET_6, signal->length - L2CAP_SIZE_6, &cfg, NULL);
1537
1538 if (cfg.rfc.mode == L2CAP_ENHANCED_RETRANSMISSION_MODE) {
1539 chan->rcfg.rfc.retransmissionTimeout = cfg.rfc.retransmissionTimeout;
1540 chan->rcfg.rfc.monitorTimeout = cfg.rfc.monitorTimeout;
1541 chan->rcfg.rfc.txWindowSize = cfg.rfc.txWindowSize;
1542 }
1543 }
1544
1545 if (flag & 0x0001) { // continuation flag is set
1546 L2capConfigurationReqContinuation(conn, chan);
1547 return;
1548 }
1549
1550 if (result == L2CAP_SUCCESS) {
1551 chan->cfgState |= L2CAP_CONFIG_STATE_OUT_DONE;
1552 L2capChannelConnected(chan);
1553 }
1554
1555 if (result == L2CAP_PENDING) {
1556 signal->code = L2CAP_CONFIGURATION_REQUEST;
1557 L2capCreatePendingRequest(
1558 conn->pendingList, chan->lcid, signal, L2CAP_DEFAULT_ERTX, L2capResponseTimeoutCallback);
1559 }
1560
1561 psm = L2capGetPsm(chan->lpsm);
1562 if (psm != NULL) {
1563 LOG_DEBUG("L2capCallback recvConfigRsp:%{public}d begin, cid = 0x%04X, result = %hu", __LINE__, cid, result);
1564 psm->service.recvConfigRsp(cid, &cfg, result, psm->ctx);
1565 LOG_DEBUG("L2capCallback recvConfigRsp:%{public}d end", __LINE__);
1566 }
1567
1568 return;
1569 }
1570
1571 NO_SANITIZE("cfi")
L2capProcessConnectionReq(uint16_t aclHandle,const L2capSignalHeader * signal,uint8_t * data)1572 static void L2capProcessConnectionReq(uint16_t aclHandle, const L2capSignalHeader *signal, uint8_t *data)
1573 {
1574 L2capConnection *conn = NULL;
1575 L2capChannel *chan = NULL;
1576 L2capPsm *psm = NULL;
1577 uint16_t lpsm;
1578 uint16_t cid;
1579
1580 conn = L2capGetConnection(aclHandle);
1581 if (conn == NULL) {
1582 return;
1583 }
1584
1585 BTM_ExitSniffMode(&(conn->addr));
1586
1587 lpsm = L2capLe16ToCpu(data + 0);
1588 cid = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1589
1590 psm = L2capGetPsm(lpsm);
1591 if (psm == NULL) {
1592 L2capChannel tchan;
1593
1594 tchan.lcid = 0;
1595 tchan.rcid = cid;
1596 L2capSendConnectionRsp(
1597 conn, &tchan, signal->identifier, L2CAP_PSM_NOT_SUPPORTED, L2CAP_NO_FURTHER_INFORMATION_AVAILABLE);
1598 return;
1599 }
1600
1601 chan = L2capNewChannel(conn, lpsm, lpsm);
1602 chan->rcid = cid;
1603 chan->state = L2CAP_CHANNEL_CONNECT_IN_REQ;
1604 chan->connIdentifier = signal->identifier;
1605
1606 if (conn->info.state == L2CAP_INFO_STATE_NONE) {
1607 L2capSendConnectionRsp(
1608 conn, chan, signal->identifier, L2CAP_CONNECTION_PENDING, L2CAP_NO_FURTHER_INFORMATION_AVAILABLE);
1609 L2capSendInformationReq(conn, L2CAP_INFORMATION_TYPE_EXTENDED_FEATURE);
1610 } else if (conn->info.state == L2CAP_INFO_STATE_DONE) {
1611 L2capConnectionInfo connInfo = {0};
1612
1613 LOG_DEBUG("L2capCallback recvConnectionReq:%{public}d begin, aclHandle = %hu, cid = 0x%04X, id = %hhu, lpsm = %hu",
1614 __LINE__,
1615 aclHandle,
1616 chan->lcid,
1617 signal->identifier,
1618 psm->lpsm);
1619
1620 connInfo.handle = aclHandle;
1621 (void)memcpy_s(&(connInfo.addr), sizeof(BtAddr), &(conn->addr), sizeof(BtAddr));
1622 psm->service.recvConnectionReq(chan->lcid, signal->identifier, &connInfo, psm->lpsm, psm->ctx);
1623 LOG_DEBUG("L2capCallback recvConnectionReq:%{public}d end", __LINE__);
1624 } else {
1625 L2capSendConnectionRsp(
1626 conn, chan, signal->identifier, L2CAP_CONNECTION_PENDING, L2CAP_NO_FURTHER_INFORMATION_AVAILABLE);
1627 }
1628
1629 return;
1630 }
1631
L2capProcessConnectionRsp(uint16_t aclHandle,L2capSignalHeader * signal,uint8_t * data)1632 static void L2capProcessConnectionRsp(uint16_t aclHandle, L2capSignalHeader *signal, uint8_t *data)
1633 {
1634 L2capConnection *conn = NULL;
1635 L2capChannel *chan = NULL;
1636 L2capPsm *psm = NULL;
1637 uint16_t lcid;
1638 uint16_t rcid;
1639 uint16_t result;
1640 uint16_t status;
1641
1642 conn = L2capGetConnection(aclHandle);
1643 if (conn == NULL) {
1644 return;
1645 }
1646
1647 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1648
1649 rcid = L2capLe16ToCpu(data + 0);
1650 lcid = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1651 result = L2capLe16ToCpu(data + L2CAP_OFFSET_4);
1652 status = L2capLe16ToCpu(data + L2CAP_OFFSET_6);
1653
1654 chan = L2capGetChannel(conn, lcid);
1655 if (chan == NULL) {
1656 return;
1657 }
1658
1659 psm = L2capGetPsm(chan->lpsm);
1660
1661 chan->rcid = rcid;
1662
1663 if (result == L2CAP_CONNECTION_SUCCESSFUL) {
1664 chan->state = L2CAP_CHANNEL_CONFIGING;
1665 }
1666
1667 if (result == L2CAP_CONNECTION_PENDING) {
1668 signal->code = L2CAP_CONNECTION_REQUEST;
1669 L2capCreatePendingRequest(
1670 conn->pendingList, chan->lcid, signal, L2CAP_DEFAULT_ERTX, L2capResponseTimeoutCallback);
1671 }
1672
1673 if (result != L2CAP_CONNECTION_SUCCESSFUL && result != L2CAP_CONNECTION_PENDING) {
1674 L2capDeleteChannel(conn, chan, 1);
1675 }
1676
1677 if (psm != NULL) {
1678 L2capConnectionInfo connInfo = {0};
1679
1680 LOG_DEBUG("L2capCallback recvConnectionRsp:%{public}d begin, aclHandle = %hu, cid = 0x%04X, result = %{public}d, status = %hu",
1681 __LINE__,
1682 aclHandle,
1683 lcid,
1684 result,
1685 status);
1686
1687 connInfo.handle = aclHandle;
1688 (void)memcpy_s(&(connInfo.addr), sizeof(BtAddr), &(conn->addr), sizeof(BtAddr));
1689 psm->service.recvConnectionRsp(lcid, &connInfo, result, status, psm->ctx);
1690 LOG_DEBUG("L2capCallback recvConnectionRsp:%{public}d end", __LINE__);
1691 }
1692
1693 return;
1694 }
1695
L2capProcessDisconnectionReq(uint16_t aclHandle,L2capSignalHeader * signal,uint8_t * data)1696 static void L2capProcessDisconnectionReq(uint16_t aclHandle, L2capSignalHeader *signal, uint8_t *data)
1697 {
1698 L2capConnection *conn = NULL;
1699 L2capChannel *chan = NULL;
1700 uint16_t lcid;
1701 uint16_t rcid;
1702 L2capPsm *psm = NULL;
1703
1704 conn = L2capGetConnection(aclHandle);
1705 if (conn == NULL) {
1706 return;
1707 }
1708
1709 BTM_ExitSniffMode(&(conn->addr));
1710
1711 lcid = L2capLe16ToCpu(data + 0);
1712 rcid = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1713
1714 chan = L2capGetChannel(conn, lcid);
1715 if (chan == NULL) {
1716 uint16_t rejCid[2] = {lcid, rcid};
1717
1718 L2capSendCommandReject(
1719 aclHandle, L2CAP_SIGNALING_CHANNEL, signal->identifier, L2CAP_INVALID_CID_IN_REQUEST, rejCid);
1720 return;
1721 }
1722
1723 // this case is for both side call disconnect at same time
1724 if (chan->state == L2CAP_CHANNEL_DISCONNECT_OUT_REQ) {
1725 L2capSendDisconnectionRsp(conn, chan, signal->identifier);
1726 return;
1727 }
1728
1729 chan->state = L2CAP_CHANNEL_DISCONNECT_IN_REQ;
1730
1731 psm = L2capGetPsm(chan->lpsm);
1732 if (psm != NULL) {
1733 LOG_DEBUG(
1734 "L2capCallback recvDisconnectionReq:%{public}d begin, cid = 0x%04X, id = %hhu", __LINE__, lcid, signal->identifier);
1735 psm->service.recvDisconnectionReq(lcid, signal->identifier, psm->ctx);
1736 LOG_DEBUG("L2capCallback recvDisconnectionReq:%{public}d end", __LINE__);
1737 }
1738
1739 return;
1740 }
1741
1742 NO_SANITIZE("cfi")
L2capProcessDisconnectionRsp(uint16_t aclHandle,const L2capSignalHeader * signal,uint8_t * data)1743 static void L2capProcessDisconnectionRsp(uint16_t aclHandle, const L2capSignalHeader *signal, uint8_t *data)
1744 {
1745 L2capConnection *conn = NULL;
1746 L2capChannel *chan = NULL;
1747 L2capPsm *psm = NULL;
1748 uint16_t lcid;
1749 uint16_t rcid;
1750
1751 conn = L2capGetConnection(aclHandle);
1752 if (conn == NULL) {
1753 return;
1754 }
1755
1756 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1757
1758 rcid = L2capLe16ToCpu(data + 0);
1759 lcid = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1760
1761 chan = L2capGetChannel(conn, lcid);
1762 if (chan == NULL) {
1763 return;
1764 }
1765
1766 if (chan->rcid != rcid) {
1767 return;
1768 }
1769
1770 psm = L2capGetPsm(chan->lpsm);
1771
1772 L2capDeleteChannel(conn, chan, 1);
1773
1774 if (psm != NULL) {
1775 LOG_DEBUG("L2capCallback recvDisconnectionRsp:%{public}d begin, cid = 0x%04X", __LINE__, lcid);
1776 psm->service.recvDisconnectionRsp(lcid, psm->ctx);
1777 LOG_DEBUG("L2capCallback recvDisconnectionRsp:%{public}d end", __LINE__);
1778 }
1779
1780 return;
1781 }
1782
L2capProcessInformationReq(uint16_t aclHandle,const L2capSignalHeader * signal,uint8_t * data)1783 static void L2capProcessInformationReq(uint16_t aclHandle, const L2capSignalHeader *signal, uint8_t *data)
1784 {
1785 L2capConnection *conn = NULL;
1786 uint16_t infoType;
1787
1788 conn = L2capGetConnection(aclHandle);
1789 if (conn == NULL) {
1790 return;
1791 }
1792
1793 infoType = L2capLe16ToCpu(data + 0);
1794
1795 L2capSendInformationRsp(conn, signal->identifier, infoType);
1796 return;
1797 }
1798
L2capIncomingConnection(const L2capConnection * conn,const L2capChannel * chan)1799 static void L2capIncomingConnection(const L2capConnection *conn, const L2capChannel *chan)
1800 {
1801 L2capPsm *psm = NULL;
1802 L2capConnectionInfo connInfo = {0};
1803
1804 psm = L2capGetPsm(chan->lpsm);
1805 if (psm != NULL) {
1806 LOG_DEBUG("L2capCallback recvConnectionReq:%{public}d begin, aclHandle = %hu, cid = 0x%04X, id = %hhu, lpsm = %hu",
1807 __LINE__,
1808 conn->aclHandle,
1809 chan->lcid,
1810 chan->connIdentifier,
1811 chan->lpsm);
1812
1813 connInfo.handle = conn->aclHandle;
1814 (void)memcpy_s(&(connInfo.addr), sizeof(BtAddr), &(conn->addr), sizeof(BtAddr));
1815 psm->service.recvConnectionReq(chan->lcid, chan->connIdentifier, &connInfo, chan->lpsm, psm->ctx);
1816 LOG_DEBUG("L2capCallback recvConnectionReq:%{public}d end", __LINE__);
1817 }
1818
1819 return;
1820 }
1821
L2capProcessInformationRsp(uint16_t aclHandle,const L2capSignalHeader * signal,uint8_t * data)1822 static void L2capProcessInformationRsp(uint16_t aclHandle, const L2capSignalHeader *signal, uint8_t *data)
1823 {
1824 L2capConnection *conn = NULL;
1825 uint16_t infoType;
1826 uint16_t result;
1827
1828 conn = L2capGetConnection(aclHandle);
1829 if (conn == NULL) {
1830 return;
1831 }
1832
1833 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1834
1835 infoType = L2capLe16ToCpu(data + 0);
1836 result = L2capLe16ToCpu(data + L2CAP_OFFSET_2);
1837 if (infoType == L2CAP_INFORMATION_TYPE_EXTENDED_FEATURE) {
1838 if (result == 0) {
1839 (void)memcpy_s(conn->info.extendedFeature, L2CAP_SIZE_4, data + L2CAP_OFFSET_4, L2CAP_SIZE_4);
1840 }
1841
1842 if (conn->info.extendedFeature[0] & L2CAP_FEATURE_FIXED_CHANNELS) {
1843 L2capSendInformationReq(conn, L2CAP_INFORMATION_TYPE_FIXED_CHANNEL);
1844 } else {
1845 conn->info.state = L2CAP_INFO_STATE_DONE;
1846 }
1847 } else { // L2CAP_INFORMATION_TYPE_FIXED_CHANNEL
1848 if (result == 0) {
1849 (void)memcpy_s(conn->info.fixedChannel, L2CAP_SIZE_8, data + L2CAP_OFFSET_4, L2CAP_SIZE_8);
1850 }
1851
1852 conn->info.state = L2CAP_INFO_STATE_DONE;
1853 }
1854
1855 if (conn->info.state == L2CAP_INFO_STATE_DONE) {
1856 L2capChannel *chan = NULL;
1857 ListNode *node = NULL;
1858
1859 node = ListGetFirstNode(conn->chanList);
1860 while (node != NULL) {
1861 chan = ListGetNodeData(node);
1862 if (chan->state == L2CAP_CHANNEL_IDLE) {
1863 L2capSendConnectionReq(conn, chan);
1864 } else {
1865 L2capIncomingConnection(conn, chan);
1866 }
1867
1868 node = ListGetNextNode(node);
1869 }
1870 }
1871
1872 return;
1873 }
1874
L2capProcessEchoReq(uint16_t aclHandle,const L2capSignalHeader * signal,const uint8_t * data)1875 static void L2capProcessEchoReq(uint16_t aclHandle, const L2capSignalHeader *signal, const uint8_t *data)
1876 {
1877 L2capInstance *inst = L2capGetInstance();
1878 L2capConnection *conn = NULL;
1879
1880 conn = L2capGetConnection(aclHandle);
1881 if (conn == NULL) {
1882 return;
1883 }
1884
1885 if (inst->echo.cb.recvEchoReq != NULL) {
1886 inst->echo.cb.recvEchoReq(aclHandle, signal->identifier, data, signal->length, inst->echo.ctx);
1887 } else {
1888 L2capSendEchoRsp(conn, signal->identifier, NULL, 0);
1889 }
1890
1891 return;
1892 }
1893
L2capProcessEchoRsp(uint16_t aclHandle,const L2capSignalHeader * signal,const uint8_t * data)1894 static void L2capProcessEchoRsp(uint16_t aclHandle, const L2capSignalHeader *signal, const uint8_t *data)
1895 {
1896 L2capInstance *inst = L2capGetInstance();
1897 L2capConnection *conn = NULL;
1898
1899 conn = L2capGetConnection(aclHandle);
1900 if (conn == NULL) {
1901 return;
1902 }
1903
1904 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1905
1906 if (inst->echo.cb.recvEchoRsp != NULL) {
1907 inst->echo.cb.recvEchoRsp(aclHandle, data, signal->length, inst->echo.ctx);
1908 }
1909
1910 return;
1911 }
1912
L2capProcessCommandReject(uint16_t aclHandle,const L2capSignalHeader * signal,const uint8_t * data)1913 static void L2capProcessCommandReject(uint16_t aclHandle, const L2capSignalHeader *signal, const uint8_t *data)
1914 {
1915 L2capConnection *conn = NULL;
1916 L2capChannel *chan = NULL;
1917 uint16_t reason;
1918
1919 conn = L2capGetConnection(aclHandle);
1920 if (conn == NULL) {
1921 return;
1922 }
1923
1924 reason = L2capLe16ToCpu(data + 0);
1925 if (reason == L2CAP_INVALID_CID_IN_REQUEST) {
1926 L2capPsm *psm = NULL;
1927 uint16_t lcid;
1928
1929 lcid = L2capLe16ToCpu(data + L2CAP_OFFSET_4);
1930 chan = L2capGetChannel(conn, lcid);
1931 if (chan != NULL) {
1932 psm = L2capGetPsm(chan->lpsm);
1933 if (psm != NULL) {
1934 LOG_DEBUG("L2capCallback disconnectAbnormal:%{public}d begin, cid = 0x%04X, reason = 0", __LINE__, lcid);
1935 psm->service.disconnectAbnormal(lcid, 0, psm->ctx);
1936 LOG_DEBUG("L2capCallback disconnectAbnormal:%{public}d end", __LINE__);
1937 }
1938
1939 L2capDeleteChannel(conn, chan, 1);
1940 }
1941 } else if ((reason == L2CAP_COMMAND_NOT_UNDERSTOOD) || (reason == L2CAP_SIGNAL_MTU_EXCEEDED)) {
1942 L2capPendingRequest *req = NULL;
1943
1944 req = L2capGetPendingRequest(conn->pendingList, signal->identifier);
1945 if (req != NULL) {
1946 chan = L2capGetChannel(conn, req->lcid);
1947 if (chan != NULL) {
1948 L2capSendDisconnectionReq(conn, chan);
1949 }
1950 }
1951 }
1952
1953 L2capDestroyPendingRequest(conn->pendingList, signal->identifier);
1954 return;
1955 }
1956
L2capSignal(uint16_t aclHandle,L2capSignalHeader * signal,uint8_t * data)1957 static void L2capSignal(uint16_t aclHandle, L2capSignalHeader *signal, uint8_t *data)
1958 {
1959 switch (signal->code) {
1960 case L2CAP_COMMAND_REJECT:
1961 L2capProcessCommandReject(aclHandle, signal, data);
1962 break;
1963 case L2CAP_CONNECTION_REQUEST:
1964 L2capProcessConnectionReq(aclHandle, signal, data);
1965 break;
1966 case L2CAP_CONNECTION_RESPONSE:
1967 L2capProcessConnectionRsp(aclHandle, signal, data);
1968 break;
1969 case L2CAP_CONFIGURATION_REQUEST:
1970 L2capProcessConfigurationReq(aclHandle, signal, data);
1971 break;
1972 case L2CAP_CONFIGURATION_RESPONSE:
1973 L2capProcessConfigurationRsp(aclHandle, signal, data);
1974 break;
1975 case L2CAP_DISCONNECTION_REQUEST:
1976 L2capProcessDisconnectionReq(aclHandle, signal, data);
1977 break;
1978 case L2CAP_DISCONNECTION_RESPONSE:
1979 L2capProcessDisconnectionRsp(aclHandle, signal, data);
1980 break;
1981 case L2CAP_ECHO_REQUEST:
1982 L2capProcessEchoReq(aclHandle, signal, data);
1983 break;
1984 case L2CAP_ECHO_RESPONSE:
1985 L2capProcessEchoRsp(aclHandle, signal, data);
1986 break;
1987 case L2CAP_INFORMATION_REQUEST:
1988 L2capProcessInformationReq(aclHandle, signal, data);
1989 break;
1990 case L2CAP_INFORMATION_RESPONSE:
1991 L2capProcessInformationRsp(aclHandle, signal, data);
1992 break;
1993 default:
1994 L2capSendCommandReject(
1995 aclHandle, L2CAP_SIGNALING_CHANNEL, signal->identifier, L2CAP_COMMAND_NOT_UNDERSTOOD, NULL);
1996 break;
1997 }
1998
1999 return;
2000 }
2001
L2capProcessSignal(uint16_t aclHandle,const Packet * pkt)2002 static void L2capProcessSignal(uint16_t aclHandle, const Packet *pkt)
2003 {
2004 uint8_t buff[L2CAP_SIGNAL_MTU] = {0};
2005 uint16_t length;
2006 uint16_t offset = 0;
2007 L2capSignalHeader signal = {0};
2008
2009 length = PacketSize(pkt);
2010 if (length > L2CAP_SIGNAL_MTU) {
2011 PacketRead(pkt, buff, 0, L2CAP_SIGNAL_HEADER_LENGTH);
2012 L2capSendCommandReject(aclHandle, L2CAP_SIGNALING_CHANNEL, buff[1], L2CAP_SIGNAL_MTU_EXCEEDED, NULL);
2013 return;
2014 }
2015
2016 if (length < L2CAP_SIGNAL_HEADER_LENGTH) {
2017 PacketRead(pkt, buff, 0, L2CAP_SIGNAL_HEADER_LENGTH);
2018 L2capSendCommandReject(aclHandle, L2CAP_SIGNALING_CHANNEL, buff[1], L2CAP_COMMAND_NOT_UNDERSTOOD, NULL);
2019 return;
2020 }
2021
2022 PacketRead(pkt, buff, 0, length);
2023 while (length >= L2CAP_SIGNAL_HEADER_LENGTH) {
2024 signal.code = buff[offset + 0];
2025 signal.identifier = buff[offset + L2CAP_OFFSET_1];
2026 signal.length = L2capLe16ToCpu(buff + offset + L2CAP_OFFSET_2);
2027
2028 uint16_t signalLength = signal.length + L2CAP_SIGNAL_HEADER_LENGTH;
2029 if (length < signalLength || signal.length > L2CAP_SIGNAL_MTU) {
2030 L2capSendCommandReject(
2031 aclHandle, L2CAP_SIGNALING_CHANNEL, signal.identifier, L2CAP_COMMAND_NOT_UNDERSTOOD, NULL);
2032 break;
2033 }
2034
2035 L2capSignal(aclHandle, &signal, buff + offset + L2CAP_SIGNAL_HEADER_LENGTH);
2036
2037 offset += signalLength;
2038 length -= signalLength;
2039 }
2040
2041 return;
2042 }
2043
L2capProcessStreamData(L2capChannel * chan,Packet * pkt)2044 static Packet *L2capProcessStreamData(L2capChannel *chan, Packet *pkt)
2045 {
2046 uint8_t header[6] = {0};
2047 Packet *outPkt = NULL;
2048 L2capErfcIControl *iCtrl = NULL;
2049
2050 if (chan->lcfg.fcs == 0x01) {
2051 if (L2capCheckCrc(pkt) != BT_SUCCESS) {
2052 return NULL;
2053 }
2054 }
2055
2056 PacketExtractHead(pkt, header, sizeof(header));
2057
2058 iCtrl = (L2capErfcIControl *)(header + L2CAP_HEADER_LENGTH);
2059 if (iCtrl->type == L2CAP_IFRAME) {
2060 outPkt = L2capStreamProcessIFrame(chan, pkt, iCtrl);
2061 }
2062
2063 return outPkt;
2064 }
2065
L2capProcessErfcData(L2capConnection * conn,L2capChannel * chan,Packet * pkt)2066 static Packet *L2capProcessErfcData(L2capConnection *conn, L2capChannel *chan, Packet *pkt)
2067 {
2068 uint8_t header[6] = {0};
2069 Packet *outPkt = NULL;
2070
2071 if (chan->lcfg.fcs == 0x01) {
2072 if (L2capCheckCrc(pkt) != BT_SUCCESS) {
2073 return NULL;
2074 }
2075 }
2076
2077 PacketExtractHead(pkt, header, sizeof(header));
2078 if (header[L2CAP_HEADER_LENGTH] & L2CAP_SFRAME) {
2079 L2capErfcProcessSFrame(conn, chan, header + L2CAP_HEADER_LENGTH);
2080 } else { // I Frame
2081 outPkt = L2capErfcProcessIFrame(conn, chan, pkt, header + L2CAP_HEADER_LENGTH);
2082 }
2083
2084 return outPkt;
2085 }
2086
L2capProcessBasicData(const L2capChannel * chan,Packet * pkt)2087 static int L2capProcessBasicData(const L2capChannel *chan, Packet *pkt)
2088 {
2089 uint16_t length;
2090 uint8_t header[L2CAP_HEADER_LENGTH] = {0};
2091
2092 PacketExtractHead(pkt, header, sizeof(header));
2093 length = PacketSize(pkt);
2094 if (length > chan->lcfg.mtu) {
2095 return BT_BAD_PARAM;
2096 }
2097
2098 return BT_SUCCESS;
2099 }
2100
L2capProcessData(uint16_t aclHandle,uint16_t cid,Packet * pkt)2101 NO_SANITIZE("cfi") static void L2capProcessData(uint16_t aclHandle, uint16_t cid, Packet *pkt)
2102 {
2103 L2capConnection *conn = NULL;
2104 L2capChannel *chan = NULL;
2105 L2capPsm *psm = NULL;
2106 Packet *outPkt = NULL;
2107
2108 L2capGetChannel3(aclHandle, cid, &conn, &chan);
2109 if (chan == NULL) {
2110 return;
2111 }
2112
2113 if (chan->state != L2CAP_CHANNEL_CONNECTED) {
2114 return;
2115 }
2116
2117 psm = L2capGetPsm(chan->lpsm);
2118 if (psm == NULL) {
2119 return;
2120 }
2121
2122 switch (chan->lcfg.rfc.mode) {
2123 case L2CAP_BASIC_MODE:
2124 if (L2capProcessBasicData(chan, pkt) == BT_SUCCESS) {
2125 LOG_DEBUG("L2capCallback recvData: begin, cid = 0x%04X, pktLen = %u", cid, PacketSize(pkt));
2126 psm->service.recvData(cid, pkt, psm->ctx);
2127 LOG_DEBUG("L2capCallback recvData:%{public}d end", __LINE__);
2128 }
2129 break;
2130 case L2CAP_ENHANCED_RETRANSMISSION_MODE:
2131 outPkt = L2capProcessErfcData(conn, chan, pkt);
2132 if (outPkt != NULL) {
2133 LOG_DEBUG("L2capCallback recvData: begin, cid = 0x%04X, pktLen = %u", cid, PacketSize(outPkt));
2134 psm->service.recvData(cid, outPkt, psm->ctx);
2135 LOG_DEBUG("L2capCallback recvData:%{public}d end", __LINE__);
2136
2137 PacketFree(outPkt);
2138 }
2139 break;
2140 case L2CAP_STREAM_MODE:
2141 outPkt = L2capProcessStreamData(chan, pkt);
2142 if (outPkt != NULL) {
2143 LOG_DEBUG("L2capCallback recvData: begin, cid = 0x%04X, pktLen = %{public}d", cid, PacketSize(outPkt));
2144 psm->service.recvData(cid, outPkt, psm->ctx);
2145 LOG_DEBUG("L2capCallback recvData:%{public}d end", __LINE__);
2146
2147 PacketFree(outPkt);
2148 }
2149 break;
2150 default:
2151 break;
2152 }
2153
2154 return;
2155 }
2156
L2capReceivePacket(uint16_t handle,uint16_t cid,Packet * pkt)2157 int L2capReceivePacket(uint16_t handle, uint16_t cid, Packet *pkt)
2158 {
2159 uint8_t header[L2CAP_HEADER_LENGTH] = {0};
2160
2161 if (L2capInitialized() != BT_SUCCESS) {
2162 return BT_BAD_STATUS;
2163 }
2164
2165 switch (cid) {
2166 case L2CAP_SIGNALING_CHANNEL:
2167 PacketExtractHead(pkt, header, sizeof(header));
2168 L2capProcessSignal(handle, pkt);
2169 break;
2170 default:
2171 L2capProcessData(handle, cid, pkt);
2172 break;
2173 }
2174
2175 return BT_SUCCESS;
2176 }
2177
L2capAclConnectFailed(List * chanList,uint8_t status)2178 static void L2capAclConnectFailed(List *chanList, uint8_t status)
2179 {
2180 L2capChannel *chan = NULL;
2181 ListNode *node = NULL;
2182
2183 if (chanList == NULL) {
2184 return;
2185 }
2186
2187 while (1) {
2188 node = ListGetFirstNode(chanList);
2189 if (node == NULL) {
2190 break;
2191 }
2192
2193 chan = ListGetNodeData(node);
2194 L2capDisconnectAbnormal(chan, status);
2195
2196 ListRemoveNode(chanList, chan);
2197 L2capDestroyChannel(chan);
2198 }
2199
2200 ListDelete(chanList);
2201 return;
2202 }
2203
L2capConnectComplete(const BtAddr * addr,uint16_t handle,uint8_t status)2204 int L2capConnectComplete(const BtAddr *addr, uint16_t handle, uint8_t status)
2205 {
2206 L2capConnection *conn = NULL;
2207
2208 if (L2capInitialized() != BT_SUCCESS) {
2209 return BT_BAD_STATUS;
2210 }
2211
2212 if (status != 0) {
2213 List *chanList = NULL;
2214
2215 conn = L2capGetConnection2(addr);
2216 if (conn != NULL) {
2217 chanList = conn->chanList;
2218 conn->chanList = NULL;
2219
2220 L2capDeleteConnection(conn);
2221 }
2222
2223 L2capAclConnectFailed(chanList, status);
2224 return BT_BAD_STATUS;
2225 }
2226
2227 conn = L2capGetConnection2(addr);
2228 if (conn == NULL) {
2229 conn = L2capNewConnection(addr, handle);
2230 }
2231
2232 conn->aclHandle = handle;
2233 conn->state = L2CAP_CONNECTION_CONNECTED;
2234
2235 if (ListGetFirstNode(conn->chanList) != NULL) {
2236 L2capAddConnectionRef(handle);
2237 L2capSendInformationReq(conn, L2CAP_INFORMATION_TYPE_EXTENDED_FEATURE);
2238 }
2239
2240 return BT_SUCCESS;
2241 }
2242
L2capAclDisconnected(L2capConnection * conn,uint8_t status,uint8_t reason)2243 static void L2capAclDisconnected(L2capConnection *conn, uint8_t status, uint8_t reason)
2244 {
2245 L2capChannel *chan = NULL;
2246 ListNode *node = NULL;
2247 List *chanList = conn->chanList;
2248 uint8_t infoState = conn->info.state;
2249
2250 conn->chanList = NULL;
2251 L2capDeleteConnection(conn);
2252
2253 while (1) {
2254 node = ListGetFirstNode(chanList);
2255 if (node == NULL) {
2256 break;
2257 }
2258
2259 chan = ListGetNodeData(node);
2260 ListRemoveNode(chanList, chan);
2261
2262 if (infoState != L2CAP_INFO_STATE_DONE) {
2263 L2capDisconnectAbnormal(chan, reason);
2264 L2capDestroyChannel(chan);
2265 continue;
2266 }
2267
2268 if (chan->state == L2CAP_CHANNEL_IDLE) {
2269 L2capDisconnectAbnormal(chan, L2CAP_STATE_COLLISION);
2270 } else {
2271 L2capDisconnectAbnormal(chan, reason);
2272 }
2273
2274 L2capDestroyChannel(chan);
2275 }
2276
2277 ListDelete(chanList);
2278 return;
2279 }
2280
L2capDisconnectComplete(uint16_t handle,uint8_t status,uint8_t reason)2281 int L2capDisconnectComplete(uint16_t handle, uint8_t status, uint8_t reason)
2282 {
2283 L2capConnection *conn = NULL;
2284
2285 if (L2capInitialized() != BT_SUCCESS) {
2286 return BT_BAD_STATUS;
2287 }
2288
2289 conn = L2capGetConnection(handle);
2290 if (conn == NULL) {
2291 return BT_BAD_PARAM;
2292 }
2293
2294 L2capAclDisconnected(conn, status, reason);
2295 return BT_SUCCESS;
2296 }
2297