1 /*
2 * SPDX-FileCopyrightText: Copyright (c) 2024 Carl Zeiss Meditec AG
3 * SPDX-FileCopyrightText: Copyright (c) 2024 Jilay Sandeep Pandya
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT adi_tmc5041
8
9 #include <stdlib.h>
10
11 #include <zephyr/drivers/stepper.h>
12 #include <zephyr/drivers/stepper/stepper_trinamic.h>
13
14 #include "adi_tmc_spi.h"
15 #include "adi_tmc5xxx_common.h"
16
17 #include <zephyr/logging/log.h>
18
19 LOG_MODULE_REGISTER(tmc5041, CONFIG_STEPPER_LOG_LEVEL);
20
21 struct tmc5041_data {
22 struct k_sem sem;
23 };
24
25 struct tmc5041_config {
26 const uint32_t gconf;
27 struct spi_dt_spec spi;
28 const uint32_t clock_frequency;
29 };
30
31 struct tmc5041_stepper_data {
32 struct k_work_delayable stallguard_dwork;
33 /* Work item to run the callback in a thread context. */
34 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
35 struct k_work_delayable rampstat_callback_dwork;
36 #endif
37 /* device pointer required to access config in k_work */
38 const struct device *stepper;
39 stepper_event_callback_t callback;
40 void *event_cb_user_data;
41 };
42
43 struct tmc5041_stepper_config {
44 const uint8_t index;
45 const uint16_t default_micro_step_res;
46 const int8_t sg_threshold;
47 const bool is_sg_enabled;
48 const uint32_t sg_velocity_check_interval_ms;
49 const uint32_t sg_threshold_velocity;
50 /* parent controller required for bus communication */
51 const struct device *controller;
52 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMP_GEN
53 const struct tmc_ramp_generator_data default_ramp_config;
54 #endif
55 };
56
tmc5041_write(const struct device * dev,const uint8_t reg_addr,const uint32_t reg_val)57 static int tmc5041_write(const struct device *dev, const uint8_t reg_addr, const uint32_t reg_val)
58 {
59 const struct tmc5041_config *config = dev->config;
60 struct tmc5041_data *data = dev->data;
61 const struct spi_dt_spec bus = config->spi;
62 int err;
63
64 k_sem_take(&data->sem, K_FOREVER);
65
66 err = tmc_spi_write_register(&bus, TMC5XXX_WRITE_BIT, reg_addr, reg_val);
67
68 k_sem_give(&data->sem);
69
70 if (err) {
71 LOG_ERR("Failed to write register 0x%x with value 0x%x", reg_addr, reg_val);
72 return err;
73 }
74 return 0;
75 }
76
tmc5041_read(const struct device * dev,const uint8_t reg_addr,uint32_t * reg_val)77 static int tmc5041_read(const struct device *dev, const uint8_t reg_addr, uint32_t *reg_val)
78 {
79 const struct tmc5041_config *config = dev->config;
80 struct tmc5041_data *data = dev->data;
81 const struct spi_dt_spec bus = config->spi;
82 int err;
83
84 k_sem_take(&data->sem, K_FOREVER);
85
86 err = tmc_spi_read_register(&bus, TMC5XXX_ADDRESS_MASK, reg_addr, reg_val);
87
88 k_sem_give(&data->sem);
89
90 if (err) {
91 LOG_ERR("Failed to read register 0x%x", reg_addr);
92 return err;
93 }
94 return 0;
95 }
96
tmc5041_stepper_set_event_callback(const struct device * dev,stepper_event_callback_t callback,void * user_data)97 static int tmc5041_stepper_set_event_callback(const struct device *dev,
98 stepper_event_callback_t callback, void *user_data)
99 {
100 struct tmc5041_stepper_data *data = dev->data;
101
102 data->callback = callback;
103 data->event_cb_user_data = user_data;
104 return 0;
105 }
106
stallguard_enable(const struct device * dev,const bool enable)107 static int stallguard_enable(const struct device *dev, const bool enable)
108 {
109 const struct tmc5041_stepper_config *config = dev->config;
110 uint32_t reg_value;
111 int err;
112
113 err = tmc5041_read(config->controller, TMC5041_SWMODE(config->index), ®_value);
114 if (err) {
115 LOG_ERR("Failed to read SWMODE register");
116 return -EIO;
117 }
118
119 if (enable) {
120 reg_value |= TMC5XXX_SW_MODE_SG_STOP_ENABLE;
121
122 int32_t actual_velocity;
123
124 err = tmc5041_read(config->controller, TMC5041_VACTUAL(config->index),
125 &actual_velocity);
126 if (err) {
127 LOG_ERR("Failed to read VACTUAL register");
128 return -EIO;
129 }
130
131 actual_velocity = (actual_velocity << (31 - TMC_RAMP_VACTUAL_SHIFT)) >>
132 (31 - TMC_RAMP_VACTUAL_SHIFT);
133 LOG_DBG("actual velocity: %d", actual_velocity);
134
135 if (abs(actual_velocity) < config->sg_threshold_velocity) {
136 return -EAGAIN;
137 }
138 } else {
139 reg_value &= ~TMC5XXX_SW_MODE_SG_STOP_ENABLE;
140 }
141 err = tmc5041_write(config->controller, TMC5041_SWMODE(config->index), reg_value);
142 if (err) {
143 LOG_ERR("Failed to write SWMODE register");
144 return -EIO;
145 }
146 return 0;
147 }
148
stallguard_work_handler(struct k_work * work)149 static void stallguard_work_handler(struct k_work *work)
150 {
151 struct k_work_delayable *dwork = k_work_delayable_from_work(work);
152 struct tmc5041_stepper_data *stepper_data =
153 CONTAINER_OF(dwork, struct tmc5041_stepper_data, stallguard_dwork);
154 int err;
155 const struct tmc5041_stepper_config *stepper_config = stepper_data->stepper->config;
156
157 err = stallguard_enable(stepper_data->stepper, true);
158 if (err == -EAGAIN) {
159 LOG_ERR("retrying stallguard activation");
160 k_work_reschedule(dwork, K_MSEC(stepper_config->sg_velocity_check_interval_ms));
161 }
162 if (err == -EIO) {
163 LOG_ERR("Failed to enable stallguard because of I/O error");
164 return;
165 }
166 }
167
168 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
169
execute_callback(const struct device * dev,const enum stepper_event event)170 static void execute_callback(const struct device *dev, const enum stepper_event event)
171 {
172 struct tmc5041_stepper_data *data = dev->data;
173
174 if (!data->callback) {
175 LOG_WRN_ONCE("No callback registered");
176 return;
177 }
178 data->callback(dev, event, data->event_cb_user_data);
179 }
180
rampstat_work_handler(struct k_work * work)181 static void rampstat_work_handler(struct k_work *work)
182 {
183 struct k_work_delayable *dwork = k_work_delayable_from_work(work);
184
185 struct tmc5041_stepper_data *stepper_data =
186 CONTAINER_OF(dwork, struct tmc5041_stepper_data, rampstat_callback_dwork);
187 const struct tmc5041_stepper_config *stepper_config = stepper_data->stepper->config;
188
189 __ASSERT_NO_MSG(stepper_config->controller != NULL);
190
191 uint32_t drv_status;
192 int err;
193
194 err = tmc5041_read(stepper_config->controller, TMC5041_DRVSTATUS(stepper_config->index),
195 &drv_status);
196 if (err != 0) {
197 LOG_ERR("%s: Failed to read DRVSTATUS register", stepper_data->stepper->name);
198 return;
199 }
200
201 if (FIELD_GET(TMC5XXX_DRV_STATUS_SG_STATUS_MASK, drv_status) == 1U) {
202 LOG_INF("%s: Stall detected", stepper_data->stepper->name);
203 err = tmc5041_write(stepper_config->controller,
204 TMC5041_RAMPMODE(stepper_config->index),
205 TMC5XXX_RAMPMODE_HOLD_MODE);
206 if (err != 0) {
207 LOG_ERR("%s: Failed to stop motor", stepper_data->stepper->name);
208 return;
209 }
210 }
211
212 uint32_t rampstat_value;
213
214 err = tmc5041_read(stepper_config->controller, TMC5041_RAMPSTAT(stepper_config->index),
215 &rampstat_value);
216 if (err != 0) {
217 LOG_ERR("%s: Failed to read RAMPSTAT register", stepper_data->stepper->name);
218 return;
219 }
220
221 const uint8_t ramp_stat_values = FIELD_GET(TMC5XXX_RAMPSTAT_INT_MASK, rampstat_value);
222
223 if (ramp_stat_values > 0) {
224 switch (ramp_stat_values) {
225
226 case TMC5XXX_STOP_LEFT_EVENT:
227 LOG_DBG("RAMPSTAT %s:Left end-stop detected", stepper_data->stepper->name);
228 execute_callback(stepper_data->stepper,
229 STEPPER_EVENT_LEFT_END_STOP_DETECTED);
230 break;
231
232 case TMC5XXX_STOP_RIGHT_EVENT:
233 LOG_DBG("RAMPSTAT %s:Right end-stop detected", stepper_data->stepper->name);
234 execute_callback(stepper_data->stepper,
235 STEPPER_EVENT_RIGHT_END_STOP_DETECTED);
236 break;
237
238 case TMC5XXX_POS_REACHED_EVENT:
239 LOG_DBG("RAMPSTAT %s:Position reached", stepper_data->stepper->name);
240 execute_callback(stepper_data->stepper, STEPPER_EVENT_STEPS_COMPLETED);
241 break;
242
243 case TMC5XXX_STOP_SG_EVENT:
244 LOG_DBG("RAMPSTAT %s:Stall detected", stepper_data->stepper->name);
245 stallguard_enable(stepper_data->stepper, false);
246 execute_callback(stepper_data->stepper, STEPPER_EVENT_STALL_DETECTED);
247 break;
248 default:
249 LOG_ERR("Illegal ramp stat bit field");
250 break;
251 }
252 } else {
253 k_work_reschedule(
254 &stepper_data->rampstat_callback_dwork,
255 K_MSEC(CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
256 }
257 }
258
259 #endif
260
tmc5041_stepper_enable(const struct device * dev,const bool enable)261 static int tmc5041_stepper_enable(const struct device *dev, const bool enable)
262 {
263 LOG_DBG("Stepper motor controller %s %s", dev->name, enable ? "enabled" : "disabled");
264 const struct tmc5041_stepper_config *config = dev->config;
265 uint32_t reg_value;
266 int err;
267
268 err = tmc5041_read(config->controller, TMC5041_CHOPCONF(config->index), ®_value);
269 if (err != 0) {
270 return -EIO;
271 }
272
273 if (enable) {
274 reg_value |= TMC5XXX_CHOPCONF_DRV_ENABLE_MASK;
275 } else {
276 reg_value &= ~TMC5XXX_CHOPCONF_DRV_ENABLE_MASK;
277 }
278
279 err = tmc5041_write(config->controller, TMC5041_CHOPCONF(config->index), reg_value);
280 if (err != 0) {
281 return -EIO;
282 }
283 return 0;
284 }
285
tmc5041_stepper_is_moving(const struct device * dev,bool * is_moving)286 static int tmc5041_stepper_is_moving(const struct device *dev, bool *is_moving)
287 {
288 const struct tmc5041_stepper_config *config = dev->config;
289 uint32_t reg_value;
290 int err;
291
292 err = tmc5041_read(config->controller, TMC5041_DRVSTATUS(config->index), ®_value);
293
294 if (err != 0) {
295 LOG_ERR("%s: Failed to read DRVSTATUS register", dev->name);
296 return -EIO;
297 }
298
299 *is_moving = (FIELD_GET(TMC5XXX_DRV_STATUS_STST_BIT, reg_value) != 1U);
300 LOG_DBG("Stepper motor controller %s is moving: %d", dev->name, *is_moving);
301 return 0;
302 }
303
tmc5041_stepper_move_by(const struct device * dev,const int32_t micro_steps)304 static int tmc5041_stepper_move_by(const struct device *dev, const int32_t micro_steps)
305 {
306 const struct tmc5041_stepper_config *config = dev->config;
307 struct tmc5041_stepper_data *data = dev->data;
308 int err;
309
310 if (config->is_sg_enabled) {
311 err = stallguard_enable(dev, false);
312 if (err != 0) {
313 return -EIO;
314 }
315 }
316
317 int32_t position;
318
319 err = stepper_get_actual_position(dev, &position);
320 if (err != 0) {
321 return -EIO;
322 }
323 int32_t target_position = position + micro_steps;
324
325 err = tmc5041_write(config->controller, TMC5041_RAMPMODE(config->index),
326 TMC5XXX_RAMPMODE_POSITIONING_MODE);
327 if (err != 0) {
328 return -EIO;
329 }
330 LOG_DBG("Stepper motor controller %s moved to %d by steps: %d", dev->name, target_position,
331 micro_steps);
332 err = tmc5041_write(config->controller, TMC5041_XTARGET(config->index), target_position);
333 if (err != 0) {
334 return -EIO;
335 }
336
337 if (config->is_sg_enabled) {
338 k_work_reschedule(&data->stallguard_dwork,
339 K_MSEC(config->sg_velocity_check_interval_ms));
340 }
341 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
342 if (data->callback) {
343 k_work_reschedule(
344 &data->rampstat_callback_dwork,
345 K_MSEC(CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
346 }
347 #endif
348 return 0;
349 }
350
tmc5041_stepper_set_max_velocity(const struct device * dev,uint32_t velocity)351 static int tmc5041_stepper_set_max_velocity(const struct device *dev, uint32_t velocity)
352 {
353 const struct tmc5041_stepper_config *config = dev->config;
354 const struct tmc5041_config *tmc5041_config = config->controller->config;
355 const uint32_t clock_frequency = tmc5041_config->clock_frequency;
356 uint32_t velocity_fclk;
357 int err;
358
359 velocity_fclk = tmc5xxx_calculate_velocity_from_hz_to_fclk(velocity, clock_frequency);
360
361 err = tmc5041_write(config->controller, TMC5041_VMAX(config->index), velocity_fclk);
362 if (err != 0) {
363 LOG_ERR("%s: Failed to set max velocity", dev->name);
364 return -EIO;
365 }
366 return 0;
367 }
368
tmc5041_stepper_set_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution res)369 static int tmc5041_stepper_set_micro_step_res(const struct device *dev,
370 enum stepper_micro_step_resolution res)
371 {
372 const struct tmc5041_stepper_config *config = dev->config;
373 uint32_t reg_value;
374 int err;
375
376 err = tmc5041_read(config->controller, TMC5041_CHOPCONF(config->index), ®_value);
377 if (err != 0) {
378 return -EIO;
379 }
380
381 reg_value &= ~TMC5XXX_CHOPCONF_MRES_MASK;
382 reg_value |= ((MICRO_STEP_RES_INDEX(STEPPER_MICRO_STEP_256) - LOG2(res))
383 << TMC5XXX_CHOPCONF_MRES_SHIFT);
384
385 err = tmc5041_write(config->controller, TMC5041_CHOPCONF(config->index), reg_value);
386 if (err != 0) {
387 return -EIO;
388 }
389
390 LOG_DBG("Stepper motor controller %s set micro step resolution to 0x%x", dev->name,
391 reg_value);
392 return 0;
393 }
394
tmc5041_stepper_get_micro_step_res(const struct device * dev,enum stepper_micro_step_resolution * res)395 static int tmc5041_stepper_get_micro_step_res(const struct device *dev,
396 enum stepper_micro_step_resolution *res)
397 {
398 const struct tmc5041_stepper_config *config = dev->config;
399 uint32_t reg_value;
400 int err;
401
402 err = tmc5041_read(config->controller, TMC5041_CHOPCONF(config->index), ®_value);
403 if (err != 0) {
404 return -EIO;
405 }
406 reg_value &= TMC5XXX_CHOPCONF_MRES_MASK;
407 reg_value >>= TMC5XXX_CHOPCONF_MRES_SHIFT;
408 *res = (1 << (MICRO_STEP_RES_INDEX(STEPPER_MICRO_STEP_256) - reg_value));
409 LOG_DBG("Stepper motor controller %s get micro step resolution: %d", dev->name, *res);
410 return 0;
411 }
412
tmc5041_stepper_set_reference_position(const struct device * dev,const int32_t position)413 static int tmc5041_stepper_set_reference_position(const struct device *dev, const int32_t position)
414 {
415 const struct tmc5041_stepper_config *config = dev->config;
416 int err;
417
418 err = tmc5041_write(config->controller, TMC5041_RAMPMODE(config->index),
419 TMC5XXX_RAMPMODE_HOLD_MODE);
420 if (err != 0) {
421 return -EIO;
422 }
423
424 err = tmc5041_write(config->controller, TMC5041_XACTUAL(config->index), position);
425 if (err != 0) {
426 return -EIO;
427 }
428 LOG_DBG("Stepper motor controller %s set actual position to %d", dev->name, position);
429 return 0;
430 }
431
tmc5041_stepper_get_actual_position(const struct device * dev,int32_t * position)432 static int tmc5041_stepper_get_actual_position(const struct device *dev, int32_t *position)
433 {
434 const struct tmc5041_stepper_config *config = dev->config;
435 int err;
436
437 err = tmc5041_read(config->controller, TMC5041_XACTUAL(config->index), position);
438 if (err != 0) {
439 return -EIO;
440 }
441 LOG_DBG("%s actual position: %d", dev->name, *position);
442 return 0;
443 }
444
tmc5041_stepper_move_to(const struct device * dev,const int32_t micro_steps)445 static int tmc5041_stepper_move_to(const struct device *dev, const int32_t micro_steps)
446 {
447 LOG_DBG("Stepper motor controller %s set target position to %d", dev->name, micro_steps);
448 const struct tmc5041_stepper_config *config = dev->config;
449 struct tmc5041_stepper_data *data = dev->data;
450 int err;
451
452 if (config->is_sg_enabled) {
453 stallguard_enable(dev, false);
454 }
455
456 err = tmc5041_write(config->controller, TMC5041_RAMPMODE(config->index),
457 TMC5XXX_RAMPMODE_POSITIONING_MODE);
458 if (err != 0) {
459 return -EIO;
460 }
461 err = tmc5041_write(config->controller, TMC5041_XTARGET(config->index), micro_steps);
462 if (err != 0) {
463 return -EIO;
464 }
465
466 if (config->is_sg_enabled) {
467 k_work_reschedule(&data->stallguard_dwork,
468 K_MSEC(config->sg_velocity_check_interval_ms));
469 }
470 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
471 if (data->callback) {
472 k_work_reschedule(
473 &data->rampstat_callback_dwork,
474 K_MSEC(CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
475 }
476 #endif
477 return 0;
478 }
479
tmc5041_stepper_run(const struct device * dev,const enum stepper_direction direction,const uint32_t velocity)480 static int tmc5041_stepper_run(const struct device *dev, const enum stepper_direction direction,
481 const uint32_t velocity)
482 {
483 LOG_DBG("Stepper motor controller %s run with velocity %d", dev->name, velocity);
484 const struct tmc5041_stepper_config *config = dev->config;
485 const struct tmc5041_config *tmc5041_config = config->controller->config;
486 struct tmc5041_stepper_data *data = dev->data;
487 const uint32_t clock_frequency = tmc5041_config->clock_frequency;
488 uint32_t velocity_fclk;
489 int err;
490
491 velocity_fclk = tmc5xxx_calculate_velocity_from_hz_to_fclk(velocity, clock_frequency);
492
493 if (config->is_sg_enabled) {
494 err = stallguard_enable(dev, false);
495 if (err != 0) {
496 return -EIO;
497 }
498 }
499
500 switch (direction) {
501 case STEPPER_DIRECTION_POSITIVE:
502 err = tmc5041_write(config->controller, TMC5041_RAMPMODE(config->index),
503 TMC5XXX_RAMPMODE_POSITIVE_VELOCITY_MODE);
504 if (err != 0) {
505 return -EIO;
506 }
507 err = tmc5041_write(config->controller, TMC5041_VMAX(config->index), velocity_fclk);
508 if (err != 0) {
509 return -EIO;
510 }
511 break;
512
513 case STEPPER_DIRECTION_NEGATIVE:
514 err = tmc5041_write(config->controller, TMC5041_RAMPMODE(config->index),
515 TMC5XXX_RAMPMODE_NEGATIVE_VELOCITY_MODE);
516 if (err != 0) {
517 return -EIO;
518 }
519 err = tmc5041_write(config->controller, TMC5041_VMAX(config->index), velocity_fclk);
520 if (err != 0) {
521 return -EIO;
522 }
523 break;
524 }
525
526 if (config->is_sg_enabled) {
527 k_work_reschedule(&data->stallguard_dwork,
528 K_MSEC(config->sg_velocity_check_interval_ms));
529 }
530 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
531 if (data->callback) {
532 k_work_reschedule(
533 &data->rampstat_callback_dwork,
534 K_MSEC(CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
535 }
536 #endif
537 return 0;
538 }
539
540 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMP_GEN
541
tmc5041_stepper_set_ramp(const struct device * dev,const struct tmc_ramp_generator_data * ramp_data)542 int tmc5041_stepper_set_ramp(const struct device *dev,
543 const struct tmc_ramp_generator_data *ramp_data)
544 {
545 LOG_DBG("Stepper motor controller %s set ramp", dev->name);
546 const struct tmc5041_stepper_config *config = dev->config;
547 int err;
548
549 err = tmc5041_write(config->controller, TMC5041_VSTART(config->index), ramp_data->vstart);
550 if (err != 0) {
551 return -EIO;
552 }
553 err = tmc5041_write(config->controller, TMC5041_A1(config->index), ramp_data->a1);
554 if (err != 0) {
555 return -EIO;
556 }
557 err = tmc5041_write(config->controller, TMC5041_AMAX(config->index), ramp_data->amax);
558 if (err != 0) {
559 return -EIO;
560 }
561 err = tmc5041_write(config->controller, TMC5041_D1(config->index), ramp_data->d1);
562 if (err != 0) {
563 return -EIO;
564 }
565 err = tmc5041_write(config->controller, TMC5041_DMAX(config->index), ramp_data->dmax);
566 if (err != 0) {
567 return -EIO;
568 }
569 err = tmc5041_write(config->controller, TMC5041_V1(config->index), ramp_data->v1);
570 if (err != 0) {
571 return -EIO;
572 }
573 err = tmc5041_write(config->controller, TMC5041_VMAX(config->index), ramp_data->vmax);
574 if (err != 0) {
575 return -EIO;
576 }
577 err = tmc5041_write(config->controller, TMC5041_VSTOP(config->index), ramp_data->vstop);
578 if (err != 0) {
579 return -EIO;
580 }
581 err = tmc5041_write(config->controller, TMC5041_TZEROWAIT(config->index),
582 ramp_data->tzerowait);
583 if (err != 0) {
584 return -EIO;
585 }
586 err = tmc5041_write(config->controller, TMC5041_VHIGH(config->index), ramp_data->vhigh);
587 if (err != 0) {
588 return -EIO;
589 }
590 err = tmc5041_write(config->controller, TMC5041_VCOOLTHRS(config->index),
591 ramp_data->vcoolthrs);
592 if (err != 0) {
593 return -EIO;
594 }
595 err = tmc5041_write(config->controller, TMC5041_IHOLD_IRUN(config->index),
596 ramp_data->iholdrun);
597 if (err != 0) {
598 return -EIO;
599 }
600 return 0;
601 }
602
603 #endif
604
tmc5041_init(const struct device * dev)605 static int tmc5041_init(const struct device *dev)
606 {
607 LOG_DBG("TMC5041 stepper motor controller %s initialized", dev->name);
608 struct tmc5041_data *data = dev->data;
609 const struct tmc5041_config *config = dev->config;
610 int err;
611
612 k_sem_init(&data->sem, 1, 1);
613
614 if (!spi_is_ready_dt(&config->spi)) {
615 LOG_ERR("SPI bus is not ready");
616 return -ENODEV;
617 }
618
619 /* Init non motor-index specific registers here. */
620 LOG_DBG("GCONF: %d", config->gconf);
621 err = tmc5041_write(dev, TMC5XXX_GCONF, config->gconf);
622 if (err != 0) {
623 return -EIO;
624 }
625
626 /* Read GSTAT register values to clear any errors SPI Datagram. */
627 uint32_t gstat_value;
628
629 err = tmc5041_read(dev, TMC5XXX_GSTAT, &gstat_value);
630 if (err != 0) {
631 return -EIO;
632 }
633
634 LOG_DBG("Device %s initialized", dev->name);
635 return 0;
636 }
637
tmc5041_stepper_init(const struct device * dev)638 static int tmc5041_stepper_init(const struct device *dev)
639 {
640 const struct tmc5041_stepper_config *stepper_config = dev->config;
641 struct tmc5041_stepper_data *data = dev->data;
642 int err;
643
644 LOG_DBG("Controller: %s, Stepper: %s", stepper_config->controller->name, dev->name);
645
646 if (stepper_config->is_sg_enabled) {
647 k_work_init_delayable(&data->stallguard_dwork, stallguard_work_handler);
648
649 err = tmc5041_write(stepper_config->controller,
650 TMC5041_SWMODE(stepper_config->index), BIT(10));
651 if (err != 0) {
652 return -EIO;
653 }
654
655 LOG_DBG("Setting stall guard to %d with delay %d ms", stepper_config->sg_threshold,
656 stepper_config->sg_velocity_check_interval_ms);
657 if (!IN_RANGE(stepper_config->sg_threshold, TMC5XXX_SG_MIN_VALUE,
658 TMC5XXX_SG_MAX_VALUE)) {
659 LOG_ERR("Stallguard threshold out of range");
660 return -EINVAL;
661 }
662
663 int32_t stall_guard_threshold = (int32_t)stepper_config->sg_threshold;
664
665 err = tmc5041_write(
666 stepper_config->controller, TMC5041_COOLCONF(stepper_config->index),
667 stall_guard_threshold << TMC5XXX_COOLCONF_SG2_THRESHOLD_VALUE_SHIFT);
668 if (err != 0) {
669 return -EIO;
670 }
671 err = stallguard_enable(dev, true);
672 if (err == -EAGAIN) {
673 LOG_ERR("retrying stallguard activation");
674 k_work_reschedule(&data->stallguard_dwork,
675 K_MSEC(stepper_config->sg_velocity_check_interval_ms));
676 }
677 }
678
679 #ifdef CONFIG_STEPPER_ADI_TMC5041_RAMP_GEN
680 err = tmc5041_stepper_set_ramp(dev, &stepper_config->default_ramp_config);
681 if (err != 0) {
682 return -EIO;
683 }
684 #endif
685
686 #if CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL
687 k_work_init_delayable(&data->rampstat_callback_dwork, rampstat_work_handler);
688 k_work_reschedule(&data->rampstat_callback_dwork,
689 K_MSEC(CONFIG_STEPPER_ADI_TMC5041_RAMPSTAT_POLL_INTERVAL_IN_MSEC));
690 #endif
691 err = tmc5041_stepper_set_micro_step_res(dev, stepper_config->default_micro_step_res);
692 if (err != 0) {
693 return -EIO;
694 }
695 return 0;
696 }
697
698 #define TMC5041_SHAFT_CONFIG(child) \
699 (DT_PROP(child, invert_direction) << TMC5041_GCONF_SHAFT_SHIFT(DT_REG_ADDR(child))) |
700
701 #define TMC5041_STEPPER_CONFIG_DEFINE(child) \
702 COND_CODE_1(DT_PROP_EXISTS(child, stallguard_threshold_velocity), \
703 BUILD_ASSERT(DT_PROP(child, stallguard_threshold_velocity), \
704 "stallguard threshold velocity must be a positive value"), ()); \
705 IF_ENABLED(CONFIG_STEPPER_ADI_TMC5041_RAMP_GEN, (CHECK_RAMP_DT_DATA(child))); \
706 static const struct tmc5041_stepper_config tmc5041_stepper_config_##child = { \
707 .controller = DEVICE_DT_GET(DT_PARENT(child)), \
708 .default_micro_step_res = DT_PROP(child, micro_step_res), \
709 .index = DT_REG_ADDR(child), \
710 .sg_threshold = DT_PROP(child, stallguard2_threshold), \
711 .sg_threshold_velocity = DT_PROP(child, stallguard_threshold_velocity), \
712 .sg_velocity_check_interval_ms = DT_PROP(child, \
713 stallguard_velocity_check_interval_ms), \
714 .is_sg_enabled = DT_PROP(child, activate_stallguard2), \
715 IF_ENABLED(CONFIG_STEPPER_ADI_TMC5041_RAMP_GEN, \
716 (.default_ramp_config = TMC_RAMP_DT_SPEC_GET(child))) };
717
718 #define TMC5041_STEPPER_DATA_DEFINE(child) \
719 static struct tmc5041_stepper_data tmc5041_stepper_data_##child = { \
720 .stepper = DEVICE_DT_GET(child),};
721
722 #define TMC5041_STEPPER_API_DEFINE(child) \
723 static DEVICE_API(stepper, tmc5041_stepper_api_##child) = { \
724 .enable = tmc5041_stepper_enable, \
725 .is_moving = tmc5041_stepper_is_moving, \
726 .move_by = tmc5041_stepper_move_by, \
727 .set_max_velocity = tmc5041_stepper_set_max_velocity, \
728 .set_micro_step_res = tmc5041_stepper_set_micro_step_res, \
729 .get_micro_step_res = tmc5041_stepper_get_micro_step_res, \
730 .set_reference_position = tmc5041_stepper_set_reference_position, \
731 .get_actual_position = tmc5041_stepper_get_actual_position, \
732 .move_to = tmc5041_stepper_move_to, \
733 .run = tmc5041_stepper_run, \
734 .set_event_callback = tmc5041_stepper_set_event_callback, };
735
736 #define TMC5041_STEPPER_DEFINE(child) \
737 DEVICE_DT_DEFINE(child, tmc5041_stepper_init, NULL, &tmc5041_stepper_data_##child, \
738 &tmc5041_stepper_config_##child, POST_KERNEL, \
739 CONFIG_STEPPER_INIT_PRIORITY, &tmc5041_stepper_api_##child);
740
741 #define TMC5041_DEFINE(inst) \
742 BUILD_ASSERT(DT_INST_CHILD_NUM(inst) <= 2, "tmc5041 can drive two steppers at max"); \
743 BUILD_ASSERT((DT_INST_PROP(inst, clock_frequency) > 0), \
744 "clock frequency must be non-zero positive value"); \
745 static struct tmc5041_data tmc5041_data_##inst; \
746 static const struct tmc5041_config tmc5041_config_##inst = { \
747 .gconf = ( \
748 (DT_INST_PROP(inst, poscmp_enable) << TMC5041_GCONF_POSCMP_ENABLE_SHIFT) | \
749 (DT_INST_PROP(inst, test_mode) << TMC5041_GCONF_TEST_MODE_SHIFT) | \
750 DT_INST_FOREACH_CHILD(inst, TMC5041_SHAFT_CONFIG) \
751 (DT_INST_PROP(inst, lock_gconf) << TMC5041_LOCK_GCONF_SHIFT)), \
752 .spi = SPI_DT_SPEC_INST_GET(inst, (SPI_OP_MODE_MASTER | SPI_TRANSFER_MSB | \
753 SPI_MODE_CPOL | SPI_MODE_CPHA | SPI_WORD_SET(8)), 0), \
754 .clock_frequency = DT_INST_PROP(inst, clock_frequency),}; \
755 DT_INST_FOREACH_CHILD(inst, TMC5041_STEPPER_CONFIG_DEFINE); \
756 DT_INST_FOREACH_CHILD(inst, TMC5041_STEPPER_DATA_DEFINE); \
757 DT_INST_FOREACH_CHILD(inst, TMC5041_STEPPER_API_DEFINE); \
758 DT_INST_FOREACH_CHILD(inst, TMC5041_STEPPER_DEFINE); \
759 DEVICE_DT_INST_DEFINE(inst, tmc5041_init, NULL, &tmc5041_data_##inst, \
760 &tmc5041_config_##inst, POST_KERNEL, CONFIG_STEPPER_INIT_PRIORITY,\
761 NULL);
762
763 DT_INST_FOREACH_STATUS_OKAY(TMC5041_DEFINE)
764