diff --git a/libctru/include/3ds/synchronization.h b/libctru/include/3ds/synchronization.h index d09bc7f..551884a 100644 --- a/libctru/include/3ds/synchronization.h +++ b/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); diff --git a/libctru/source/synchronization.c b/libctru/source/synchronization.c index 15bc35c..6b60e6b 100644 --- a/libctru/source/synchronization.c +++ b/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); +}