core: mutex: add support timeout condvar

Add support timeout condvar based on timeout notify

Signed-off-by: Gavin Liu <gavin.liu@mediatek.com>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>
Reviewed-by: Etienne Carriere <etienne.carriere@foss.st.com>

core/include/kernel/mutex.h

@@ -95,10 +95,26 @@ void condvar_broadcast_debug(struct condvar *cv, const char *fname, int lineno);
 void condvar_wait_debug(struct condvar *cv, struct mutex *m,
 			const char *fname, int lineno);
 #define condvar_wait(cv, m) condvar_wait_debug((cv), (m), __FILE__, __LINE__)
+
+/*
+ * Return TEE_ERROR_TIMEOUT if the normal world returns before
+ * the condvar has been signaled.
+ */
+TEE_Result condvar_wait_timeout_debug(struct condvar *cv, struct mutex *m,
+				      uint32_t timeout_ms, const char *fname,
+				      int lineno);
+#define condvar_wait_timeout(cv, m, timeout_ms) \
+	condvar_wait_timeout_debug((cv), (m), (timeout_ms), __FILE__, __LINE__)
 #else
 void condvar_signal(struct condvar *cv);
 void condvar_broadcast(struct condvar *cv);
 void condvar_wait(struct condvar *cv, struct mutex *m);
+/*
+ * Return TEE_ERROR_TIMEOUT if the normal world returns before
+ * the condvar has been signaled.
+ */
+TEE_Result condvar_wait_timeout(struct condvar *cv, struct mutex *m,
+				uint32_t timeout_ms);
 #endif
 
 #endif /*__KERNEL_MUTEX_H*/

core/kernel/mutex.c

@@ -61,7 +61,7 @@ static void __mutex_lock(struct mutex *m, const char *fname, int lineno)
 			 * Someone else is holding the lock, wait in normal
 			 * world for the lock to become available.
 			 */
-			wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+			wq_wait_final(&m->wq, &wqe, 0, m, fname, lineno);
 		} else
 			return;
 	}
@@ -206,7 +206,7 @@ static void __mutex_read_lock(struct mutex *m, const char *fname, int lineno)
 			 * Someone else is holding the lock, wait in normal
 			 * world for the lock to become available.
 			 */
-			wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+			wq_wait_final(&m->wq, &wqe, 0, m, fname, lineno);
 		} else
 			return;
 	}
@@ -427,13 +427,15 @@ void condvar_broadcast(struct condvar *cv)
 }
 #endif /*CFG_MUTEX_DEBUG*/
 
-static void __condvar_wait(struct condvar *cv, struct mutex *m,
-			const char *fname, int lineno)
+static TEE_Result __condvar_wait_timeout(struct condvar *cv, struct mutex *m,
+					 uint32_t timeout_ms, const char *fname,
+					 int lineno)
 {
-	uint32_t old_itr_status;
-	struct wait_queue_elem wqe;
-	short old_state;
-	short new_state;
+	TEE_Result res = TEE_SUCCESS;
+	uint32_t old_itr_status = 0;
+	struct wait_queue_elem wqe = { };
+	short old_state = 0;
+	short new_state = 0;
 
 	mutex_unlock_check(m);
 
@@ -468,23 +470,38 @@ static void __condvar_wait(struct condvar *cv, struct mutex *m,
 	if (!new_state)
 		wq_wake_next(&m->wq, m, fname, lineno);
 
-	wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+	res = wq_wait_final(&m->wq, &wqe, timeout_ms, m, fname, lineno);
 
 	if (old_state > 0)
 		mutex_read_lock(m);
 	else
 		mutex_lock(m);
+
+	return res;
 }
 
 #ifdef CFG_MUTEX_DEBUG
 void condvar_wait_debug(struct condvar *cv, struct mutex *m,
 			const char *fname, int lineno)
 {
-	__condvar_wait(cv, m, fname, lineno);
+	__condvar_wait_timeout(cv, m, 0, fname, lineno);
+}
+
+TEE_Result condvar_wait_timeout_debug(struct condvar *cv, struct mutex *m,
+				      uint32_t timeout_ms, const char *fname,
+				      int lineno)
+{
+	return __condvar_wait_timeout(cv, m, timeout_ms, fname, lineno);
 }
 #else
 void condvar_wait(struct condvar *cv, struct mutex *m)
 {
-	__condvar_wait(cv, m, NULL, -1);
+	__condvar_wait_timeout(cv, m, 0, NULL, -1);
+}
+
+TEE_Result condvar_wait_timeout(struct condvar *cv, struct mutex *m,
+				uint32_t timeout_ms)
+{
+	return __condvar_wait_timeout(cv, m, timeout_ms, NULL, -1);
 }
 #endif