diff --git a/thermosphere/src/spinlock.h b/thermosphere/src/spinlock.h new file mode 100644 index 000000000..c6b948bf4 --- /dev/null +++ b/thermosphere/src/spinlock.h @@ -0,0 +1,98 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#include "utils.h" +#include "sysreg.h" +#include "core_ctx.h" + +typedef struct Spinlock { + u32 lock; +} Spinlock; + +typedef struct RecursiveSpinlock { + Spinlock lock; + u32 count; + u32 tag; +} RecursiveSpinlock; + +static inline u64 maskIrq(void) +{ + u64 ret = GET_SYSREG(daif); + SET_SYSREG(daifset, BITL(1)); + return ret; +} + +static inline u64 unmaskIrq(void) +{ + u64 ret = GET_SYSREG(daif); + SET_SYSREG(daifclr, BITL(1)); + return ret; +} + +static inline void restoreInterruptFlags(u64 flags) +{ + SET_SYSREG(daif, flags); +} + +void spinlockLock(Spinlock *lock); +void spinlockUnlock(Spinlock *lock); + +static inline u64 spinlockLockMaskIrq(Spinlock *lock) +{ + u64 ret = maskIrq(); + spinlockLock(lock); + return ret; +} + +static inline void spinlockLockRestoreIrq(Spinlock *lock, u64 flags) +{ + spinlockUnlock(lock); + restoreInterruptFlags(flags); +} + +static inline void recursiveSpinlockLock(RecursiveSpinlock *lock) +{ + if (lock->tag != currentCoreCtx->coreId + 1) { + spinlockLock(&lock->lock); + lock->tag = currentCoreCtx->coreId + 1; + lock->count = 0; + } else { + ++lock->count; + } +} + +static inline void recursiveSpinlockUnlock(RecursiveSpinlock *lock) +{ + if (--lock->count == 0) { + lock->tag = 0; + spinlockUnlock(&lock->lock); + } +} + +static inline u64 recursiveSpinlockLockMaskIrq(RecursiveSpinlock *lock) +{ + u64 ret = maskIrq(); + recursiveSpinlockLock(lock); + return ret; +} + +static inline void recursiveSpinlockLockRestoreIrq(RecursiveSpinlock *lock, u64 flags) +{ + recursiveSpinlockUnlock(lock); + restoreInterruptFlags(flags); +} diff --git a/thermosphere/src/spinlock.s b/thermosphere/src/spinlock.s new file mode 100644 index 000000000..171d7f8bf --- /dev/null +++ b/thermosphere/src/spinlock.s @@ -0,0 +1,45 @@ +/* + * Copyright (c) 2019 Atmosphère-NX + * + * This program is free software; you can redistribute it and/or modify it + * under the terms and conditions of the GNU General Public License, + * version 2, as published by the Free Software Foundation. + * + * This program is distributed in the hope it will be useful, but WITHOUT + * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or + * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for + * more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +// From Arm TF + + +/* + * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved. + * + * SPDX-License-Identifier: BSD-3-Clause + */ + +.global spinlockLock +.type spinlockLock, %function +spinlockLock: + mov w2, #1 + sevl + l1: + wfe + l2: + ldaxr w1, [x0] + cbnz w1, l1 + stxr w1, w2, [x0] + cbnz w1, l2 + ret + +.global spinlockUnlock +.type spinlockUnlock, %function +spinlockUnlock: + stlr wzr, [x0] + sev + ret