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 <stdbool.h>
17 #include <stdint.h>
18 
19 #include "att.h"
20 #include "btm.h"
21 
22 #include "platform/include/allocator.h"
23 #include "platform/include/list.h"
24 
25 #include "hci/hci.h"
26 #include "hci/hci_error.h"
27 #include "l2cap/l2cap_core.h"
28 #include "rfcomm/rfcomm_defs.h"
29 #include "smp/smp_def.h"
30 
31 #include "btm_snoop_filter.h"
32 
33 #define HCI_PACKET_BOUNDARY_CONTINUING 0x01
34 
35 #define L2CAP_SIGNALING_CHANNEL 0x0001
36 #define L2CAP_LE_ATTRIBUTE_PROTOCOL 0x0004
37 #define L2CAP_LE_SIGNALING_CHANNEL 0x0005
38 #define L2CAP_LE_SECURITY_MANAGER_PROTOCOL 0x0006
39 #define L2CAP_BREDR_SECURITY_MANAGER 0x0007
40 
41 #define L2CAP_CONNECTION_REQUEST 0x02
42 #define L2CAP_CONNECTION_RESPONSE 0x03
43 #define L2CAP_CONFIGURATION_REQUEST 0x04
44 #define L2CAP_CONFIGURATION_RESPONSE 0x05
45 #define L2CAP_DISCONNECTION_REQUEST 0x06
46 #define L2CAP_DISCONNECTION_RESPONSE 0x07
47 
48 #define L2CAP_CONNECTION_SUCCESSFUL 0x0000
49 #define L2CAP_CONNECTION_PENDING 0x0001
50 
51 #define L2CAP_INVALID_CHANNEL_ID 0x0000
52 
53 #define L2CAP_SDP_PSM 0x0001
54 #define L2CAP_RFCOMM_PSM 0x0003
55 #define L2CAP_AVCTP_PSM 0x0017
56 #define L2CAP_AVDTP_PSM 0x0019
57 #define L2CAP_AVCTP_BROWSING_PSM 0x001B
58 #define L2CAP_ATT_PSM 0x001F
59 
60 #define AVDT_MEDIA_PACKET_HEADER_LEN 12
61 
62 #define AVCT_PACKET_HEADER_LEN 3
63 #define AVRCP_COMPANY_ID 3
64 #define AVRCP_PDU_ID_GET_ELEMENT_ATTR 0x20
65 #define AVRCP_BROWSING_PDU_ID_GET_ITEM_ATTR 0x73
66 
67 typedef enum {
68     DISCONNECTED,
69     CONNECTING,
70     CONNECTED,
71 } ConnStatus;
72 
73 typedef enum {
74     BREDR,
75     LE,
76 } AclType;
77 
78 typedef struct {
79     AclType type;
80     uint16_t handle;
81     BtAddr addr;
82     bool prevPktIsFiltered;
83 } AclConnInfo;
84 
85 typedef struct {
86     ConnStatus status;
87     uint8_t module;
88     uint8_t headerLength;
89     AclConnInfo *acl;
90     uint16_t localCid;
91     uint16_t remoteCid;
92     bool isInitiator;
93     bool isAvdtpMedia;
94 } FilterL2capChannelConnInfo;
95 
96 typedef struct {
97     uint16_t aclHandle;
98     uint16_t remoteCid;
99     uint8_t dlci;
100 } RemoteSearchInfo;
101 
102 #pragma pack(1)
103 typedef struct {
104     uint16_t handle : 12;
105     uint16_t pbFlag : 2;
106     uint16_t bcFlag : 2;
107     uint16_t dataTotalLength;
108 } HciAclDataHeader;
109 
110 typedef struct {
111     uint8_t eventCode;
112     uint8_t parameterTotalLength;
113 } HciEventHeader;
114 
115 typedef struct {
116     uint16_t length;
117     uint16_t channelId;
118 } L2capBasicHeader;
119 
120 typedef struct {
121     uint16_t length;
122     uint16_t channelId;
123     uint16_t control;
124 } L2capNotBasicHeader;
125 
126 typedef struct {
127     uint8_t code;
128     uint8_t identifier;
129     uint16_t length;
130 } L2capSignalingHeader;
131 #pragma pack()
132 
133 static List *g_aclConnList = NULL;
134 static List *g_filterL2capConnList = NULL;
135 static List *g_filterRfcommConnList = NULL;
136 
FreeListNodeData(void * data)137 static void FreeListNodeData(void *data)
138 {
139     MEM_MALLOC.free(data);
140 }
141 
BtmEnableSnoopFilterAcl(void)142 void BtmEnableSnoopFilterAcl(void)
143 {
144     if (g_aclConnList == NULL) {
145         g_aclConnList = ListCreate(FreeListNodeData);
146     }
147     if (g_filterL2capConnList == NULL) {
148         g_filterL2capConnList = ListCreate(FreeListNodeData);
149     }
150     if (g_filterRfcommConnList == NULL) {
151         g_filterRfcommConnList = ListCreate(FreeListNodeData);
152     }
153 
154     BTM_AddLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_RFCOMM, L2CAP_RFCOMM_PSM);
155     BTM_AddLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVDTP, L2CAP_AVDTP_PSM);
156     BTM_AddLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVCTP, L2CAP_AVCTP_PSM);
157     BTM_AddLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVCTP_BROWSING, L2CAP_AVCTP_BROWSING_PSM);
158     BTM_AddLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_ATT, L2CAP_ATT_PSM);
159 }
160 
BtmDisableSnoopFilterAcl(void)161 void BtmDisableSnoopFilterAcl(void)
162 {
163     if (g_filterL2capConnList != NULL) {
164         ListDelete(g_filterL2capConnList);
165         g_filterL2capConnList = NULL;
166     }
167     if (g_filterRfcommConnList != NULL) {
168         ListDelete(g_filterRfcommConnList);
169         g_filterRfcommConnList = NULL;
170     }
171     if (g_aclConnList != NULL) {
172         ListDelete(g_aclConnList);
173         g_aclConnList = NULL;
174     }
175 
176     BTM_RemoveLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_RFCOMM, L2CAP_RFCOMM_PSM);
177     BTM_RemoveLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVDTP, L2CAP_AVDTP_PSM);
178     BTM_RemoveLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVCTP, L2CAP_AVCTP_PSM);
179     BTM_RemoveLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_AVCTP_BROWSING, L2CAP_AVCTP_BROWSING_PSM);
180     BTM_RemoveLocalL2capPsmForLogging(BTM_HCI_LOG_FILTER_MODULE_ATT, L2CAP_ATT_PSM);
181 }
182 
FindAclConnInfoByHandle(void * nodeData,void * handle)183 static bool FindAclConnInfoByHandle(void *nodeData, void *handle)
184 {
185     if (((AclConnInfo *)nodeData)->handle == *(uint16_t *)handle) {
186         return true;
187     } else {
188         return false;
189     }
190 }
191 
FindL2capConnByLocalCid(void * nodeData,void * cid)192 static bool FindL2capConnByLocalCid(void *nodeData, void *cid)
193 {
194     if (((FilterL2capChannelConnInfo *)nodeData)->localCid == *(uint16_t *)cid) {
195         return true;
196     } else {
197         return false;
198     }
199 }
200 
FindL2capConnByRemoteInfo(void * nodeData,void * info)201 static bool FindL2capConnByRemoteInfo(void *nodeData, void *info)
202 {
203     if (((FilterL2capChannelConnInfo *)nodeData)->remoteCid == ((RemoteSearchInfo *)info)->remoteCid &&
204         ((FilterL2capChannelConnInfo *)nodeData)->acl->handle == ((RemoteSearchInfo *)info)->aclHandle) {
205         return true;
206     } else {
207         return false;
208     }
209 }
210 
ExistL2capConnIsAvdtpSignal(uint16_t aclHandle)211 static bool ExistL2capConnIsAvdtpSignal(uint16_t aclHandle)
212 {
213     ListNode *node = ListGetFirstNode(g_filterL2capConnList);
214     while (node != NULL) {
215         FilterL2capChannelConnInfo *l2capConn = ListGetNodeData(node);
216         node = ListGetNextNode(node);
217 
218         if (l2capConn->acl->handle == aclHandle && l2capConn->module == BTM_HCI_LOG_FILTER_MODULE_AVDTP &&
219             l2capConn->status == CONNECTED && !l2capConn->isAvdtpMedia) {
220             return true;
221         }
222     }
223     return false;
224 }
225 
AddL2capConnection(ConnStatus status,uint8_t module,uint16_t aclHandle,uint16_t localCid,uint16_t remoteCid)226 static FilterL2capChannelConnInfo *AddL2capConnection(
227     ConnStatus status, uint8_t module, uint16_t aclHandle, uint16_t localCid, uint16_t remoteCid)
228 {
229     FilterL2capChannelConnInfo *l2capInfo = MEM_MALLOC.alloc(sizeof(FilterL2capChannelConnInfo));
230     if (l2capInfo != NULL) {
231         AclConnInfo *aclConnInfo = ListForEachData(g_aclConnList, FindAclConnInfoByHandle, &aclHandle);
232         if (aclConnInfo != NULL) {
233             l2capInfo->acl = aclConnInfo;
234             l2capInfo->module = module;
235             l2capInfo->status = status;
236             l2capInfo->localCid = localCid;
237             l2capInfo->remoteCid = remoteCid;
238             l2capInfo->isAvdtpMedia = false;
239             l2capInfo->isInitiator = false;
240             l2capInfo->headerLength = sizeof(L2capBasicHeader);
241             ListAddLast(g_filterL2capConnList, l2capInfo);
242         }
243     }
244 
245     return l2capInfo;
246 }
247 
RemoveL2capConnectionByLocalCid(uint16_t localCid)248 static void RemoveL2capConnectionByLocalCid(uint16_t localCid)
249 {
250     FilterL2capChannelConnInfo *l2capInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByLocalCid, &localCid);
251     if (l2capInfo != NULL) {
252         ListRemoveNode(g_filterL2capConnList, l2capInfo);
253     }
254 }
255 
RemoveL2capConnectionByAclHandle(uint16_t aclHandle)256 static void RemoveL2capConnectionByAclHandle(uint16_t aclHandle)
257 {
258     ListNode *node = ListGetFirstNode(g_filterL2capConnList);
259     while (node != NULL) {
260         FilterL2capChannelConnInfo *l2capInfo = ListGetNodeData(node);
261         node = ListGetNextNode(node);
262 
263         if (l2capInfo->acl->handle == aclHandle) {
264             ListRemoveNode(g_filterL2capConnList, l2capInfo);
265         }
266     }
267 }
268 
AddAclConnection(AclType type,uint16_t handle,const HciBdAddr * addr,uint8_t addrType)269 static void AddAclConnection(AclType type, uint16_t handle, const HciBdAddr *addr, uint8_t addrType)
270 {
271     AclConnInfo *info = MEM_MALLOC.alloc(sizeof(AclConnInfo));
272     if (info != NULL) {
273         info->handle = handle;
274         info->type = type;
275         (void)memcpy_s(info->addr.addr, sizeof(info->addr.addr), addr->raw, sizeof(addr->raw));
276         info->addr.type = addrType;
277         info->prevPktIsFiltered = false;
278         ListAddLast(g_aclConnList, info);
279     }
280 }
281 
RemoveAclConnection(uint16_t handle)282 static void RemoveAclConnection(uint16_t handle)
283 {
284     RemoveL2capConnectionByAclHandle(handle);
285 
286     AclConnInfo *info = ListForEachData(g_aclConnList, FindAclConnInfoByHandle, &handle);
287     if (info != NULL) {
288         ListRemoveNode(g_aclConnList, info);
289     }
290 }
291 
SetAclPrevPktIsFiltered(uint16_t handle,bool isFiltered)292 static void SetAclPrevPktIsFiltered(uint16_t handle, bool isFiltered)
293 {
294     AclConnInfo *info = ListForEachData(g_aclConnList, FindAclConnInfoByHandle, &handle);
295     if (info != NULL) {
296         info->prevPktIsFiltered = isFiltered;
297     }
298 }
299 
ProcessHciLeConnInfo(const uint8_t ** data,uint16_t originalLength)300 static void ProcessHciLeConnInfo(const uint8_t **data, uint16_t originalLength)
301 {
302     uint16_t offset = sizeof(HciEventHeader);
303     const uint8_t *subEventCode = *data + offset;
304     offset += sizeof(uint8_t);
305     switch (*subEventCode) {
306         case HCI_LE_CONNECTION_COMPLETE_EVENT: {
307             HciLeConnectionCompleteEventParam *evtParam = (HciLeConnectionCompleteEventParam *)(*data + offset);
308             if (evtParam->status == HCI_SUCCESS) {
309                 AddAclConnection(LE, evtParam->connectionHandle, &evtParam->peerAddress, evtParam->peerAddressType);
310             }
311             AddL2capConnection(CONNECTED,
312                 BTM_HCI_LOG_FILTER_MODULE_ATT,
313                 evtParam->connectionHandle,
314                 L2CAP_LE_ATTRIBUTE_PROTOCOL,
315                 L2CAP_LE_ATTRIBUTE_PROTOCOL);
316             AddL2capConnection(CONNECTED,
317                 BTM_HCI_LOG_FILTER_MODULE_SM,
318                 evtParam->connectionHandle,
319                 L2CAP_LE_SECURITY_MANAGER_PROTOCOL,
320                 L2CAP_LE_SECURITY_MANAGER_PROTOCOL);
321             break;
322         }
323         case HCI_LE_ENHANCED_CONNECTION_COMPLETE_EVENT: {
324             HciLeEnhancedConnectionCompleteEventParam *evtParam =
325                 (HciLeEnhancedConnectionCompleteEventParam *)(*data + offset);
326             if (evtParam->status == HCI_SUCCESS) {
327                 AddAclConnection(LE, evtParam->connectionHandle, &evtParam->peerAddress, evtParam->peerAddressType);
328             }
329             AddL2capConnection(CONNECTED,
330                 BTM_HCI_LOG_FILTER_MODULE_ATT,
331                 evtParam->connectionHandle,
332                 L2CAP_LE_ATTRIBUTE_PROTOCOL,
333                 L2CAP_LE_ATTRIBUTE_PROTOCOL);
334             AddL2capConnection(CONNECTED,
335                 BTM_HCI_LOG_FILTER_MODULE_SM,
336                 evtParam->connectionHandle,
337                 L2CAP_LE_SECURITY_MANAGER_PROTOCOL,
338                 L2CAP_LE_SECURITY_MANAGER_PROTOCOL);
339             break;
340         }
341         default:
342             break;
343     }
344 }
345 
ProcessHciConnInfo(const uint8_t ** data,uint16_t originalLength)346 static void ProcessHciConnInfo(const uint8_t **data, uint16_t originalLength)
347 {
348     uint16_t offset = 0;
349     HciEventHeader *header = (HciEventHeader *)*data;
350     offset += sizeof(HciEventHeader);
351 
352     switch (header->eventCode) {
353         case HCI_CONNECTION_COMPLETE_EVENT: {
354             HciConnectionCompleteEventParam *evtParam = (HciConnectionCompleteEventParam *)(*data + offset);
355             if ((evtParam->linkType == HCI_LINK_TYPE_ACL) && (evtParam->status == HCI_SUCCESS)) {
356                 AddAclConnection(BREDR, evtParam->connectionHandle, &evtParam->bdAddr, BT_PUBLIC_DEVICE_ADDRESS);
357                 AddL2capConnection(CONNECTED,
358                     BTM_HCI_LOG_FILTER_MODULE_SM,
359                     evtParam->connectionHandle,
360                     L2CAP_BREDR_SECURITY_MANAGER,
361                     L2CAP_BREDR_SECURITY_MANAGER);
362             }
363             break;
364         }
365         case HCI_DISCONNECT_COMPLETE_EVENT: {
366             HciDisconnectCompleteEventParam *evtParam = (HciDisconnectCompleteEventParam *)(*data + offset);
367             if (evtParam->status == HCI_SUCCESS) {
368                 RemoveAclConnection(evtParam->connectionHandle);
369             }
370             break;
371         }
372         case HCI_LE_META_EVENT:
373             ProcessHciLeConnInfo(data, originalLength);
374             break;
375         default:
376             break;
377     }
378 }
379 
BtmFilterCheckAndSaveAclConnInfo(const uint8_t ** data,uint16_t originalLength)380 void BtmFilterCheckAndSaveAclConnInfo(const uint8_t **data, uint16_t originalLength)
381 {
382     HciEventHeader *header = (HciEventHeader *)*data;
383 
384     switch (header->eventCode) {
385         case HCI_CONNECTION_COMPLETE_EVENT:
386             ProcessHciConnInfo(data, originalLength);
387             break;
388         case HCI_DISCONNECT_COMPLETE_EVENT:
389             ProcessHciConnInfo(data, originalLength);
390             break;
391         case HCI_LE_META_EVENT: {
392             uint16_t offset = sizeof(HciEventHeader);
393             const uint8_t *subEventCode = *data + offset;
394 
395             switch (*subEventCode) {
396                 case HCI_LE_CONNECTION_COMPLETE_EVENT:
397                     ProcessHciConnInfo(data, originalLength);
398                     break;
399                 case HCI_LE_ENHANCED_CONNECTION_COMPLETE_EVENT:
400                     ProcessHciConnInfo(data, originalLength);
401                     break;
402                 default:
403                     break;
404             }
405         } break;
406         default:
407             break;
408     }
409 }
410 
ProcessL2capConnectionRequest(uint8_t type,const AclConnInfo * aclInfo,const L2capSignalingHeader * l2capsignalingHeader,const uint8_t * data)411 static void ProcessL2capConnectionRequest(
412     uint8_t type, const AclConnInfo *aclInfo, const L2capSignalingHeader *l2capsignalingHeader, const uint8_t *data)
413 {
414     uint16_t offset = 0;
415     uint16_t *psm = (uint16_t *)(data + offset);
416     offset += sizeof(uint16_t);
417     uint16_t *scid = (uint16_t *)(data + offset);
418     offset += sizeof(uint16_t);
419 
420     if (l2capsignalingHeader->length < offset) {
421         return;
422     }
423 
424     BtmSnoopFilterInfo cmpinfo = {
425         .isLocal = (type == TRANSMISSON_TYPE_H2C_DATA) ? false : true,
426         .l2capPsm = *psm,
427     };
428     if (!cmpinfo.isLocal) {
429         cmpinfo.remoteAddr = aclInfo->addr;
430     }
431     BtmSnoopFilterInfo *info = ListForEachData(BtmGetFilterInfoList(), BtmFindFilterInfoByInfoUsePsm, &cmpinfo);
432     if (info != NULL) {
433         FilterL2capChannelConnInfo *l2capInfo = NULL;
434         if (type == TRANSMISSON_TYPE_H2C_DATA) {
435             l2capInfo = AddL2capConnection(CONNECTING, info->module, aclInfo->handle, *scid, L2CAP_INVALID_CHANNEL_ID);
436         } else {
437             l2capInfo = AddL2capConnection(CONNECTING, info->module, aclInfo->handle, L2CAP_INVALID_CHANNEL_ID, *scid);
438         }
439         if (l2capInfo != NULL) {
440             l2capInfo->isInitiator = (type == TRANSMISSON_TYPE_H2C_DATA);
441             if (*psm == L2CAP_AVDTP_PSM && ExistL2capConnIsAvdtpSignal(aclInfo->handle)) {
442                 l2capInfo->isAvdtpMedia = true;
443             }
444         }
445     }
446 }
447 
ProcessL2capConnectionResponse(uint8_t type,const AclConnInfo * aclInfo,const L2capSignalingHeader * l2capsignalingHeader,const uint8_t * data)448 static void ProcessL2capConnectionResponse(
449     uint8_t type, const AclConnInfo *aclInfo, const L2capSignalingHeader *l2capsignalingHeader, const uint8_t *data)
450 {
451     uint16_t offset = 0;
452     uint16_t *dcid = (uint16_t *)(data + offset);
453     offset += sizeof(uint16_t);
454     uint16_t *scid = (uint16_t *)(data + offset);
455     offset += sizeof(uint16_t);
456     uint16_t *result = (uint16_t *)(data + offset);
457     offset += sizeof(uint16_t);
458 
459     if (l2capsignalingHeader->length < offset || *result == L2CAP_CONNECTION_PENDING) {
460         return;
461     }
462 
463     FilterL2capChannelConnInfo *connInfo = NULL;
464     if (type == TRANSMISSON_TYPE_H2C_DATA) {
465         RemoteSearchInfo remoteInfo = {
466             .aclHandle = aclInfo->handle,
467             .remoteCid = *scid,
468         };
469         connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByRemoteInfo, &remoteInfo);
470     } else {
471         connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByLocalCid, scid);
472     }
473 
474     if (connInfo != NULL) {
475         if (*result == L2CAP_CONNECTION_SUCCESSFUL) {
476             if (type == TRANSMISSON_TYPE_H2C_DATA) {
477                 connInfo->localCid = *dcid;
478             } else {
479                 connInfo->remoteCid = *dcid;
480             }
481             connInfo->status = CONNECTED;
482         } else {
483             ListRemoveNode(g_filterL2capConnList, connInfo);
484         }
485     }
486 }
487 
ProcessL2capConfigurationRequest(uint8_t type,const AclConnInfo * aclInfo,const L2capSignalingHeader * l2capsignalingHeader,const uint8_t * data)488 static void ProcessL2capConfigurationRequest(
489     uint8_t type, const AclConnInfo *aclInfo, const L2capSignalingHeader *l2capsignalingHeader, const uint8_t *data)
490 {
491     uint16_t offset = 0;
492     uint16_t *dcid = (uint16_t *)(data + offset);
493     offset += sizeof(uint16_t);
494     offset += sizeof(uint16_t); /* flag */
495 
496     while (offset + sizeof(uint8_t) + sizeof(uint8_t) < l2capsignalingHeader->length) {
497         uint8_t *optionType = (uint8_t *)(data + offset);
498         offset += sizeof(uint8_t);
499         uint8_t *length = (uint8_t *)(data + offset);
500         offset += sizeof(uint8_t);
501         switch (*optionType) {
502             case L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL: {
503                 uint8_t mode = *(uint8_t *)(data + offset);
504                 offset += sizeof(uint8_t);
505 
506                 FilterL2capChannelConnInfo *connInfo = NULL;
507                 if (type == TRANSMISSON_TYPE_H2C_DATA) {
508                     RemoteSearchInfo remoteInfo = {
509                         .aclHandle = aclInfo->handle,
510                         .remoteCid = *dcid,
511                     };
512                     connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByRemoteInfo, &remoteInfo);
513                 } else {
514                     connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByLocalCid, dcid);
515                 }
516 
517                 if (connInfo != NULL) {
518                     connInfo->headerLength =
519                         (mode == L2CAP_BASIC_MODE) ? sizeof(L2capBasicHeader) : sizeof(L2capNotBasicHeader);
520                 }
521             } break;
522             default:
523                 break;
524         }
525         offset += *length;
526     }
527 }
528 
ProcessL2capConfigurationResponse(uint8_t type,const AclConnInfo * aclInfo,const L2capSignalingHeader * l2capsignalingHeader,const uint8_t * data)529 static void ProcessL2capConfigurationResponse(
530     uint8_t type, const AclConnInfo *aclInfo, const L2capSignalingHeader *l2capsignalingHeader, const uint8_t *data)
531 {
532     uint16_t offset = 0;
533     uint16_t *scid = (uint16_t *)(data + offset);
534     offset += sizeof(uint16_t);
535     offset += sizeof(uint16_t); /* flag */
536     offset += sizeof(uint16_t); /* result */
537 
538     if (offset == l2capsignalingHeader->length) {
539         return;
540     }
541 
542     while (offset + sizeof(uint8_t) + sizeof(uint8_t) < l2capsignalingHeader->length) {
543         uint8_t *optionType = (uint8_t *)(data + offset);
544         offset += sizeof(uint8_t);
545         uint8_t *length = (uint8_t *)(data + offset);
546         offset += sizeof(uint8_t);
547         switch (*optionType) {
548             case L2CAP_OPTION_RETRANSMISSION_AND_FLOW_CONTROL: {
549                 uint8_t mode = *(uint8_t *)(data + offset);
550                 offset += sizeof(uint8_t);
551 
552                 FilterL2capChannelConnInfo *connInfo = NULL;
553                 if (type == TRANSMISSON_TYPE_H2C_DATA) {
554                     RemoteSearchInfo remoteInfo = {
555                         .aclHandle = aclInfo->handle,
556                         .remoteCid = *scid,
557                     };
558                     connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByRemoteInfo, &remoteInfo);
559                 } else {
560                     connInfo = ListForEachData(g_filterL2capConnList, FindL2capConnByLocalCid, scid);
561                 }
562 
563                 if (connInfo != NULL) {
564                     connInfo->headerLength =
565                         (mode == L2CAP_BASIC_MODE) ? sizeof(L2capBasicHeader) : sizeof(L2capNotBasicHeader);
566                 }
567             } break;
568             default:
569                 break;
570         }
571         offset += *length;
572     }
573 }
574 
ProcessL2capDisconnectionResponse(uint8_t type,const AclConnInfo * aclInfo,const L2capSignalingHeader * l2capsignalingHeader,const uint8_t * data)575 static void ProcessL2capDisconnectionResponse(
576     uint8_t type, const AclConnInfo *aclInfo, const L2capSignalingHeader *l2capsignalingHeader, const uint8_t *data)
577 {
578     uint16_t offset = 0;
579     uint16_t *dcid = (uint16_t *)(data + offset);
580     offset += sizeof(uint16_t);
581     uint16_t *scid = (uint16_t *)(data + offset);
582     offset += sizeof(uint16_t);
583 
584     if (l2capsignalingHeader->length < offset) {
585         return;
586     }
587 
588     RemoveL2capConnectionByLocalCid((type == TRANSMISSON_TYPE_H2C_DATA) ? *dcid : *scid);
589 }
590 
ProcessL2capConnInfo(uint8_t type,const AclConnInfo * aclInfo,const uint8_t * data,uint16_t originalLength)591 static void ProcessL2capConnInfo(uint8_t type, const AclConnInfo *aclInfo, const uint8_t *data, uint16_t originalLength)
592 {
593     uint16_t offset = 0;
594     L2capBasicHeader *l2capHeader = (L2capBasicHeader *)(data + offset);
595     offset += sizeof(L2capBasicHeader);
596     L2capSignalingHeader *l2capsignalingHeader = (L2capSignalingHeader *)(data + offset);
597     offset += sizeof(L2capSignalingHeader);
598 
599     if (l2capHeader->length < offset) {
600         return;
601     }
602 
603     switch (l2capsignalingHeader->code) {
604         case L2CAP_CONNECTION_REQUEST:
605             ProcessL2capConnectionRequest(type, aclInfo, l2capsignalingHeader, data + offset);
606             break;
607         case L2CAP_CONNECTION_RESPONSE:
608             ProcessL2capConnectionResponse(type, aclInfo, l2capsignalingHeader, data + offset);
609             break;
610         case L2CAP_CONFIGURATION_REQUEST:
611             ProcessL2capConfigurationRequest(type, aclInfo, l2capsignalingHeader, data + offset);
612             break;
613         case L2CAP_CONFIGURATION_RESPONSE:
614             ProcessL2capConfigurationResponse(type, aclInfo, l2capsignalingHeader, data + offset);
615             break;
616         case L2CAP_DISCONNECTION_REQUEST:
617             break;
618         case L2CAP_DISCONNECTION_RESPONSE:
619             ProcessL2capDisconnectionResponse(type, aclInfo, l2capsignalingHeader, data + offset);
620             break;
621         default:
622             break;
623     }
624 }
625 
SearchL2capChannelConnInfo(uint8_t type,uint16_t aclHandle,uint16_t channelId)626 static FilterL2capChannelConnInfo *SearchL2capChannelConnInfo(uint8_t type, uint16_t aclHandle, uint16_t channelId)
627 {
628     if (type == TRANSMISSON_TYPE_H2C_DATA) {
629         RemoteSearchInfo remoteInfo = {
630             .aclHandle = aclHandle,
631             .remoteCid = channelId,
632         };
633         return ListForEachData(g_filterL2capConnList, FindL2capConnByRemoteInfo, &remoteInfo);
634     } else {
635         return ListForEachData(g_filterL2capConnList, FindL2capConnByLocalCid, &channelId);
636     }
637 }
638 
RfcommDlciFilterCheck(uint8_t type,uint16_t aclHandle,uint16_t channelId,uint8_t dlci,uint8_t * module)639 static bool RfcommDlciFilterCheck(uint8_t type, uint16_t aclHandle, uint16_t channelId, uint8_t dlci, uint8_t *module)
640 {
641     AclConnInfo *aclInfo = ListForEachData(g_aclConnList, FindAclConnInfoByHandle, &aclHandle);
642     if (aclInfo == NULL) {
643         return false;
644     }
645 
646     FilterL2capChannelConnInfo *l2capInfo = SearchL2capChannelConnInfo(type, aclHandle, channelId);
647     if (l2capInfo == NULL) {
648         return false;
649     }
650     BtmSnoopFilterInfo cmpinfo = {
651         .isLocal = !(l2capInfo->isInitiator ^ (dlci & 0x01)),
652         .rfcommScn = dlci >> RFCOMM_DLCI_SHIFT_SCN,
653     };
654     if (!cmpinfo.isLocal) {
655         cmpinfo.remoteAddr = aclInfo->addr;
656     }
657     BtmSnoopFilterInfo *btmSnoopFilterInfo =
658         ListForEachData(BtmGetFilterInfoList(), BtmFindFilterInfoByInfoUseScn, &cmpinfo);
659     if (btmSnoopFilterInfo == NULL) {
660         return false;
661     }
662     *module = btmSnoopFilterInfo->module;
663 
664     return true;
665 }
666 
RfcommDataCheckFrameTypeUih(const uint8_t ** data,uint16_t originalLength,FilterL2capChannelConnInfo * l2capInfo,uint16_t * offset,uint16_t * payloadLen)667 static bool RfcommDataCheckFrameTypeUih(const uint8_t **data, uint16_t originalLength,
668     FilterL2capChannelConnInfo *l2capInfo, uint16_t *offset, uint16_t *payloadLen)
669 {
670     *offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength;
671     *offset += sizeof(uint8_t);  // address
672     const uint8_t *frameType = *data + *offset;
673     *offset += sizeof(uint8_t);  // frameType
674 
675     if ((*frameType & (~PF)) != FRAME_TYPE_UIH) {
676         return false;
677     }
678 
679     const uint8_t *length = *data + *offset;
680     if (*length & 0x01) {
681         *offset += sizeof(uint8_t);
682         if (payloadLen != NULL) {
683             *payloadLen = *length >> 1;
684         }
685     } else {
686         *offset += sizeof(uint16_t);
687         if (payloadLen != NULL) {
688             *payloadLen = *(uint16_t *)length >> 1;
689         }
690     }
691 
692     if ((*frameType & (PF)) == PF) {
693         *offset += sizeof(uint8_t); /* credit */
694     }
695     return true;
696 }
697 
RfcommDataFilterUseHfp(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)698 static bool RfcommDataFilterUseHfp(
699     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
700 {
701     uint16_t offset;
702     uint16_t payloadLen;
703     char *filterAT[] = {"CLCC", "CLIP", "CNUM", "BINP", "CCWA"};
704 
705     if (!RfcommDataCheckFrameTypeUih(data, originalLength, l2capInfo, &offset, &payloadLen)) {
706         return false;
707     }
708 
709     if (payloadLen < strlen("AT+")) {
710         return false;
711     }
712 
713     {
714         char *at = "ATD";
715         if (!memcmp(*data + offset, at, strlen(at))) {
716             offset += strlen(at);
717             BtmChangeIncludeLength(includedLength, offset);
718             uint8_t *copyData = BtmCreateFilterBuffer(includedLength, *data);
719             *data = copyData;
720             return true;
721         }
722     }
723 
724     offset += strlen("AT+"); /* "AT+" or "\r\n+" */
725     for (uint8_t ii = 0; ii < sizeof(filterAT) / sizeof(filterAT[0]); ii++) {
726         char *at = filterAT[ii];
727         if (payloadLen < strlen("AT+") + strlen(at)) {
728             continue;
729         } else {
730             if (!memcmp(*data + offset, at, strlen(at))) {
731                 offset += strlen(at) + 1; /* '?' '=' other */
732                 BtmChangeIncludeLength(includedLength, offset);
733                 *data = BtmCreateFilterBuffer(includedLength, *data);
734                 return true;
735             }
736         }
737     }
738     return false;
739 }
740 
RfcommDataFilterUseOther(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)741 static bool RfcommDataFilterUseOther(
742     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
743 {
744     uint16_t offset;
745 
746     if (RfcommDataCheckFrameTypeUih(data, originalLength, l2capInfo, &offset, NULL)) {
747         BtmChangeIncludeLength(includedLength, offset);
748         *data = BtmCreateFilterBuffer(includedLength, *data);
749         return true;
750     }
751     return false;
752 }
753 
IsL2capSignalingChannelData(const uint8_t * data,uint16_t originalLength)754 static bool IsL2capSignalingChannelData(const uint8_t *data, uint16_t originalLength)
755 {
756     uint16_t offset = 0;
757     offset += sizeof(uint16_t);
758     uint16_t channelID = *(uint16_t *)(data + offset);
759     if (channelID == L2CAP_SIGNALING_CHANNEL) {
760         return true;
761     } else {
762         return false;
763     }
764 }
765 
L2capDataFilterUseGoepPbap(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)766 static bool L2capDataFilterUseGoepPbap(
767     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
768 {
769     BtmChangeIncludeLength(includedLength, sizeof(HciAclDataHeader) + l2capInfo->headerLength);
770     *data = BtmCreateFilterBuffer(includedLength, *data);
771     return true;
772 }
773 
L2capDataFilterUseGoepMap(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)774 static bool L2capDataFilterUseGoepMap(
775     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
776 {
777     BtmChangeIncludeLength(includedLength, sizeof(HciAclDataHeader) + l2capInfo->headerLength);
778     *data = BtmCreateFilterBuffer(includedLength, *data);
779     return true;
780 }
781 
L2capDataFilterUseRfcomm(uint8_t type,const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)782 static bool L2capDataFilterUseRfcomm(uint8_t type, const uint8_t **data, uint16_t originalLength,
783     uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
784 {
785     uint16_t offset = 0;
786     HciAclDataHeader *hciHeader = (HciAclDataHeader *)(*data + offset);
787     offset += sizeof(HciAclDataHeader);
788     L2capBasicHeader *l2capHeader = (L2capBasicHeader *)(*data + offset);
789     offset += l2capInfo->headerLength;
790     const uint8_t *address = *data + offset;
791 
792     uint8_t dlci = *address >> RFCOMM_SHIFT_DLCI;
793     if (dlci == 0) {
794         return false;
795     }
796     uint8_t module;
797     bool isFiltered = false;
798 
799     if (RfcommDlciFilterCheck(type, hciHeader->handle, l2capHeader->channelId, dlci, &module)) {
800         switch (module) {
801             case BTM_HCI_LOG_FILTER_MODULE_HFP:
802                 isFiltered = RfcommDataFilterUseHfp(data, originalLength, includedLength, l2capInfo);
803                 break;
804             default:
805                 isFiltered = RfcommDataFilterUseOther(data, originalLength, includedLength, l2capInfo);
806                 break;
807         }
808     } else {
809         isFiltered = RfcommDataFilterUseOther(data, originalLength, includedLength, l2capInfo);
810     }
811     return isFiltered;
812 }
813 
L2capDataFilterUseAvdtp(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)814 static bool L2capDataFilterUseAvdtp(
815     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
816 {
817     uint16_t offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength + AVDT_MEDIA_PACKET_HEADER_LEN;
818 
819     BtmChangeIncludeLength(includedLength, offset);
820     *data = BtmCreateFilterBuffer(includedLength, *data);
821     return true;
822 }
823 
L2capDataFilterUseAvctp(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)824 static bool L2capDataFilterUseAvctp(
825     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
826 {
827     uint16_t offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength + AVCT_PACKET_HEADER_LEN;
828     offset += sizeof(uint8_t) + sizeof(uint8_t);
829     const uint8_t *opCode = *data + offset;
830     offset += sizeof(uint8_t);
831 
832     if (*opCode != 0x00) {
833         return false;
834     }
835 
836     offset += AVRCP_COMPANY_ID;
837     const uint8_t *pduId = *data + offset;
838     offset += sizeof(uint8_t);
839     if (*pduId == AVRCP_PDU_ID_GET_ELEMENT_ATTR) {
840         BtmChangeIncludeLength(includedLength, offset);
841         *data = BtmCreateFilterBuffer(includedLength, *data);
842     }
843 
844     return false;
845 }
846 
L2capDataFilterUseAvctpBrowsing(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)847 static bool L2capDataFilterUseAvctpBrowsing(
848     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
849 {
850     uint16_t offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength + AVCT_PACKET_HEADER_LEN;
851     const uint8_t *pduId = *data + offset;
852     offset += sizeof(uint8_t);
853     if (*pduId == AVRCP_BROWSING_PDU_ID_GET_ITEM_ATTR) {
854         BtmChangeIncludeLength(includedLength, offset);
855         *data = BtmCreateFilterBuffer(includedLength, *data);
856     }
857 
858     return false;
859 }
860 
L2capDataFilterUseAtt(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)861 static bool L2capDataFilterUseAtt(
862     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
863 {
864     uint16_t offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength;
865     const uint8_t *opcode = (*data + offset);
866     offset += sizeof(uint8_t);
867 
868     switch (*opcode) {
869         case FIND_INFORMATION_RESPONSE:
870         case FIND_BY_TYPE_VALUE_REQUEST:
871         case READ_BY_TYPE_RESPONSE:
872         case READ_RESPONSE:
873         case READ_BLOB_RESPONSE:
874         case READ_MULTIPLE_RESPONSE:
875         case READ_BY_GROUP_TYPE_RESPONSE:
876         case WRITE_REQUEST:
877         case WRITE_COMMAND:
878         case PREPARE_WRITE_REQUEST:
879         case PREPARE_WRITE_RESPONSE:
880         case READ_MULTIPLE_VARIABLE_RESPONSE:
881         case MULTIPLE_HANDLE_VALUE_NOTIFICATION:
882         case HANDLE_VALUE_NOTIFICATION:
883         case HANDLE_VALUE_INDICATION:
884         case SIGNED_WRITE_COMMAND:
885             BtmChangeIncludeLength(includedLength, offset);
886             *data = BtmCreateFilterBuffer(includedLength, *data);
887             break;
888         default:
889             return false;
890     }
891 
892     return true;
893 }
894 
L2capDataFilterUseSmp(const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength,FilterL2capChannelConnInfo * l2capInfo)895 static bool L2capDataFilterUseSmp(
896     const uint8_t **data, uint16_t originalLength, uint16_t *includedLength, FilterL2capChannelConnInfo *l2capInfo)
897 {
898     uint16_t offset = sizeof(HciAclDataHeader) + l2capInfo->headerLength;
899     const uint8_t *opcode = (*data + offset);
900     offset += sizeof(uint8_t);
901 
902     switch (*opcode) {
903         case SMP_CODE_PAIRING_CFM:
904         case SMP_CODE_PAIRING_RAND:
905         case SMP_CODE_ENCRYPTION_INFO:
906         case SMP_CODE_MASTER_IDENTITY:
907         case SMP_CODE_IDENTITY_INFO:
908         case SMP_CODE_SIGNING_INFO:
909         case SMP_CODE_PAIRING_PUBLIC_KEY:
910         case SMP_CODE_PAIRING_DHKEY_CHECK:
911             BtmChangeIncludeLength(includedLength, offset);
912             *data = BtmCreateFilterBuffer(includedLength, *data);
913             break;
914         case SMP_CODE_IDENTITY_ADDR_INFO:
915             offset += sizeof(uint8_t);
916             uint8_t *copyData = BtmCreateFilterBuffer(includedLength, *data);
917             BtmFilterAddress(copyData + offset, BT_ADDRESS_SIZE);
918             *data = copyData;
919             break;
920         default:
921             return false;
922     }
923 
924     return true;
925 }
926 
L2capDataCheckFilter(uint8_t type,FilterL2capChannelConnInfo * connInfo,const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength)927 static void L2capDataCheckFilter(uint8_t type, FilterL2capChannelConnInfo *connInfo, const uint8_t **data,
928     uint16_t originalLength, uint16_t *includedLength)
929 {
930     bool isFiltered;
931 
932     switch (connInfo->module) {
933         case BTM_HCI_LOG_FILTER_MODULE_PBAP:
934             isFiltered = L2capDataFilterUseGoepPbap(data, originalLength, includedLength, connInfo);
935             break;
936         case BTM_HCI_LOG_FILTER_MODULE_MAP:
937             isFiltered = L2capDataFilterUseGoepMap(data, originalLength, includedLength, connInfo);
938             break;
939         case BTM_HCI_LOG_FILTER_MODULE_RFCOMM:
940             isFiltered = L2capDataFilterUseRfcomm(type, data, originalLength, includedLength, connInfo);
941             break;
942         case BTM_HCI_LOG_FILTER_MODULE_AVCTP:
943             isFiltered = L2capDataFilterUseAvctp(data, originalLength, includedLength, connInfo);
944             break;
945         case BTM_HCI_LOG_FILTER_MODULE_AVCTP_BROWSING:
946             isFiltered = L2capDataFilterUseAvctpBrowsing(data, originalLength, includedLength, connInfo);
947             break;
948         case BTM_HCI_LOG_FILTER_MODULE_AVDTP:
949             if (connInfo->isAvdtpMedia) {
950                 isFiltered = L2capDataFilterUseAvdtp(data, originalLength, includedLength, connInfo);
951             } else {
952                 isFiltered = false;
953             }
954             break;
955         case BTM_HCI_LOG_FILTER_MODULE_ATT:
956             isFiltered = L2capDataFilterUseAtt(data, originalLength, includedLength, connInfo);
957             break;
958         case BTM_HCI_LOG_FILTER_MODULE_SM:
959             isFiltered = L2capDataFilterUseSmp(data, originalLength, includedLength, connInfo);
960             break;
961         default:
962             return;
963     }
964     SetAclPrevPktIsFiltered(connInfo->acl->handle, isFiltered);
965 }
966 
BtmFilterAclData(uint8_t type,const uint8_t ** data,uint16_t originalLength,uint16_t * includedLength)967 void BtmFilterAclData(uint8_t type, const uint8_t **data, uint16_t originalLength, uint16_t *includedLength)
968 {
969     uint16_t offset = 0;
970     HciAclDataHeader *hciHeader = (HciAclDataHeader *)(*data + offset);
971     offset += sizeof(HciAclDataHeader);
972 
973     uint16_t handle = hciHeader->handle;
974     AclConnInfo *aclInfo = ListForEachData(g_aclConnList, FindAclConnInfoByHandle, &handle);
975     if (aclInfo == NULL) {
976         return;
977     }
978 
979     if (hciHeader->pbFlag == HCI_PACKET_BOUNDARY_CONTINUING) {
980         if (aclInfo->prevPktIsFiltered) {
981             BtmChangeIncludeLength(includedLength, sizeof(HciAclDataHeader));
982             *data = BtmCreateFilterBuffer(includedLength, *data);
983         }
984         return;
985     } else {
986         aclInfo->prevPktIsFiltered = false;
987     }
988 
989     if (IsL2capSignalingChannelData(*data + offset, originalLength)) {
990         ProcessL2capConnInfo(type, aclInfo, *data + offset, originalLength);
991         return;
992     }
993 
994     L2capBasicHeader *l2capHeader = (L2capBasicHeader *)(*data + offset);
995 
996     FilterL2capChannelConnInfo *connInfo = SearchL2capChannelConnInfo(type, hciHeader->handle, l2capHeader->channelId);
997     if (connInfo == NULL) {
998         return;
999     }
1000 
1001     L2capDataCheckFilter(type, connInfo, data, originalLength, includedLength);
1002 }
1003