1 /*
2 * Copyright (C) 2021 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 "chre/pal/util/wifi_pal_convert.h"
18
19 // Constants defining the number of bits per LCI IE field.
20 #define LCI_IE_UNCERTAINTY_BITS 6
21 #define LCI_IE_LAT_LONG_BITS 34
22 #define LCI_IE_ALT_TYPE_BITS 4
23 #define LCI_IE_ALT_BITS 30
24
25 // The LCI subelement ID.
26 #define LCI_SUBELEMENT_ID 0
27
28 /************************************************
29 * Private functions
30 ***********************************************/
31
32 /**
33 * Reverses the bit positions in a byte.
34 *
35 * @param input The input byte.
36 *
37 * @return The output byte with reversed bits.
38 */
reverseBits(uint8_t input)39 static uint8_t reverseBits(uint8_t input) {
40 uint8_t output = 0;
41 for (size_t i = 0; i < 8; i++) {
42 output <<= 1;
43 uint8_t tmp = (uint8_t)(input & 1);
44 output |= tmp;
45 input >>= 1;
46 }
47
48 return output;
49 }
50
51 /**
52 * @param buf A non-null pointer to a buffer.
53 * @param bufferBitOffset The bit offset with respect to the buffer pointer.
54 *
55 * @return The bit value of the desired bit offset.
56 */
getBitAtBitOffsetInByteArray(const uint8_t * buf,size_t bufferBitOffset)57 static uint64_t getBitAtBitOffsetInByteArray(const uint8_t *buf,
58 size_t bufferBitOffset) {
59 size_t index = bufferBitOffset / 8;
60 size_t offsetInByte = bufferBitOffset % 8;
61 return ((buf[index] & 0x80 >> offsetInByte) != 0);
62 }
63
64 /**
65 * Returns the field value of the LCI IE buffer.
66 *
67 * The user must invoke this method in order of the IE data fields, providing
68 * the number of bits the field is encoded as in numBits, and updating
69 * bufferBitPos sequentially.
70 *
71 * @param buf A non-null pointer to a buffer.
72 * @param numBits The number of bits the value is encoded as.
73 * @param bufferBitPos The current bit position. This value will be updated as a
74 * result of this function invocation, and will be incremented by numBits.
75 *
76 * @return The field value.
77 */
getField(const uint8_t * buf,size_t numBits,size_t * bufferBitPos)78 static uint64_t getField(const uint8_t *buf, size_t numBits,
79 size_t *bufferBitPos) {
80 uint64_t field = 0;
81 for (size_t i = 0; i < numBits; i++) {
82 // Per specs, we need to store the bits in MSB first per field,
83 // so we store the bits in reverse order (since we have reverse the bits
84 // per byte earlier).
85 field |= getBitAtBitOffsetInByteArray(buf, *bufferBitPos + i) << i;
86 }
87
88 *bufferBitPos += numBits;
89 return field;
90 }
91
convert34BitTwosComplementToInt64(uint64_t input)92 static int64_t convert34BitTwosComplementToInt64(uint64_t input) {
93 // This is 34 bits, so we need to sign extend
94 if ((input & 0x200000000) != 0) {
95 input |= 0xFFFFFFFC00000000;
96 }
97
98 return (int64_t)input;
99 }
100
convert30BitTwosComplementToInt32(uint32_t input)101 static int32_t convert30BitTwosComplementToInt32(uint32_t input) {
102 // This is 30 bits, so we need to sign extend
103 if ((input & 0x20000000) != 0) {
104 input |= 0xC0000000;
105 }
106
107 return (int32_t)input;
108 }
109
decodeLciSubelement(const uint8_t * lciSubelement,struct chreWifiLci * out)110 static void decodeLciSubelement(const uint8_t *lciSubelement,
111 struct chreWifiLci *out) {
112 uint8_t lciDataTmp[CHRE_LCI_SUBELEMENT_DATA_LEN_BYTES];
113 size_t bufferBitPos = 0;
114 uint64_t x;
115
116 // First, reverse the bits to get the LSB first per specs.
117 for (size_t i = 0; i < CHRE_LCI_SUBELEMENT_DATA_LEN_BYTES; i++) {
118 lciDataTmp[i] = reverseBits(lciSubelement[i]);
119 }
120
121 out->latitudeUncertainty =
122 (uint8_t)getField(lciDataTmp, LCI_IE_UNCERTAINTY_BITS, &bufferBitPos);
123
124 x = getField(lciDataTmp, LCI_IE_LAT_LONG_BITS, &bufferBitPos);
125 out->latitude = convert34BitTwosComplementToInt64(x);
126
127 out->longitudeUncertainty =
128 (uint8_t)getField(lciDataTmp, LCI_IE_UNCERTAINTY_BITS, &bufferBitPos);
129
130 x = getField(lciDataTmp, LCI_IE_LAT_LONG_BITS, &bufferBitPos);
131 out->longitude = convert34BitTwosComplementToInt64(x);
132
133 out->altitudeType =
134 (uint8_t)getField(lciDataTmp, LCI_IE_ALT_TYPE_BITS, &bufferBitPos);
135 out->altitudeUncertainty =
136 (uint8_t)getField(lciDataTmp, LCI_IE_UNCERTAINTY_BITS, &bufferBitPos);
137
138 x = getField(lciDataTmp, LCI_IE_ALT_BITS, &bufferBitPos);
139 out->altitude = convert30BitTwosComplementToInt32((uint32_t)x);
140 }
141
142 /************************************************
143 * Public functions
144 ***********************************************/
chreWifiLciFromIe(const uint8_t * ieData,size_t len,struct chreWifiRangingResult * outResult)145 bool chreWifiLciFromIe(const uint8_t *ieData, size_t len,
146 struct chreWifiRangingResult *outResult) {
147 bool success = false;
148 const size_t kHeaderLen =
149 CHRE_LCI_IE_HEADER_LEN_BYTES + CHRE_LCI_SUBELEMENT_HEADER_LEN_BYTES;
150 if (len >= kHeaderLen) {
151 size_t pos = CHRE_LCI_IE_HEADER_LEN_BYTES;
152
153 uint8_t subelementId = ieData[pos++];
154 uint8_t subelementLength = ieData[pos++];
155 if ((subelementId == LCI_SUBELEMENT_ID) &&
156 (len >= kHeaderLen + subelementLength)) {
157 success = true;
158 if (subelementLength < CHRE_LCI_SUBELEMENT_DATA_LEN_BYTES) {
159 outResult->flags = 0;
160 } else {
161 outResult->flags = CHRE_WIFI_RTT_RESULT_HAS_LCI;
162 decodeLciSubelement(&ieData[pos], &outResult->lci);
163 }
164 }
165 }
166
167 return success;
168 }
169