diff --git a/Android.mk b/Android.mk
index 98ce3a6..f46276c 100644
--- a/Android.mk
+++ b/Android.mk
@@ -15,6 +15,22 @@
 ifeq ($(strip $(BOARD_USES_DRM_HWCOMPOSER)),true)
 
 LOCAL_PATH := $(call my-dir)
+
+# =====================
+# libdrmhwc_utils.a
+# =====================
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+	worker.cpp
+
+LOCAL_MODULE := libdrmhwc_utils
+
+include $(BUILD_STATIC_LIBRARY)
+
+# =====================
+# hwcomposer.drm.so
+# =====================
 include $(CLEAR_VARS)
 
 LOCAL_SHARED_LIBRARIES := \
@@ -28,6 +44,7 @@
 	libui \
 	libutils
 
+LOCAL_STATIC_LIBRARIES := libdrmhwc_utils
 
 LOCAL_C_INCLUDES := \
 	external/drm_gralloc \
@@ -38,11 +55,9 @@
 	system/core/libsync/include \
 
 LOCAL_SRC_FILES := \
-	autolock.cpp \
 	drmresources.cpp \
 	drmcomposition.cpp \
 	drmcompositor.cpp \
-	drmcompositorworker.cpp \
 	drmconnector.cpp \
 	drmcrtc.cpp \
 	drmdisplaycomposition.cpp \
@@ -60,8 +75,7 @@
 	platformnv.cpp \
 	separate_rects.cpp \
 	virtualcompositorworker.cpp \
-	vsyncworker.cpp \
-	worker.cpp
+	vsyncworker.cpp
 
 LOCAL_CPPFLAGS += \
 	-DHWC2_USE_CPP11 \
@@ -80,4 +94,5 @@
 LOCAL_MODULE_SUFFIX := $(TARGET_SHLIB_SUFFIX)
 include $(BUILD_SHARED_LIBRARY)
 
+include $(call all-makefiles-under,$(LOCAL_PATH))
 endif
diff --git a/autolock.cpp b/autolock.cpp
deleted file mode 100644
index 1a2ded7..0000000
--- a/autolock.cpp
+++ /dev/null
@@ -1,56 +0,0 @@
-/*
- * Copyright (C) 2015 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.
- */
-
-#define ATRACE_TAG ATRACE_TAG_GRAPHICS
-#define LOG_TAG "hwc-drm-auto-lock"
-
-#include "autolock.h"
-
-#include <errno.h>
-#include <pthread.h>
-
-#include <cutils/log.h>
-
-namespace android {
-
-int AutoLock::Lock() {
-  if (locked_) {
-    ALOGE("Invalid attempt to double lock AutoLock %s", name_);
-    return -EINVAL;
-  }
-  int ret = pthread_mutex_lock(mutex_);
-  if (ret) {
-    ALOGE("Failed to acquire %s lock %d", name_, ret);
-    return ret;
-  }
-  locked_ = true;
-  return 0;
-}
-
-int AutoLock::Unlock() {
-  if (!locked_) {
-    ALOGE("Invalid attempt to unlock unlocked AutoLock %s", name_);
-    return -EINVAL;
-  }
-  int ret = pthread_mutex_unlock(mutex_);
-  if (ret) {
-    ALOGE("Failed to release %s lock %d", name_, ret);
-    return ret;
-  }
-  locked_ = false;
-  return 0;
-}
-}
diff --git a/autolock.h b/autolock.h
deleted file mode 100644
index 3b824e2..0000000
--- a/autolock.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * Copyright (C) 2015 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 <pthread.h>
-
-namespace android {
-
-class AutoLock {
- public:
-  AutoLock(pthread_mutex_t *mutex, const char *const name)
-      : mutex_(mutex), name_(name) {
-  }
-  ~AutoLock() {
-    if (locked_)
-      Unlock();
-  }
-
-  AutoLock(const AutoLock &rhs) = delete;
-  AutoLock &operator=(const AutoLock &rhs) = delete;
-
-  int Lock();
-  int Unlock();
-
- private:
-  pthread_mutex_t *const mutex_;
-  bool locked_ = false;
-  const char *const name_;
-};
-}
diff --git a/drmcompositorworker.cpp b/drmcompositorworker.cpp
deleted file mode 100644
index 9804322..0000000
--- a/drmcompositorworker.cpp
+++ /dev/null
@@ -1,87 +0,0 @@
-/*
- * Copyright (C) 2015 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.
- */
-
-#define LOG_TAG "hwc-drm-compositor-worker"
-
-#include "drmdisplaycompositor.h"
-#include "drmcompositorworker.h"
-#include "worker.h"
-
-#include <stdlib.h>
-
-#include <cutils/log.h>
-#include <hardware/hardware.h>
-
-namespace android {
-
-static const int64_t kSquashWait = 500000000LL;
-
-DrmCompositorWorker::DrmCompositorWorker(DrmDisplayCompositor *compositor)
-    : Worker("drm-compositor", HAL_PRIORITY_URGENT_DISPLAY),
-      compositor_(compositor) {
-}
-
-DrmCompositorWorker::~DrmCompositorWorker() {
-}
-
-int DrmCompositorWorker::Init() {
-  return InitWorker();
-}
-
-void DrmCompositorWorker::Routine() {
-  int ret;
-  if (!compositor_->HaveQueuedComposites()) {
-    ret = Lock();
-    if (ret) {
-      ALOGE("Failed to lock worker, %d", ret);
-      return;
-    }
-
-    // Only use a timeout if we didn't do a SquashAll last time. This will
-    // prevent wait_ret == -ETIMEDOUT which would trigger a SquashAll and be a
-    // pointless drain on resources.
-    int wait_ret = did_squash_all_ ? WaitForSignalOrExitLocked()
-                                   : WaitForSignalOrExitLocked(kSquashWait);
-
-    ret = Unlock();
-    if (ret) {
-      ALOGE("Failed to unlock worker, %d", ret);
-      return;
-    }
-
-    switch (wait_ret) {
-      case 0:
-        break;
-      case -EINTR:
-        return;
-      case -ETIMEDOUT:
-        ret = compositor_->SquashAll();
-        if (ret)
-          ALOGE("Failed to squash all %d", ret);
-        did_squash_all_ = true;
-        return;
-      default:
-        ALOGE("Failed to wait for signal, %d", wait_ret);
-        return;
-    }
-  }
-
-  ret = compositor_->Composite();
-  if (ret)
-    ALOGE("Failed to composite! %d", ret);
-  did_squash_all_ = false;
-}
-}
diff --git a/drmcompositorworker.h b/drmcompositorworker.h
deleted file mode 100644
index 731bc65..0000000
--- a/drmcompositorworker.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*
- * Copyright (C) 2015 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 ANDROID_DRM_COMPOSITOR_WORKER_H_
-#define ANDROID_DRM_COMPOSITOR_WORKER_H_
-
-#include "worker.h"
-
-namespace android {
-
-class DrmDisplayCompositor;
-
-class DrmCompositorWorker : public Worker {
- public:
-  DrmCompositorWorker(DrmDisplayCompositor *compositor);
-  ~DrmCompositorWorker() override;
-
-  int Init();
-
- protected:
-  void Routine() override;
-
-  DrmDisplayCompositor *compositor_;
-  bool did_squash_all_ = false;
-};
-}
-
-#endif
diff --git a/drmdisplaycompositor.cpp b/drmdisplaycompositor.cpp
index 4ca9302..642a1c7 100644
--- a/drmdisplaycompositor.cpp
+++ b/drmdisplaycompositor.cpp
@@ -19,10 +19,13 @@
 
 #include "drmdisplaycompositor.h"
 
-#include <pthread.h>
 #include <sched.h>
 #include <stdlib.h>
 #include <time.h>
+#include <algorithm>
+#include <bitset>
+#include <cinttypes>
+#include <mutex>
 #include <sstream>
 #include <vector>
 
@@ -31,7 +34,6 @@
 #include <sync/sync.h>
 #include <utils/Trace.h>
 
-#include "autolock.h"
 #include "drmcrtc.h"
 #include "drmplane.h"
 #include "drmresources.h"
@@ -41,6 +43,8 @@
 
 namespace android {
 
+static const int64_t kSquashWait = 500LL;
+
 void SquashState::Init(DrmHwcLayer *layers, size_t num_layers) {
   generation_number_++;
   valid_history_ = 0;
@@ -177,83 +181,53 @@
 }
 
 DrmDisplayCompositor::FrameWorker::FrameWorker(DrmDisplayCompositor *compositor)
-    : Worker("frame-worker", HAL_PRIORITY_URGENT_DISPLAY),
+    : QueueWorker("frame-worker", HAL_PRIORITY_URGENT_DISPLAY),
       compositor_(compositor) {
 }
 
-DrmDisplayCompositor::FrameWorker::~FrameWorker() {
-}
-
 int DrmDisplayCompositor::FrameWorker::Init() {
+  set_max_queue_size(DRM_DISPLAY_COMPOSITOR_MAX_QUEUE_DEPTH);
   return InitWorker();
 }
 
 void DrmDisplayCompositor::FrameWorker::QueueFrame(
     std::unique_ptr<DrmDisplayComposition> composition, int status) {
-  Lock();
+  std::unique_ptr<FrameState> frame(
+      new FrameState(std::move(composition), status));
 
-  // Block queue if it gets too large. Otherwise composition will
-  // start stacking up and eat limited resources (file descriptors)
-  // allocated for these.
-  while (frame_queue_.size() >= DRM_DISPLAY_COMPOSITOR_MAX_QUEUE_DEPTH) {
-    Unlock();
-    sched_yield();
-    Lock();
+  auto start = std::chrono::high_resolution_clock::now();
+  int ret = QueueWork(std::move(frame));
+  if (ret) {
+    ALOGE("Unable to queue frame work (%d)", ret);
+    // TODO: error handling (timeout or exit)
+    return;
   }
+  auto end = std::chrono::high_resolution_clock::now();
 
-  FrameState frame;
-  frame.composition = std::move(composition);
-  frame.status = status;
-  frame_queue_.push(std::move(frame));
-  SignalLocked();
-  Unlock();
+  uint64_t duration_us =
+      std::chrono::duration_cast<std::chrono::microseconds>(end - start)
+          .count();
+  if (duration_us > max_duration_us)
+    max_duration_us = duration_us;
 }
 
-void DrmDisplayCompositor::FrameWorker::Routine() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock worker, %d", ret);
-    return;
-  }
-
-  int wait_ret = 0;
-  if (frame_queue_.empty()) {
-    wait_ret = WaitForSignalOrExitLocked();
-  }
-
-  FrameState frame;
-  if (!frame_queue_.empty()) {
-    frame = std::move(frame_queue_.front());
-    frame_queue_.pop();
-  }
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock worker, %d", ret);
-    return;
-  }
-
-  if (wait_ret == -EINTR) {
-    return;
-  } else if (wait_ret) {
-    ALOGE("Failed to wait for signal, %d", wait_ret);
-    return;
-  }
-  compositor_->ApplyFrame(std::move(frame.composition), frame.status);
+void DrmDisplayCompositor::FrameWorker::ProcessWork(
+    std::unique_ptr<FrameState> frame) {
+  compositor_->ApplyFrame(std::move(frame->composition), frame->status);
 }
 
 DrmDisplayCompositor::DrmDisplayCompositor()
-    : drm_(NULL),
+    : QueueWorker("drm-compositor", HAL_PRIORITY_URGENT_DISPLAY),
+      drm_(NULL),
       display_(-1),
-      worker_(this),
       frame_worker_(this),
-      initialized_(false),
       active_(false),
       use_hw_overlays_(true),
       framebuffer_index_(0),
       squash_framebuffer_index_(0),
       dump_frames_composited_(0),
-      dump_last_timestamp_ns_(0) {
+      dump_last_timestamp_ns_(0),
+      max_duration_us(0) {
   struct timespec ts;
   if (clock_gettime(CLOCK_MONOTONIC, &ts))
     return;
@@ -261,58 +235,32 @@
 }
 
 DrmDisplayCompositor::~DrmDisplayCompositor() {
-  if (!initialized_)
+  if (!initialized())
     return;
 
-  worker_.Exit();
   frame_worker_.Exit();
+  Exit();
 
-  int ret = pthread_mutex_lock(&lock_);
-  if (ret)
-    ALOGE("Failed to acquire compositor lock %d", ret);
+  std::lock_guard<std::mutex> lk(mutex_);
 
   if (mode_.blob_id)
     drm_->DestroyPropertyBlob(mode_.blob_id);
   if (mode_.old_blob_id)
     drm_->DestroyPropertyBlob(mode_.old_blob_id);
 
-  while (!composite_queue_.empty()) {
-    composite_queue_.front().reset();
-    composite_queue_.pop();
-  }
   active_composition_.reset();
-
-  ret = pthread_mutex_unlock(&lock_);
-  if (ret)
-    ALOGE("Failed to acquire compositor lock %d", ret);
-
-  pthread_mutex_destroy(&lock_);
 }
 
 int DrmDisplayCompositor::Init(DrmResources *drm, int display) {
   drm_ = drm;
   display_ = display;
 
-  int ret = pthread_mutex_init(&lock_, NULL);
-  if (ret) {
-    ALOGE("Failed to initialize drm compositor lock %d\n", ret);
-    return ret;
-  }
-  ret = worker_.Init();
-  if (ret) {
-    pthread_mutex_destroy(&lock_);
-    ALOGE("Failed to initialize compositor worker %d\n", ret);
-    return ret;
-  }
-  ret = frame_worker_.Init();
-  if (ret) {
-    pthread_mutex_destroy(&lock_);
-    ALOGE("Failed to initialize frame worker %d\n", ret);
-    return ret;
-  }
+  frame_worker_.Init();
 
-  initialized_ = true;
-  return 0;
+  set_max_queue_size(DRM_DISPLAY_COMPOSITOR_MAX_QUEUE_DEPTH);
+  set_idle_timeout(kSquashWait);
+
+  return InitWorker();
 }
 
 std::unique_ptr<DrmDisplayComposition> DrmDisplayCompositor::CreateComposition()
@@ -343,29 +291,23 @@
       return -ENOENT;
   }
 
-  int ret = pthread_mutex_lock(&lock_);
+  auto start = std::chrono::high_resolution_clock::now();
+
+  int ret = QueueWork(std::move(composition));
   if (ret) {
-    ALOGE("Failed to acquire compositor lock %d", ret);
+    ALOGE("Unable to queue work (%d)", ret);
+    // TODO: error handling (timeout or exit)
     return ret;
   }
 
-  // Block the queue if it gets too large. Otherwise, SurfaceFlinger will start
-  // to eat our buffer handles when we get about 1 second behind.
-  while (composite_queue_.size() >= DRM_DISPLAY_COMPOSITOR_MAX_QUEUE_DEPTH) {
-    pthread_mutex_unlock(&lock_);
-    sched_yield();
-    pthread_mutex_lock(&lock_);
-  }
+  auto end = std::chrono::high_resolution_clock::now();
 
-  composite_queue_.push(std::move(composition));
+  uint64_t duration_us =
+      std::chrono::duration_cast<std::chrono::microseconds>(end - start)
+          .count();
+  if (duration_us > max_duration_us)
+    max_duration_us = duration_us;
 
-  ret = pthread_mutex_unlock(&lock_);
-  if (ret) {
-    ALOGE("Failed to release compositor lock %d", ret);
-    return ret;
-  }
-
-  worker_.Signal();
   return 0;
 }
 
@@ -878,11 +820,7 @@
 }
 
 void DrmDisplayCompositor::ClearDisplay() {
-  AutoLock lock(&lock_, "compositor");
-  int ret = lock.Lock();
-  if (ret)
-    return;
-
+  std::lock_guard<std::mutex> lk(mutex_);
   if (!active_composition_)
     return;
 
@@ -913,19 +851,12 @@
   if (active_composition_)
     active_composition_->SignalCompositionDone();
 
-  ret = pthread_mutex_lock(&lock_);
-  if (ret)
-    ALOGE("Failed to acquire lock for active_composition swap");
-
+  std::lock_guard<std::mutex> lk(mutex_);
   active_composition_.swap(composition);
-
-  if (!ret)
-    ret = pthread_mutex_unlock(&lock_);
-  if (ret)
-    ALOGE("Failed to release lock for active_composition swap");
 }
 
-int DrmDisplayCompositor::Composite() {
+void DrmDisplayCompositor::ProcessWork(
+    std::unique_ptr<DrmDisplayComposition> composition) {
   ATRACE_CALL();
 
   if (!pre_compositor_) {
@@ -933,39 +864,17 @@
     int ret = pre_compositor_->Init();
     if (ret) {
       ALOGE("Failed to initialize OpenGL compositor %d", ret);
-      return ret;
+      return;
     }
   }
 
-  int ret = pthread_mutex_lock(&lock_);
-  if (ret) {
-    ALOGE("Failed to acquire compositor lock %d", ret);
-    return ret;
-  }
-  if (composite_queue_.empty()) {
-    ret = pthread_mutex_unlock(&lock_);
-    if (ret)
-      ALOGE("Failed to release compositor lock %d", ret);
-    return ret;
-  }
-
-  std::unique_ptr<DrmDisplayComposition> composition(
-      std::move(composite_queue_.front()));
-
-  composite_queue_.pop();
-
-  ret = pthread_mutex_unlock(&lock_);
-  if (ret) {
-    ALOGE("Failed to release compositor lock %d", ret);
-    return ret;
-  }
-
+  int ret;
   switch (composition->type()) {
     case DRM_COMPOSITION_TYPE_FRAME:
       ret = PrepareFrame(composition.get());
       if (ret) {
         ALOGE("Failed to prepare frame for display %d", display_);
-        return ret;
+        return;
       }
       if (composition->geometry_changed()) {
         // Send the composition to the kernel to ensure we can commit it. This
@@ -991,7 +900,7 @@
           // to signal the release fences from that composition to avoid
           // hanging.
           ClearDisplay();
-          return ret;
+          return;
         }
       }
       frame_worker_.QueueFrame(std::move(composition), ret);
@@ -1000,7 +909,7 @@
       ret = ApplyDpms(composition.get());
       if (ret)
         ALOGE("Failed to apply dpms for display %d", display_);
-      return ret;
+      break;
     case DRM_COMPOSITION_TYPE_MODESET:
       mode_.mode = composition->display_mode();
       if (mode_.blob_id)
@@ -1008,41 +917,19 @@
       std::tie(ret, mode_.blob_id) = CreateModeBlob(mode_.mode);
       if (ret) {
         ALOGE("Failed to create mode blob for display %d", display_);
-        return ret;
+        return;
       }
       mode_.needs_modeset = true;
-      return 0;
+      break;
     default:
       ALOGE("Unknown composition type %d", composition->type());
-      return -EINVAL;
+      break;
   }
-
-  return ret;
-}
-
-bool DrmDisplayCompositor::HaveQueuedComposites() const {
-  int ret = pthread_mutex_lock(&lock_);
-  if (ret) {
-    ALOGE("Failed to acquire compositor lock %d", ret);
-    return false;
-  }
-
-  bool empty_ret = !composite_queue_.empty();
-
-  ret = pthread_mutex_unlock(&lock_);
-  if (ret) {
-    ALOGE("Failed to release compositor lock %d", ret);
-    return false;
-  }
-
-  return empty_ret;
 }
 
 int DrmDisplayCompositor::SquashAll() {
-  AutoLock lock(&lock_, "compositor");
-  int ret = lock.Lock();
-  if (ret)
-    return ret;
+  std::unique_lock<std::mutex> lk(mutex_);
+  int ret;
 
   if (!active_composition_)
     return 0;
@@ -1051,7 +938,7 @@
   ret = SquashFrame(active_composition_.get(), comp.get());
 
   // ApplyFrame needs the lock
-  lock.Unlock();
+  lk.unlock();
 
   if (!ret)
     ApplyFrame(std::move(comp), 0);
@@ -1183,17 +1070,13 @@
 }
 
 void DrmDisplayCompositor::Dump(std::ostringstream *out) const {
-  int ret = pthread_mutex_lock(&lock_);
-  if (ret)
-    return;
-
+  std::lock_guard<std::mutex> lk(mutex_);
   uint64_t num_frames = dump_frames_composited_;
   dump_frames_composited_ = 0;
 
   struct timespec ts;
-  ret = clock_gettime(CLOCK_MONOTONIC, &ts);
+  int ret = clock_gettime(CLOCK_MONOTONIC, &ts);
   if (ret) {
-    pthread_mutex_unlock(&lock_);
     return;
   }
 
@@ -1207,11 +1090,21 @@
 
   dump_last_timestamp_ns_ = cur_ts;
 
+  *out << "----Jank Stats: "
+       << " compositor_max_q_wait_us=" << max_duration_us
+       << " frameworker_max_q_wait_us=" << frame_worker_.max_duration_us
+       << "\n";
+
+  max_duration_us = 0;
+  frame_worker_.max_duration_us = 0;
+
   if (active_composition_)
     active_composition_->Dump(out);
 
   squash_state_.Dump(out);
+}
 
-  pthread_mutex_unlock(&lock_);
+void DrmDisplayCompositor::ProcessIdle() {
+  SquashAll();
 }
 }
diff --git a/drmdisplaycompositor.h b/drmdisplaycompositor.h
index 9487cac..961fe72 100644
--- a/drmdisplaycompositor.h
+++ b/drmdisplaycompositor.h
@@ -17,13 +17,13 @@
 #ifndef ANDROID_DRM_DISPLAY_COMPOSITOR_H_
 #define ANDROID_DRM_DISPLAY_COMPOSITOR_H_
 
-#include "drmhwcomposer.h"
 #include "drmcomposition.h"
-#include "drmcompositorworker.h"
 #include "drmframebuffer.h"
+#include "drmhwcomposer.h"
+#include "queue_worker.h"
 #include "separate_rects.h"
 
-#include <pthread.h>
+#include <chrono>
 #include <memory>
 #include <queue>
 #include <sstream>
@@ -81,7 +81,7 @@
   std::vector<Region> regions_;
 };
 
-class DrmDisplayCompositor {
+class DrmDisplayCompositor : public QueueWorker<DrmDisplayComposition> {
  public:
   DrmDisplayCompositor();
   ~DrmDisplayCompositor();
@@ -90,39 +90,42 @@
 
   std::unique_ptr<DrmDisplayComposition> CreateComposition() const;
   int QueueComposition(std::unique_ptr<DrmDisplayComposition> composition);
-  int Composite();
+  void ProcessWork(std::unique_ptr<DrmDisplayComposition> composition);
+  void ProcessIdle();
   int SquashAll();
   void Dump(std::ostringstream *out) const;
 
   std::tuple<uint32_t, uint32_t, int> GetActiveModeResolution();
 
-  bool HaveQueuedComposites() const;
-
   SquashState *squash_state() {
     return &squash_state_;
   }
 
  private:
   struct FrameState {
+    FrameState(std::unique_ptr<DrmDisplayComposition> composition, int status)
+        : composition(std::move(composition)), status(status) {
+    }
+
     std::unique_ptr<DrmDisplayComposition> composition;
     int status = 0;
   };
 
-  class FrameWorker : public Worker {
+  class FrameWorker : public QueueWorker<FrameState> {
    public:
     FrameWorker(DrmDisplayCompositor *compositor);
-    ~FrameWorker() override;
 
     int Init();
     void QueueFrame(std::unique_ptr<DrmDisplayComposition> composition,
                     int status);
 
+    mutable uint64_t max_duration_us;
+
    protected:
-    void Routine() override;
+    void ProcessWork(std::unique_ptr<FrameState> frame);
 
    private:
     DrmDisplayCompositor *compositor_;
-    std::queue<FrameState> frame_queue_;
   };
 
   struct ModeState {
@@ -158,13 +161,10 @@
   DrmResources *drm_;
   int display_;
 
-  DrmCompositorWorker worker_;
   FrameWorker frame_worker_;
 
-  std::queue<std::unique_ptr<DrmDisplayComposition>> composite_queue_;
   std::unique_ptr<DrmDisplayComposition> active_composition_;
 
-  bool initialized_;
   bool active_;
   bool use_hw_overlays_;
 
@@ -179,12 +179,13 @@
   DrmFramebuffer squash_framebuffers_[2];
 
   // mutable since we need to acquire in HaveQueuedComposites
-  mutable pthread_mutex_t lock_;
+  mutable std::mutex mutex_;
 
   // State tracking progress since our last Dump(). These are mutable since
   // we need to reset them on every Dump() call.
   mutable uint64_t dump_frames_composited_;
   mutable uint64_t dump_last_timestamp_ns_;
+  mutable uint64_t max_duration_us;
 };
 }
 
diff --git a/drmeventlistener.cpp b/drmeventlistener.cpp
index 2607eef..7a21980 100644
--- a/drmeventlistener.cpp
+++ b/drmeventlistener.cpp
@@ -21,10 +21,10 @@
 #include "drmeventlistener.h"
 #include "drmresources.h"
 
-#include <assert.h>
 #include <linux/netlink.h>
 #include <sys/socket.h>
 
+#include <assert.h>
 #include <cutils/log.h>
 #include <xf86drm.h>
 
diff --git a/hwcomposer.cpp b/hwcomposer.cpp
index e0483e9..875056d 100644
--- a/hwcomposer.cpp
+++ b/hwcomposer.cpp
@@ -33,7 +33,6 @@
 
 #include <errno.h>
 #include <fcntl.h>
-#include <pthread.h>
 #include <sys/param.h>
 #include <sys/resource.h>
 #include <xf86drm.h>
@@ -488,7 +487,8 @@
 
   struct hwc_context_t *ctx = (struct hwc_context_t *)&dev->common;
   hwc_drm_display_t *hd = &ctx->displays[display];
-  return hd->vsync_worker.VSyncControl(enabled);
+  hd->vsync_worker.VSyncControl(enabled);
+  return 0;
 }
 
 static int hwc_set_power_mode(struct hwc_composer_device_1 *dev, int display,
diff --git a/queue_worker.h b/queue_worker.h
new file mode 100644
index 0000000..7e96eec
--- /dev/null
+++ b/queue_worker.h
@@ -0,0 +1,157 @@
+/*
+ * Copyright (C) 2016 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 ANDROID_QUEUE_WORKER_H_
+#define ANDROID_QUEUE_WORKER_H_
+
+#include "worker.h"
+
+#include <queue>
+
+namespace android {
+
+template <typename T>
+class QueueWorker : public Worker {
+ public:
+  static const size_t kDefaultMaxQueueSize = 2;
+  static const int64_t kTimeoutDisabled = -1;
+
+  QueueWorker(const char *name, int priority)
+      : Worker(name, priority),
+        max_queue_size_(kDefaultMaxQueueSize),
+        queue_timeout_ms_(kTimeoutDisabled),
+        idle_timeout_ms_(kTimeoutDisabled),
+        idled_out_(false) {
+  }
+
+  int QueueWork(std::unique_ptr<T> workitem);
+
+  bool IsWorkPending() const {
+    return !queue_.empty();
+  }
+  bool idle() const {
+    return idled_out_;
+  }
+
+  int64_t idle_timeout() {
+    return idle_timeout_ms_;
+  }
+  void set_idle_timeout(int64_t timeout_ms) {
+    idle_timeout_ms_ = timeout_ms;
+  }
+
+  int64_t queue_timeout() {
+    return queue_timeout_ms_;
+  }
+  void set_queue_timeout(int64_t timeout_ms) {
+    queue_timeout_ms_ = timeout_ms;
+  }
+
+  size_t max_queue_size() const {
+    return max_queue_size_;
+  }
+  void set_max_queue_size(size_t size) {
+    max_queue_size_ = size;
+  }
+
+ protected:
+  virtual void ProcessWork(std::unique_ptr<T> workitem) = 0;
+  virtual void ProcessIdle(){}
+  virtual void Routine();
+
+  template <typename Predicate>
+  int WaitCond(std::unique_lock<std::mutex> &lock, Predicate pred,
+               int64_t max_msecs);
+
+ private:
+  std::queue<std::unique_ptr<T>> queue_;
+  size_t max_queue_size_;
+  int64_t queue_timeout_ms_;
+  int64_t idle_timeout_ms_;
+  bool idled_out_;
+};
+
+template <typename T>
+template <typename Predicate>
+int QueueWorker<T>::WaitCond(std::unique_lock<std::mutex> &lock, Predicate pred,
+                             int64_t max_msecs) {
+  bool ret = true;
+  auto wait_func = [&] { return pred() || should_exit(); };
+
+  if (max_msecs < 0) {
+    cond_.wait(lock, wait_func);
+  } else {
+    auto timeout = std::chrono::milliseconds(max_msecs);
+    ret = cond_.wait_for(lock, timeout, wait_func);
+  }
+
+  if (!ret)
+    return -ETIMEDOUT;
+  else if (should_exit())
+    return -EINTR;
+
+  return 0;
+}
+
+template <typename T>
+void QueueWorker<T>::Routine() {
+  std::unique_lock<std::mutex> lk(mutex_);
+  std::unique_ptr<T> workitem;
+
+  auto wait_func = [&] { return !queue_.empty(); };
+  int ret =
+      WaitCond(lk, wait_func, idled_out_ ? kTimeoutDisabled : idle_timeout_ms_);
+  switch (ret) {
+    case 0:
+      break;
+    case -ETIMEDOUT:
+      ProcessIdle();
+      idled_out_ = true;
+      return;
+    case -EINTR:
+    default:
+      return;
+  }
+
+  if (!queue_.empty()) {
+    workitem = std::move(queue_.front());
+    queue_.pop();
+  }
+  lk.unlock();
+  cond_.notify_all();
+
+  idled_out_ = false;
+  ProcessWork(std::move(workitem));
+}
+
+template <typename T>
+int QueueWorker<T>::QueueWork(std::unique_ptr<T> workitem) {
+  std::unique_lock<std::mutex> lk(mutex_);
+
+  auto wait_func = [&] { return queue_.size() < max_queue_size_; };
+  int ret = WaitCond(lk, wait_func, queue_timeout_ms_);
+  if (ret)
+    return ret;
+
+  queue_.push(std::move(workitem));
+  lk.unlock();
+
+  cond_.notify_one();
+
+  return 0;
+}
+};
+#endif
diff --git a/tests/Android.mk b/tests/Android.mk
new file mode 100644
index 0000000..b86cca6
--- /dev/null
+++ b/tests/Android.mk
@@ -0,0 +1,13 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+	queue_worker_test.cpp \
+	worker_test.cpp
+
+LOCAL_MODULE := hwc-drm-tests
+LOCAL_STATIC_LIBRARIES := libdrmhwc_utils
+LOCAL_C_INCLUDES := external/drm_hwcomposer
+
+include $(BUILD_NATIVE_TEST)
diff --git a/tests/queue_worker_test.cpp b/tests/queue_worker_test.cpp
new file mode 100644
index 0000000..d1c0470
--- /dev/null
+++ b/tests/queue_worker_test.cpp
@@ -0,0 +1,201 @@
+#include <gtest/gtest.h>
+#include <hardware/hardware.h>
+
+#include <chrono>
+#include <mutex>
+
+#include "queue_worker.h"
+
+using android::QueueWorker;
+
+#define UNUSED_ARG(x) (void)(x)
+
+struct TestData {
+  TestData(int val) : value(val) {
+  }
+  virtual ~TestData() {
+  }
+
+  virtual void CheckValue(int prev_value) {
+    ASSERT_EQ(prev_value + 1, value);
+  }
+
+  int value;
+};
+
+struct TestQueueWorker : public QueueWorker<TestData> {
+  TestQueueWorker()
+      : QueueWorker("test-queueworker", HAL_PRIORITY_URGENT_DISPLAY), value(0) {
+  }
+
+  int Init() {
+    return InitWorker();
+  }
+
+  void ProcessWork(std::unique_ptr<TestData> data) {
+    std::lock_guard<std::mutex> blk(block);
+    data->CheckValue(value);
+    {
+      std::lock_guard<std::mutex> lk(lock);
+      value = data->value;
+    }
+    cond.notify_one();
+  }
+
+  void ProcessIdle() {
+    ASSERT_FALSE(idle());
+  }
+
+  std::mutex lock;
+  std::mutex block;
+  std::condition_variable cond;
+  int value;
+};
+
+struct QueueWorkerTest : public testing::Test {
+  static const int kTimeoutMs = 1000;
+  TestQueueWorker qw;
+
+  virtual void SetUp() {
+    qw.Init();
+  }
+  bool QueueValue(int val) {
+    std::unique_ptr<TestData> data(new TestData(val));
+    return !qw.QueueWork(std::move(data));
+  }
+
+  bool WaitFor(int val, int timeout_ms = kTimeoutMs) {
+    std::unique_lock<std::mutex> lk(qw.lock);
+
+    auto timeout = std::chrono::milliseconds(timeout_ms);
+    return qw.cond.wait_for(lk, timeout, [&] { return qw.value == val; });
+  }
+};
+
+struct IdleQueueWorkerTest : public QueueWorkerTest {
+  const int64_t kIdleTimeoutMs = 100;
+
+  virtual void SetUp() {
+    qw.set_idle_timeout(kIdleTimeoutMs);
+    qw.Init();
+  }
+};
+
+TEST_F(QueueWorkerTest, single_queue) {
+  // already isInitialized so should fail
+  ASSERT_NE(qw.Init(), 0);
+
+  ASSERT_EQ(qw.value, 0);
+  ASSERT_TRUE(QueueValue(1));
+  ASSERT_TRUE(WaitFor(1));
+  ASSERT_EQ(qw.value, 1);
+  ASSERT_FALSE(qw.IsWorkPending());
+}
+
+TEST_F(QueueWorkerTest, multiple_waits) {
+  for (int i = 1; i <= 100; i++) {
+    ASSERT_TRUE(QueueValue(i));
+    ASSERT_TRUE(WaitFor(i));
+    ASSERT_EQ(qw.value, i);
+    ASSERT_FALSE(qw.IsWorkPending());
+  }
+}
+
+TEST_F(QueueWorkerTest, multiple_queue) {
+  for (int i = 1; i <= 100; i++) {
+    ASSERT_TRUE(QueueValue(i));
+  }
+  ASSERT_TRUE(WaitFor(100));
+  ASSERT_EQ(qw.value, 100);
+  ASSERT_FALSE(qw.IsWorkPending());
+}
+
+TEST_F(QueueWorkerTest, blocking) {
+  // First wait for inital value to be setup
+  ASSERT_TRUE(QueueValue(1));
+  ASSERT_TRUE(WaitFor(1));
+
+  // Block processing and fill up the queue
+  std::unique_lock<std::mutex> lk(qw.block);
+  size_t expected_value = qw.max_queue_size() + 2;
+  for (size_t i = 2; i <= expected_value; i++) {
+    ASSERT_TRUE(QueueValue(i));
+  }
+
+  qw.set_queue_timeout(100);
+  // any additional queueing should fail
+  ASSERT_FALSE(QueueValue(expected_value + 1));
+
+  // make sure value is not changed while blocked
+  {
+    std::unique_lock<std::mutex> lock(qw.lock);
+    auto timeout = std::chrono::milliseconds(100);
+    ASSERT_FALSE(
+        qw.cond.wait_for(lock, timeout, [&] { return qw.value != 1; }));
+  }
+  ASSERT_EQ(qw.value, 1);
+  ASSERT_TRUE(qw.IsWorkPending());
+
+  // unblock and wait for value to be reached
+  lk.unlock();
+  ASSERT_TRUE(WaitFor(expected_value));
+  ASSERT_FALSE(qw.IsWorkPending());
+}
+
+TEST_F(QueueWorkerTest, exit_slow) {
+  struct SlowData : public TestData {
+    SlowData(int val) : TestData(val) {
+    }
+    void CheckValue(int prev_value) {
+      UNUSED_ARG(prev_value);
+
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    }
+  };
+  std::unique_ptr<SlowData> data(new SlowData(1));
+  ASSERT_EQ(qw.QueueWork(std::move(data)), 0);
+  data = std::unique_ptr<SlowData>(new SlowData(2));
+  ASSERT_EQ(qw.QueueWork(std::move(data)), 0);
+  qw.Exit();
+  ASSERT_FALSE(qw.initialized());
+}
+
+TEST_F(QueueWorkerTest, exit_empty) {
+  qw.Exit();
+  ASSERT_FALSE(qw.initialized());
+}
+
+TEST_F(QueueWorkerTest, queue_worker_noidling) {
+  ASSERT_TRUE(QueueValue(1));
+  ASSERT_TRUE(WaitFor(1));
+
+  ASSERT_FALSE(qw.idle());
+  auto timeout = std::chrono::milliseconds(200);
+  std::this_thread::sleep_for(timeout);
+  ASSERT_FALSE(qw.idle());
+}
+
+TEST_F(IdleQueueWorkerTest, queue_worker_idling) {
+  ASSERT_TRUE(QueueValue(1));
+  ASSERT_TRUE(WaitFor(1));
+  ASSERT_FALSE(qw.idle());
+
+  auto timeout = std::chrono::milliseconds(kIdleTimeoutMs + 10);
+  std::this_thread::sleep_for(timeout);
+  ASSERT_TRUE(qw.idle());
+  ASSERT_TRUE(QueueValue(2));
+  ASSERT_TRUE(WaitFor(2));
+  ASSERT_FALSE(qw.idle());
+
+  std::this_thread::sleep_for(3 * timeout);
+  ASSERT_TRUE(qw.idle());
+
+  ASSERT_TRUE(QueueValue(3));
+  ASSERT_TRUE(WaitFor(3));
+  for (int i = 4; i <= 100; i++) {
+    QueueValue(i);
+  }
+  ASSERT_FALSE(qw.idle());
+  qw.Exit();
+  ASSERT_FALSE(qw.initialized());
+}
\ No newline at end of file
diff --git a/tests/worker_test.cpp b/tests/worker_test.cpp
new file mode 100644
index 0000000..38f91db
--- /dev/null
+++ b/tests/worker_test.cpp
@@ -0,0 +1,110 @@
+#include <gtest/gtest.h>
+#include <hardware/hardware.h>
+
+#include <chrono>
+
+#include "worker.h"
+
+using android::Worker;
+
+struct TestWorker : public Worker {
+  TestWorker()
+      : Worker("test-worker", HAL_PRIORITY_URGENT_DISPLAY),
+        value(0),
+        enabled_(false) {
+  }
+
+  int Init() {
+    return InitWorker();
+  }
+
+  void Routine() {
+    Lock();
+    if (!enabled_) {
+      int ret = WaitForSignalOrExitLocked();
+      if (ret == -EINTR) {
+        Unlock();
+        return;
+      }
+      // should only reached here if it was enabled
+      if (!enabled_)
+        printf("Shouldn't reach here while disabled %d %d\n", value, ret);
+    }
+    value++;
+    Unlock();
+  }
+
+  void Control(bool enable) {
+    bool changed = false;
+    Lock();
+    if (enabled_ != enable) {
+      enabled_ = enable;
+      changed = true;
+    }
+    Unlock();
+
+    if (enable && changed)
+      Signal();
+  }
+
+  int value;
+
+ private:
+  bool enabled_;
+};
+
+struct WorkerTest : public testing::Test {
+  TestWorker worker;
+
+  virtual void SetUp() {
+    worker.Init();
+  }
+
+  void small_delay() {
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  }
+};
+
+TEST_F(WorkerTest, test_worker) {
+  // already isInitialized so should fail
+  ASSERT_TRUE(worker.initialized());
+
+  int val = worker.value;
+  small_delay();
+
+  // value shouldn't change when isInitialized
+  ASSERT_EQ(val, worker.value);
+
+  worker.Control(true);
+  small_delay();
+
+  // while locked, value shouldn't be changing
+  worker.Lock();
+  val = worker.value;
+  small_delay();
+  ASSERT_EQ(val, worker.value);
+  worker.Unlock();
+
+  small_delay();
+  // value should be different now
+  ASSERT_NE(val, worker.value);
+
+  worker.Control(false);
+  worker.Lock();
+  val = worker.value;
+  worker.Unlock();
+  small_delay();
+
+  // value should be same
+  ASSERT_EQ(val, worker.value);
+
+  worker.Exit();
+  ASSERT_FALSE(worker.initialized());
+}
+
+TEST_F(WorkerTest, exit_while_running) {
+  worker.Control(true);
+
+  std::this_thread::sleep_for(std::chrono::milliseconds(50));
+  worker.Exit();
+}
diff --git a/virtualcompositorworker.cpp b/virtualcompositorworker.cpp
index 92a1634..c1a6d2f 100644
--- a/virtualcompositorworker.cpp
+++ b/virtualcompositorworker.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 The Android Open Source Project
+ * Copyright (C) 2015-2016 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.
@@ -17,15 +17,8 @@
 #define LOG_TAG "hwc-virtual-compositor-worker"
 
 #include "virtualcompositorworker.h"
-#include "worker.h"
-
-#include <errno.h>
-#include <stdlib.h>
 
 #include <cutils/log.h>
-#include <hardware/hardware.h>
-#include <hardware/hwcomposer.h>
-#include <sched.h>
 #include <sw_sync.h>
 #include <sync/sync.h>
 
@@ -35,7 +28,7 @@
 static const int kAcquireWaitTimeoutMs = 3000;
 
 VirtualCompositorWorker::VirtualCompositorWorker()
-    : Worker("virtual-compositor", HAL_PRIORITY_URGENT_DISPLAY),
+    : QueueWorker("virtual-compositor", HAL_PRIORITY_URGENT_DISPLAY),
       timeline_fd_(-1),
       timeline_(0),
       timeline_current_(0) {
@@ -56,6 +49,8 @@
     return ret;
   }
   timeline_fd_ = ret;
+
+  set_max_queue_size(kMaxQueueDepth);
   return InitWorker();
 }
 
@@ -81,50 +76,7 @@
 
   composition->release_timeline = timeline_;
 
-  Lock();
-  while (composite_queue_.size() >= kMaxQueueDepth) {
-    Unlock();
-    sched_yield();
-    Lock();
-  }
-
-  composite_queue_.push(std::move(composition));
-  SignalLocked();
-  Unlock();
-}
-
-void VirtualCompositorWorker::Routine() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock worker, %d", ret);
-    return;
-  }
-
-  int wait_ret = 0;
-  if (composite_queue_.empty()) {
-    wait_ret = WaitForSignalOrExitLocked();
-  }
-
-  std::unique_ptr<VirtualComposition> composition;
-  if (!composite_queue_.empty()) {
-    composition = std::move(composite_queue_.front());
-    composite_queue_.pop();
-  }
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock worker, %d", ret);
-    return;
-  }
-
-  if (wait_ret == -EINTR) {
-    return;
-  } else if (wait_ret) {
-    ALOGE("Failed to wait for signal, %d", wait_ret);
-    return;
-  }
-
-  Compose(std::move(composition));
+  QueueWork(std::move(composition));
 }
 
 int VirtualCompositorWorker::CreateNextTimelineFence() {
@@ -144,7 +96,7 @@
   return ret;
 }
 
-void VirtualCompositorWorker::Compose(
+void VirtualCompositorWorker::ProcessWork(
     std::unique_ptr<VirtualComposition> composition) {
   if (!composition.get())
     return;
diff --git a/virtualcompositorworker.h b/virtualcompositorworker.h
index 1fc5e43..885cf31 100644
--- a/virtualcompositorworker.h
+++ b/virtualcompositorworker.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 The Android Open Source Project
+ * Copyright (C) 2015-2016 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.
@@ -18,13 +18,17 @@
 #define ANDROID_VIRTUAL_COMPOSITOR_WORKER_H_
 
 #include "drmhwcomposer.h"
-#include "worker.h"
-
-#include <queue>
+#include "queue_worker.h"
 
 namespace android {
 
-class VirtualCompositorWorker : public Worker {
+struct VirtualComposition {
+  UniqueFd outbuf_acquire_fence;
+  std::vector<UniqueFd> layer_acquire_fences;
+  int release_timeline;
+};
+
+class VirtualCompositorWorker : public QueueWorker<VirtualComposition> {
  public:
   VirtualCompositorWorker();
   ~VirtualCompositorWorker() override;
@@ -33,20 +37,12 @@
   void QueueComposite(hwc_display_contents_1_t *dc);
 
  protected:
-  void Routine() override;
+  void ProcessWork(std::unique_ptr<VirtualComposition> composition);
 
  private:
-  struct VirtualComposition {
-    UniqueFd outbuf_acquire_fence;
-    std::vector<UniqueFd> layer_acquire_fences;
-    int release_timeline;
-  };
-
   int CreateNextTimelineFence();
   int FinishComposition(int timeline);
-  void Compose(std::unique_ptr<VirtualComposition> composition);
 
-  std::queue<std::unique_ptr<VirtualComposition>> composite_queue_;
   int timeline_fd_;
   int timeline_;
   int timeline_current_;
diff --git a/vsyncworker.cpp b/vsyncworker.cpp
index cc9c96b..2177521 100644
--- a/vsyncworker.cpp
+++ b/vsyncworker.cpp
@@ -48,41 +48,19 @@
   return InitWorker();
 }
 
-int VSyncWorker::RegisterCallback(std::shared_ptr<VsyncCallback> callback) {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock vsync worker lock %d\n", ret);
-    return ret;
-  }
-
+void VSyncWorker::RegisterCallback(std::shared_ptr<VsyncCallback> callback) {
+  Lock();
   callback_ = callback;
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock vsync worker lock %d\n", ret);
-    return ret;
-  }
-  return 0;
+  Unlock();
 }
 
-int VSyncWorker::VSyncControl(bool enabled) {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock vsync worker lock %d\n", ret);
-    return ret;
-  }
-
+void VSyncWorker::VSyncControl(bool enabled) {
+  Lock();
   enabled_ = enabled;
   last_timestamp_ = -1;
-  int signal_ret = SignalLocked();
+  Unlock();
 
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock vsync worker lock %d\n", ret);
-    return ret;
-  }
-
-  return signal_ret;
+  Signal();
 }
 
 /*
@@ -135,12 +113,9 @@
 }
 
 void VSyncWorker::Routine() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock worker %d", ret);
-    return;
-  }
+  int ret;
 
+  Lock();
   if (!enabled_) {
     ret = WaitForSignalOrExitLocked();
     if (ret == -EINTR) {
@@ -152,10 +127,7 @@
   int display = display_;
   std::shared_ptr<VsyncCallback> callback(callback_);
 
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock worker %d", ret);
-  }
+  Unlock();
 
   if (!enabled)
     return;
diff --git a/vsyncworker.h b/vsyncworker.h
index a1ba1a5..787152e 100644
--- a/vsyncworker.h
+++ b/vsyncworker.h
@@ -41,9 +41,9 @@
   ~VSyncWorker() override;
 
   int Init(DrmResources *drm, int display);
-  int RegisterCallback(std::shared_ptr<VsyncCallback> callback);
+  void RegisterCallback(std::shared_ptr<VsyncCallback> callback);
 
-  int VSyncControl(bool enabled);
+  void VSyncControl(bool enabled);
 
  protected:
   void Routine() override;
diff --git a/worker.cpp b/worker.cpp
index 1cebedc..da6c580 100644
--- a/worker.cpp
+++ b/worker.cpp
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 The Android Open Source Project
+ * Copyright (C) 2015-2016 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.
@@ -14,190 +14,81 @@
  * limitations under the License.
  */
 
-#define LOG_TAG "hwc-drm-worker"
-
 #include "worker.h"
 
-#include <errno.h>
-#include <pthread.h>
-#include <stdlib.h>
+#include <sys/prctl.h>
 #include <sys/resource.h>
-#include <sys/signal.h>
-#include <time.h>
-
-#include <cutils/log.h>
 
 namespace android {
 
-static const int64_t kBillion = 1000000000LL;
-
 Worker::Worker(const char *name, int priority)
     : name_(name), priority_(priority), exit_(false), initialized_(false) {
 }
 
 Worker::~Worker() {
-  if (!initialized_)
-    return;
-
-  pthread_kill(thread_, SIGTERM);
-  pthread_cond_destroy(&cond_);
-  pthread_mutex_destroy(&lock_);
+  Exit();
 }
 
 int Worker::InitWorker() {
-  pthread_condattr_t cond_attr;
-  pthread_condattr_init(&cond_attr);
-  pthread_condattr_setclock(&cond_attr, CLOCK_MONOTONIC);
-  int ret = pthread_cond_init(&cond_, &cond_attr);
-  if (ret) {
-    ALOGE("Failed to int thread %s condition %d", name_.c_str(), ret);
-    return ret;
-  }
+  std::lock_guard<std::mutex> lk(mutex_);
+  if (initialized())
+    return -EALREADY;
 
-  ret = pthread_mutex_init(&lock_, NULL);
-  if (ret) {
-    ALOGE("Failed to init thread %s lock %d", name_.c_str(), ret);
-    pthread_cond_destroy(&cond_);
-    return ret;
-  }
-
-  ret = pthread_create(&thread_, NULL, InternalRoutine, this);
-  if (ret) {
-    ALOGE("Could not create thread %s %d", name_.c_str(), ret);
-    pthread_mutex_destroy(&lock_);
-    pthread_cond_destroy(&cond_);
-    return ret;
-  }
+  thread_ = std::unique_ptr<std::thread>(
+      new std::thread(&Worker::InternalRoutine, this));
   initialized_ = true;
+  exit_ = false;
+
   return 0;
 }
 
-bool Worker::initialized() const {
-  return initialized_;
-}
-
-int Worker::Lock() {
-  return pthread_mutex_lock(&lock_);
-}
-
-int Worker::Unlock() {
-  return pthread_mutex_unlock(&lock_);
-}
-
-int Worker::SignalLocked() {
-  return SignalThreadLocked(false);
-}
-
-int Worker::ExitLocked() {
-  int signal_ret = SignalThreadLocked(true);
-  if (signal_ret)
-    ALOGE("Failed to signal thread %s with exit %d", name_.c_str(), signal_ret);
-
-  int join_ret = pthread_join(thread_, NULL);
-  if (join_ret && join_ret != ESRCH)
-    ALOGE("Failed to join thread %s in exit %d", name_.c_str(), join_ret);
-
-  return signal_ret | join_ret;
-}
-
-int Worker::Signal() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to acquire lock in Signal() %d\n", ret);
-    return ret;
+void Worker::Exit() {
+  std::unique_lock<std::mutex> lk(mutex_);
+  exit_ = true;
+  if (initialized()) {
+    lk.unlock();
+    cond_.notify_all();
+    thread_->join();
+    initialized_ = false;
   }
-
-  int signal_ret = SignalLocked();
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to release lock in Signal() %d\n", ret);
-    return ret;
-  }
-  return signal_ret;
-}
-
-int Worker::Exit() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to acquire lock in Exit() %d\n", ret);
-    return ret;
-  }
-
-  int exit_ret = ExitLocked();
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to release lock in Exit() %d\n", ret);
-    return ret;
-  }
-  return exit_ret;
 }
 
 int Worker::WaitForSignalOrExitLocked(int64_t max_nanoseconds) {
-  if (exit_)
+  int ret = 0;
+  if (should_exit())
     return -EINTR;
 
-  int ret = 0;
+  std::unique_lock<std::mutex> lk(mutex_, std::adopt_lock);
   if (max_nanoseconds < 0) {
-    ret = pthread_cond_wait(&cond_, &lock_);
-  } else {
-    struct timespec abs_deadline;
-    ret = clock_gettime(CLOCK_MONOTONIC, &abs_deadline);
-    if (ret)
-      return ret;
-    int64_t nanos = (int64_t)abs_deadline.tv_nsec + max_nanoseconds;
-    abs_deadline.tv_sec += nanos / kBillion;
-    abs_deadline.tv_nsec = nanos % kBillion;
-    ret = pthread_cond_timedwait(&cond_, &lock_, &abs_deadline);
-    if (ret == ETIMEDOUT)
-      ret = -ETIMEDOUT;
+    cond_.wait(lk);
+  } else if (std::cv_status::timeout ==
+             cond_.wait_for(lk, std::chrono::nanoseconds(max_nanoseconds))) {
+    ret = -ETIMEDOUT;
   }
 
-  if (exit_)
-    return -EINTR;
+  // exit takes precedence on timeout
+  if (should_exit())
+    ret = -EINTR;
+
+  // release leaves mutex locked when going out of scope
+  lk.release();
 
   return ret;
 }
 
-// static
-void *Worker::InternalRoutine(void *arg) {
-  Worker *worker = (Worker *)arg;
+void Worker::InternalRoutine() {
+  setpriority(PRIO_PROCESS, 0, priority_);
+  prctl(PR_SET_NAME, name_.c_str());
 
-  setpriority(PRIO_PROCESS, 0, worker->priority_);
+  std::unique_lock<std::mutex> lk(mutex_, std::defer_lock);
 
   while (true) {
-    int ret = worker->Lock();
-    if (ret) {
-      ALOGE("Failed to lock %s thread %d", worker->name_.c_str(), ret);
-      continue;
-    }
+    lk.lock();
+    if (should_exit())
+      return;
+    lk.unlock();
 
-    bool exit = worker->exit_;
-
-    ret = worker->Unlock();
-    if (ret) {
-      ALOGE("Failed to unlock %s thread %d", worker->name_.c_str(), ret);
-      break;
-    }
-    if (exit)
-      break;
-
-    worker->Routine();
+    Routine();
   }
-  return NULL;
-}
-
-int Worker::SignalThreadLocked(bool exit) {
-  if (exit)
-    exit_ = exit;
-
-  int ret = pthread_cond_signal(&cond_);
-  if (ret) {
-    ALOGE("Failed to signal condition on %s thread %d", name_.c_str(), ret);
-    return ret;
-  }
-
-  return 0;
 }
 }
diff --git a/worker.h b/worker.h
index 7015178..8f6295b 100644
--- a/worker.h
+++ b/worker.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2015 The Android Open Source Project
+ * Copyright (C) 2015-2016 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.
@@ -17,33 +17,39 @@
 #ifndef ANDROID_WORKER_H_
 #define ANDROID_WORKER_H_
 
-#include <pthread.h>
 #include <stdint.h>
+#include <stdlib.h>
 #include <string>
 
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
 namespace android {
 
 class Worker {
  public:
-  int Lock();
-  int Unlock();
+  void Lock() {
+    mutex_.lock();
+  }
+  void Unlock() {
+    mutex_.unlock();
+  }
 
-  // Must be called with the lock acquired
-  int SignalLocked();
-  int ExitLocked();
+  void Signal() {
+    cond_.notify_all();
+  }
+  void Exit();
 
-  // Convenience versions of above, acquires the lock
-  int Signal();
-  int Exit();
+  bool initialized() const {
+    return initialized_;
+  }
 
  protected:
   Worker(const char *name, int priority);
   virtual ~Worker();
 
   int InitWorker();
-
-  bool initialized() const;
-
   virtual void Routine() = 0;
 
   /*
@@ -54,22 +60,22 @@
    */
   int WaitForSignalOrExitLocked(int64_t max_nanoseconds = -1);
 
- private:
-  static void *InternalRoutine(void *worker);
+  bool should_exit() const {
+    return exit_;
+  }
 
-  // Must be called with the lock acquired
-  int SignalThreadLocked(bool exit);
+  std::mutex mutex_;
+  std::condition_variable cond_;
+
+ private:
+  void InternalRoutine();
 
   std::string name_;
   int priority_;
 
-  pthread_t thread_;
-  pthread_mutex_t lock_;
-  pthread_cond_t cond_;
-
+  std::unique_ptr<std::thread> thread_;
   bool exit_;
   bool initialized_;
 };
 }
-
 #endif