1 /*
2 * SPDX-FileCopyrightText: Copyright (c) 2024 Fabian Blatz <fabianblatz@gmail.com>
3 * SPDX-License-Identifier: Apache-2.0
4 */
5
6 #include "step_dir_stepper_common.h"
7
8 #include <zephyr/logging/log.h>
9 LOG_MODULE_REGISTER(step_dir_stepper, CONFIG_STEPPER_LOG_LEVEL);
10
step_dir_stepper_perform_step(const struct device * dev)11 static inline int step_dir_stepper_perform_step(const struct device *dev)
12 {
13 const struct step_dir_stepper_common_config *config = dev->config;
14 struct step_dir_stepper_common_data *data = dev->data;
15 int ret;
16
17 switch (data->direction) {
18 case STEPPER_DIRECTION_POSITIVE:
19 ret = gpio_pin_set_dt(&config->dir_pin, 1);
20 break;
21 case STEPPER_DIRECTION_NEGATIVE:
22 ret = gpio_pin_set_dt(&config->dir_pin, 0);
23 break;
24 default:
25 LOG_ERR("Unsupported direction: %d", data->direction);
26 return -ENOTSUP;
27 }
28 if (ret < 0) {
29 LOG_ERR("Failed to set direction: %d", ret);
30 return ret;
31 }
32
33 ret = gpio_pin_toggle_dt(&config->step_pin);
34 if (ret < 0) {
35 LOG_ERR("Failed to toggle step pin: %d", ret);
36 return ret;
37 }
38
39 if (!config->dual_edge) {
40 ret = gpio_pin_toggle_dt(&config->step_pin);
41 if (ret < 0) {
42 LOG_ERR("Failed to toggle step pin: %d", ret);
43 return ret;
44 }
45 }
46
47 if (data->direction == STEPPER_DIRECTION_POSITIVE) {
48 data->actual_position++;
49 } else {
50 data->actual_position--;
51 }
52
53 return 0;
54 }
55
stepper_trigger_callback(const struct device * dev,enum stepper_event event)56 static void stepper_trigger_callback(const struct device *dev, enum stepper_event event)
57 {
58 struct step_dir_stepper_common_data *data = dev->data;
59
60 if (!data->callback) {
61 LOG_WRN_ONCE("No callback set");
62 return;
63 }
64
65 if (!k_is_in_isr()) {
66 data->callback(dev, event, data->event_cb_user_data);
67 return;
68 }
69
70 #ifdef CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS
71 /* Dispatch to msgq instead of raising directly */
72 int ret = k_msgq_put(&data->event_msgq, &event, K_NO_WAIT);
73
74 if (ret != 0) {
75 LOG_WRN("Failed to put event in msgq: %d", ret);
76 }
77
78 ret = k_work_submit(&data->event_callback_work);
79 if (ret < 0) {
80 LOG_ERR("Failed to submit work item: %d", ret);
81 }
82 #else
83 LOG_WRN_ONCE("Event callback called from ISR context without ISR safe events enabled");
84 #endif /* CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS */
85 }
86
87 #ifdef CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS
stepper_work_event_handler(struct k_work * work)88 static void stepper_work_event_handler(struct k_work *work)
89 {
90 struct step_dir_stepper_common_data *data =
91 CONTAINER_OF(work, struct step_dir_stepper_common_data, event_callback_work);
92 enum stepper_event event;
93 int ret;
94
95 ret = k_msgq_get(&data->event_msgq, &event, K_NO_WAIT);
96 if (ret != 0) {
97 return;
98 }
99
100 /* Run the callback */
101 if (data->callback != NULL) {
102 data->callback(data->dev, event, data->event_cb_user_data);
103 }
104
105 /* If there are more pending events, resubmit this work item to handle them */
106 if (k_msgq_num_used_get(&data->event_msgq) > 0) {
107 k_work_submit(work);
108 }
109 }
110 #endif /* CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS */
111
update_remaining_steps(struct step_dir_stepper_common_data * data)112 static void update_remaining_steps(struct step_dir_stepper_common_data *data)
113 {
114 const struct step_dir_stepper_common_config *config = data->dev->config;
115
116 if (data->step_count > 0) {
117 data->step_count--;
118 } else if (data->step_count < 0) {
119 data->step_count++;
120 } else {
121 stepper_trigger_callback(data->dev, STEPPER_EVENT_STEPS_COMPLETED);
122 config->timing_source->stop(data->dev);
123 }
124 }
125
update_direction_from_step_count(const struct device * dev)126 static void update_direction_from_step_count(const struct device *dev)
127 {
128 struct step_dir_stepper_common_data *data = dev->data;
129
130 if (data->step_count > 0) {
131 data->direction = STEPPER_DIRECTION_POSITIVE;
132 } else if (data->step_count < 0) {
133 data->direction = STEPPER_DIRECTION_NEGATIVE;
134 } else {
135 LOG_ERR("Step count is zero");
136 }
137 }
138
position_mode_task(const struct device * dev)139 static void position_mode_task(const struct device *dev)
140 {
141 struct step_dir_stepper_common_data *data = dev->data;
142 const struct step_dir_stepper_common_config *config = dev->config;
143
144 if (data->step_count) {
145 (void)step_dir_stepper_perform_step(dev);
146 }
147
148 update_remaining_steps(dev->data);
149
150 if (config->timing_source->needs_reschedule(dev) && data->step_count != 0) {
151 (void)config->timing_source->start(dev);
152 }
153 }
154
velocity_mode_task(const struct device * dev)155 static void velocity_mode_task(const struct device *dev)
156 {
157 const struct step_dir_stepper_common_config *config = dev->config;
158
159 (void)step_dir_stepper_perform_step(dev);
160
161 if (config->timing_source->needs_reschedule(dev)) {
162 (void)config->timing_source->start(dev);
163 }
164 }
165
stepper_handle_timing_signal(const struct device * dev)166 void stepper_handle_timing_signal(const struct device *dev)
167 {
168 struct step_dir_stepper_common_data *data = dev->data;
169
170 K_SPINLOCK(&data->lock) {
171 switch (data->run_mode) {
172 case STEPPER_RUN_MODE_POSITION:
173 position_mode_task(dev);
174 break;
175 case STEPPER_RUN_MODE_VELOCITY:
176 velocity_mode_task(dev);
177 break;
178 default:
179 LOG_WRN("Unsupported run mode: %d", data->run_mode);
180 break;
181 }
182 }
183 }
184
step_dir_stepper_common_init(const struct device * dev)185 int step_dir_stepper_common_init(const struct device *dev)
186 {
187 const struct step_dir_stepper_common_config *config = dev->config;
188 int ret;
189
190 if (!gpio_is_ready_dt(&config->step_pin) || !gpio_is_ready_dt(&config->dir_pin)) {
191 LOG_ERR("GPIO pins are not ready");
192 return -ENODEV;
193 }
194
195 ret = gpio_pin_configure_dt(&config->step_pin, GPIO_OUTPUT);
196 if (ret < 0) {
197 LOG_ERR("Failed to configure step pin: %d", ret);
198 return ret;
199 }
200
201 ret = gpio_pin_configure_dt(&config->dir_pin, GPIO_OUTPUT);
202 if (ret < 0) {
203 LOG_ERR("Failed to configure dir pin: %d", ret);
204 return ret;
205 }
206
207 if (config->timing_source->init) {
208 ret = config->timing_source->init(dev);
209 if (ret < 0) {
210 LOG_ERR("Failed to initialize timing source: %d", ret);
211 return ret;
212 }
213 }
214
215 #ifdef CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS
216 struct step_dir_stepper_common_data *data = dev->data;
217
218 k_msgq_init(&data->event_msgq, data->event_msgq_buffer, sizeof(enum stepper_event),
219 CONFIG_STEPPER_STEP_DIR_EVENT_QUEUE_LEN);
220 k_work_init(&data->event_callback_work, stepper_work_event_handler);
221 #endif /* CONFIG_STEPPER_STEP_DIR_GENERATE_ISR_SAFE_EVENTS */
222
223 return 0;
224 }
225
step_dir_stepper_common_move_by(const struct device * dev,const int32_t micro_steps)226 int step_dir_stepper_common_move_by(const struct device *dev, const int32_t micro_steps)
227 {
228 struct step_dir_stepper_common_data *data = dev->data;
229 const struct step_dir_stepper_common_config *config = dev->config;
230
231 if (data->microstep_interval_ns == 0) {
232 LOG_ERR("Step interval not set or invalid step interval set");
233 return -EINVAL;
234 }
235
236 K_SPINLOCK(&data->lock) {
237 data->run_mode = STEPPER_RUN_MODE_POSITION;
238 data->step_count = micro_steps;
239 config->timing_source->update(dev, data->microstep_interval_ns);
240 update_direction_from_step_count(dev);
241 config->timing_source->start(dev);
242 }
243
244 return 0;
245 }
246
step_dir_stepper_common_set_microstep_interval(const struct device * dev,const uint64_t microstep_interval_ns)247 int step_dir_stepper_common_set_microstep_interval(const struct device *dev,
248 const uint64_t microstep_interval_ns)
249 {
250 struct step_dir_stepper_common_data *data = dev->data;
251 const struct step_dir_stepper_common_config *config = dev->config;
252
253 if (microstep_interval_ns == 0) {
254 LOG_ERR("Step interval cannot be zero");
255 return -EINVAL;
256 }
257
258 K_SPINLOCK(&data->lock) {
259 data->microstep_interval_ns = microstep_interval_ns;
260 config->timing_source->update(dev, microstep_interval_ns);
261 }
262
263 return 0;
264 }
265
step_dir_stepper_common_set_reference_position(const struct device * dev,const int32_t value)266 int step_dir_stepper_common_set_reference_position(const struct device *dev, const int32_t value)
267 {
268 struct step_dir_stepper_common_data *data = dev->data;
269
270 K_SPINLOCK(&data->lock) {
271 data->actual_position = value;
272 }
273
274 return 0;
275 }
276
step_dir_stepper_common_get_actual_position(const struct device * dev,int32_t * value)277 int step_dir_stepper_common_get_actual_position(const struct device *dev, int32_t *value)
278 {
279 struct step_dir_stepper_common_data *data = dev->data;
280
281 K_SPINLOCK(&data->lock) {
282 *value = data->actual_position;
283 }
284
285 return 0;
286 }
287
step_dir_stepper_common_move_to(const struct device * dev,const int32_t value)288 int step_dir_stepper_common_move_to(const struct device *dev, const int32_t value)
289 {
290 struct step_dir_stepper_common_data *data = dev->data;
291 const struct step_dir_stepper_common_config *config = dev->config;
292
293 if (data->microstep_interval_ns == 0) {
294 LOG_ERR("Step interval not set or invalid step interval set");
295 return -EINVAL;
296 }
297
298 K_SPINLOCK(&data->lock) {
299 data->run_mode = STEPPER_RUN_MODE_POSITION;
300 data->step_count = value - data->actual_position;
301 config->timing_source->update(dev, data->microstep_interval_ns);
302 update_direction_from_step_count(dev);
303 config->timing_source->start(dev);
304 }
305
306 return 0;
307 }
308
step_dir_stepper_common_is_moving(const struct device * dev,bool * is_moving)309 int step_dir_stepper_common_is_moving(const struct device *dev, bool *is_moving)
310 {
311 const struct step_dir_stepper_common_config *config = dev->config;
312
313 *is_moving = config->timing_source->is_running(dev);
314 return 0;
315 }
316
step_dir_stepper_common_run(const struct device * dev,const enum stepper_direction direction)317 int step_dir_stepper_common_run(const struct device *dev, const enum stepper_direction direction)
318 {
319 struct step_dir_stepper_common_data *data = dev->data;
320 const struct step_dir_stepper_common_config *config = dev->config;
321
322 K_SPINLOCK(&data->lock) {
323 data->run_mode = STEPPER_RUN_MODE_VELOCITY;
324 data->direction = direction;
325 config->timing_source->update(dev, data->microstep_interval_ns);
326 config->timing_source->start(dev);
327 }
328
329 return 0;
330 }
331
step_dir_stepper_common_stop(const struct device * dev)332 int step_dir_stepper_common_stop(const struct device *dev)
333 {
334 const struct step_dir_stepper_common_config *config = dev->config;
335 int ret;
336
337 ret = config->timing_source->stop(dev);
338 if (ret != 0) {
339 LOG_ERR("Failed to stop timing source: %d", ret);
340 return ret;
341 }
342
343 stepper_trigger_callback(dev, STEPPER_EVENT_STOPPED);
344 return 0;
345 }
346
step_dir_stepper_common_set_event_callback(const struct device * dev,stepper_event_callback_t callback,void * user_data)347 int step_dir_stepper_common_set_event_callback(const struct device *dev,
348 stepper_event_callback_t callback, void *user_data)
349 {
350 struct step_dir_stepper_common_data *data = dev->data;
351
352 data->callback = callback;
353 data->event_cb_user_data = user_data;
354 return 0;
355 }
356
