drivers: hwspinlock: implement sqn hwspinlock driver

When we lock an hwspinlock, we must write the CPU identifier to
the hwspinlock register. If we want to unlock the locked hwspinlock,
we have to rewrite the same CPU identifier.

To define the CPU identifier, we use affinity 1 and affinity 2 fields
of the MPIDR register.

Signed-off-by: Aziz Idomar <[email protected]>

Author: Aziz Idomar

drivers/hwspinlock/CMakeLists.txt

@@ -2,4 +2,5 @@
 
 zephyr_library()
 
+zephyr_library_sources_ifdef(CONFIG_SQN_HWSPINLOCK sqn_hwspinlock.c)
 zephyr_library_sources_ifdef(CONFIG_USERSPACE hwspinlock_handlers.c)

drivers/hwspinlock/Kconfig

@@ -16,4 +16,6 @@ config HWSPINLOCK_INIT_PRIORITY
 	help
 	  HW spinlock driver device initialization priority.
 
+source "drivers/hwspinlock/Kconfig.sqn"
+
 endif

drivers/hwspinlock/Kconfig.sqn

@@ -0,0 +1,20 @@
+# Sequans HW spinlock configuration
+
+# Copyright (c) 2023 Sequans Communications.
+# SPDX-License-Identifier: Apache-2.0
+
+config SQN_HWSPINLOCK
+	bool "Sequans HW spinlock Driver"
+	default y
+	depends on DT_HAS_SQN_HWSPINLOCK_ENABLED
+	help
+	  Enable HW spinlock for SQN
+
+if SQN_HWSPINLOCK
+config SQN_HWSPINLOCK_RELAX_TIME
+	int "Sequans HW spinlock relax time"
+	default 50
+	help
+	  Default HW spinlock relax time in microseconds.
+
+endif #SQN_HWSPINLOCK

drivers/hwspinlock/sqn_hwspinlock.c

@@ -0,0 +1,151 @@
+/*
+ * Copyright (c) 2023 Sequans Communications
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#define DT_DRV_COMPAT sqn_hwspinlock
+
+#include <zephyr/device.h>
+#include <zephyr/kernel.h>
+#include <zephyr/sys/sys_io.h>
+#include <zephyr/drivers/hwspinlock.h>
+
+#include <zephyr/sys/printk.h>
+#include <zephyr/logging/log.h>
+LOG_MODULE_REGISTER(sqn_hwspinlock);
+
+struct sqn_hwspinlock_data {
+	DEVICE_MMIO_RAM;
+};
+
+struct sqn_hwspinlock_config {
+	DEVICE_MMIO_ROM;
+	uint32_t num_locks;
+};
+
+static inline mem_addr_t get_lock_addr(const struct device *dev, uint32_t id)
+{
+	return (mem_addr_t)(DEVICE_MMIO_GET(dev) + id * sizeof(uint32_t));
+}
+
+/*
+ * To define CPU id, we use the affinity2 and affinity1
+ * fields of the MPIDR register.
+ */
+static uint8_t mpidr_to_cpuid(uint64_t mpidr_val)
+{
+	uint8_t cpuid = ((mpidr_val >> 8) & 0x0F) | ((mpidr_val >> 12) & 0xF0);
+
+	return ++cpuid;
+}
+
+static int sqn_hwspinlock_trylock(const struct device *dev, uint32_t id)
+{
+	const struct sqn_hwspinlock_config *config = dev->config;
+	uint8_t cpuid;
+
+	if (id > config->num_locks)
+		return -EINVAL;
+
+	/*
+	 * If the register value is equal to cpuid, this means that the current
+	 * core has already locked the HW spinlock.
+	 * If not, we try to lock the HW spinlock by writing cpuid, then check
+	 * whether it is locked.
+	 */
+
+	cpuid = mpidr_to_cpuid(read_mpidr_el1());
+	if (sys_read8(get_lock_addr(dev, id)) == cpuid)
+		return 0;
+
+	sys_write8(cpuid, get_lock_addr(dev, id));
+	if (sys_read8(get_lock_addr(dev, id)) == cpuid)
+		return 0;
+
+	return -EBUSY;
+}
+
+static void sqn_hwspinlock_lock(const struct device *dev, uint32_t id)
+{
+	const struct sqn_hwspinlock_config *config = dev->config;
+	uint8_t cpuid;
+
+	if (id > config->num_locks) {
+		LOG_ERR("unsupported hwspinlock id '%d'", id);
+		return;
+	}
+
+	/*
+	 * Writing cpuid is equivalent to trying to lock HW spinlock, after
+	 * which we check whether we've locked by reading the register value
+	 * and comparing it with cpuid.
+	 */
+
+	cpuid = mpidr_to_cpuid(read_mpidr_el1());
+	if (sys_read8(get_lock_addr(dev, id)) == 0) {
+		sys_write8(cpuid, get_lock_addr(dev, id));
+	}
+
+	while (sys_read8(get_lock_addr(dev, id)) != cpuid) {
+		k_busy_wait(CONFIG_SQN_HWSPINLOCK_RELAX_TIME);
+		sys_write8(cpuid, get_lock_addr(dev, id));
+	}
+}
+
+static void sqn_hwspinlock_unlock(const struct device *dev, uint32_t id)
+{
+	const struct sqn_hwspinlock_config *config = dev->config;
+	uint8_t cpuid;
+
+	if (id > config->num_locks) {
+		LOG_ERR("unsupported hwspinlock id '%d'", id);
+		return;
+	}
+
+	/*
+	 * If the HW spinlock register value is equal to the cpuid and we write
+	 * the cpuid, then the register value will be 0. So to unlock the
+	 * hwspinlock, we write cpuid.
+	 */
+
+	cpuid = mpidr_to_cpuid(read_mpidr_el1());
+	sys_write8(cpuid, get_lock_addr(dev, id));
+}
+
+static uint32_t sqn_hwspinlock_get_max_id(const struct device *dev)
+{
+	const struct sqn_hwspinlock_config *config = dev->config;
+
+	return config->num_locks;
+}
+
+static const struct hwspinlock_driver_api hwspinlock_api = {
+	.trylock = sqn_hwspinlock_trylock,
+	.lock = sqn_hwspinlock_lock,
+	.unlock = sqn_hwspinlock_unlock,
+	.get_max_id = sqn_hwspinlock_get_max_id,
+};
+
+static int sqn_hwspinlock_init(const struct device *dev)
+{
+	DEVICE_MMIO_MAP(dev, K_MEM_CACHE_NONE);
+
+	return 0;
+}
+
+#define SQN_HWSPINLOCK_INIT(idx)                                                \
+	static struct sqn_hwspinlock_data sqn_hwspinlock##idx##_data;           \
+	static struct sqn_hwspinlock_config sqn_hwspinlock##idx##_config = {    \
+		DEVICE_MMIO_ROM_INIT(DT_DRV_INST(idx)),                         \
+		.num_locks = DT_INST_PROP(idx, num_locks),                      \
+	};                                                                      \
+	DEVICE_DT_INST_DEFINE(idx,                                              \
+			      sqn_hwspinlock_init,                              \
+			      NULL,                                             \
+			      &sqn_hwspinlock##idx##_data,                      \
+			      &sqn_hwspinlock##idx##_config,                    \
+			      PRE_KERNEL_1, CONFIG_HWSPINLOCK_INIT_PRIORITY,    \
+			      &hwspinlock_api)
+
+DT_INST_FOREACH_STATUS_OKAY(SQN_HWSPINLOCK_INIT);

dts/bindings/hwspinlock/sqn,hwspinlock.yaml

@@ -0,0 +1,16 @@
+# Copyright (c) 2023 Sequans Communications
+# SPDX-License-Identifier: Apache-2.0
+
+description: SQN Hardware spinlocks
+
+compatible: "sqn,hwspinlock"
+
+include: base.yaml
+
+properties:
+  reg:
+    required: true
+
+  num_locks:
+    type: int
+    required: true