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 * If the register value is equal to cpuid, this means that the current
53 * core has already locked the HW spinlock.
54 * If not, we try to lock the HW spinlock by writing cpuid, then check
55 * whether it is locked.
56 */
57
58 cpuid = mpidr_to_cpuid(read_mpidr_el1());
59 if (sys_read8(get_lock_addr(dev, id)) == cpuid)
60 return 0;
61
62 sys_write8(cpuid, get_lock_addr(dev, id));
63 if (sys_read8(get_lock_addr(dev, id)) == cpuid)
64 return 0;
65
66 return -EBUSY;
67 }
68
sqn_hwspinlock_lock(const struct device * dev,uint32_t id)69 static void sqn_hwspinlock_lock(const struct device *dev, uint32_t id)
70 {
71 const struct sqn_hwspinlock_config *config = dev->config;
72 uint8_t cpuid;
73
74 if (id > config->num_locks) {
75 LOG_ERR("unsupported hwspinlock id '%d'", id);
76 return;
77 }
78
79 /*
80 * Writing cpuid is equivalent to trying to lock HW spinlock, after
81 * which we check whether we've locked by reading the register value
82 * and comparing it with cpuid.
83 */
84
85 cpuid = mpidr_to_cpuid(read_mpidr_el1());
86 if (sys_read8(get_lock_addr(dev, id)) == 0) {
87 sys_write8(cpuid, get_lock_addr(dev, id));
88 }
89
90 while (sys_read8(get_lock_addr(dev, id)) != cpuid) {
91 k_busy_wait(CONFIG_SQN_HWSPINLOCK_RELAX_TIME);
92 sys_write8(cpuid, get_lock_addr(dev, id));
93 }
94 }
95
sqn_hwspinlock_unlock(const struct device * dev,uint32_t id)96 static void sqn_hwspinlock_unlock(const struct device *dev, uint32_t id)
97 {
98 const struct sqn_hwspinlock_config *config = dev->config;
99 uint8_t cpuid;
100
101 if (id > config->num_locks) {
102 LOG_ERR("unsupported hwspinlock id '%d'", id);
103 return;
104 }
105
106 /*
107 * If the HW spinlock register value is equal to the cpuid and we write
108 * the cpuid, then the register value will be 0. So to unlock the
109 * hwspinlock, we write cpuid.
110 */
111
112 cpuid = mpidr_to_cpuid(read_mpidr_el1());
113 sys_write8(cpuid, get_lock_addr(dev, id));
114 }
115
sqn_hwspinlock_get_max_id(const struct device * dev)116 static uint32_t sqn_hwspinlock_get_max_id(const struct device *dev)
117 {
118 const struct sqn_hwspinlock_config *config = dev->config;
119
120 return config->num_locks;
121 }
122
123 static const struct hwspinlock_driver_api hwspinlock_api = {
124 .trylock = sqn_hwspinlock_trylock,
125 .lock = sqn_hwspinlock_lock,
126 .unlock = sqn_hwspinlock_unlock,
127 .get_max_id = sqn_hwspinlock_get_max_id,
128 };
129
sqn_hwspinlock_init(const struct device * dev)130 static int sqn_hwspinlock_init(const struct device *dev)
131 {
132 DEVICE_MMIO_MAP(dev, K_MEM_CACHE_NONE);
133
134 return 0;
135 }
136
137 #define SQN_HWSPINLOCK_INIT(idx) \
138 static struct sqn_hwspinlock_data sqn_hwspinlock##idx##_data; \
139 static const struct sqn_hwspinlock_config sqn_hwspinlock##idx##_config = { \
140 DEVICE_MMIO_ROM_INIT(DT_DRV_INST(idx)), \
141 .num_locks = DT_INST_PROP(idx, num_locks), \
142 }; \
143 DEVICE_DT_INST_DEFINE(idx, \
144 sqn_hwspinlock_init, \
145 NULL, \
146 &sqn_hwspinlock##idx##_data, \
147 &sqn_hwspinlock##idx##_config, \
148 PRE_KERNEL_1, CONFIG_HWSPINLOCK_INIT_PRIORITY, \
149 &hwspinlock_api)
150
151 DT_INST_FOREACH_STATUS_OKAY(SQN_HWSPINLOCK_INIT);
152
