core: notif: add support timeout notify

Add support timeout notification, allowing to
avoid waiting indefinitely for the completion of an event

Signed-off-by: Gavin Liu <gavin.liu@mediatek.com>

core/include/kernel/notif.h

@@ -106,6 +106,11 @@ void notif_free_async_value(uint32_t value);
  */
 TEE_Result notif_wait(uint32_t value);
 
+/*
+ * Wait timeout in normal world for a value to be sent by notif_send()
+ */
+TEE_Result notif_wait_timeout(uint32_t value, uint32_t tmo);
+
 /*
  * Send an asynchronous value, note that it must be <= NOTIF_ASYNC_VALUE_MAX
  */
@@ -124,6 +129,13 @@ static inline void notif_send_async(uint32_t value __unused)
  */
 TEE_Result notif_send_sync(uint32_t value);
 
+/*
+ * Send a synchronous value with timeout, note that it must be <=
+ * NOTIF_VALUE_MAX. The notification is synchronous even if the value
+ * happens to belong in the asynchronous range.
+ */
+TEE_Result notif_send_sync_timeout(uint32_t value, uint32_t tmo);
+
 /*
  * Called by device drivers.
  */

core/include/kernel/wait_queue.h

@@ -48,6 +48,11 @@ static inline void wq_wait_init(struct wait_queue *wq,
 void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
 		   const void *sync_obj, const char *fname, int lineno);
 
+/* Waits for the wait queue element to the awakened or timed out */
+uint32_t wq_wait_final_timeout(struct wait_queue *wq, struct wait_queue_elem *wqe,
+			       uint32_t tmo, const void *sync_obj, const char *fname,
+			       int lineno);
+
 /* Wakes up the first wait queue element in the wait queue, if there is one */
 void wq_wake_next(struct wait_queue *wq, const void *sync_obj,
 		const char *fname, int lineno);

core/kernel/notif.c

@@ -117,19 +117,29 @@ void notif_deliver_event(enum notif_event ev)
 }
 #endif /*CFG_CORE_ASYNC_NOTIF*/
 
-static TEE_Result notif_rpc(uint32_t func, uint32_t value)
+static TEE_Result notif_rpc(uint32_t func, uint32_t value, uint32_t tmo)
 {
-	struct thread_param params = THREAD_PARAM_VALUE(IN, func, value, 0);
+	struct thread_param params = THREAD_PARAM_VALUE(IN, func, value, tmo);
 
 	return thread_rpc_cmd(OPTEE_RPC_CMD_NOTIFICATION, 1, &params);
 }
 
 TEE_Result notif_wait(uint32_t value)
 {
-	return notif_rpc(OPTEE_RPC_NOTIFICATION_WAIT, value);
+	return notif_rpc(OPTEE_RPC_NOTIFICATION_WAIT, value, 0);
 }
 
 TEE_Result notif_send_sync(uint32_t value)
 {
-	return notif_rpc(OPTEE_RPC_NOTIFICATION_SEND, value);
+	return notif_rpc(OPTEE_RPC_NOTIFICATION_SEND, value, 0);
+}
+
+TEE_Result notif_wait_timeout(uint32_t value, uint32_t tmo)
+{
+	return notif_rpc(OPTEE_RPC_NOTIFICATION_WAIT, value, tmo);
+}
+
+TEE_Result notif_send_sync_timeout(uint32_t value, uint32_t tmo)
+{
+	return notif_rpc(OPTEE_RPC_NOTIFICATION_SEND, value, tmo);
 }

core/kernel/wait_queue.c

@@ -20,7 +20,7 @@ void wq_init(struct wait_queue *wq)
 	*wq = (struct wait_queue)WAIT_QUEUE_INITIALIZER;
 }
 
-static void do_notif(TEE_Result (*fn)(uint32_t), int id,
+static uint32_t do_notif(TEE_Result (*fn)(uint32_t, uint32_t), int id, uint32_t tmo,
 		     const char *cmd_str __maybe_unused,
 		     const void *sync_obj __maybe_unused,
 		     const char *fname, int lineno __maybe_unused)
@@ -33,9 +33,14 @@ static void do_notif(TEE_Result (*fn)(uint32_t), int id,
 	else
 		DMSG("%s thread %d %p", cmd_str, id, sync_obj);
 
-	res = fn(id + NOTIF_SYNC_VALUE_BASE);
+	res = fn(id + NOTIF_SYNC_VALUE_BASE, tmo);
 	if (res)
 		DMSG("%s thread %d res %#"PRIx32, cmd_str, id, res);
+
+	if (res != TEE_SUCCESS && res != TEE_ERROR_BUSY)
+		DMSG("%s thread %u ret 0x%x", cmd_str, id, res);
+
+	return res;
 }
 
 static void slist_add_tail(struct wait_queue *wq, struct wait_queue_elem *wqe)
@@ -69,24 +74,45 @@ void wq_wait_init_condvar(struct wait_queue *wq, struct wait_queue_elem *wqe,
 	cpu_spin_unlock_xrestore(&wq_spin_lock, old_itr_status);
 }
 
-void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
-		   const void *sync_obj, const char *fname, int lineno)
+static uint32_t wq_wait_final_helper(struct wait_queue *wq,
+				     struct wait_queue_elem *wqe,
+				     uint32_t tmo, const void *sync_obj,
+				     const char *fname, int lineno)
 {
 	uint32_t old_itr_status;
 	unsigned done;
+	uint32_t ret;
 
 	do {
-		do_notif(notif_wait, wqe->handle,
+		ret = do_notif(notif_wait_timeout, wqe->handle, tmo,
 			 "sleep", sync_obj, fname, lineno);
 
 		old_itr_status = cpu_spin_lock_xsave(&wq_spin_lock);
 
+		if (ret == TEE_ERROR_BUSY) /* timed-out */
+			wqe->done = true;
+
 		done = wqe->done;
 		if (done)
 			SLIST_REMOVE(wq, wqe, wait_queue_elem, link);
 
 		cpu_spin_unlock_xrestore(&wq_spin_lock, old_itr_status);
 	} while (!done);
+
+	return ret;
+}
+
+void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
+		   const void *sync_obj, const char *fname, int lineno)
+{
+	wq_wait_final_helper(wq, wqe, 0, sync_obj, fname, lineno);
+}
+
+uint32_t wq_wait_final_timeout(struct wait_queue *wq, struct wait_queue_elem *wqe,
+			       uint32_t tmo, const void *sync_obj,
+			       const char *fname, int lineno)
+{
+	return wq_wait_final_helper(wq, wqe, tmo, sync_obj, fname, lineno);
 }
 
 void wq_wake_next(struct wait_queue *wq, const void *sync_obj,
@@ -130,7 +156,7 @@ void wq_wake_next(struct wait_queue *wq, const void *sync_obj,
 		cpu_spin_unlock_xrestore(&wq_spin_lock, old_itr_status);
 
 		if (do_wakeup)
-			do_notif(notif_send_sync, handle,
+			do_notif(notif_send_sync_timeout, handle, 0,
 				 "wake ", sync_obj, fname, lineno);
 
 		if (!do_wakeup || !wake_read)