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), &reg_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), &reg_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), &reg_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), &reg_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), &reg_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