diff --git a/Android.bp b/Android.bp
index dd84737..70e0574 100644
--- a/Android.bp
+++ b/Android.bp
@@ -45,4 +45,5 @@
         "-g",
     ],
 
+    require_root: true,
 }
diff --git a/gnss/1.0/vts/functional/Android.bp b/gnss/1.0/vts/functional/Android.bp
index 505cb41..d73b32e 100644
--- a/gnss/1.0/vts/functional/Android.bp
+++ b/gnss/1.0/vts/functional/Android.bp
@@ -19,5 +19,5 @@
     defaults: ["VtsHalTargetTestDefaults"],
     srcs: ["VtsHalGnssV1_0TargetTest.cpp"],
     static_libs: ["android.hardware.gnss@1.0"],
-    test_suites: ["general-tests"],
+    test_suites: ["general-tests", "vts-core"],
 }
diff --git a/gnss/1.0/vts/functional/VtsHalGnssV1_0TargetTest.cpp b/gnss/1.0/vts/functional/VtsHalGnssV1_0TargetTest.cpp
index c26f60a..1a80ecf 100644
--- a/gnss/1.0/vts/functional/VtsHalGnssV1_0TargetTest.cpp
+++ b/gnss/1.0/vts/functional/VtsHalGnssV1_0TargetTest.cpp
@@ -16,11 +16,11 @@
 
 #define LOG_TAG "VtsHalGnssV1_0TargetTest"
 #include <android/hardware/gnss/1.0/IGnss.h>
+#include <gtest/gtest.h>
+#include <hidl/GtestPrinter.h>
+#include <hidl/ServiceManagement.h>
 #include <log/log.h>
 
-#include <VtsHalHidlTargetTestBase.h>
-#include <VtsHalHidlTargetTestEnvBase.h>
-
 #include <chrono>
 #include <condition_variable>
 #include <mutex>
@@ -42,23 +42,8 @@
 bool sAgpsIsPresent = false;  // if SUPL or XTRA assistance available
 bool sSignalIsWeak = false;   // if GNSS signals are weak (e.g. light indoor)
 
-// Test environment for GNSS HIDL HAL.
-class GnssHidlEnvironment : public ::testing::VtsHalHidlTargetTestEnvBase {
- public:
-  // get the test environment singleton
-  static GnssHidlEnvironment* Instance() {
-    static GnssHidlEnvironment* instance = new GnssHidlEnvironment;
-    return instance;
-  }
-
-  virtual void registerTestServices() override { registerTestService<IGnss>(); }
-
- private:
-  GnssHidlEnvironment() {}
-};
-
 // The main test class for GNSS HAL.
-class GnssHalTest : public ::testing::VtsHalHidlTargetTestBase {
+class GnssHalTest : public testing::TestWithParam<std::string> {
  public:
   virtual void SetUp() override {
     // Clean between tests
@@ -67,8 +52,7 @@
     info_called_count_ = 0;
     notify_count_ = 0;
 
-    gnss_hal_ = ::testing::VtsHalHidlTargetTestBase::getService<IGnss>(
-        GnssHidlEnvironment::Instance()->getServiceName<IGnss>());
+    gnss_hal_ = IGnss::getService(GetParam());
     ASSERT_NE(gnss_hal_, nullptr);
 
     gnss_cb_ = new GnssCallback(*this);
@@ -344,14 +328,14 @@
  * Since this is just the basic operation of SetUp() and TearDown(),
  * the function definition is intentionally empty
  */
-TEST_F(GnssHalTest, SetCallbackCapabilitiesCleanup) {}
+TEST_P(GnssHalTest, SetCallbackCapabilitiesCleanup) {}
 
 /*
  * GetLocation:
  * Turns on location, waits 45 second for at least 5 locations,
  * and checks them for reasonable validity.
  */
-TEST_F(GnssHalTest, GetLocation) {
+TEST_P(GnssHalTest, GetLocation) {
 #define MIN_INTERVAL_MSEC 500
 #define PREFERRED_ACCURACY 0   // Ideally perfect (matches GnssLocationProvider)
 #define PREFERRED_TIME_MSEC 0  // Ideally immediate
@@ -391,7 +375,7 @@
  * InjectDelete:
  * Ensures that calls to inject and/or delete information state are handled.
  */
-TEST_F(GnssHalTest, InjectDelete) {
+TEST_P(GnssHalTest, InjectDelete) {
   // confidently, well north of Alaska
   auto result = gnss_hal_->injectLocation(80.0, -170.0, 1000.0);
 
@@ -424,7 +408,7 @@
  *   null or actual extension, no crash.
  * Confirms year-based required extensions (Measurement & Debug) are present
  */
-TEST_F(GnssHalTest, GetAllExtensions) {
+TEST_P(GnssHalTest, GetAllExtensions) {
   // Basic call-is-handled checks
   auto gnssXtra = gnss_hal_->getExtensionXtra();
   ASSERT_TRUE(gnssXtra.isOk());
@@ -470,7 +454,7 @@
  * MeasurementCapabilities:
  * Verifies that modern hardware supports measurement capabilities.
  */
-TEST_F(GnssHalTest, MeasurementCapabilites) {
+TEST_P(GnssHalTest, MeasurementCapabilites) {
   if (info_called_count_ > 0 && last_info_.yearOfHw >= 2016) {
     EXPECT_TRUE(last_capabilities_ & IGnssCallback::Capabilities::MEASUREMENTS);
   }
@@ -480,16 +464,19 @@
  * SchedulingCapabilities:
  * Verifies that 2018+ hardware supports Scheduling capabilities.
  */
-TEST_F(GnssHalTest, SchedulingCapabilities) {
+TEST_P(GnssHalTest, SchedulingCapabilities) {
     if (info_called_count_ > 0 && last_info_.yearOfHw >= 2018) {
         EXPECT_TRUE(last_capabilities_ & IGnssCallback::Capabilities::SCHEDULING);
     }
 }
 
+INSTANTIATE_TEST_SUITE_P(
+        PerInstance, GnssHalTest,
+        testing::ValuesIn(android::hardware::getAllHalInstanceNames(IGnss::descriptor)),
+        android::hardware::PrintInstanceNameToString);
+
 int main(int argc, char** argv) {
-  ::testing::AddGlobalTestEnvironment(GnssHidlEnvironment::Instance());
   ::testing::InitGoogleTest(&argc, argv);
-  GnssHidlEnvironment::Instance()->init(&argc, argv);
   /*
    * These arguments not used by automated VTS testing.
    * Only for use in manual testing, when wanting to run
@@ -502,7 +489,6 @@
         sSignalIsWeak = true;
     }
   }
-  int status = RUN_ALL_TESTS();
-  ALOGI("Test result = %d", status);
-  return status;
-}
+
+  return RUN_ALL_TESTS();
+}
\ No newline at end of file
diff --git a/graphics/mapper/4.0/IMapper.hal b/graphics/mapper/4.0/IMapper.hal
index f5df04b..85c7c81 100644
--- a/graphics/mapper/4.0/IMapper.hal
+++ b/graphics/mapper/4.0/IMapper.hal
@@ -202,6 +202,9 @@
      * the buffer. If the bytesPerStride is unknown or variable, a value of -1
      * should be returned.
      *
+     * The locked buffer must adhere to the format requested at allocation time
+     * in the BufferDescriptorInfo.
+     *
      * @param buffer Buffer to lock.
      * @param cpuUsage CPU usage flags to request. See +ndk
      *     libnativewindow#AHardwareBuffer_UsageFlags for possible values.
diff --git a/sensors/2.0/multihal/Android.bp b/sensors/2.0/multihal/Android.bp
index 710835f..5c67676 100644
--- a/sensors/2.0/multihal/Android.bp
+++ b/sensors/2.0/multihal/Android.bp
@@ -28,6 +28,7 @@
         "libpower",
         "libutils",
     ],
+    cflags: ["-DLOG_TAG=\"SensorsMultiHal\""],
 }
 
 cc_binary {
@@ -45,7 +46,6 @@
     ],
     init_rc: ["android.hardware.sensors@2.0-service-multihal.rc"],
     vintf_fragments: ["android.hardware.sensors@2.0-multihal.xml"],
-    cflags: ["-DLOG_TAG=\"SensorsMultiHal\""],
 }
 
 cc_library_headers {
@@ -66,4 +66,7 @@
     export_header_lib_headers: [
         "android.hardware.sensors@2.0-multihal.header",
     ],
+    shared_libs: [
+        "libutils",
+    ],
 }
diff --git a/sensors/2.0/multihal/HalProxy.cpp b/sensors/2.0/multihal/HalProxy.cpp
index 5aa3b3d..d667218 100644
--- a/sensors/2.0/multihal/HalProxy.cpp
+++ b/sensors/2.0/multihal/HalProxy.cpp
@@ -24,6 +24,7 @@
 
 #include <dlfcn.h>
 
+#include <cinttypes>
 #include <fstream>
 #include <functional>
 #include <thread>
@@ -35,6 +36,9 @@
 namespace implementation {
 
 using ::android::hardware::sensors::V2_0::EventQueueFlagBits;
+using ::android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
+using ::android::hardware::sensors::V2_0::implementation::getTimeNow;
+using ::android::hardware::sensors::V2_0::implementation::kWakelockTimeoutNs;
 
 typedef ISensorsSubHal*(SensorsHalGetSubHalFunc)(uint32_t*);
 
@@ -53,23 +57,23 @@
 HalProxy::HalProxy() {
     const char* kMultiHalConfigFile = "/vendor/etc/sensors/hals.conf";
     initializeSubHalListFromConfigFile(kMultiHalConfigFile);
-    initializeSubHalCallbacksAndSensorList();
+    init();
 }
 
 HalProxy::HalProxy(std::vector<ISensorsSubHal*>& subHalList) : mSubHalList(subHalList) {
-    initializeSubHalCallbacksAndSensorList();
+    init();
 }
 
 HalProxy::~HalProxy() {
-    {
-        std::lock_guard<std::mutex> lockGuard(mEventQueueWriteMutex);
-        mPendingWritesRun = false;
-        mEventQueueWriteCV.notify_one();
-    }
+    mThreadsRun.store(false);
+    mWakelockCV.notify_one();
+    mEventQueueWriteCV.notify_one();
     if (mPendingWritesThread.joinable()) {
         mPendingWritesThread.join();
     }
-    // TODO: Cleanup wakeup thread once it is implemented
+    if (mWakelockThread.joinable()) {
+        mWakelockThread.join();
+    }
 }
 
 Return<void> HalProxy::getSensorsList(getSensorsList_cb _hidl_cb) {
@@ -140,7 +144,7 @@
     }
 
     mPendingWritesThread = std::thread(startPendingWritesThread, this);
-    // TODO: start threads to read wake locks.
+    mWakelockThread = std::thread(startWakelockThread, this);
 
     for (size_t i = 0; i < mSubHalList.size(); i++) {
         auto subHal = mSubHalList[i];
@@ -322,7 +326,7 @@
     }
 }
 
-void HalProxy::initializeSubHalCallbacksAndSensorList() {
+void HalProxy::init() {
     initializeSubHalCallbacks();
     initializeSensorList();
 }
@@ -334,11 +338,12 @@
 void HalProxy::handlePendingWrites() {
     // TODO: Find a way to optimize locking strategy maybe using two mutexes instead of one.
     std::unique_lock<std::mutex> lock(mEventQueueWriteMutex);
-    while (mPendingWritesRun) {
+    while (mThreadsRun.load()) {
         mEventQueueWriteCV.wait(
-                lock, [&] { return !mPendingWriteEventsQueue.empty() || !mPendingWritesRun; });
-        if (!mPendingWriteEventsQueue.empty() && mPendingWritesRun) {
-            std::vector<Event>& pendingWriteEvents = mPendingWriteEventsQueue.front();
+                lock, [&] { return !mPendingWriteEventsQueue.empty() || !mThreadsRun.load(); });
+        if (mThreadsRun.load()) {
+            std::vector<Event>& pendingWriteEvents = mPendingWriteEventsQueue.front().first;
+            size_t numWakeupEvents = mPendingWriteEventsQueue.front().second;
             size_t eventQueueSize = mEventQueue->getQuantumCount();
             size_t numToWrite = std::min(pendingWriteEvents.size(), eventQueueSize);
             lock.unlock();
@@ -348,10 +353,16 @@
                         pendingWriteEvents.data(), numToWrite,
                         static_cast<uint32_t>(EventQueueFlagBits::EVENTS_READ),
                         static_cast<uint32_t>(EventQueueFlagBits::READ_AND_PROCESS),
-                        kWakelockTimeoutNs, mEventQueueFlag)) {
+                        kPendingWriteTimeoutNs, mEventQueueFlag)) {
                 ALOGE("Dropping %zu events after blockingWrite failed.", numToWrite);
-            } else {
-                mEventQueueFlag->wake(static_cast<uint32_t>(EventQueueFlagBits::READ_AND_PROCESS));
+                if (numWakeupEvents > 0) {
+                    if (pendingWriteEvents.size() > eventQueueSize) {
+                        decrementRefCountAndMaybeReleaseWakelock(
+                                countNumWakeupEvents(pendingWriteEvents, eventQueueSize));
+                    } else {
+                        decrementRefCountAndMaybeReleaseWakelock(numWakeupEvents);
+                    }
+                }
             }
             lock.lock();
             if (pendingWriteEvents.size() > eventQueueSize) {
@@ -366,9 +377,60 @@
     }
 }
 
-void HalProxy::postEventsToMessageQueue(const std::vector<Event>& events) {
+void HalProxy::startWakelockThread(HalProxy* halProxy) {
+    halProxy->handleWakelocks();
+}
+
+void HalProxy::handleWakelocks() {
+    std::unique_lock<std::recursive_mutex> lock(mWakelockMutex);
+    while (mThreadsRun.load()) {
+        mWakelockCV.wait(lock, [&] { return mWakelockRefCount > 0 || !mThreadsRun.load(); });
+        if (mThreadsRun.load()) {
+            int64_t timeLeft;
+            if (sharedWakelockDidTimeout(&timeLeft)) {
+                resetSharedWakelock();
+            } else {
+                uint32_t numWakeLocksProcessed;
+                lock.unlock();
+                bool success = mWakeLockQueue->readBlocking(
+                        &numWakeLocksProcessed, 1, 0,
+                        static_cast<uint32_t>(WakeLockQueueFlagBits::DATA_WRITTEN), timeLeft);
+                lock.lock();
+                if (success) {
+                    decrementRefCountAndMaybeReleaseWakelock(
+                            static_cast<size_t>(numWakeLocksProcessed));
+                }
+            }
+        }
+    }
+    resetSharedWakelock();
+}
+
+bool HalProxy::sharedWakelockDidTimeout(int64_t* timeLeft) {
+    bool didTimeout;
+    int64_t duration = getTimeNow() - mWakelockTimeoutStartTime;
+    if (duration > kWakelockTimeoutNs) {
+        didTimeout = true;
+    } else {
+        didTimeout = false;
+        *timeLeft = kWakelockTimeoutNs - duration;
+    }
+    return didTimeout;
+}
+
+void HalProxy::resetSharedWakelock() {
+    std::lock_guard<std::recursive_mutex> lockGuard(mWakelockMutex);
+    decrementRefCountAndMaybeReleaseWakelock(mWakelockRefCount);
+    mWakelockTimeoutResetTime = getTimeNow();
+}
+
+void HalProxy::postEventsToMessageQueue(const std::vector<Event>& events, size_t numWakeupEvents,
+                                        ScopedWakelock wakelock) {
     size_t numToWrite = 0;
     std::lock_guard<std::mutex> lock(mEventQueueWriteMutex);
+    if (wakelock.isLocked()) {
+        incrementRefCountAndMaybeAcquireWakelock(numWakeupEvents);
+    }
     if (mPendingWriteEventsQueue.empty()) {
         numToWrite = std::min(events.size(), mEventQueue->availableToWrite());
         if (numToWrite > 0) {
@@ -384,28 +446,37 @@
     if (numToWrite < events.size()) {
         // TODO: Bound the mPendingWriteEventsQueue so that we do not trigger OOMs if framework
         // stalls
-        mPendingWriteEventsQueue.push(
-                std::vector<Event>(events.begin() + numToWrite, events.end()));
+        std::vector<Event> eventsLeft(events.begin() + numToWrite, events.end());
+        mPendingWriteEventsQueue.push({eventsLeft, numWakeupEvents});
         mEventQueueWriteCV.notify_one();
     }
 }
 
-// TODO: Implement the wakelock timeout in these next two methods. Also pass in the subhal
-// index for better tracking.
-
-void HalProxy::incrementRefCountAndMaybeAcquireWakelock() {
-    std::lock_guard<std::mutex> lockGuard(mWakelockRefCountMutex);
+bool HalProxy::incrementRefCountAndMaybeAcquireWakelock(size_t delta,
+                                                        int64_t* timeoutStart /* = nullptr */) {
+    if (!mThreadsRun.load()) return false;
+    std::lock_guard<std::recursive_mutex> lockGuard(mWakelockMutex);
     if (mWakelockRefCount == 0) {
-        acquire_wake_lock(PARTIAL_WAKE_LOCK, kWakeLockName);
+        acquire_wake_lock(PARTIAL_WAKE_LOCK, kWakelockName);
+        mWakelockCV.notify_one();
     }
-    mWakelockRefCount++;
+    mWakelockTimeoutStartTime = getTimeNow();
+    mWakelockRefCount += delta;
+    if (timeoutStart != nullptr) {
+        *timeoutStart = mWakelockTimeoutStartTime;
+    }
+    return true;
 }
 
-void HalProxy::decrementRefCountAndMaybeReleaseWakelock() {
-    std::lock_guard<std::mutex> lockGuard(mWakelockRefCountMutex);
-    mWakelockRefCount--;
+void HalProxy::decrementRefCountAndMaybeReleaseWakelock(size_t delta,
+                                                        int64_t timeoutStart /* = -1 */) {
+    if (!mThreadsRun.load()) return;
+    std::lock_guard<std::recursive_mutex> lockGuard(mWakelockMutex);
+    if (timeoutStart == -1) timeoutStart = mWakelockTimeoutResetTime;
+    if (mWakelockRefCount == 0 || timeoutStart < mWakelockTimeoutResetTime) return;
+    mWakelockRefCount -= std::min(mWakelockRefCount, delta);
     if (mWakelockRefCount == 0) {
-        release_wake_lock(kWakeLockName);
+        release_wake_lock(kWakelockName);
     }
 }
 
@@ -427,6 +498,17 @@
     return mSubHalList[static_cast<size_t>(sensorHandle >> 24)];
 }
 
+size_t HalProxy::countNumWakeupEvents(const std::vector<Event>& events, size_t n) {
+    size_t numWakeupEvents = 0;
+    for (size_t i = 0; i < n; i++) {
+        int32_t sensorHandle = events[i].sensorHandle;
+        if (mSensors[sensorHandle].flags & static_cast<uint32_t>(V1_0::SensorFlagBits::WAKE_UP)) {
+            numWakeupEvents++;
+        }
+    }
+    return numWakeupEvents;
+}
+
 uint32_t HalProxy::clearSubHalIndex(uint32_t sensorHandle) {
     return sensorHandle & (~kSensorHandleSubHalIndexMask);
 }
@@ -436,7 +518,7 @@
 }
 
 void HalProxyCallback::postEvents(const std::vector<Event>& events, ScopedWakelock wakelock) {
-    (void)wakelock;
+    if (events.empty() || !mHalProxy->areThreadsRunning()) return;
     size_t numWakeupEvents;
     std::vector<Event> processedEvents = processEvents(events, &numWakeupEvents);
     if (numWakeupEvents > 0) {
@@ -450,8 +532,7 @@
                     " w/ index %zu.",
                     mSubHalIndex);
     }
-
-    mHalProxy->postEventsToMessageQueue(processedEvents);
+    mHalProxy->postEventsToMessageQueue(events, numWakeupEvents, std::move(wakelock));
 }
 
 ScopedWakelock HalProxyCallback::createScopedWakelock(bool lock) {
@@ -461,13 +542,13 @@
 
 std::vector<Event> HalProxyCallback::processEvents(const std::vector<Event>& events,
                                                    size_t* numWakeupEvents) const {
-    std::vector<Event> eventsOut;
     *numWakeupEvents = 0;
+    std::vector<Event> eventsOut;
     for (Event event : events) {
         event.sensorHandle = setSubHalIndex(event.sensorHandle, mSubHalIndex);
         eventsOut.push_back(event);
-        if ((mHalProxy->getSensorInfo(event.sensorHandle).flags & V1_0::SensorFlagBits::WAKE_UP) !=
-            0) {
+        const SensorInfo& sensor = mHalProxy->getSensorInfo(event.sensorHandle);
+        if ((sensor.flags & V1_0::SensorFlagBits::WAKE_UP) != 0) {
             (*numWakeupEvents)++;
         }
     }
diff --git a/sensors/2.0/multihal/ScopedWakelock.cpp b/sensors/2.0/multihal/ScopedWakelock.cpp
index 0430fa3..d85d4a7 100644
--- a/sensors/2.0/multihal/ScopedWakelock.cpp
+++ b/sensors/2.0/multihal/ScopedWakelock.cpp
@@ -22,18 +22,22 @@
 namespace V2_0 {
 namespace implementation {
 
+int64_t getTimeNow() {
+    return std::chrono::duration_cast<std::chrono::nanoseconds>(
+                   std::chrono::system_clock::now().time_since_epoch())
+            .count();
+}
+
 ScopedWakelock::ScopedWakelock(IScopedWakelockRefCounter* refCounter, bool locked)
     : mRefCounter(refCounter), mLocked(locked) {
-    // TODO: Move this implementation into HalProxy object instead
     if (mLocked) {
-        mRefCounter->incrementRefCountAndMaybeAcquireWakelock();
+        mLocked = mRefCounter->incrementRefCountAndMaybeAcquireWakelock(1, &mCreatedAtTimeNs);
     }
 }
 
 ScopedWakelock::~ScopedWakelock() {
-    // TODO: Move this implementation into HalProxy object instead
     if (mLocked) {
-        mRefCounter->decrementRefCountAndMaybeReleaseWakelock();
+        mRefCounter->decrementRefCountAndMaybeReleaseWakelock(1, mCreatedAtTimeNs);
     }
 }
 
diff --git a/sensors/2.0/multihal/android.hardware.sensors@2.0-service-multihal.rc b/sensors/2.0/multihal/android.hardware.sensors@2.0-service-multihal.rc
index 1671689..a4da3b0 100644
--- a/sensors/2.0/multihal/android.hardware.sensors@2.0-service-multihal.rc
+++ b/sensors/2.0/multihal/android.hardware.sensors@2.0-service-multihal.rc
@@ -1,6 +1,7 @@
 service vendor.sensors-hal-2-0-multihal /vendor/bin/hw/android.hardware.sensors@2.0-service.multihal
     class hal
     user system
-    group system
+    group system wakelock
+    writepid /dev/cpuset/system-background/tasks
     capabilities BLOCK_SUSPEND
     rlimit rtprio 10 10
diff --git a/sensors/2.0/multihal/include/HalProxy.h b/sensors/2.0/multihal/include/HalProxy.h
index 47571a6..6592afe 100644
--- a/sensors/2.0/multihal/include/HalProxy.h
+++ b/sensors/2.0/multihal/include/HalProxy.h
@@ -16,9 +16,11 @@
 
 #pragma once
 
+#include "ScopedWakelock.h"
 #include "SubHal.h"
 
 #include <android/hardware/sensors/2.0/ISensors.h>
+#include <android/hardware/sensors/2.0/types.h>
 #include <fmq/MessageQueue.h>
 #include <hardware_legacy/power.h>
 #include <hidl/MQDescriptor.h>
@@ -30,6 +32,7 @@
 #include <mutex>
 #include <queue>
 #include <thread>
+#include <utility>
 
 namespace android {
 namespace hardware {
@@ -100,45 +103,41 @@
     Return<void> onDynamicSensorsDisconnected(const hidl_vec<int32_t>& dynamicSensorHandlesRemoved,
                                               int32_t subHalIndex);
 
-    // Below methods follow IScopedWakelockRefCounter
-
-    /**
-     * Increment ref count and maybe acquire wakelock.
-     */
-    void incrementRefCountAndMaybeAcquireWakelock() override;
-
-    /**
-     * Decrement ref count and maybe release wakelock.
-     */
-    void decrementRefCountAndMaybeReleaseWakelock() override;
-
     // Below methods are for HalProxyCallback
 
     /**
      * Post events to the event message queue if there is room to write them. Otherwise post the
-     * remaining events to a background thread for a blocking write with a 5 second timeout.
+     * remaining events to a background thread for a blocking write with a kPendingWriteTimeoutNs
+     * timeout.
      *
      * @param events The list of events to post to the message queue.
+     * @param numWakeupEvents The number of wakeup events in events.
+     * @param wakelock The wakelock associated with this post of events.
      */
-    void postEventsToMessageQueue(const std::vector<Event>& events);
+    void postEventsToMessageQueue(const std::vector<Event>& events, size_t numWakeupEvents,
+                                  ScopedWakelock wakelock);
 
     /**
-     * Get the SensorInfo object associated with the sensorHandle.
+     * Get the sensor info associated with that sensorHandle.
      *
-     * @param sensorHandle The sensorHandle for the sensor.
+     * @param sensorHandle The sensor handle.
      *
-     * @return The sensor info for the sensor.
+     * @return The sensor info object in the mapping.
      */
-    const SensorInfo& getSensorInfo(uint32_t sensorHandle) const {
-        return mSensors.at(sensorHandle);
-    }
+    const SensorInfo& getSensorInfo(uint32_t sensorHandle) { return mSensors[sensorHandle]; }
+
+    bool areThreadsRunning() { return mThreadsRun.load(); }
+
+    // Below methods are from IScopedWakelockRefCounter interface
+    bool incrementRefCountAndMaybeAcquireWakelock(size_t delta,
+                                                  int64_t* timeoutStart = nullptr) override;
+
+    void decrementRefCountAndMaybeReleaseWakelock(size_t delta, int64_t timeoutStart = -1) override;
 
   private:
     using EventMessageQueue = MessageQueue<Event, kSynchronizedReadWrite>;
     using WakeLockMessageQueue = MessageQueue<uint32_t, kSynchronizedReadWrite>;
 
-    const char* kWakeLockName = "SensorsHAL_WAKEUP";
-
     /**
      * The Event FMQ where sensor events are written
      */
@@ -185,23 +184,17 @@
     //! The single subHal that supports directChannel reporting.
     ISensorsSubHal* mDirectChannelSubHal = nullptr;
 
-    //! The mutex for the event queue.
-    std::mutex mEventQueueMutex;
-
     //! The timeout for each pending write on background thread for events.
-    static const int64_t kWakelockTimeoutNs = 5 * INT64_C(1000000000) /* 5 seconds */;
-
-    //! The scoped wakelock ref count.
-    size_t mWakelockRefCount = 0;
-
-    //! The mutex guarding the mWakelockRefCount variable
-    std::mutex mWakelockRefCountMutex;
+    static const int64_t kPendingWriteTimeoutNs = 5 * INT64_C(1000000000) /* 5 seconds */;
 
     //! The bit mask used to get the subhal index from a sensor handle.
     static constexpr uint32_t kSensorHandleSubHalIndexMask = 0xFF000000;
 
-    //! The events that were not able to be written to fmq right away
-    std::queue<std::vector<Event>> mPendingWriteEventsQueue;
+    /**
+     * A FIFO queue of pairs of vector of events and the number of wakeup events in that vector
+     * which are waiting to be written to the events fmq in the background thread.
+     */
+    std::queue<std::pair<std::vector<Event>, size_t>> mPendingWriteEventsQueue;
 
     //! The mutex protecting writing to the fmq and the pending events queue
     std::mutex mEventQueueWriteMutex;
@@ -212,12 +205,32 @@
     //! The thread object ptr that handles pending writes
     std::thread mPendingWritesThread;
 
-    //! The bool indicating whether to end the pending writes background thread or not
-    bool mPendingWritesRun = true;
+    //! The thread object that handles wakelocks
+    std::thread mWakelockThread;
+
+    //! The bool indicating whether to end the threads started in initialize
+    std::atomic_bool mThreadsRun = true;
 
     //! The mutex protecting access to the dynamic sensors added and removed methods.
     std::mutex mDynamicSensorsMutex;
 
+    // WakelockRefCount membar vars below
+
+    //! The mutex protecting the wakelock refcount and subsequent wakelock releases and
+    //! acquisitions
+    std::recursive_mutex mWakelockMutex;
+
+    std::condition_variable_any mWakelockCV;
+
+    //! The refcount of how many ScopedWakelocks and pending wakeup events are active
+    size_t mWakelockRefCount = 0;
+
+    int64_t mWakelockTimeoutStartTime = getTimeNow();
+
+    int64_t mWakelockTimeoutResetTime = getTimeNow();
+
+    const char* kWakelockName = "SensorsMultiHal";
+
     /**
      * Initialize the list of SubHal objects in mSubHalList by reading from dynamic libraries
      * listed in a config file.
@@ -236,9 +249,9 @@
     void initializeSensorList();
 
     /**
-     * Calls the above two helper methods which are shared in both ctors.
+     * Calls the helper methods that all ctors use.
      */
-    void initializeSubHalCallbacksAndSensorList();
+    void init();
 
     /**
      * Starts the thread that handles pending writes to event fmq.
@@ -251,6 +264,31 @@
     void handlePendingWrites();
 
     /**
+     * Starts the thread that handles decrementing the ref count on wakeup events processed by the
+     * framework and timing out wakelocks.
+     *
+     * @param halProxy The HalProxy object pointer.
+     */
+    static void startWakelockThread(HalProxy* halProxy);
+
+    //! Handles the wakelocks.
+    void handleWakelocks();
+
+    /**
+     * @param timeLeft The variable that should be set to the timeleft before timeout will occur or
+     * unmodified if timeout occurred.
+     *
+     * @return true if the shared wakelock has been held passed the timeout and should be released
+     */
+    bool sharedWakelockDidTimeout(int64_t* timeLeft);
+
+    /**
+     * Reset all the member variables associated with the wakelock ref count and maybe release
+     * the shared wakelock.
+     */
+    void resetSharedWakelock();
+
+    /**
      * Clear direct channel flags if the HalProxy has already chosen a subhal as its direct channel
      * subhal. Set the directChannelSubHal pointer to the subHal passed in if this is the first
      * direct channel enabled sensor seen.
@@ -269,6 +307,16 @@
      */
     ISensorsSubHal* getSubHalForSensorHandle(uint32_t sensorHandle);
 
+    /**
+     * Count the number of wakeup events in the first n events of the vector.
+     *
+     * @param events The vector of Event objects.
+     * @param n The end index not inclusive of events to consider.
+     *
+     * @return The number of wakeup events of the considered events.
+     */
+    size_t countNumWakeupEvents(const std::vector<Event>& events, size_t n);
+
     /*
      * Clear out the subhal index bytes from a sensorHandle.
      *
diff --git a/sensors/2.0/multihal/include/ScopedWakelock.h b/sensors/2.0/multihal/include/ScopedWakelock.h
index a151f15..aa6d9db 100644
--- a/sensors/2.0/multihal/include/ScopedWakelock.h
+++ b/sensors/2.0/multihal/include/ScopedWakelock.h
@@ -16,7 +16,7 @@
 
 #pragma once
 
-#include <mutex>
+#include <android/hardware/sensors/2.0/types.h>
 
 namespace android {
 namespace hardware {
@@ -24,10 +24,36 @@
 namespace V2_0 {
 namespace implementation {
 
-class IScopedWakelockRefCounter {
+using ::android::hardware::sensors::V2_0::SensorTimeout;
+
+const int64_t kWakelockTimeoutNs =
+        static_cast<int64_t>(SensorTimeout::WAKE_LOCK_SECONDS) * INT64_C(1000000000);
+
+int64_t getTimeNow();
+
+class IScopedWakelockRefCounter : public RefBase {
   public:
-    virtual void incrementRefCountAndMaybeAcquireWakelock() = 0;
-    virtual void decrementRefCountAndMaybeReleaseWakelock() = 0;
+    /**
+     * Increment the wakelock ref count and maybe acquire the shared wakelock if incrementing
+     * from 0 then return the time of incrementing back to caller.
+     *
+     * @param delta The amount to change ref count by.
+     * @param timeoutStart The ptr to the timestamp in ns that the increment occurred which will be
+     *        set in the function or nullptr if not specified.
+     *
+     * @return true if successfully incremented the wakelock ref count.
+     */
+    virtual bool incrementRefCountAndMaybeAcquireWakelock(size_t delta,
+                                                          int64_t* timeoutStart = nullptr) = 0;
+    /**
+     * Decrement the wakelock ref count and maybe release wakelock if ref count ends up 0.
+     *
+     * @param delta The amount to change ref count by.
+     * @param timeoutStart The timestamp in ns that the calling context kept track of when
+     *        incrementing the ref count or -1 by default
+     */
+    virtual void decrementRefCountAndMaybeReleaseWakelock(size_t delta,
+                                                          int64_t timeoutStart = -1) = 0;
     // Virtual dtor needed for compilation success
     virtual ~IScopedWakelockRefCounter(){};
 };
@@ -64,6 +90,7 @@
   private:
     friend class HalProxyCallback;
     IScopedWakelockRefCounter* mRefCounter;
+    int64_t mCreatedAtTimeNs;
     bool mLocked;
     ScopedWakelock(IScopedWakelockRefCounter* refCounter, bool locked);
     ScopedWakelock(const ScopedWakelock&) = delete;
diff --git a/sensors/2.0/multihal/tests/Android.bp b/sensors/2.0/multihal/tests/Android.bp
index aa44687..e3a4af1 100644
--- a/sensors/2.0/multihal/tests/Android.bp
+++ b/sensors/2.0/multihal/tests/Android.bp
@@ -35,6 +35,9 @@
     static_libs: [
         "android.hardware.sensors@2.0-HalProxy",
     ],
+    cflags: [
+        "-DLOG_TAG=\"FakeSubHal\""
+    ],
 }
 
 cc_library {
@@ -87,4 +90,7 @@
         "libutils",
     ],
     test_suites: ["device-tests"],
+    cflags: [
+        "-DLOG_TAG=\"HalProxyUnitTests\"",
+    ],
 }
diff --git a/sensors/2.0/multihal/tests/HalProxy_test.cpp b/sensors/2.0/multihal/tests/HalProxy_test.cpp
index c8fbb73..fa527c9 100644
--- a/sensors/2.0/multihal/tests/HalProxy_test.cpp
+++ b/sensors/2.0/multihal/tests/HalProxy_test.cpp
@@ -29,6 +29,7 @@
 
 namespace {
 
+using ::android::hardware::EventFlag;
 using ::android::hardware::hidl_vec;
 using ::android::hardware::MessageQueue;
 using ::android::hardware::Return;
@@ -36,7 +37,9 @@
 using ::android::hardware::sensors::V1_0::SensorFlagBits;
 using ::android::hardware::sensors::V1_0::SensorInfo;
 using ::android::hardware::sensors::V1_0::SensorType;
+using ::android::hardware::sensors::V2_0::EventQueueFlagBits;
 using ::android::hardware::sensors::V2_0::ISensorsCallback;
+using ::android::hardware::sensors::V2_0::WakeLockQueueFlagBits;
 using ::android::hardware::sensors::V2_0::implementation::HalProxy;
 using ::android::hardware::sensors::V2_0::implementation::HalProxyCallback;
 using ::android::hardware::sensors::V2_0::subhal::implementation::AddAndRemoveDynamicSensorsSubHal;
@@ -123,6 +126,16 @@
 void testSensorsListForOneDirectChannelEnabledSubHal(const std::vector<SensorInfo>& sensorsList,
                                                      size_t enabledSubHalIndex);
 
+void ackWakeupEventsToHalProxy(size_t numEvents, std::unique_ptr<WakeupMessageQueue>& wakelockQueue,
+                               EventFlag* wakelockQueueFlag);
+
+bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueue>& eventQueue,
+                          EventFlag* eventQueueFlag);
+
+std::unique_ptr<EventMessageQueue> makeEventFMQ(size_t size);
+
+std::unique_ptr<WakeupMessageQueue> makeWakelockFMQ(size_t size);
+
 /**
  * Construct and return a HIDL Event type thats sensorHandle refers to a proximity sensor
  *    which is a wakeup type sensor.
@@ -269,10 +282,8 @@
     AllSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
@@ -288,10 +299,8 @@
     AllSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
@@ -306,17 +315,24 @@
     AllSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
+    EventFlag* eventQueueFlag;
+    EventFlag::createEventFlag(eventQueue->getEventFlagWord(), &eventQueueFlag);
+
+    EventFlag* wakelockQueueFlag;
+    EventFlag::createEventFlag(wakeLockQueue->getEventFlagWord(), &wakelockQueueFlag);
+
     std::vector<Event> events{makeProximityEvent()};
     subHal.postEvents(events, true /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), 1);
+
+    readEventsOutOfQueue(1, eventQueue, eventQueueFlag);
+    ackWakeupEventsToHalProxy(1, wakeLockQueue, wakelockQueueFlag);
 }
 
 TEST(HalProxyTest, PostMultipleWakeupEvents) {
@@ -325,17 +341,24 @@
     AllSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
+    EventFlag* eventQueueFlag;
+    EventFlag::createEventFlag(eventQueue->getEventFlagWord(), &eventQueueFlag);
+
+    EventFlag* wakelockQueueFlag;
+    EventFlag::createEventFlag(wakeLockQueue->getEventFlagWord(), &wakelockQueueFlag);
+
     std::vector<Event> events = makeMultipleProximityEvents(kNumEvents);
     subHal.postEvents(events, true /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kNumEvents);
+
+    readEventsOutOfQueue(kNumEvents, eventQueue, eventQueueFlag);
+    ackWakeupEventsToHalProxy(kNumEvents, wakeLockQueue, wakelockQueueFlag);
 }
 
 TEST(HalProxyTest, PostEventsMultipleSubhals) {
@@ -344,10 +367,8 @@
     AllSensorsSubHal subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
@@ -367,27 +388,26 @@
     AllSensorsSubHal subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
+    EventFlag* eventQueueFlag;
+    EventFlag::createEventFlag(eventQueue->getEventFlagWord(), &eventQueueFlag);
+
     std::vector<Event> events = makeMultipleAccelerometerEvents(kNumEvents);
     subHal1.postEvents(events, false /* wakeup */);
 
     EXPECT_EQ(eventQueue->availableToRead(), kQueueSize);
 
-    Event eventOut;
-    // writeblock 1 event out of queue, timeout for half a second
-    EXPECT_TRUE(eventQueue->readBlocking(&eventOut, 1, 500000000));
+    // readblock a full queue size worth of events out of queue, timeout for half a second
+    EXPECT_TRUE(readEventsOutOfQueue(kQueueSize, eventQueue, eventQueueFlag));
 
-    // Sleep for a half second so that blocking write has time complete in background thread
-    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+    // proxy background thread should have wrote remaining events when it saw space
+    EXPECT_TRUE(readEventsOutOfQueue(kNumEvents - kQueueSize, eventQueue, eventQueueFlag));
 
-    // proxy background thread should have wrote last event when it saw space
-    EXPECT_EQ(eventQueue->availableToRead(), kQueueSize);
+    EXPECT_EQ(eventQueue->availableToRead(), 0);
 }
 
 TEST(HalProxyTest, PostEventsMultipleSubhalsThreaded) {
@@ -396,10 +416,8 @@
     AllSensorsSubHal subHal1, subHal2;
     std::vector<ISensorsSubHal*> subHals{&subHal1, &subHal2};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
 
@@ -420,10 +438,8 @@
     AllSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
 
-    std::unique_ptr<EventMessageQueue> eventQueue =
-            std::make_unique<EventMessageQueue>(kQueueSize, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(kQueueSize, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(kQueueSize);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(kQueueSize);
     ::android::sp<ISensorsCallback> callback = new SensorsCallback();
     HalProxy proxy(subHals);
     proxy.initialize(*eventQueue->getDesc(), *wakeLockQueue->getDesc(), callback);
@@ -445,9 +461,8 @@
     AddAndRemoveDynamicSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = std::make_unique<EventMessageQueue>(0, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(0, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(0);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(0);
 
     std::vector<SensorInfo> sensorsToConnect;
     std::vector<int32_t> sensorHandlesToExpect;
@@ -474,9 +489,8 @@
     AddAndRemoveDynamicSensorsSubHal subHal;
     std::vector<ISensorsSubHal*> subHals{&subHal};
     HalProxy proxy(subHals);
-    std::unique_ptr<EventMessageQueue> eventQueue = std::make_unique<EventMessageQueue>(0, true);
-    std::unique_ptr<WakeupMessageQueue> wakeLockQueue =
-            std::make_unique<WakeupMessageQueue>(0, true);
+    std::unique_ptr<EventMessageQueue> eventQueue = makeEventFMQ(0);
+    std::unique_ptr<WakeupMessageQueue> wakeLockQueue = makeWakelockFMQ(0);
 
     std::vector<SensorInfo> sensorsToConnect;
     std::vector<int32_t> sensorHandlesToExpect;
@@ -548,6 +562,31 @@
     }
 }
 
+void ackWakeupEventsToHalProxy(size_t numEvents, std::unique_ptr<WakeupMessageQueue>& wakelockQueue,
+                               EventFlag* wakelockQueueFlag) {
+    uint32_t numEventsUInt32 = static_cast<uint32_t>(numEvents);
+    wakelockQueue->write(&numEventsUInt32);
+    wakelockQueueFlag->wake(static_cast<uint32_t>(WakeLockQueueFlagBits::DATA_WRITTEN));
+}
+
+bool readEventsOutOfQueue(size_t numEvents, std::unique_ptr<EventMessageQueue>& eventQueue,
+                          EventFlag* eventQueueFlag) {
+    constexpr int64_t kReadBlockingTimeout = INT64_C(500000000);
+    std::vector<Event> events(numEvents);
+    return eventQueue->readBlocking(events.data(), numEvents,
+                                    static_cast<uint32_t>(EventQueueFlagBits::EVENTS_READ),
+                                    static_cast<uint32_t>(EventQueueFlagBits::READ_AND_PROCESS),
+                                    kReadBlockingTimeout, eventQueueFlag);
+}
+
+std::unique_ptr<EventMessageQueue> makeEventFMQ(size_t size) {
+    return std::make_unique<EventMessageQueue>(size, true);
+}
+
+std::unique_ptr<WakeupMessageQueue> makeWakelockFMQ(size_t size) {
+    return std::make_unique<WakeupMessageQueue>(size, true);
+}
+
 Event makeProximityEvent() {
     Event event;
     event.timestamp = 0xFF00FF00;
diff --git a/tv/tuner/1.0/default/Demux.cpp b/tv/tuner/1.0/default/Demux.cpp
index b18d4df..8bb79f9 100644
--- a/tv/tuner/1.0/default/Demux.cpp
+++ b/tv/tuner/1.0/default/Demux.cpp
@@ -479,7 +479,8 @@
             if (prefix == 0x000001) {
                 // TODO handle mulptiple Pes filters
                 mPesSizeLeft =
-                        (mFilterOutputs[filterId][i + 7] << 8) | mFilterOutputs[filterId][i + 8];
+                        (mFilterOutputs[filterId][i + 8] << 8) | mFilterOutputs[filterId][i + 9];
+                mPesSizeLeft += 6;
                 ALOGD("[Demux] pes data length %d", mPesSizeLeft);
             } else {
                 continue;
@@ -489,7 +490,7 @@
         int endPoint = min(184, mPesSizeLeft);
         // append data and check size
         vector<uint8_t>::const_iterator first = mFilterOutputs[filterId].begin() + i + 4;
-        vector<uint8_t>::const_iterator last = mFilterOutputs[filterId].begin() + i + 3 + endPoint;
+        vector<uint8_t>::const_iterator last = mFilterOutputs[filterId].begin() + i + 4 + endPoint;
         mPesOutput.insert(mPesOutput.end(), first, last);
         // size does not match then continue
         mPesSizeLeft -= endPoint;
@@ -800,7 +801,7 @@
 void Demux::maySendFilterStatusCallback(uint32_t filterId) {
     std::lock_guard<std::mutex> lock(mFilterStatusLock);
     int availableToRead = mFilterMQs[filterId]->availableToRead();
-    int availableToWrite = mInputMQ->availableToWrite();
+    int availableToWrite = mFilterMQs[filterId]->availableToWrite();
     int fmqSize = mFilterMQs[filterId]->getQuantumCount();
 
     DemuxFilterStatus newStatus =
@@ -885,7 +886,6 @@
                     byteBuffer[index] = static_cast<uint8_t>(buffer[index]);
                 }
                 startTsFilter(byteBuffer);
-                inputData.seekg(packetSize, inputData.cur);
             }
             startFilterDispatcher();
             sleep(1);