diff --git a/camera/QCamera2/HAL3/QCamera3HWI.cpp b/camera/QCamera2/HAL3/QCamera3HWI.cpp
index 227ca00..e4782aa 100644
--- a/camera/QCamera2/HAL3/QCamera3HWI.cpp
+++ b/camera/QCamera2/HAL3/QCamera3HWI.cpp
@@ -50,6 +50,7 @@
 #include "QCamera3Channel.h"
 #include "QCamera3PostProc.h"
 #include "QCamera3VendorTags.h"
+#include "cam_cond.h"
 
 using namespace android;
 
@@ -364,7 +365,8 @@
     // TODO: hardcode for now until mctl add support for min_num_pp_bufs
     //TBD - To see if this hardcoding is needed. Check by printing if this is filled by mctl to 3
     gCamCapability[cameraId]->min_num_pp_bufs = 3;
-    pthread_cond_init(&mRequestCond, NULL);
+
+    PTHREAD_COND_INIT(&mRequestCond);
     mPendingLiveRequest = 0;
     mCurrentRequestId = -1;
     pthread_mutex_init(&mMutex, NULL);
@@ -3826,7 +3828,7 @@
     // Added a timed condition wait
     struct timespec ts;
     uint8_t isValidTimeout = 1;
-    rc = clock_gettime(CLOCK_REALTIME, &ts);
+    rc = clock_gettime(CLOCK_MONOTONIC, &ts);
     if (rc < 0) {
       isValidTimeout = 0;
       ALOGE("%s: Error reading the real time clock!!", __func__);
diff --git a/camera/QCamera2/stack/common/cam_cond.h b/camera/QCamera2/stack/common/cam_cond.h
new file mode 100644
index 0000000..52a6d71
--- /dev/null
+++ b/camera/QCamera2/stack/common/cam_cond.h
@@ -0,0 +1,34 @@
+/*
+ * 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 CAM_COND_H
+#define CAM_COND_H
+
+#define PTHREAD_COND_INIT(cond) \
+  ({                                   \
+    int rc = 0;                       \
+    pthread_condattr_t cond_attr;     \
+    rc = pthread_condattr_init(&cond_attr);   \
+    if (rc == 0) {                            \
+      rc = pthread_condattr_setclock(&cond_attr, CLOCK_MONOTONIC);  \
+      if (rc == 0) {                                 \
+        rc = pthread_cond_init(cond, &cond_attr);  \
+      } \
+    } \
+    rc; \
+  })
+
+#endif // CAM_COND_H
diff --git a/camera/QCamera2/stack/common/cam_semaphore.h b/camera/QCamera2/stack/common/cam_semaphore.h
index a52f907..f4260c2 100644
--- a/camera/QCamera2/stack/common/cam_semaphore.h
+++ b/camera/QCamera2/stack/common/cam_semaphore.h
@@ -30,6 +30,11 @@
 #ifndef __QCAMERA_SEMAPHORE_H__
 #define __QCAMERA_SEMAPHORE_H__
 
+// System dependencies
+#include <pthread.h>
+#include <errno.h>
+#include "cam_cond.h"
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -48,7 +53,7 @@
 static inline void cam_sem_init(cam_semaphore_t *s, int n)
 {
     pthread_mutex_init(&(s->mutex), NULL);
-    pthread_cond_init(&(s->cond), NULL);
+    PTHREAD_COND_INIT(&(s->cond));
     s->val = n;
 }
 
@@ -71,6 +76,27 @@
     return rc;
 }
 
+static inline int cam_sem_timedwait(cam_semaphore_t *s, const struct timespec *abs_timeout)
+{
+    int rc = 0;
+    pthread_mutex_lock(&(s->mutex));
+    while (s->val == 0 && rc != ETIMEDOUT)
+        rc = pthread_cond_timedwait(&(s->cond), &(s->mutex), abs_timeout);
+
+    if (s->val > 0)
+        s->val--;
+
+    pthread_mutex_unlock(&(s->mutex));
+
+    /* sem_timedwait returns -1 for failure case, and failure code is in errno
+     */
+    if (rc != 0) {
+        errno = rc;
+        rc = -1;
+    }
+    return rc;
+}
+
 static inline void cam_sem_destroy(cam_semaphore_t *s)
 {
     pthread_mutex_destroy(&(s->mutex));
diff --git a/camera/QCamera2/stack/mm-camera-interface/Android.mk b/camera/QCamera2/stack/mm-camera-interface/Android.mk
index ff71773..cef137d 100644
--- a/camera/QCamera2/stack/mm-camera-interface/Android.mk
+++ b/camera/QCamera2/stack/mm-camera-interface/Android.mk
@@ -27,6 +27,8 @@
 LOCAL_COPY_HEADERS_TO := mm-camera-interface
 LOCAL_COPY_HEADERS += ../common/cam_intf.h
 LOCAL_COPY_HEADERS += ../common/cam_types.h
+LOCAL_COPY_HEADERS += ../common/cam_cond.h
+LOCAL_COPY_HEADERS += ../common/cam_semaphore.h
 
 LOCAL_C_INCLUDES := \
     $(LOCAL_PATH)/inc \
diff --git a/camera/QCamera2/stack/mm-camera-interface/src/mm_camera.c b/camera/QCamera2/stack/mm-camera-interface/src/mm_camera.c
index 1cf357f..9e6f30f 100644
--- a/camera/QCamera2/stack/mm-camera-interface/src/mm_camera.c
+++ b/camera/QCamera2/stack/mm-camera-interface/src/mm_camera.c
@@ -43,6 +43,7 @@
 #include "mm_camera_sock.h"
 #include "mm_camera_interface.h"
 #include "mm_camera.h"
+#include "cam_cond.h"
 
 #define SET_PARM_BIT32(parm, parm_arr) \
     (parm_arr[parm/32] |= (1<<(parm%32)))
@@ -340,7 +341,7 @@
 
     pthread_mutex_init(&my_obj->cb_lock, NULL);
     pthread_mutex_init(&my_obj->evt_lock, NULL);
-    pthread_cond_init(&my_obj->evt_cond, NULL);
+    PTHREAD_COND_INIT(&my_obj->evt_cond);
 
     CDBG("%s : Launch evt Thread in Cam Open",__func__);
     snprintf(my_obj->evt_thread.threadName, THREAD_NAME_SIZE, "CAM_Dispatch");
@@ -1713,7 +1714,7 @@
 
     pthread_mutex_lock(&my_obj->evt_lock);
     while (!(my_obj->evt_rcvd.server_event_type & evt_mask)) {
-        clock_gettime(CLOCK_REALTIME, &ts);
+        clock_gettime(CLOCK_MONOTONIC, &ts);
         ts.tv_sec += WAIT_TIMEOUT;
         rc = pthread_cond_timedwait(&my_obj->evt_cond, &my_obj->evt_lock, &ts);
         if (rc == ETIMEDOUT) {
diff --git a/camera/QCamera2/stack/mm-camera-test/Android.mk b/camera/QCamera2/stack/mm-camera-test/Android.mk
index ddfc976..43c4371 100644
--- a/camera/QCamera2/stack/mm-camera-test/Android.mk
+++ b/camera/QCamera2/stack/mm-camera-test/Android.mk
@@ -222,3 +222,19 @@
 
 LOCAL_MODULE:= libmm-qcamera
 include $(BUILD_SHARED_LIBRARY)
+
+# Build cam_semaphore_tests
+include $(CLEAR_VARS)
+
+LOCAL_SRC_FILES := src/cam_semaphore_tests.cpp
+
+LOCAL_C_INCLUDES += $(LOCAL_PATH)/../common
+
+LOCAL_CFLAGS := -Wall -Wextra -Werror
+
+LOCAL_MODULE := cam_semaphore_tests
+LOCAL_MODULE_TAGS := tests
+
+include $(BUILD_NATIVE_TEST)
+
+LOCAL_PATH := $(OLD_LOCAL_PATH)
diff --git a/camera/QCamera2/stack/mm-camera-test/src/cam_semaphore_tests.cpp b/camera/QCamera2/stack/mm-camera-test/src/cam_semaphore_tests.cpp
new file mode 100644
index 0000000..98ec45b
--- /dev/null
+++ b/camera/QCamera2/stack/mm-camera-test/src/cam_semaphore_tests.cpp
@@ -0,0 +1,77 @@
+/*
+ * Copyright 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.
+ */
+
+//#define LOG_NDEBUG 0
+#define LOG_TAG "cam_semaphore_tests"
+#include <utils/Log.h>
+
+#include <gtest/gtest.h>
+
+#include "cam_semaphore.h"
+
+#define NS_PER_S 1000000000
+
+//10 ms is about standard timer resolution for most non-RTOS.
+#define TIME_THRESHOLD_IN_NS  10000000
+
+static inline void timespec_add_ms(timespec& ts, size_t ms) {
+    ts.tv_sec  += ms / 1000;
+    ts.tv_nsec += (ms % 1000) * 1000000;
+    if (ts.tv_nsec >= NS_PER_S) {
+        ts.tv_sec++;
+        ts.tv_nsec -= NS_PER_S;
+    }
+}
+
+static inline int64_t time_diff(timespec& ts_start, timespec& ts_end) {
+    if (ts_start.tv_sec == ts_end.tv_sec) {
+        return (int64_t)ts_end.tv_nsec - ts_start.tv_nsec;
+    } else {
+        return (int64_t)(ts_end.tv_sec - 1 - ts_start.tv_sec) * NS_PER_S +
+                ts_end.tv_nsec + NS_PER_S - ts_start.tv_nsec;
+    }
+}
+
+// Test cam_semaphore_timedwait
+TEST(cam_semaphore_tests, cam_semaphore_timedwait) {
+
+    cam_semaphore_t sem;
+    cam_sem_init(&sem, 0);
+
+    // Test timeout
+    timespec ts;
+    ASSERT_EQ(0, clock_gettime(CLOCK_MONOTONIC, &ts));
+    timespec_add_ms(ts, 100);
+
+    errno = 0;
+    ASSERT_EQ(-1, cam_sem_timedwait(&sem, &ts));
+    timespec ts_end;
+    clock_gettime(CLOCK_MONOTONIC, &ts_end);
+
+    ASSERT_EQ(ETIMEDOUT, errno);
+    // Check time after timeout ~= time before call + timeout
+    ASSERT_GE(time_diff(ts, ts_end), 0);
+    ASSERT_LT(time_diff(ts, ts_end), TIME_THRESHOLD_IN_NS);
+
+    // Test successful wait
+    ASSERT_EQ(0, clock_gettime(CLOCK_MONOTONIC, &ts));
+    timespec_add_ms(ts, 100);
+
+    errno = 0;
+    cam_sem_post(&sem);
+    ASSERT_EQ(0, cam_sem_timedwait(&sem, &ts));
+    ASSERT_EQ(0, errno);
+}