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->max_velocity == 0) {
232 LOG_ERR("Velocity not set or invalid velocity 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->max_velocity);
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_max_velocity(const struct device * dev,const uint32_t velocity)247 int step_dir_stepper_common_set_max_velocity(const struct device *dev, const uint32_t velocity)
248 {
249 struct step_dir_stepper_common_data *data = dev->data;
250 const struct step_dir_stepper_common_config *config = dev->config;
251
252 if (velocity == 0) {
253 LOG_ERR("Velocity cannot be zero");
254 return -EINVAL;
255 }
256
257 if (velocity > USEC_PER_SEC) {
258 LOG_ERR("Velocity cannot be greater than %d micro steps per second", USEC_PER_SEC);
259 return -EINVAL;
260 }
261
262 K_SPINLOCK(&data->lock) {
263 data->max_velocity = velocity;
264 config->timing_source->update(dev, velocity);
265 }
266
267 return 0;
268 }
269
step_dir_stepper_common_set_reference_position(const struct device * dev,const int32_t value)270 int step_dir_stepper_common_set_reference_position(const struct device *dev, const int32_t value)
271 {
272 struct step_dir_stepper_common_data *data = dev->data;
273
274 K_SPINLOCK(&data->lock) {
275 data->actual_position = value;
276 }
277
278 return 0;
279 }
280
step_dir_stepper_common_get_actual_position(const struct device * dev,int32_t * value)281 int step_dir_stepper_common_get_actual_position(const struct device *dev, int32_t *value)
282 {
283 struct step_dir_stepper_common_data *data = dev->data;
284
285 K_SPINLOCK(&data->lock) {
286 *value = data->actual_position;
287 }
288
289 return 0;
290 }
291
step_dir_stepper_common_move_to(const struct device * dev,const int32_t value)292 int step_dir_stepper_common_move_to(const struct device *dev, const int32_t value)
293 {
294 struct step_dir_stepper_common_data *data = dev->data;
295 const struct step_dir_stepper_common_config *config = dev->config;
296
297 if (data->max_velocity == 0) {
298 LOG_ERR("Velocity not set or invalid velocity set");
299 return -EINVAL;
300 }
301
302 K_SPINLOCK(&data->lock) {
303 data->run_mode = STEPPER_RUN_MODE_POSITION;
304 data->step_count = value - data->actual_position;
305 config->timing_source->update(dev, data->max_velocity);
306 update_direction_from_step_count(dev);
307 config->timing_source->start(dev);
308 }
309
310 return 0;
311 }
312
step_dir_stepper_common_is_moving(const struct device * dev,bool * is_moving)313 int step_dir_stepper_common_is_moving(const struct device *dev, bool *is_moving)
314 {
315 const struct step_dir_stepper_common_config *config = dev->config;
316
317 *is_moving = config->timing_source->is_running(dev);
318 return 0;
319 }
320
step_dir_stepper_common_run(const struct device * dev,const enum stepper_direction direction,const uint32_t velocity)321 int step_dir_stepper_common_run(const struct device *dev, const enum stepper_direction direction,
322 const uint32_t velocity)
323 {
324 struct step_dir_stepper_common_data *data = dev->data;
325 const struct step_dir_stepper_common_config *config = dev->config;
326
327 K_SPINLOCK(&data->lock) {
328 data->run_mode = STEPPER_RUN_MODE_VELOCITY;
329 data->direction = direction;
330 data->max_velocity = velocity;
331 config->timing_source->update(dev, velocity);
332 if (velocity != 0) {
333 config->timing_source->start(dev);
334 } else {
335 config->timing_source->stop(dev);
336 }
337 }
338
339 return 0;
340 }
341
step_dir_stepper_common_set_event_callback(const struct device * dev,stepper_event_callback_t callback,void * user_data)342 int step_dir_stepper_common_set_event_callback(const struct device *dev,
343 stepper_event_callback_t callback, void *user_data)
344 {
345 struct step_dir_stepper_common_data *data = dev->data;
346
347 data->callback = callback;
348 data->event_cb_user_data = user_data;
349 return 0;
350 }
351
