drm_hwcomposer: Move event worker into VSyncWorker

Using the new Worker interface, split out the event worker
now that it just handles vsyncs.

Change-Id: I7ebc6237b10411fdba3b8826935921b8c83c4b6c
Signed-off-by: Sean Paul <seanpaul@chromium.org>
diff --git a/Android.mk b/Android.mk
index 262ddfe..8024900 100644
--- a/Android.mk
+++ b/Android.mk
@@ -48,6 +48,7 @@
 	drmproperty.cpp \
 	hwcomposer.cpp \
         nvimporter.cpp \
+	vsyncworker.cpp \
 	worker.cpp
 
 ifeq ($(strip $(BOARD_DRM_HWCOMPOSER_BUFFER_IMPORTER)),nvidia-gralloc)
diff --git a/hwcomposer.cpp b/hwcomposer.cpp
index a809c8b..afb0ebe 100644
--- a/hwcomposer.cpp
+++ b/hwcomposer.cpp
@@ -19,6 +19,7 @@
 #include "drm_hwcomposer.h"
 #include "drmresources.h"
 #include "importer.h"
+#include "vsyncworker.h"
 
 #include <errno.h>
 #include <fcntl.h>
@@ -42,21 +43,13 @@
 
 namespace android {
 
-struct hwc_worker {
-  pthread_t thread;
-  pthread_mutex_t lock;
-  pthread_cond_t cond;
-  bool exit;
-};
-
 typedef struct hwc_drm_display {
   struct hwc_context_t *ctx;
   int display;
 
   std::vector<uint32_t> config_ids;
 
-  bool enable_vsync_events;
-  unsigned int vsync_sequence;
+  VSyncWorker vsync_worker;
 } hwc_drm_display_t;
 
 struct hwc_context_t {
@@ -74,8 +67,6 @@
   hwc_composer_device_1_t device;
   hwc_procs_t const *procs;
 
-  struct hwc_worker event_worker;
-
   DisplayMap displays;
   DrmResources drm;
   Importer *importer;
@@ -147,87 +138,6 @@
   delete composition;
 }
 
-static int hwc_queue_vblank_event(struct hwc_drm_display *hd) {
-  DrmCrtc *crtc = hd->ctx->drm.GetCrtcForDisplay(hd->display);
-  if (!crtc) {
-    ALOGE("Failed to get crtc for display");
-    return -ENODEV;
-  }
-
-  drmVBlank vblank;
-  memset(&vblank, 0, sizeof(vblank));
-
-  uint32_t high_crtc = (crtc->pipe() << DRM_VBLANK_HIGH_CRTC_SHIFT);
-  vblank.request.type = (drmVBlankSeqType)(
-      DRM_VBLANK_ABSOLUTE | DRM_VBLANK_NEXTONMISS | DRM_VBLANK_EVENT |
-      (high_crtc & DRM_VBLANK_HIGH_CRTC_MASK));
-  vblank.request.signal = (unsigned long)hd;
-  vblank.request.sequence = hd->vsync_sequence + 1;
-
-  int ret = drmWaitVBlank(hd->ctx->drm.fd(), &vblank);
-  if (ret) {
-    ALOGE("Failed to wait for vblank %d", ret);
-    return ret;
-  }
-
-  return 0;
-}
-
-static void hwc_vblank_event_handler(int /* fd */, unsigned int sequence,
-                                     unsigned int tv_sec, unsigned int tv_usec,
-                                     void *user_data) {
-  struct hwc_drm_display *hd = (struct hwc_drm_display *)user_data;
-
-  if (!hd->enable_vsync_events || !hd->ctx->procs->vsync)
-    return;
-
-  /*
-   * Discard duplicate vsync (can happen when enabling vsync events while
-   * already processing vsyncs).
-   */
-  if (sequence <= hd->vsync_sequence)
-    return;
-
-  hd->vsync_sequence = sequence;
-  int ret = hwc_queue_vblank_event(hd);
-  if (ret)
-    ALOGE("Failed to queue vblank event ret=%d", ret);
-
-  int64_t timestamp =
-      (int64_t)tv_sec * 1000 * 1000 * 1000 + (int64_t)tv_usec * 1000;
-  hd->ctx->procs->vsync(hd->ctx->procs, hd->display, timestamp);
-}
-
-static void *hwc_event_worker(void *arg) {
-  setpriority(PRIO_PROCESS, 0, HAL_PRIORITY_URGENT_DISPLAY);
-
-  struct hwc_context_t *ctx = (struct hwc_context_t *)arg;
-  do {
-    fd_set fds;
-    FD_ZERO(&fds);
-    FD_SET(ctx->drm.fd(), &fds);
-
-    drmEventContext event_context;
-    event_context.version = DRM_EVENT_CONTEXT_VERSION;
-    event_context.page_flip_handler = NULL;
-    event_context.vblank_handler = hwc_vblank_event_handler;
-
-    int ret;
-    do {
-      ret = select(ctx->drm.fd() + 1, &fds, NULL, NULL, NULL);
-    } while (ret == -1 && errno == EINTR);
-
-    if (ret != 1) {
-      ALOGE("Failed waiting for drm event\n");
-      continue;
-    }
-
-    drmHandleEvent(ctx->drm.fd(), &event_context);
-  } while (true);
-
-  return NULL;
-}
-
 static int hwc_add_layer(int display, hwc_context_t *ctx, hwc_layer_1_t *layer,
                          Composition *composition) {
   hwc_drm_bo_t bo;
@@ -322,36 +232,12 @@
 
 static int hwc_event_control(struct hwc_composer_device_1 *dev, int display,
                              int event, int enabled) {
-  struct hwc_context_t *ctx = (struct hwc_context_t *)&dev->common;
-  struct hwc_drm_display *hd = &ctx->displays[display];
   if (event != HWC_EVENT_VSYNC || (enabled != 0 && enabled != 1))
     return -EINVAL;
 
-  DrmCrtc *crtc = ctx->drm.GetCrtcForDisplay(display);
-  if (!crtc) {
-    ALOGD("Can't service events for display %d, no crtc", display);
-    return -EINVAL;
-  }
-
-  hd->enable_vsync_events = !!enabled;
-
-  if (!hd->enable_vsync_events)
-    return 0;
-
-  /*
-   * Note that it's possible that the event worker is already waiting for
-   * a vsync, and this will be a duplicate request. In that event, we'll
-   * end up firing the event handler twice, and it will discard the second
-   * event. Not ideal, but not worth introducing a bunch of additional
-   * logic/locks/state for.
-   */
-  int ret = hwc_queue_vblank_event(hd);
-  if (ret) {
-    ALOGE("Failed to queue vblank event ret=%d", ret);
-    return ret;
-  }
-
-  return 0;
+  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);
 }
 
 static int hwc_set_power_mode(struct hwc_composer_device_1 *dev, int display,
@@ -396,6 +282,11 @@
   struct hwc_context_t *ctx = (struct hwc_context_t *)&dev->common;
 
   ctx->procs = procs;
+
+  for (hwc_context_t::DisplayMapIter iter = ctx->displays.begin();
+       iter != ctx->displays.end(); ++iter) {
+    iter->second.vsync_worker.SetProcs(procs);
+  }
 }
 
 static int hwc_get_display_configs(struct hwc_composer_device_1 *dev,
@@ -519,67 +410,12 @@
   return ret;
 }
 
-static int hwc_destroy_worker(struct hwc_worker *worker) {
-  int ret = pthread_mutex_lock(&worker->lock);
-  if (ret) {
-    ALOGE("Failed to lock in destroy() %d", ret);
-    return ret;
-  }
-
-  worker->exit = true;
-
-  ret |= pthread_cond_signal(&worker->cond);
-  if (ret)
-    ALOGE("Failed to signal cond in destroy() %d", ret);
-
-  ret |= pthread_mutex_unlock(&worker->lock);
-  if (ret)
-    ALOGE("Failed to unlock in destroy() %d", ret);
-
-  ret |= pthread_join(worker->thread, NULL);
-  if (ret && ret != ESRCH)
-    ALOGE("Failed to join thread in destroy() %d", ret);
-
-  return ret;
-}
-
 static int hwc_device_close(struct hw_device_t *dev) {
   struct hwc_context_t *ctx = (struct hwc_context_t *)dev;
-
-  if (hwc_destroy_worker(&ctx->event_worker))
-    ALOGE("Destroy event worker failed");
-
   delete ctx;
   return 0;
 }
 
-static int hwc_initialize_worker(struct hwc_worker *worker,
-                                 void *(*routine)(void *), void *arg) {
-  int ret = pthread_cond_init(&worker->cond, NULL);
-  if (ret) {
-    ALOGE("Failed to create worker condition %d", ret);
-    return ret;
-  }
-
-  ret = pthread_mutex_init(&worker->lock, NULL);
-  if (ret) {
-    ALOGE("Failed to initialize worker lock %d", ret);
-    pthread_cond_destroy(&worker->cond);
-    return ret;
-  }
-
-  worker->exit = false;
-
-  ret = pthread_create(&worker->thread, NULL, routine, arg);
-  if (ret) {
-    ALOGE("Could not create worker thread %d", ret);
-    pthread_mutex_destroy(&worker->lock);
-    pthread_cond_destroy(&worker->cond);
-    return ret;
-  }
-  return 0;
-}
-
 /*
  * TODO: This function sets the active config to the first one in the list. This
  * should be fixed such that it selects the preferred mode for the display, or
@@ -606,8 +442,6 @@
   hwc_drm_display_t *hd = &ctx->displays[display];
   hd->ctx = ctx;
   hd->display = display;
-  hd->enable_vsync_events = false;
-  hd->vsync_sequence = 0;
 
   int ret = hwc_set_initial_config(hd);
   if (ret) {
@@ -615,6 +449,12 @@
     return ret;
   }
 
+  ret = hd->vsync_worker.Init(&ctx->drm, display);
+  if (ret) {
+    ALOGE("Failed to create event worker for display %d %d\n", display, ret);
+    return ret;
+  }
+
   return 0;
 }
 
@@ -666,13 +506,6 @@
     return ret;
   }
 
-  ret = hwc_initialize_worker(&ctx->event_worker, hwc_event_worker, ctx);
-  if (ret) {
-    ALOGE("Failed to create event worker %d\n", ret);
-    delete ctx;
-    return ret;
-  }
-
   ctx->device.common.tag = HARDWARE_DEVICE_TAG;
   ctx->device.common.version = HWC_DEVICE_API_VERSION_1_4;
   ctx->device.common.module = const_cast<hw_module_t *>(module);
diff --git a/vsyncworker.cpp b/vsyncworker.cpp
new file mode 100644
index 0000000..9626022
--- /dev/null
+++ b/vsyncworker.cpp
@@ -0,0 +1,203 @@
+/*
+ * 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-vsync-worker"
+
+#include "drmresources.h"
+#include "vsyncworker.h"
+#include "worker.h"
+
+#include <map>
+#include <stdlib.h>
+#include <time.h>
+#include <xf86drm.h>
+#include <xf86drmMode.h>
+
+#include <cutils/log.h>
+#include <hardware/hardware.h>
+
+namespace android {
+
+VSyncWorker::VSyncWorker()
+    : Worker("vsync", HAL_PRIORITY_URGENT_DISPLAY),
+      drm_(NULL),
+      procs_(NULL),
+      display_(-1),
+      last_timestamp_(-1) {
+}
+
+VSyncWorker::~VSyncWorker() {
+}
+
+int VSyncWorker::Init(DrmResources *drm, int display) {
+  drm_ = drm;
+  display_ = display;
+
+  return InitWorker();
+}
+
+int VSyncWorker::SetProcs(hwc_procs_t const *procs) {
+  int ret = Lock();
+  if (ret) {
+    ALOGE("Failed to lock vsync worker lock %d\n", ret);
+    return ret;
+  }
+
+  procs_ = procs;
+
+  ret = Unlock();
+  if (ret) {
+    ALOGE("Failed to unlock vsync worker lock %d\n", ret);
+    return ret;
+  }
+  return 0;
+}
+
+int VSyncWorker::VSyncControl(bool enabled) {
+  int ret = Lock();
+  if (ret) {
+    ALOGE("Failed to lock vsync worker lock %d\n", ret);
+    return ret;
+  }
+
+  enabled_ = enabled;
+  last_timestamp_ = -1;
+  int signal_ret = SignalLocked();
+
+  ret = Unlock();
+  if (ret) {
+    ALOGE("Failed to unlock vsync worker lock %d\n", ret);
+    return ret;
+  }
+
+  return signal_ret;
+}
+
+/*
+ * Returns the timestamp of the next vsync in phase with last_timestamp_.
+ * For example:
+ *  last_timestamp_ = 137
+ *  frame_ns = 50
+ *  current = 683
+ *
+ *  ret = (50 * ((683 - 137)/50 + 1)) + 137
+ *  ret = 687
+ *
+ *  Thus, we must sleep until timestamp 687 to maintain phase with the last
+ *  timestamp.
+ */
+int64_t VSyncWorker::GetPhasedVSync(int64_t frame_ns, int64_t current) {
+  if (last_timestamp_ < 0)
+    return current + frame_ns;
+
+  return frame_ns * ((current - last_timestamp_) / frame_ns + 1) +
+         last_timestamp_;
+}
+
+static const int64_t kOneSecondNs = 1 * 1000 * 1000 * 1000;
+
+int VSyncWorker::SyntheticWaitVBlank(int64_t *timestamp) {
+  struct timespec vsync;
+  int ret = clock_gettime(CLOCK_MONOTONIC, &vsync);
+
+  int64_t refresh = 60;  // Default to 60Hz refresh rate
+  DrmConnector *conn = drm_->GetConnectorForDisplay(display_);
+  if (conn && conn->active_mode().v_refresh())
+    refresh = conn->active_mode().v_refresh();
+  else
+    ALOGW("Vsync worker active with conn=%p refresh=%d\n", conn,
+          conn ? conn->active_mode().v_refresh() : -1);
+
+  int64_t phased_timestamp = GetPhasedVSync(
+      kOneSecondNs / refresh, vsync.tv_sec * kOneSecondNs + vsync.tv_nsec);
+  vsync.tv_sec = phased_timestamp / kOneSecondNs;
+  vsync.tv_nsec = phased_timestamp - (vsync.tv_sec * kOneSecondNs);
+  do {
+    ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &vsync, NULL);
+  } while (ret == -1 && errno == EINTR);
+  if (ret)
+    return ret;
+
+  *timestamp = (int64_t)vsync.tv_sec * kOneSecondNs + (int64_t)vsync.tv_nsec;
+  return 0;
+}
+
+void VSyncWorker::Routine() {
+  int ret = Lock();
+  if (ret) {
+    ALOGE("Failed to lock worker %d", ret);
+    return;
+  }
+
+  if (!enabled_) {
+    ret = WaitForSignalOrExitLocked();
+    if (ret == -EINTR) {
+      return;
+    }
+  }
+
+  bool enabled = enabled_;
+  int display = display_;
+  hwc_procs_t const *procs = procs_;
+
+  ret = Unlock();
+  if (ret) {
+    ALOGE("Failed to unlock worker %d", ret);
+  }
+
+  if (!enabled)
+    return;
+
+  DrmCrtc *crtc = drm_->GetCrtcForDisplay(display);
+  if (!crtc) {
+    ALOGE("Failed to get crtc for display");
+    return;
+  }
+  uint32_t high_crtc = (crtc->pipe() << DRM_VBLANK_HIGH_CRTC_SHIFT);
+
+  drmVBlank vblank;
+  memset(&vblank, 0, sizeof(vblank));
+  vblank.request.type = (drmVBlankSeqType)(
+      DRM_VBLANK_RELATIVE | (high_crtc & DRM_VBLANK_HIGH_CRTC_MASK));
+  vblank.request.sequence = 1;
+
+  int64_t timestamp;
+  ret = drmWaitVBlank(drm_->fd(), &vblank);
+  if (ret == -EINTR) {
+    return;
+  } else if (ret) {
+    ret = SyntheticWaitVBlank(&timestamp);
+    if (ret)
+      return;
+  } else {
+    timestamp = (int64_t)vblank.reply.tval_sec * kOneSecondNs +
+                (int64_t)vblank.reply.tval_usec * 1000;
+  }
+
+  /*
+   * There's a race here where a change in procs_ will not take effect until
+   * the next subsequent requested vsync. This is unavoidable since we can't
+   * call the vsync hook while holding the thread lock.
+   *
+   * We could shorten the race window by caching procs_ right before calling
+   * the hook. However, in practice, procs_ is only updated once, so it's not
+   * worth the overhead.
+   */
+  if (procs && procs->vsync)
+    procs->vsync(procs, display, timestamp);
+  last_timestamp_ = timestamp;
+}
+}
diff --git a/vsyncworker.h b/vsyncworker.h
new file mode 100644
index 0000000..ce7b94a
--- /dev/null
+++ b/vsyncworker.h
@@ -0,0 +1,57 @@
+/*
+ * 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_EVENT_WORKER_H_
+#define ANDROID_EVENT_WORKER_H_
+
+#include "drmresources.h"
+#include "worker.h"
+
+#include <map>
+#include <stdint.h>
+
+#include <hardware/hardware.h>
+#include <hardware/hwcomposer.h>
+
+namespace android {
+
+class VSyncWorker : public Worker {
+ public:
+  VSyncWorker();
+  ~VSyncWorker();
+
+  int Init(DrmResources *drm, int display);
+  int SetProcs(hwc_procs_t const *procs);
+
+  int VSyncControl(bool enabled);
+
+ protected:
+  virtual void Routine();
+
+ private:
+  int64_t GetPhasedVSync(int64_t frame_ns, int64_t current);
+  int SyntheticWaitVBlank(int64_t *timestamp);
+
+  DrmResources *drm_;
+  hwc_procs_t const *procs_;
+
+  int display_;
+  bool enabled_;
+  int64_t last_timestamp_;
+};
+}
+
+#endif