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{{0x14, 0x09, 0x54, 0xFF, 0xFF, 0x20, 0x42, 0x4C,
27                                                     0x45, 0x05, 0x12, 0xFF, 0x00, 0xE8, 0x03, 0x02,
28                                                     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, size_t position) {
34     auto data_start = ad.begin() + position;
35 
36     // Traxxas - bad name length
37     if ((ad.size() - position) >= 18 && std::equal(data_start, data_start + 3, trx_quirk.begin()) &&
38         std::equal(data_start + 5, data_start + 11, trx_quirk.begin() + 5) &&
39         std::equal(data_start + 12, data_start + 18, trx_quirk.begin() + 12)) {
40       return true;
41     }
42 
43     return false;
44   }
45 
46 public:
RemoveTrailingZeros(std::vector<uint8_t> & ad)47   static void RemoveTrailingZeros(std::vector<uint8_t>& ad) {
48     size_t position = 0;
49 
50     size_t ad_len = ad.size();
51     while (position != ad_len) {
52       uint8_t len = ad[position];
53 
54       // A field length of 0 would be invalid as it should at least contain the
55       // EIR field type. However, some existing devices send zero padding at the
56       // end of advertisement. If this is the case, cut the zero padding from
57       // end of the packet. Otherwise i.e. gluing scan response to advertise
58       // data will result in data with zero padding in the middle.
59       if (len == 0) {
60         ad.erase(ad.begin() + position, ad.end());
61         return;
62       }
63 
64       if (position + len >= ad_len) {
65         return;
66       }
67 
68       position += len + 1;
69     }
70   }
71 
72   /**
73    * Return true if this |ad| represent properly formatted advertising data.
74    */
IsValid(const std::vector<uint8_t> & ad)75   static bool IsValid(const std::vector<uint8_t>& ad) {
76     size_t position = 0;
77 
78     size_t ad_len = ad.size();
79     while (position != ad_len) {
80       uint8_t len = ad[position];
81 
82       // A field length of 0 would be invalid as it should at least contain the
83       // EIR field type. However, some existing devices send zero padding at the
84       // end of advertisement. If this is the case, treat the packet as valid.
85       if (len == 0) {
86         for (size_t i = position + 1; i < ad_len; i++) {
87           if (ad[i] != 0) {
88             return false;
89           }
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)) {
98           return true;
99         }
100 
101         return false;
102       }
103 
104       position += len + 1;
105     }
106 
107     return true;
108   }
109 
110   /**
111    * This function returns a pointer inside the |ad| array of length |ad_len|
112    * where a field of |type| is located, together with its length in |p_length|
113    */
GetFieldByType(const uint8_t * ad,size_t ad_len,uint8_t type,uint8_t * p_length)114   static const uint8_t* GetFieldByType(const uint8_t* ad, size_t ad_len, uint8_t type,
115                                        uint8_t* p_length) {
116     size_t position = 0;
117 
118     while (position != ad_len) {
119       uint8_t len = ad[position];
120 
121       if (len == 0) {
122         break;
123       }
124       if (position + len >= ad_len) {
125         break;
126       }
127 
128       uint8_t adv_type = ad[position + 1];
129 
130       if (adv_type == type) {
131         /* length doesn't include itself */
132         *p_length = len - 1; /* minus the length of type */
133         return ad + position + 2;
134       }
135 
136       position += len + 1; /* skip the length of data */
137     }
138 
139     *p_length = 0;
140     return NULL;
141   }
142 
143   /**
144    * This function returns a pointer inside the |adv| where a field of |type| is
145    * located, together with it' length in |p_length|
146    */
GetFieldByType(std::vector<uint8_t> const & ad,uint8_t type,uint8_t * p_length)147   static const uint8_t* GetFieldByType(std::vector<uint8_t> const& ad, uint8_t type,
148                                        uint8_t* p_length) {
149     return GetFieldByType(ad.data(), ad.size(), type, p_length);
150   }
151 };
152