stack/include/advertise_data_parser.h

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