diff --git a/Android.mk b/Android.mk
index 98ce3a6..2c4f7e3 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 \
@@ -60,8 +77,7 @@
 	platformnv.cpp \
 	separate_rects.cpp \
 	virtualcompositorworker.cpp \
-	vsyncworker.cpp \
-	worker.cpp
+	vsyncworker.cpp
 
 LOCAL_CPPFLAGS += \
 	-DHWC2_USE_CPP11 \
@@ -80,4 +96,5 @@
 LOCAL_MODULE_SUFFIX := $(TARGET_SHLIB_SUFFIX)
 include $(BUILD_SHARED_LIBRARY)
 
+include $(call all-makefiles-under,$(LOCAL_PATH))
 endif
diff --git a/drmcompositorworker.cpp b/drmcompositorworker.cpp
index 9804322..a4e7fc9 100644
--- a/drmcompositorworker.cpp
+++ b/drmcompositorworker.cpp
@@ -44,23 +44,14 @@
 void DrmCompositorWorker::Routine() {
   int ret;
   if (!compositor_->HaveQueuedComposites()) {
-    ret = Lock();
-    if (ret) {
-      ALOGE("Failed to lock worker, %d", ret);
-      return;
-    }
+    Lock();
 
     // 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;
-    }
+    Unlock();
 
     switch (wait_ret) {
       case 0:
diff --git a/drmdisplaycompositor.cpp b/drmdisplaycompositor.cpp
index 4ca9302..b0d1f1e 100644
--- a/drmdisplaycompositor.cpp
+++ b/drmdisplaycompositor.cpp
@@ -205,18 +205,14 @@
   frame.composition = std::move(composition);
   frame.status = status;
   frame_queue_.push(std::move(frame));
-  SignalLocked();
   Unlock();
+  Signal();
 }
 
 void DrmDisplayCompositor::FrameWorker::Routine() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock worker, %d", ret);
-    return;
-  }
-
   int wait_ret = 0;
+
+  Lock();
   if (frame_queue_.empty()) {
     wait_ret = WaitForSignalOrExitLocked();
   }
@@ -226,12 +222,7 @@
     frame = std::move(frame_queue_.front());
     frame_queue_.pop();
   }
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock worker, %d", ret);
-    return;
-  }
+  Unlock();
 
   if (wait_ret == -EINTR) {
     return;
diff --git a/drmhwctwo.cpp b/drmhwctwo.cpp
index 138f5fa..b13fce1 100644
--- a/drmhwctwo.cpp
+++ b/drmhwctwo.cpp
@@ -236,11 +236,7 @@
     hwc2_callback_data_t data, hwc2_function_pointer_t func) {
   supported(__func__);
   auto callback = std::make_shared<DrmVsyncCallback>(data, func);
-  int ret = vsync_worker_.RegisterCallback(std::move(callback));
-  if (ret) {
-    ALOGE("Failed to register callback d=%" PRIu64 " ret=%d", handle_, ret);
-    return HWC2::Error::BadDisplay;
-  }
+  vsync_worker_.RegisterCallback(std::move(callback));
   return HWC2::Error::None;
 }
 
diff --git a/hwcomposer.cpp b/hwcomposer.cpp
index e0483e9..c0aafef 100644
--- a/hwcomposer.cpp
+++ b/hwcomposer.cpp
@@ -488,7 +488,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/tests/Android.mk b/tests/Android.mk
new file mode 100644
index 0000000..5bbda93
--- /dev/null
+++ b/tests/Android.mk
@@ -0,0 +1,12 @@
+LOCAL_PATH := $(call my-dir)
+
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := \
+	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/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..639dc86 100644
--- a/virtualcompositorworker.cpp
+++ b/virtualcompositorworker.cpp
@@ -89,18 +89,14 @@
   }
 
   composite_queue_.push(std::move(composition));
-  SignalLocked();
   Unlock();
+  Signal();
 }
 
 void VirtualCompositorWorker::Routine() {
-  int ret = Lock();
-  if (ret) {
-    ALOGE("Failed to lock worker, %d", ret);
-    return;
-  }
-
   int wait_ret = 0;
+
+  Lock();
   if (composite_queue_.empty()) {
     wait_ret = WaitForSignalOrExitLocked();
   }
@@ -110,12 +106,7 @@
     composition = std::move(composite_queue_.front());
     composite_queue_.pop();
   }
-
-  ret = Unlock();
-  if (ret) {
-    ALOGE("Failed to unlock worker, %d", ret);
-    return;
-  }
+  Unlock();
 
   if (wait_ret == -EINTR) {
     return;
diff --git a/vsyncworker.cpp b/vsyncworker.cpp
index cc9c96b..3ad16fe 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) {
@@ -151,11 +126,7 @@
   bool enabled = enabled_;
   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..47aeb86 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,79 @@
  * 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;
-  }
+  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;
+
   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() {
+  if (initialized()) {
+    Lock();
+    exit_ = true;
+    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 lock unlocked when returning
+  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