1 /*
2 * Copyright (c) 2023, Meta
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT zephyr_devmux
8
9 #include <limits.h>
10
11 #include <zephyr/device.h>
12 #include <zephyr/drivers/misc/devmux/devmux.h>
13 #include <zephyr/kernel.h>
14 #include <zephyr/sys/util.h>
15
16 struct devmux_config {
17 const struct device **devs;
18 const size_t n_devs;
19 };
20
21 struct devmux_data {
22 struct k_spinlock lock;
23 size_t selected;
24 };
25
26 /* The number of devmux devices */
27 #define N DT_NUM_INST_STATUS_OKAY(DT_DRV_COMPAT)
28
29 static const struct device *devmux_devices[N];
30 static const struct devmux_config *devmux_configs[N];
31 static struct devmux_data *devmux_datas[N];
32
devmux_device_is_valid(const struct device * dev)33 static bool devmux_device_is_valid(const struct device *dev)
34 {
35 for (size_t i = 0; i < N; ++i) {
36 if (dev == devmux_devices[i]) {
37 return true;
38 }
39 }
40
41 return false;
42 }
43
devmux_inst_get(const struct device * dev)44 static size_t devmux_inst_get(const struct device *dev)
45 {
46 for (size_t i = 0; i < N; i++) {
47 if (dev == devmux_devices[i]) {
48 return i;
49 }
50 }
51
52 return SIZE_MAX;
53 }
54
devmux_config_get(const struct device * dev)55 const struct devmux_config *devmux_config_get(const struct device *dev)
56 {
57 for (size_t i = 0; i < N; i++) {
58 if (dev == devmux_devices[i]) {
59 return devmux_configs[i];
60 }
61 }
62
63 return NULL;
64 }
65
devmux_data_get(const struct device * dev)66 struct devmux_data *devmux_data_get(const struct device *dev)
67 {
68 for (size_t i = 0; i < N; i++) {
69 if (dev == devmux_devices[i]) {
70 return devmux_datas[i];
71 }
72 }
73
74 return NULL;
75 }
76
z_impl_devmux_select_get(const struct device * dev)77 int z_impl_devmux_select_get(const struct device *dev)
78 {
79 int index;
80 struct devmux_data *const data = devmux_data_get(dev);
81
82 if (!devmux_device_is_valid(dev)) {
83 return -EINVAL;
84 }
85
86 K_SPINLOCK(&data->lock)
87 {
88 index = data->selected;
89 }
90
91 return index;
92 }
93
94 #ifdef CONFIG_USERSPACE
z_vrfy_devmux_select_get(const struct device * dev)95 int z_vrfy_devmux_select_get(const struct device *dev)
96 {
97 return z_impl_devmux_select_get(dev);
98 }
99 #include <zephyr/syscalls/devmux_select_get_mrsh.c>
100 #endif
101
z_impl_devmux_select_set(struct device * dev,size_t index)102 int z_impl_devmux_select_set(struct device *dev, size_t index)
103 {
104 struct devmux_data *const data = devmux_data_get(dev);
105 const struct devmux_config *config = devmux_config_get(dev);
106
107 if (!devmux_device_is_valid(dev) || index >= config->n_devs) {
108 return -EINVAL;
109 }
110
111 if (!device_is_ready(config->devs[index])) {
112 return -ENODEV;
113 }
114
115 K_SPINLOCK(&data->lock)
116 {
117 *dev = *config->devs[index];
118 data->selected = index;
119 }
120
121 return 0;
122 }
123
124 #ifdef CONFIG_USERSPACE
z_vrfy_devmux_select_set(struct device * dev,size_t index)125 int z_vrfy_devmux_select_set(struct device *dev, size_t index)
126 {
127 return z_impl_devmux_select_set(dev, index);
128 }
129 #include <zephyr/syscalls/devmux_select_set_mrsh.c>
130 #endif
131
devmux_init(struct device * const dev)132 static int devmux_init(struct device *const dev)
133 {
134 size_t inst = devmux_inst_get(dev);
135 struct devmux_data *const data = dev->data;
136 const struct devmux_config *config = dev->config;
137 size_t sel = data->selected;
138
139 devmux_configs[inst] = config;
140 devmux_datas[inst] = data;
141
142 if (!device_is_ready(config->devs[sel])) {
143 return -ENODEV;
144 }
145
146 *dev = *config->devs[sel];
147
148 return 0;
149 }
150
151 #define DEVMUX_PHANDLE_TO_DEVICE(node_id, prop, idx) \
152 DEVICE_DT_GET(DT_PHANDLE_BY_IDX(node_id, prop, idx))
153
154 #define DEVMUX_PHANDLE_DEVICES(_n) \
155 DT_INST_FOREACH_PROP_ELEM_SEP(_n, devices, DEVMUX_PHANDLE_TO_DEVICE, (,))
156
157 #define DEVMUX_SELECTED(_n) DT_INST_PROP(_n, selected)
158
159 #define DEVMUX_DEFINE(_n) \
160 BUILD_ASSERT(DT_INST_PROP_OR(_n, zephyr_mutable, 0), \
161 "devmux nodes must contain the 'zephyr,mutable' property"); \
162 BUILD_ASSERT(DT_INST_PROP_LEN(_n, devices) > 0, "devices array must have non-zero size"); \
163 BUILD_ASSERT(DT_INST_PROP_LEN(_n, devices) <= INT_MAX, \
164 "devices array must be less than INT_MAX"); \
165 BUILD_ASSERT(DEVMUX_SELECTED(_n) >= 0, "selected must be > 0"); \
166 BUILD_ASSERT(DEVMUX_SELECTED(_n) < DT_INST_PROP_LEN(_n, devices), \
167 "selected must be within bounds of devices phandle array"); \
168 static const struct device *demux_devs_##_n[] = {DEVMUX_PHANDLE_DEVICES(_n)}; \
169 static const struct devmux_config devmux_config_##_n = { \
170 .devs = demux_devs_##_n, \
171 .n_devs = DT_INST_PROP_LEN(_n, devices), \
172 }; \
173 static struct devmux_data devmux_data_##_n = { \
174 .selected = DEVMUX_SELECTED(_n), \
175 }; \
176 \
177 DEVICE_DT_INST_DEFINE(_n, devmux_init, NULL, &devmux_data_##_n, &devmux_config_##_n, \
178 PRE_KERNEL_1, CONFIG_DEVMUX_INIT_PRIORITY, NULL);
179
180 DT_INST_FOREACH_STATUS_OKAY(DEVMUX_DEFINE)
181
182 #define DEVMUX_DEVICE_GET(_n) DEVICE_DT_INST_GET(_n),
183 static const struct device *devmux_devices[] = {DT_INST_FOREACH_STATUS_OKAY(DEVMUX_DEVICE_GET)};
184
