1 /****************************************************************************** 2 * 3 * Copyright 2017 The Android Open Source Project 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at: 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 * 17 ******************************************************************************/ 18 19 #pragma once 20 21 #include <array> 22 #include <cstdint> 23 #include <vector> 24 25 // Scan Response data from Traxxas 26 static constexpr std::array<uint8_t, 18> trx_quirk{ 27 {0x14, 0x09, 0x54, 0xFF, 0xFF, 0x20, 0x42, 0x4C, 0x45, 0x05, 0x12, 0xFF, 28 0x00, 0xE8, 0x03, 0x02, 0x0A, 0x00}}; 29 30 class AdvertiseDataParser { 31 // Return true if the packet is malformed, but should be considered valid for 32 // compatibility with already existing devices MalformedPacketQuirk(const std::vector<uint8_t> & ad,size_t position)33 static bool MalformedPacketQuirk(const std::vector<uint8_t>& ad, 34 size_t position) { 35 auto data_start = ad.begin() + position; 36 37 // Traxxas - bad name length 38 if ((ad.size() - position) >= 18 && 39 std::equal(data_start, data_start + 3, trx_quirk.begin()) && 40 std::equal(data_start + 5, data_start + 11, trx_quirk.begin() + 5) && 41 std::equal(data_start + 12, data_start + 18, trx_quirk.begin() + 12)) { 42 return true; 43 } 44 45 return false; 46 } 47 48 public: RemoveTrailingZeros(std::vector<uint8_t> & ad)49 static void RemoveTrailingZeros(std::vector<uint8_t>& ad) { 50 size_t position = 0; 51 52 size_t ad_len = ad.size(); 53 while (position != ad_len) { 54 uint8_t len = ad[position]; 55 56 // A field length of 0 would be invalid as it should at least contain the 57 // EIR field type. However, some existing devices send zero padding at the 58 // end of advertisement. If this is the case, cut the zero padding from 59 // end of the packet. Otherwise i.e. gluing scan response to advertise 60 // data will result in data with zero padding in the middle. 61 if (len == 0) { 62 ad.erase(ad.begin() + position, ad.end()); 63 return; 64 } 65 66 if (position + len >= ad_len) { 67 return; 68 } 69 70 position += len + 1; 71 } 72 } 73 74 /** 75 * Return true if this |ad| represent properly formatted advertising data. 76 */ IsValid(const std::vector<uint8_t> & ad)77 static bool IsValid(const std::vector<uint8_t>& ad) { 78 size_t position = 0; 79 80 size_t ad_len = ad.size(); 81 while (position != ad_len) { 82 uint8_t len = ad[position]; 83 84 // A field length of 0 would be invalid as it should at least contain the 85 // EIR field type. However, some existing devices send zero padding at the 86 // end of advertisement. If this is the case, treat the packet as valid. 87 if (len == 0) { 88 for (size_t i = position + 1; i < ad_len; i++) { 89 if (ad[i] != 0) return false; 90 } 91 return true; 92 } 93 94 // If the length of the current field would exceed the total data length, 95 // then the data is badly formatted. 96 if (position + len >= ad_len) { 97 if (MalformedPacketQuirk(ad, position)) return true; 98 99 return false; 100 } 101 102 position += len + 1; 103 } 104 105 return true; 106 } 107 108 /** 109 * This function returns a pointer inside the |ad| array of length |ad_len| 110 * where a field of |type| is located, together with its length in |p_length| 111 */ GetFieldByType(const uint8_t * ad,size_t ad_len,uint8_t type,uint8_t * p_length)112 static const uint8_t* GetFieldByType(const uint8_t* ad, size_t ad_len, 113 uint8_t type, uint8_t* p_length) { 114 size_t position = 0; 115 116 while (position != ad_len) { 117 uint8_t len = ad[position]; 118 119 if (len == 0) break; 120 if (position + len >= ad_len) break; 121 122 uint8_t adv_type = ad[position + 1]; 123 124 if (adv_type == type) { 125 /* length doesn't include itself */ 126 *p_length = len - 1; /* minus the length of type */ 127 return ad + position + 2; 128 } 129 130 position += len + 1; /* skip the length of data */ 131 } 132 133 *p_length = 0; 134 return NULL; 135 } 136 137 /** 138 * This function returns a pointer inside the |adv| where a field of |type| is 139 * located, together with it' length in |p_length| 140 */ GetFieldByType(std::vector<uint8_t> const & ad,uint8_t type,uint8_t * p_length)141 static const uint8_t* GetFieldByType(std::vector<uint8_t> const& ad, 142 uint8_t type, uint8_t* p_length) { 143 return GetFieldByType(ad.data(), ad.size(), type, p_length); 144 } 145 }; 146