Implement light semaphores (#369)

libctru/include/3ds/synchronization.h

@@ -19,6 +19,14 @@ typedef struct
 	LightLock lock; ///< Lock used for sticky timer operation
 } LightEvent;
 
+/// A light semaphore.
+typedef struct
+{
+	s32 current_count;      ///< The current release count of the semaphore
+	s16 num_threads_acq;    ///< Number of threads concurrently acquiring the semaphore
+	s16 max_count;          ///< The maximum release count of the semaphore
+} LightSemaphore;
+
 /// Performs a Data Synchronization Barrier operation.
 static inline void __dsb(void)
 {
@@ -56,6 +64,56 @@ static inline bool __strex(s32* addr, s32 val)
 	return res;
 }
 
+/**
+ * @brief Performs a ldrexh operation.
+ * @param addr Address to perform the operation on.
+ * @return The resulting value.
+ */
+static inline u16 __ldrexh(u16* addr)
+{
+	u16 val;
+	__asm__ __volatile__("ldrexh %[val], %[addr]" : [val] "=r" (val) : [addr] "Q" (*addr));
+	return val;
+}
+
+/**
+ * @brief Performs a strexh operation.
+ * @param addr Address to perform the operation on.
+ * @param val Value to store.
+ * @return Whether the operation was successful.
+ */
+static inline bool __strexh(u16* addr, u16 val)
+{
+	bool res;
+	__asm__ __volatile__("strexh %[res], %[val], %[addr]" : [res] "=&r" (res) : [val] "r" (val), [addr] "Q" (*addr));
+	return res;
+}
+
+/**
+ * @brief Performs a ldrexb operation.
+ * @param addr Address to perform the operation on.
+ * @return The resulting value.
+ */
+static inline u8 __ldrexb(u8* addr)
+{
+	u8 val;
+	__asm__ __volatile__("ldrexb %[val], %[addr]" : [val] "=r" (val) : [addr] "Q" (*addr));
+	return val;
+}
+
+/**
+ * @brief Performs a strexb operation.
+ * @param addr Address to perform the operation on.
+ * @param val Value to store.
+ * @return Whether the operation was successful.
+ */
+static inline bool __strexb(u8* addr, u8 val)
+{
+	bool res;
+	__asm__ __volatile__("strexb %[res], %[val], %[addr]" : [res] "=&r" (res) : [val] "r" (val), [addr] "Q" (*addr));
+	return res;
+}
+
 /// Performs an atomic pre-increment operation.
 #define AtomicIncrement(ptr) __atomic_add_fetch((u32*)(ptr), 1, __ATOMIC_SEQ_CST)
 /// Performs an atomic pre-decrement operation.
@@ -160,3 +218,25 @@ int LightEvent_TryWait(LightEvent* event);
  * @param event Pointer to the event.
  */
 void LightEvent_Wait(LightEvent* event);
+
+/**
+ * @brief Initializes a light semaphore.
+ * @param event Pointer to the semaphore.
+ * @param max_count Initial count of the semaphore.
+ * @param max_count Maximum count of the semaphore.
+ */
+void LightSemaphore_Init(LightSemaphore* semaphore, s16 initial_count, s16 max_count);
+
+/**
+ * @brief Acquires a light semaphore.
+ * @param semaphore Pointer to the semaphore.
+ * @param count Acquire count
+ */
+void LightSemaphore_Acquire(LightSemaphore* semaphore, s32 count);
+
+/**
+ * @brief Releases a light semaphore.
+ * @param semaphore Pointer to the semaphore.
+ * @param count Release count
+ */
+void LightSemaphore_Release(LightSemaphore* semaphore, s32 count);

libctru/source/synchronization.c

@@ -216,3 +216,52 @@ void LightEvent_Wait(LightEvent* event)
 		svcArbitrateAddress(arbiter, (u32)event, ARBITRATION_WAIT_IF_LESS_THAN, 0, 0);
 	}
 }
+
+void LightSemaphore_Init(LightSemaphore* semaphore, s16 initial_count, s16 max_count)
+{
+	semaphore->current_count = (s32)initial_count;
+	semaphore->num_threads_acq = 0;
+	semaphore->max_count = max_count;
+}
+
+void LightSemaphore_Acquire(LightSemaphore* semaphore, s32 count)
+{
+	s32 old_count;
+	s16 num_threads_acq;
+
+	do
+	{
+		for (;;)
+		{
+			old_count = __ldrex(&semaphore->current_count);
+			if (old_count > 0)
+				break;
+			__clrex();
+
+			do
+				num_threads_acq = (s16)__ldrexh((u16 *)&semaphore->num_threads_acq);
+			while (__strexh((u16 *)&semaphore->num_threads_acq, num_threads_acq + 1));
+
+			svcArbitrateAddress(arbiter, (u32)semaphore, ARBITRATION_WAIT_IF_LESS_THAN, count, 0);
+
+			do
+				num_threads_acq = (s16)__ldrexh((u16 *)&semaphore->num_threads_acq);
+			while (__strexh((u16 *)&semaphore->num_threads_acq, num_threads_acq - 1));
+		}
+	} while (__strex(&semaphore->current_count, old_count - count));
+}
+
+void LightSemaphore_Release(LightSemaphore* semaphore, s32 count)
+{
+	s32 old_count, new_count;
+	do
+	{
+		old_count = __ldrex(&semaphore->current_count);
+		new_count = old_count + count;
+		if (new_count >= semaphore->max_count)
+			new_count = semaphore->max_count;
+	} while (__strex(&semaphore->current_count, new_count));
+
+	if(old_count <= 0 || semaphore->num_threads_acq > 0)
+		svcArbitrateAddress(arbiter, (u32)semaphore, ARBITRATION_SIGNAL, count, 0);
+}