1 /*
2 * Copyright (c) 2023 Sequans Communications
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT sqn_hwspinlock
8
9 #include <zephyr/device.h>
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys/sys_io.h>
12 #include <zephyr/drivers/hwspinlock.h>
13
14 #include <zephyr/sys/printk.h>
15 #include <zephyr/logging/log.h>
16 LOG_MODULE_REGISTER(sqn_hwspinlock);
17
18 struct sqn_hwspinlock_data {
19 DEVICE_MMIO_RAM;
20 };
21
22 struct sqn_hwspinlock_config {
23 DEVICE_MMIO_ROM;
24 uint32_t num_locks;
25 };
26
get_lock_addr(const struct device * dev,uint32_t id)27 static inline mem_addr_t get_lock_addr(const struct device *dev, uint32_t id)
28 {
29 return (mem_addr_t)(DEVICE_MMIO_GET(dev) + id * sizeof(uint32_t));
30 }
31
32 /*
33 * To define CPU id, we use the affinity2 and affinity1
34 * fields of the MPIDR register.
35 */
mpidr_to_cpuid(uint64_t mpidr_val)36 static uint8_t mpidr_to_cpuid(uint64_t mpidr_val)
37 {
38 uint8_t cpuid = ((mpidr_val >> 8) & 0x0F) | ((mpidr_val >> 12) & 0xF0);
39
40 return ++cpuid;
41 }
42
sqn_hwspinlock_trylock(const struct device * dev,uint32_t id)43 static int sqn_hwspinlock_trylock(const struct device *dev, uint32_t id)
44 {
45 const struct sqn_hwspinlock_config *config = dev->config;
46 uint8_t cpuid;
47
48 if (id > config->num_locks) {
49 return -EINVAL;
50 }
51
52 /*
53 * If the register value is equal to cpuid, this means that the current
54 * core has already locked the HW spinlock.
55 * If not, we try to lock the HW spinlock by writing cpuid, then check
56 * whether it is locked.
57 */
58
59 cpuid = mpidr_to_cpuid(read_mpidr_el1());
60 if (sys_read8(get_lock_addr(dev, id)) == cpuid) {
61 return 0;
62 }
63
64 sys_write8(cpuid, get_lock_addr(dev, id));
65 if (sys_read8(get_lock_addr(dev, id)) == cpuid) {
66 return 0;
67 }
68
69 return -EBUSY;
70 }
71
sqn_hwspinlock_lock(const struct device * dev,uint32_t id)72 static void sqn_hwspinlock_lock(const struct device *dev, uint32_t id)
73 {
74 const struct sqn_hwspinlock_config *config = dev->config;
75 uint8_t cpuid;
76
77 if (id > config->num_locks) {
78 LOG_ERR("unsupported hwspinlock id '%d'", id);
79 return;
80 }
81
82 /*
83 * Writing cpuid is equivalent to trying to lock HW spinlock, after
84 * which we check whether we've locked by reading the register value
85 * and comparing it with cpuid.
86 */
87
88 cpuid = mpidr_to_cpuid(read_mpidr_el1());
89 if (sys_read8(get_lock_addr(dev, id)) == 0) {
90 sys_write8(cpuid, get_lock_addr(dev, id));
91 }
92
93 while (sys_read8(get_lock_addr(dev, id)) != cpuid) {
94 k_busy_wait(CONFIG_SQN_HWSPINLOCK_RELAX_TIME);
95 sys_write8(cpuid, get_lock_addr(dev, id));
96 }
97 }
98
sqn_hwspinlock_unlock(const struct device * dev,uint32_t id)99 static void sqn_hwspinlock_unlock(const struct device *dev, uint32_t id)
100 {
101 const struct sqn_hwspinlock_config *config = dev->config;
102 uint8_t cpuid;
103
104 if (id > config->num_locks) {
105 LOG_ERR("unsupported hwspinlock id '%d'", id);
106 return;
107 }
108
109 /*
110 * If the HW spinlock register value is equal to the cpuid and we write
111 * the cpuid, then the register value will be 0. So to unlock the
112 * hwspinlock, we write cpuid.
113 */
114
115 cpuid = mpidr_to_cpuid(read_mpidr_el1());
116 sys_write8(cpuid, get_lock_addr(dev, id));
117 }
118
sqn_hwspinlock_get_max_id(const struct device * dev)119 static uint32_t sqn_hwspinlock_get_max_id(const struct device *dev)
120 {
121 const struct sqn_hwspinlock_config *config = dev->config;
122
123 return config->num_locks;
124 }
125
126 static DEVICE_API(hwspinlock, hwspinlock_api) = {
127 .trylock = sqn_hwspinlock_trylock,
128 .lock = sqn_hwspinlock_lock,
129 .unlock = sqn_hwspinlock_unlock,
130 .get_max_id = sqn_hwspinlock_get_max_id,
131 };
132
sqn_hwspinlock_init(const struct device * dev)133 static int sqn_hwspinlock_init(const struct device *dev)
134 {
135 DEVICE_MMIO_MAP(dev, K_MEM_CACHE_NONE);
136
137 return 0;
138 }
139
140 #define SQN_HWSPINLOCK_INIT(idx) \
141 static struct sqn_hwspinlock_data sqn_hwspinlock##idx##_data; \
142 static const struct sqn_hwspinlock_config sqn_hwspinlock##idx##_config = { \
143 DEVICE_MMIO_ROM_INIT(DT_DRV_INST(idx)), \
144 .num_locks = DT_INST_PROP(idx, num_locks), \
145 }; \
146 DEVICE_DT_INST_DEFINE(idx, \
147 sqn_hwspinlock_init, \
148 NULL, \
149 &sqn_hwspinlock##idx##_data, \
150 &sqn_hwspinlock##idx##_config, \
151 PRE_KERNEL_1, CONFIG_HWSPINLOCK_INIT_PRIORITY, \
152 &hwspinlock_api)
153
154 DT_INST_FOREACH_STATUS_OKAY(SQN_HWSPINLOCK_INIT);
155
