1 /*
2 * Copyright 2022 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include "hci/pcap_filter.h"
18
19 #include <packet_runtime.h>
20
21 #include <algorithm>
22 #include <array>
23 #include <cstddef>
24 #include <cstdint>
25 #include <cstdio>
26 #include <cstring>
27 #include <memory>
28 #include <utility>
29 #include <vector>
30
31 #include "log.h"
32 #include "packets/hci_packets.h"
33
34 using namespace bluetooth::hci;
35
36 namespace rootcanal {
37
create_packet_view(std::vector<uint8_t> const & packet)38 static pdl::packet::slice create_packet_view(
39 std::vector<uint8_t> const& packet) {
40 // Wrap the reference to the packet in a shared_ptr with created
41 // a no-op deleter. The packet view will be short lived so there is no
42 // risk of the reference leaking.
43 return pdl::packet::slice(std::shared_ptr<std::vector<uint8_t> const>(
44 &packet, [](std::vector<uint8_t> const* /* ptr */) {}));
45 }
46
47 static std::vector<uint8_t> FilterHciAcl(std::vector<uint8_t> const& packet);
48 static std::vector<uint8_t> FilterHciSco(std::vector<uint8_t> const& packet);
49 static std::vector<uint8_t> FilterHciIso(std::vector<uint8_t> const& packet);
50
FilterHciPacket(std::vector<uint8_t> const & packet,uint8_t idc)51 std::vector<uint8_t> PcapFilter::FilterHciPacket(
52 std::vector<uint8_t> const& packet, uint8_t idc) {
53 switch (idc) {
54 case 0x1:
55 return FilterHciCommand(packet);
56 case 0x2:
57 return FilterHciAcl(packet);
58 case 0x3:
59 return FilterHciSco(packet);
60 case 0x4:
61 return FilterHciEvent(packet);
62 case 0x5:
63 return FilterHciIso(packet);
64 default:
65 break;
66 }
67 return std::vector<uint8_t>(packet);
68 }
69
FilterHciCommand(std::vector<uint8_t> const & packet)70 std::vector<uint8_t> PcapFilter::FilterHciCommand(
71 std::vector<uint8_t> const& packet) {
72 auto command = CommandView::Create(create_packet_view(packet));
73 ASSERT(command.IsValid());
74 switch (command.GetOpCode()) {
75 case OpCode::WRITE_LOCAL_NAME:
76 return FilterWriteLocalName(command);
77 case OpCode::WRITE_EXTENDED_INQUIRY_RESPONSE:
78 return FilterWriteExtendedInquiryResponse(command);
79 case OpCode::LE_SET_ADVERTISING_DATA:
80 return FilterLeSetAdvertisingData(command);
81 case OpCode::LE_SET_SCAN_RESPONSE_DATA:
82 return FilterLeSetScanResponseData(command);
83 case OpCode::LE_SET_EXTENDED_ADVERTISING_DATA:
84 return FilterLeSetExtendedAdvertisingData(command);
85 case OpCode::LE_SET_EXTENDED_SCAN_RESPONSE_DATA:
86 return FilterLeSetExtendedScanResponseData(command);
87 case OpCode::LE_SET_PERIODIC_ADVERTISING_DATA:
88 return FilterLeSetPeriodicAdvertisingData(command);
89 default:
90 break;
91 }
92 return std::vector<uint8_t>(packet);
93 }
94
FilterHciEvent(std::vector<uint8_t> const & packet)95 std::vector<uint8_t> PcapFilter::FilterHciEvent(
96 std::vector<uint8_t> const& packet) {
97 auto event = EventView::Create(create_packet_view(packet));
98 ASSERT(event.IsValid());
99 switch (event.GetEventCode()) {
100 case EventCode::LE_META_EVENT: {
101 auto le_meta_event = LeMetaEventView::Create(event);
102 ASSERT(le_meta_event.IsValid());
103 switch (le_meta_event.GetSubeventCode()) {
104 case SubeventCode::ADVERTISING_REPORT:
105 return FilterLeAdvertisingReport(le_meta_event);
106 case SubeventCode::EXTENDED_ADVERTISING_REPORT:
107 return FilterLeExtendedAdvertisingReport(le_meta_event);
108 default:
109 break;
110 }
111 break;
112 }
113 case EventCode::COMMAND_COMPLETE: {
114 auto command_complete = CommandCompleteView::Create(event);
115 ASSERT(command_complete.IsValid());
116 switch (command_complete.GetCommandOpCode()) {
117 case OpCode::READ_LOCAL_NAME:
118 return FilterReadLocalNameComplete(command_complete);
119 case OpCode::READ_EXTENDED_INQUIRY_RESPONSE:
120 return FilterReadExtendedInquiryResponseComplete(command_complete);
121 default:
122 break;
123 }
124 break;
125 }
126 case EventCode::REMOTE_NAME_REQUEST_COMPLETE:
127 return FilterRemoteNameRequestComplete(event);
128 case EventCode::EXTENDED_INQUIRY_RESULT:
129 return FilterExtendedInquiryResult(event);
130 default:
131 break;
132 }
133 return std::vector<uint8_t>(packet);
134 }
135
FilterHciAcl(std::vector<uint8_t> const & packet)136 static std::vector<uint8_t> FilterHciAcl(std::vector<uint8_t> const& packet) {
137 auto acl = AclView::Create(create_packet_view(packet));
138 std::vector<uint8_t> payload;
139 payload.resize(acl.GetPayload().size());
140 ASSERT(acl.IsValid());
141 return AclBuilder::Create(acl.GetHandle(), acl.GetPacketBoundaryFlag(),
142 acl.GetBroadcastFlag(), std::move(payload))
143 ->SerializeToBytes();
144 }
145
FilterHciSco(std::vector<uint8_t> const & packet)146 static std::vector<uint8_t> FilterHciSco(std::vector<uint8_t> const& packet) {
147 auto sco = ScoView::Create(create_packet_view(packet));
148 std::vector<uint8_t> data;
149 data.resize(sco.GetData().size());
150 ASSERT(sco.IsValid());
151 return ScoBuilder::Create(sco.GetHandle(), sco.GetPacketStatusFlag(), data)
152 ->SerializeToBytes();
153 }
154
FilterHciIso(std::vector<uint8_t> const & packet)155 static std::vector<uint8_t> FilterHciIso(std::vector<uint8_t> const& packet) {
156 auto iso = IsoView::Create(create_packet_view(packet));
157 std::vector<uint8_t> payload;
158 payload.resize(iso.GetPayload().size());
159 ASSERT(iso.IsValid());
160 return IsoBuilder::Create(iso.GetConnectionHandle(), iso.GetPbFlag(),
161 iso.GetTsFlag(), std::move(payload))
162 ->SerializeToBytes();
163 }
164
165 // Replace device names in GAP entries.
166 // TODO: extended advertising reports can be chunked across multiple
167 // events, and a single GAP data entry can be segmented in two.
168 // The filter should account for that and keep a state for partial
169 // GAP entries.
FilterGapData(uint8_t * gap_data,size_t gap_data_len)170 void PcapFilter::FilterGapData(uint8_t* gap_data, size_t gap_data_len) {
171 size_t offset = 0;
172 while ((offset + 2) <= gap_data_len) {
173 size_t length = gap_data[offset];
174 GapDataType data_type = static_cast<GapDataType>(gap_data[offset + 1]);
175
176 // Truncated entry.
177 if ((offset + length + 1) > gap_data_len) {
178 break;
179 }
180
181 // Empty entry.
182 if (length == 0) {
183 offset += 1;
184 continue;
185 }
186
187 // Apply the filter to entries that contain user data.
188 switch (data_type) {
189 case GapDataType::COMPLETE_LOCAL_NAME:
190 case GapDataType::SHORTENED_LOCAL_NAME: {
191 auto start_pos = gap_data + offset + 1;
192 auto end_pos = gap_data + offset + length;
193 std::vector<uint8_t> new_name =
194 ChangeDeviceName(std::vector<uint8_t>{start_pos, end_pos});
195 std::copy(new_name.begin(), new_name.end(), start_pos);
196 break;
197 }
198 default:
199 break;
200 }
201
202 offset += length + 1;
203 }
204 }
205
FilterGapData(std::vector<uint8_t> & gap_data)206 void PcapFilter::FilterGapData(std::vector<uint8_t>& gap_data) {
207 FilterGapData(gap_data.data(), gap_data.size());
208 }
209
210 // Replace the local device name.
FilterWriteLocalName(CommandView & command)211 std::vector<uint8_t> PcapFilter::FilterWriteLocalName(CommandView& command) {
212 auto parameters = WriteLocalNameView::Create(command);
213 ASSERT(parameters.IsValid());
214
215 std::array<uint8_t, 248> local_name =
216 ChangeDeviceName(parameters.GetLocalName());
217 return WriteLocalNameBuilder::Create(local_name)->SerializeToBytes();
218 }
219
220 // Replace the device names in the GAP entries of the extended inquiry response.
FilterWriteExtendedInquiryResponse(CommandView & command)221 std::vector<uint8_t> PcapFilter::FilterWriteExtendedInquiryResponse(
222 CommandView& command) {
223 auto parameters = WriteExtendedInquiryResponseView::Create(command);
224 ASSERT(parameters.IsValid());
225
226 std::array<uint8_t, 240> extended_inquiry_response =
227 parameters.GetExtendedInquiryResponse();
228 FilterGapData(extended_inquiry_response.data(),
229 extended_inquiry_response.size());
230 return WriteExtendedInquiryResponseBuilder::Create(
231 parameters.GetFecRequired(), extended_inquiry_response)
232 ->SerializeToBytes();
233 }
234
235 // Replace the device names in the GAP entries of the advertising data.
FilterLeSetAdvertisingData(CommandView & command)236 std::vector<uint8_t> PcapFilter::FilterLeSetAdvertisingData(
237 CommandView& command) {
238 auto parameters = LeSetAdvertisingDataView::Create(command);
239 ASSERT(parameters.IsValid());
240
241 std::vector<uint8_t> advertising_data = parameters.GetAdvertisingData();
242 FilterGapData(advertising_data);
243 return LeSetAdvertisingDataBuilder::Create(advertising_data)
244 ->SerializeToBytes();
245 }
246
247 // Replace the device names in the GAP entries of the scan response data.
FilterLeSetScanResponseData(CommandView & command)248 std::vector<uint8_t> PcapFilter::FilterLeSetScanResponseData(
249 CommandView& command) {
250 auto parameters = LeSetScanResponseDataView::Create(command);
251 ASSERT(parameters.IsValid());
252
253 std::vector<uint8_t> advertising_data = parameters.GetAdvertisingData();
254 FilterGapData(advertising_data);
255 return LeSetScanResponseDataBuilder::Create(advertising_data)
256 ->SerializeToBytes();
257 }
258
259 // Replace the device names in the GAP entries of the extended advertising data.
FilterLeSetExtendedAdvertisingData(CommandView & command)260 std::vector<uint8_t> PcapFilter::FilterLeSetExtendedAdvertisingData(
261 CommandView& command) {
262 auto parameters = LeSetExtendedAdvertisingDataView::Create(command);
263 ASSERT(parameters.IsValid());
264
265 std::vector<uint8_t> advertising_data = parameters.GetAdvertisingData();
266 FilterGapData(advertising_data);
267 return LeSetExtendedAdvertisingDataBuilder::Create(
268 parameters.GetAdvertisingHandle(), parameters.GetOperation(),
269 parameters.GetFragmentPreference(), advertising_data)
270 ->SerializeToBytes();
271 }
272
273 // Replace the device names in the GAP entries of the extended scan response
274 // data.
FilterLeSetExtendedScanResponseData(CommandView & command)275 std::vector<uint8_t> PcapFilter::FilterLeSetExtendedScanResponseData(
276 CommandView& command) {
277 auto parameters = LeSetExtendedScanResponseDataView::Create(command);
278 ASSERT(parameters.IsValid());
279
280 std::vector<uint8_t> advertising_data = parameters.GetScanResponseData();
281 FilterGapData(advertising_data);
282 return LeSetExtendedScanResponseDataBuilder::Create(
283 parameters.GetAdvertisingHandle(), parameters.GetOperation(),
284 parameters.GetFragmentPreference(), advertising_data)
285 ->SerializeToBytes();
286 }
287
288 // Replace the device names in the GAP entries of the periodic advertising
289 // data.
FilterLeSetPeriodicAdvertisingData(bluetooth::hci::CommandView & command)290 std::vector<uint8_t> PcapFilter::FilterLeSetPeriodicAdvertisingData(
291 bluetooth::hci::CommandView& command) {
292 auto parameters = LeSetPeriodicAdvertisingDataView::Create(command);
293 ASSERT(parameters.IsValid());
294
295 std::vector<uint8_t> advertising_data = parameters.GetAdvertisingData();
296 FilterGapData(advertising_data);
297 return LeSetPeriodicAdvertisingDataBuilder::Create(
298 parameters.GetAdvertisingHandle(), parameters.GetOperation(),
299 advertising_data)
300 ->SerializeToBytes();
301 }
302
303 // Replace the local device name in the read local name complete event.
FilterReadLocalNameComplete(bluetooth::hci::CommandCompleteView & command_complete)304 std::vector<uint8_t> PcapFilter::FilterReadLocalNameComplete(
305 bluetooth::hci::CommandCompleteView& command_complete) {
306 auto parameters = ReadLocalNameCompleteView::Create(command_complete);
307 ASSERT(parameters.IsValid());
308
309 std::array<uint8_t, 248> local_name = parameters.GetLocalName();
310 if (parameters.GetStatus() == ErrorCode::SUCCESS) {
311 local_name = ChangeDeviceName(local_name);
312 }
313
314 return ReadLocalNameCompleteBuilder::Create(
315 parameters.GetNumHciCommandPackets(), parameters.GetStatus(),
316 local_name)
317 ->SerializeToBytes();
318 }
319
320 // Replace the device names in the GAP entries of the extended inquiry response.
FilterReadExtendedInquiryResponseComplete(bluetooth::hci::CommandCompleteView & command_complete)321 std::vector<uint8_t> PcapFilter::FilterReadExtendedInquiryResponseComplete(
322 bluetooth::hci::CommandCompleteView& command_complete) {
323 auto parameters =
324 ReadExtendedInquiryResponseCompleteView::Create(command_complete);
325 ASSERT(parameters.IsValid());
326
327 std::array<uint8_t, 240> extended_inquiry_response =
328 parameters.GetExtendedInquiryResponse();
329 if (parameters.GetStatus() == ErrorCode::SUCCESS) {
330 FilterGapData(extended_inquiry_response.data(),
331 extended_inquiry_response.size());
332 }
333
334 return ReadExtendedInquiryResponseCompleteBuilder::Create(
335 parameters.GetNumHciCommandPackets(), parameters.GetStatus(),
336 parameters.GetFecRequired(), extended_inquiry_response)
337 ->SerializeToBytes();
338 }
339
340 // Replace the remote device name in the remote name request complete event.
FilterRemoteNameRequestComplete(bluetooth::hci::EventView & event)341 std::vector<uint8_t> PcapFilter::FilterRemoteNameRequestComplete(
342 bluetooth::hci::EventView& event) {
343 auto parameters = RemoteNameRequestCompleteView::Create(event);
344 ASSERT(parameters.IsValid());
345
346 std::array<uint8_t, 248> remote_name = parameters.GetRemoteName();
347 if (parameters.GetStatus() == ErrorCode::SUCCESS) {
348 remote_name = ChangeDeviceName(remote_name);
349 }
350
351 return RemoteNameRequestCompleteBuilder::Create(
352 parameters.GetStatus(), parameters.GetBdAddr(), remote_name)
353 ->SerializeToBytes();
354 }
355
356 // Replace the device names in the GAP entries in the extended inquiry result.
FilterExtendedInquiryResult(bluetooth::hci::EventView & event)357 std::vector<uint8_t> PcapFilter::FilterExtendedInquiryResult(
358 bluetooth::hci::EventView& event) {
359 auto parameters = ExtendedInquiryResultView::Create(event);
360 ASSERT(parameters.IsValid());
361
362 std::array<uint8_t, 240> extended_inquiry_response =
363 parameters.GetExtendedInquiryResponse();
364 FilterGapData(extended_inquiry_response.data(),
365 extended_inquiry_response.size());
366 return ExtendedInquiryResultBuilder::Create(
367 parameters.GetAddress(), parameters.GetPageScanRepetitionMode(),
368 parameters.GetClassOfDevice(), parameters.GetClockOffset(),
369 parameters.GetRssi(), extended_inquiry_response)
370 ->SerializeToBytes();
371 }
372
373 // Replace the device names in the GAP entries in the advertising report.
FilterLeAdvertisingReport(bluetooth::hci::LeMetaEventView & event)374 std::vector<uint8_t> PcapFilter::FilterLeAdvertisingReport(
375 bluetooth::hci::LeMetaEventView& event) {
376 auto parameters = LeAdvertisingReportView::Create(event);
377 ASSERT(parameters.IsValid());
378
379 std::vector<LeAdvertisingResponse> responses = parameters.GetResponses();
380 for (auto& response : responses) {
381 FilterGapData(response.advertising_data_);
382 }
383
384 return LeAdvertisingReportBuilder::Create(responses)->SerializeToBytes();
385 }
386
387 // Replace the device names in the GAP entries in the extended advertising
388 // report.
FilterLeExtendedAdvertisingReport(bluetooth::hci::LeMetaEventView & event)389 std::vector<uint8_t> PcapFilter::FilterLeExtendedAdvertisingReport(
390 bluetooth::hci::LeMetaEventView& event) {
391 auto parameters = LeExtendedAdvertisingReportView::Create(event);
392 ASSERT(parameters.IsValid());
393
394 std::vector<LeExtendedAdvertisingResponse> responses =
395 parameters.GetResponses();
396 for (auto& response : responses) {
397 FilterGapData(response.advertising_data_);
398 }
399
400 return LeExtendedAdvertisingReportBuilder::Create(responses)
401 ->SerializeToBytes();
402 }
403
404 // Generate a device name of the specified length.
405 // device_nr is a unique identifier used for the generation.
406 // padded indicates if the name should be padded to length with
407 // spaces.
generate_device_name(size_t device_nr,size_t device_name_len,bool padded)408 static std::vector<uint8_t> generate_device_name(size_t device_nr,
409 size_t device_name_len,
410 bool padded) {
411 std::vector<uint8_t> output;
412 output.resize(device_name_len + 1);
413 int written_len = std::snprintf(reinterpret_cast<char*>(output.data()),
414 output.size(), "#%02zu device", device_nr);
415 // Remove the null terminator, not used for the device name
416 // since it is framed in most cases.
417 output.resize(device_name_len);
418 // Pad the device name with spaces.
419 if (padded && written_len >= 0 && written_len < (int)output.size()) {
420 std::memset(&output[written_len], ' ', output.size() - written_len);
421 }
422 return output;
423 }
424
ChangeDeviceName(std::vector<uint8_t> const & device_name)425 std::vector<uint8_t> PcapFilter::ChangeDeviceName(
426 std::vector<uint8_t> const& device_name) {
427 for (auto const& [old_device_name, new_device_name] : device_name_map) {
428 if (old_device_name == device_name) {
429 return std::vector<uint8_t>(new_device_name);
430 }
431 }
432
433 std::vector<uint8_t> new_device_name =
434 generate_device_name(device_name_map.size(), device_name.size(), true);
435 device_name_map.push_back(std::pair{
436 std::vector<uint8_t>(device_name),
437 new_device_name,
438 });
439 return new_device_name;
440 }
441
ChangeDeviceName(std::array<uint8_t,248> const & device_name)442 std::array<uint8_t, 248> PcapFilter::ChangeDeviceName(
443 std::array<uint8_t, 248> const& device_name) {
444 for (auto const& [old_device_name, new_device_name] : device_name_map) {
445 if (std::equal(old_device_name.begin(), old_device_name.end(),
446 device_name.begin(), device_name.end())) {
447 std::array<uint8_t, 248> out_device_name{};
448 std::copy(new_device_name.begin(), new_device_name.end(),
449 out_device_name.begin());
450 return out_device_name;
451 }
452 }
453
454 std::vector<uint8_t> new_device_name =
455 generate_device_name(device_name_map.size(), device_name.size(), false);
456 std::array<uint8_t, 248> out_device_name{};
457 std::copy(new_device_name.begin(), new_device_name.end(),
458 out_device_name.begin());
459 device_name_map.push_back(std::pair{
460 std::vector<uint8_t>(device_name.begin(), device_name.end()),
461 std::move(new_device_name),
462 });
463 return out_device_name;
464 }
465
466 } // namespace rootcanal
467