dynamic_sensor: Support non-8-bit sized HID report items.

Bug: 207008609
Test: Verified dynamic sensor sampling.
Test: Verified unit tests pass.
Change-Id: I797ccabadefa0dde59fd0f3e8541f9853d657f3f

8 files changed, 236 insertions, 43 deletions

diff --git a/modules/sensors/dynamic_sensor/HidRawSensor.cpp b/modules/sensors/dynamic_sensor/HidRawSensor.cpp
index 91aed0a5..531ab9d5 100644
--- a/modules/sensors/dynamic_sensor/HidRawSensor.cpp
+++ b/modules/sensors/dynamic_sensor/HidRawSensor.cpp
@@ -19,6 +19,8 @@
 #include <utils/Errors.h>
 #include "HidLog.h"
 
+#include <HidUtils.h>
+
 #include <algorithm>
 #include <cfloat>
 #include <codecvt>
@@ -792,13 +794,12 @@ bool HidRawSensor::findSensorControlUsage(const std::vector<HidParser::ReportPac
     const HidParser::ReportItem *reportingState
             = find(packets, REPORTING_STATE, HidParser::REPORT_TYPE_FEATURE);
 
-    if (reportingState == nullptr
-            || !reportingState->isByteAligned()
-            || reportingState->bitSize != 8) {
+    if (reportingState == nullptr) {
         LOG_W << "Cannot find valid reporting state feature" << LOG_ENDL;
     } else {
         mReportingStateId = reportingState->id;
-        mReportingStateOffset = reportingState->bitOffset / 8;
+        mReportingStateBitOffset = reportingState->bitOffset;
+        mReportingStateBitSize = reportingState->bitSize;
 
         mReportingStateDisableIndex = -1;
         mReportingStateEnableIndex = -1;
@@ -824,13 +825,12 @@ bool HidRawSensor::findSensorControlUsage(const std::vector<HidParser::ReportPac
     //POWER_STATE
     const HidParser::ReportItem *powerState
             = find(packets, POWER_STATE, HidParser::REPORT_TYPE_FEATURE);
-    if (powerState == nullptr
-            || !powerState->isByteAligned()
-            || powerState->bitSize != 8) {
+    if (powerState == nullptr) {
         LOG_W << "Cannot find valid power state feature" << LOG_ENDL;
     } else {
         mPowerStateId = powerState->id;
-        mPowerStateOffset = powerState->bitOffset / 8;
+        mPowerStateBitOffset = powerState->bitOffset;
+        mPowerStateBitSize = powerState->bitSize;
 
         mPowerStateOffIndex = -1;
         mPowerStateOnIndex = -1;
@@ -857,14 +857,12 @@ bool HidRawSensor::findSensorControlUsage(const std::vector<HidParser::ReportPac
     const HidParser::ReportItem *reportInterval
             = find(packets, REPORT_INTERVAL, HidParser::REPORT_TYPE_FEATURE);
     if (reportInterval == nullptr
-            || !reportInterval->isByteAligned()
-            || reportInterval->minRaw < 0
-            || (reportInterval->bitSize != 16 && reportInterval->bitSize != 32)) {
+            || reportInterval->minRaw < 0) {
         LOG_W << "Cannot find valid report interval feature" << LOG_ENDL;
     } else {
         mReportIntervalId = reportInterval->id;
-        mReportIntervalOffset = reportInterval->bitOffset / 8;
-        mReportIntervalSize = reportInterval->bitSize / 8;
+        mReportIntervalBitOffset = reportInterval->bitOffset;
+        mReportIntervalBitSize = reportInterval->bitSize;
 
         mFeatureInfo.minDelay = std::max(static_cast<int64_t>(1), reportInterval->minRaw) * 1000;
         mFeatureInfo.maxDelay = std::min(static_cast<int64_t>(1000000),
@@ -899,9 +897,11 @@ int HidRawSensor::enable(bool enable) {
         setPowerOk = false;
         uint8_t id = static_cast<uint8_t>(mPowerStateId);
         if (device->getFeature(id, &buffer)
-                && buffer.size() > mPowerStateOffset) {
-            buffer[mPowerStateOffset] =
-                    enable ? mPowerStateOnIndex : mPowerStateOffIndex;
+                && (8 * buffer.size()) >=
+                   (mPowerStateBitOffset + mPowerStateBitSize)) {
+            uint8_t index = enable ? mPowerStateOnIndex : mPowerStateOffIndex;
+            HidUtil::copyBits(&index, &(buffer[0]), buffer.size(),
+                              0, mPowerStateBitOffset, mPowerStateBitSize);
             setPowerOk = device->setFeature(id, buffer);
         } else {
             LOG_E << "enable: changing POWER STATE failed" << LOG_ENDL;
@@ -913,10 +913,12 @@ int HidRawSensor::enable(bool enable) {
         setReportingOk = false;
         uint8_t id = static_cast<uint8_t>(mReportingStateId);
         if (device->getFeature(id, &buffer)
-                && buffer.size() > mReportingStateOffset) {
-            buffer[mReportingStateOffset]
-                    = enable ? mReportingStateEnableIndex :
-                               mReportingStateDisableIndex;
+                && (8 * buffer.size()) >
+                   (mReportingStateBitOffset + mReportingStateBitSize)) {
+            uint8_t index = enable ? mReportingStateEnableIndex :
+                                     mReportingStateDisableIndex;
+            HidUtil::copyBits(&index, &(buffer[0]), buffer.size(),0,
+                              mReportingStateBitOffset, mReportingStateBitSize);
             setReportingOk = device->setFeature(id, buffer);
         } else {
             LOG_E << "enable: changing REPORTING STATE failed" << LOG_ENDL;
@@ -949,22 +951,15 @@ int HidRawSensor::batch(int64_t samplingPeriod, int64_t batchingPeriod) {
         ok = false;
         uint8_t id = static_cast<uint8_t>(mReportIntervalId);
         if (device->getFeature(id, &buffer)
-                && buffer.size() >= mReportIntervalOffset + mReportIntervalSize) {
+                && (8 * buffer.size()) >=
+                   (mReportIntervalBitOffset + mReportIntervalBitSize)) {
             int64_t periodMs = samplingPeriod / 1000000; //ns -> ms
-            switch (mReportIntervalSize) {
-                case sizeof(uint16_t):
-                    periodMs = std::min(periodMs, static_cast<int64_t>(UINT16_MAX));
-                    buffer[mReportIntervalOffset] = periodMs & 0xFF;
-                    buffer[mReportIntervalOffset + 1] = (periodMs >> 8) & 0xFF;
-                    break;
-                case sizeof(uint32_t):
-                    periodMs = std::min(periodMs, static_cast<int64_t>(UINT32_MAX));
-                    buffer[mReportIntervalOffset] = periodMs & 0xFF;
-                    buffer[mReportIntervalOffset + 1] = (periodMs >> 8) & 0xFF;
-                    buffer[mReportIntervalOffset + 2] = (periodMs >> 16) & 0xFF;
-                    buffer[mReportIntervalOffset + 3] = (periodMs >> 24) & 0xFF;
-                    break;
-            }
+            int64_t maxPeriodMs =
+                (1LL << std::min(mReportIntervalBitSize, 63U)) - 1;
+            periodMs = std::min(periodMs, maxPeriodMs);
+            HidUtil::copyBits(&periodMs, &(buffer[0]), buffer.size(),
+                              0, mReportIntervalBitOffset,
+                              mReportIntervalBitSize);
             ok = device->setFeature(id, buffer);
         }
     }
@@ -1057,7 +1052,8 @@ std::string HidRawSensor::dump() const {
     ss << "  Power state ";
     if (mPowerStateId >= 0) {
         ss << "found, id: " << mPowerStateId
-              << " offset: " << mPowerStateOffset
+              << " bit offset: " << mPowerStateBitOffset
+              << " bit size: " << mPowerStateBitSize
               << " power off index: " << mPowerStateOffIndex
               << " power on index: " << mPowerStateOnIndex
               << LOG_ENDL;
@@ -1068,7 +1064,8 @@ std::string HidRawSensor::dump() const {
     ss << "  Reporting state ";
     if (mReportingStateId >= 0) {
         ss << "found, id: " << mReportingStateId
-              << " offset: " << mReportingStateOffset
+              << " bit offset: " << mReportingStateBitOffset
+              << " bit size: " << mReportingStateBitSize
               << " disable index: " << mReportingStateDisableIndex
               << " enable index: " << mReportingStateEnableIndex
               << LOG_ENDL;
@@ -1079,8 +1076,8 @@ std::string HidRawSensor::dump() const {
     ss << "  Report interval ";
     if (mReportIntervalId >= 0) {
         ss << "found, id: " << mReportIntervalId
-              << " offset: " << mReportIntervalOffset
-              << " size: " << mReportIntervalSize << LOG_ENDL;
+              << " bit offset: " << mReportIntervalBitOffset
+              << " bit size: " << mReportIntervalBitSize << LOG_ENDL;
     } else {
         ss << "not found" << LOG_ENDL;
     }
diff --git a/modules/sensors/dynamic_sensor/HidRawSensor.h b/modules/sensors/dynamic_sensor/HidRawSensor.h
index 201e72e9..99ddfe30 100644
--- a/modules/sensors/dynamic_sensor/HidRawSensor.h
+++ b/modules/sensors/dynamic_sensor/HidRawSensor.h
@@ -137,18 +137,20 @@ private:
 
     // Features for control sensor
     int mReportingStateId;
-    unsigned int mReportingStateOffset;
+    unsigned int mReportingStateBitOffset;
+    unsigned int mReportingStateBitSize;
     int mReportingStateDisableIndex;
     int mReportingStateEnableIndex;
 
     int mPowerStateId;
-    unsigned int mPowerStateOffset;
+    unsigned int mPowerStateBitOffset;
+    unsigned int mPowerStateBitSize;
     int mPowerStateOffIndex;
     int mPowerStateOnIndex;
 
     int mReportIntervalId;
-    unsigned int mReportIntervalOffset;
-    unsigned int mReportIntervalSize;
+    unsigned int mReportIntervalBitOffset;
+    unsigned int mReportIntervalBitSize;
 
     // Input report translate table
     std::vector<ReportTranslateRecord> mTranslateTable;
diff --git a/modules/sensors/dynamic_sensor/HidUtils/Android.bp b/modules/sensors/dynamic_sensor/HidUtils/Android.bp
index bbed0327..5823c794 100644
--- a/modules/sensors/dynamic_sensor/HidUtils/Android.bp
+++ b/modules/sensors/dynamic_sensor/HidUtils/Android.bp
@@ -45,6 +45,7 @@ cc_library {
         "HidParser.cpp",
         "HidReport.cpp",
         "HidTree.cpp",
+        "HidUtils.cpp",
     ],
     export_include_dirs: ["."],
 
@@ -99,3 +100,20 @@ cc_test_host {
 
     local_include_dirs: ["test"],
 }
+
+//
+// Test for HidUtils
+//
+cc_test_host {
+    name: "hid_utils_test",
+    defaults: ["hid_defaults"],
+
+    srcs: ["test/CopyBitsTest.cpp"],
+
+    shared_libs: [
+        "libhidparser",
+    ],
+
+    local_include_dirs: ["test"],
+}
+
diff --git a/modules/sensors/dynamic_sensor/HidUtils/HidParser.cpp b/modules/sensors/dynamic_sensor/HidUtils/HidParser.cpp
index 704a1b39..28d87d97 100644
--- a/modules/sensors/dynamic_sensor/HidUtils/HidParser.cpp
+++ b/modules/sensors/dynamic_sensor/HidUtils/HidParser.cpp
@@ -317,4 +317,5 @@ std::ostream& operator<<(std::ostream &os, const HidParser::DigestVector &digest
     os << LOG_ENDL;
     return os;
 }
+
 } // namespace HidUtil
diff --git a/modules/sensors/dynamic_sensor/HidUtils/HidParser.h b/modules/sensors/dynamic_sensor/HidUtils/HidParser.h
index 20dcf637..cb4a92a8 100644
--- a/modules/sensors/dynamic_sensor/HidUtils/HidParser.h
+++ b/modules/sensors/dynamic_sensor/HidUtils/HidParser.h
@@ -174,6 +174,7 @@ struct HidParser::ReportPacket {
 };
 
 std::ostream& operator<<(std::ostream &os, const HidParser::DigestVector &digest2);
+
 } // namespace HidUtil
 
 #endif // HIDUTIL_HIDPARSER_H_
diff --git a/modules/sensors/dynamic_sensor/HidUtils/HidUtils.cpp b/modules/sensors/dynamic_sensor/HidUtils/HidUtils.cpp
new file mode 100644
index 00000000..0cce2a39
--- /dev/null
+++ b/modules/sensors/dynamic_sensor/HidUtils/HidUtils.cpp
@@ -0,0 +1,72 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "HidUtils.h"
+#include <stdint.h>
+#include <algorithm>
+
+namespace HidUtil {
+
+void copyBits(const void *src, void *dst, size_t dst_size,
+              unsigned int src_bit_offset, unsigned int dst_bit_offset,
+              unsigned int bit_count) {
+    const uint8_t *p_src;
+    uint8_t       *p_dst;
+    uint8_t        dst_mask;
+    unsigned int   bits_rem;
+    unsigned int   bit_block_count;
+
+    // Do nothing if copying past the end of the destination buffer.
+    if ((static_cast<size_t>(dst_bit_offset) > (8 * dst_size)) ||
+        (static_cast<size_t>(bit_count) > (8 * dst_size)) ||
+        (static_cast<size_t>(dst_bit_offset + bit_count) > (8 * dst_size))) {
+        return;
+    }
+
+    // Copy bits from source to destination buffer.
+    p_src = static_cast<const uint8_t*>(src) + (src_bit_offset / 8);
+    src_bit_offset = src_bit_offset % 8;
+    p_dst = static_cast<uint8_t*>(dst) + (dst_bit_offset / 8);
+    dst_bit_offset = dst_bit_offset % 8;
+    bits_rem = bit_count;
+    while (bits_rem > 0) {
+        // Determine the size of the next block of bits to copy. The block must
+        // not cross a source or desintation byte boundary.
+        bit_block_count = std::min(bits_rem, 8 - src_bit_offset);
+        bit_block_count = std::min(bit_block_count, 8 - dst_bit_offset);
+
+        // Determine the destination bit block mask.
+        dst_mask = ((1 << bit_block_count) - 1) << dst_bit_offset;
+
+        // Copy the block of bits.
+        *p_dst = (*p_dst & ~dst_mask) |
+                 (((*p_src >> src_bit_offset) << dst_bit_offset) & dst_mask);
+
+        // Advance past the block of copied bits in the source.
+        src_bit_offset += bit_block_count;
+        p_src += src_bit_offset / 8;
+        src_bit_offset = src_bit_offset % 8;
+
+        // Advance past the block of copied bits in the destination.
+        dst_bit_offset += bit_block_count;
+        p_dst += dst_bit_offset / 8;
+        dst_bit_offset = dst_bit_offset % 8;
+
+        // Decrement the number of bits remaining.
+        bits_rem -= bit_block_count;
+    }
+}
+
+} // namespace HidUtil
diff --git a/modules/sensors/dynamic_sensor/HidUtils/HidUtils.h b/modules/sensors/dynamic_sensor/HidUtils/HidUtils.h
new file mode 100644
index 00000000..54aa31e9
--- /dev/null
+++ b/modules/sensors/dynamic_sensor/HidUtils/HidUtils.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef HIDUTIL_HIDUTILS_H_
+#define HIDUTIL_HIDUTILS_H_
+
+#include <stddef.h>
+
+namespace HidUtil {
+
+void copyBits(const void *src, void *dst, size_t dst_size,
+              unsigned int src_bit_offset, unsigned int dst_bit_offset,
+              unsigned int bit_count);
+
+} // namespace HidUtil
+
+#endif // HIDUTIL_HIDUTILS_H_
diff --git a/modules/sensors/dynamic_sensor/HidUtils/test/CopyBitsTest.cpp b/modules/sensors/dynamic_sensor/HidUtils/test/CopyBitsTest.cpp
new file mode 100644
index 00000000..1b1ca709
--- /dev/null
+++ b/modules/sensors/dynamic_sensor/HidUtils/test/CopyBitsTest.cpp
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2021 The Android Open Source Project
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "HidUtils.h"
+#include <gtest/gtest.h>
+
+using HidUtil::copyBits;
+
+TEST(CopyBitsTest, CopyBits) {
+    const struct {
+        uint32_t src;
+        uint32_t dst;
+        int src_bit_offset;
+        int dst_bit_offset;
+        int bit_count;
+        uint32_t expected_dst;
+    } kTestVectorList[] = {
+        { 0x00000005, 0x00000000,  0,  0,  8, 0x00000005 },
+        { 0x00000005, 0x00000000,  0,  4,  8, 0x00000050 },
+        { 0x0000000C, 0x00000020,  0,  4,  8, 0x000000C0 },
+        { 0x00000005, 0x0000F02F,  0,  4,  8, 0x0000F05F },
+        { 0x12345678, 0x87654321,  5, 11, 17, 0x8D159B21 },
+        { 0x12345678, 0x87654321, 11,  5, 17, 0x8748D141 },
+    };
+
+    for (auto test_vector : kTestVectorList) {
+        uint32_t dst = test_vector.dst;
+        copyBits(&(test_vector.src), &dst, sizeof(dst),
+                 test_vector.src_bit_offset, test_vector.dst_bit_offset,
+                 test_vector.bit_count);
+        EXPECT_EQ(test_vector.expected_dst, dst);
+    }
+}
+
+TEST(CopyBitsTest, Overflow) {
+    const struct {
+        uint32_t src;
+        uint32_t dst;
+        unsigned int src_bit_offset;
+        unsigned int dst_bit_offset;
+        unsigned int bit_count;
+        uint32_t expected_dst;
+    } kTestVectorList[] = {
+        { 0x000000FF, 0x00000000,  0,        0,        8, 0x000000FF },
+        { 0x000000FF, 0x00000000,  0,       24,        8, 0xFF000000 },
+        { 0x000000FF, 0x00000000,  0,       25,        8, 0x00000000 },
+        { 0x000000FF, 0x00000000,  0,       32,        8, 0x00000000 },
+        { 0x000000FF, 0x00000000,  0, UINT_MAX,        8, 0x00000000 },
+        { 0x000000FF, 0x00000000,  0,        8, UINT_MAX, 0x00000000 },
+    };
+
+    for (auto test_vector : kTestVectorList) {
+        uint32_t dst = test_vector.dst;
+        copyBits(&(test_vector.src), &dst, sizeof(dst),
+                 test_vector.src_bit_offset, test_vector.dst_bit_offset,
+                 test_vector.bit_count);
+        EXPECT_EQ(test_vector.expected_dst, dst);
+    }
+}
+