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