/*******************************************************************************
* Copyright © 2019 TRINAMIC Motion Control GmbH & Co. KG
* (now owned by Analog Devices Inc.),
*
* Copyright © 2023 Analog Devices Inc. All Rights Reserved. This software is
* proprietary & confidential to Analog Devices, Inc. and its licensors.
*******************************************************************************/


#include "Board.h"
#include "tmc/ic/TMC2209/TMC2209.h"
#include "tmc/StepDir.h"

#undef  TMC2209_MAX_VELOCITY
#define TMC2209_MAX_VELOCITY  STEPDIR_MAX_VELOCITY

// Stepdir precision: 2^17 -> 17 digits of precision
#define STEPDIR_PRECISION 131072

#define ERRORS_VM        (1<<0)
#define ERRORS_VM_UNDER  (1<<1)
#define ERRORS_VM_OVER   (1<<2)

#define VM_MIN  50   // VM[V/10] min
#define VM_MAX  390  // VM[V/10] max

#define MOTORS 1

#define VREF_FULLSCALE 2100 // mV // with R308 achievable Vref_max is ~2100mV
//#define VREF_FULLSCALE 3300 // mV // without R308 achievable Vref_max is ~2500mV


static uint32_t right(uint8_t motor, int32_t velocity);
static uint32_t left(uint8_t motor, int32_t velocity);
static uint32_t rotate(uint8_t motor, int32_t velocity);
static uint32_t stop(uint8_t motor);
static uint32_t moveTo(uint8_t motor, int32_t position);
static uint32_t moveBy(uint8_t motor, int32_t *ticks);
static uint32_t GAP(uint8_t type, uint8_t motor, int32_t *value);
static uint32_t SAP(uint8_t type, uint8_t motor, int32_t value);

static void checkErrors (uint32_t tick);
static void deInit(void);
static uint32_t userFunction(uint8_t type, uint8_t motor, int32_t *value);

static void periodicJob(uint32_t tick);
static uint8_t reset(void);
static uint8_t restore(void);
static void enableDriver(DriverState state);

static UART_Config *TMC2209_UARTChannel;
static ConfigurationTypeDef *TMC2209_config;

static uint16_t vref; // mV
static int32_t thigh;

static timer_channel timerChannel;
// Helper macro - Access the chip object in the driver boards union
#define TMC2209 (driverBoards.tmc2209)

// Helper macro - index is always 1 here (channel 1 <-> index 0, channel 2 <-> index 1)
#define TMC2209_CRC(data, length) tmc_CRC8(data, length, 1)

typedef struct
{
	IOPinTypeDef  *ENN;
	IOPinTypeDef  *SPREAD;
	IOPinTypeDef  *STEP;
	IOPinTypeDef  *DIR;
	IOPinTypeDef  *MS1_AD0;
	IOPinTypeDef  *MS2_AD1;
	IOPinTypeDef  *DIAG;
	IOPinTypeDef  *INDEX;
	IOPinTypeDef  *UC_PWM;
	IOPinTypeDef  *STDBY;
} PinsTypeDef;

static PinsTypeDef Pins;

static inline TMC2209TypeDef *motorToIC(uint8_t motor)
{
	UNUSED(motor);

	return &TMC2209;
}

static inline UART_Config *channelToUART(uint8_t channel)
{
	UNUSED(channel);

	return TMC2209_UARTChannel;
}

// => UART wrapper
// Write [writeLength] bytes from the [data] array.
// If [readLength] is greater than zero, read [readLength] bytes from the
// [data] array.
void tmc2209_readWriteArray(uint8_t channel, uint8_t *data, size_t writeLength, size_t readLength)
{
	UART_readWrite(channelToUART(channel), data, writeLength, readLength);
}
// <= UART wrapper

// => CRC wrapper
// Return the CRC8 of [length] bytes of data stored in the [data] array.
uint8_t tmc2209_CRC8(uint8_t *data, size_t length)
{
	return TMC2209_CRC(data, length);
}
// <= CRC wrapper

void tmc2209_writeRegister(uint8_t motor, uint16_t address, int32_t value)
{
	tmc2209_writeInt(motorToIC(motor), (uint8_t) address, value);

}

void tmc2209_readRegister(uint8_t motor, uint16_t address, int32_t *value)
{
	*value = tmc2209_readInt(motorToIC(motor), (uint8_t) address);
}

static uint32_t rotate(uint8_t motor, int32_t velocity)
{
	if(motor >= MOTORS)
		return TMC_ERROR_MOTOR;

	StepDir_rotate(motor, velocity);

	return TMC_ERROR_NONE;
}

static uint32_t right(uint8_t motor, int32_t velocity)
{
	return rotate(motor, velocity);
}

static uint32_t left(uint8_t motor, int32_t velocity)
{
	return rotate(motor, -velocity);
}

static uint32_t stop(uint8_t motor)
{
	return rotate(motor, 0);
}

static uint32_t moveTo(uint8_t motor, int32_t position)
{
	if(motor >= MOTORS)
		return TMC_ERROR_MOTOR;

	StepDir_moveTo(motor, position);

	return TMC_ERROR_NONE;
}

static uint32_t moveBy(uint8_t motor, int32_t *ticks)
{
	if(motor >= MOTORS)
		return TMC_ERROR_MOTOR;

	// determine actual position and add numbers of ticks to move
	*ticks += StepDir_getActualPosition(motor);

	return moveTo(motor, *ticks);
}

static uint32_t handleParameter(uint8_t readWrite, uint8_t motor, uint8_t type, int32_t *value)
{
	uint32_t errors = TMC_ERROR_NONE;
	int32_t buffer = 0;

	if(motor >= MOTORS)
		return TMC_ERROR_MOTOR;

	switch(type)
	{
	case 0:
		// Target position
		if(readWrite == READ) {
			*value = StepDir_getTargetPosition(motor);
		} else if(readWrite == WRITE) {
			StepDir_moveTo(motor, *value);
		}
		break;
	case 1:
		// Actual position
		if(readWrite == READ) {
			*value = StepDir_getActualPosition(motor);
		} else if(readWrite == WRITE) {
			StepDir_setActualPosition(motor, *value);
		}
		break;
	case 2:
		// Target speed
		if(readWrite == READ) {
			*value = StepDir_getTargetVelocity(motor);
		} else if(readWrite == WRITE) {
			StepDir_rotate(motor, *value);
		}
		break;
	case 3:
		// Actual speed
		if(readWrite == READ) {
			*value = StepDir_getActualVelocity(motor);
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	case 4:
		// Maximum speed
		if(readWrite == READ) {
			*value = StepDir_getVelocityMax(motor);
		} else if(readWrite == WRITE) {
			StepDir_setVelocityMax(motor, abs(*value));
		}
		break;
	case 5:
		// Maximum acceleration
		if(readWrite == READ) {
			*value = StepDir_getAcceleration(motor);
		} else if(readWrite == WRITE) {
			StepDir_setAcceleration(motor, *value);
		}
		break;
	case 6:
		// Maximum current
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_IHOLD_IRUN, TMC2209_IRUN_MASK, TMC2209_IRUN_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_IHOLD_IRUN, TMC2209_IRUN_MASK, TMC2209_IRUN_SHIFT, *value);
		}
		break;
	case 7:
		// Standby current
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_IHOLD_IRUN, TMC2209_IHOLD_MASK, TMC2209_IHOLD_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_IHOLD_IRUN, TMC2209_IHOLD_MASK, TMC2209_IHOLD_SHIFT, *value);
		}
		break;
	case 8:
		// Position reached flag
		if(readWrite == READ) {
			*value = (StepDir_getStatus(motor) & STATUS_TARGET_REACHED)? 1:0;
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	case 9:
		// VREF
		if (readWrite == READ) {
			*value = vref;
		} else {
			if ((uint32_t) *value < VREF_FULLSCALE) {
				vref = *value;
				Timer.setDuty(timerChannel, ((float)vref) / VREF_FULLSCALE);
			} else {
				errors |= TMC_ERROR_VALUE;
			}
		}
		break;
	case 23:
		// Speed threshold for high speed mode
		if(readWrite == READ) {
			buffer = thigh;
			*value = MIN(0xFFFFF, (1<<24) / ((buffer) ? buffer : 1));
		} else if(readWrite == WRITE) {
			*value = MIN(0xFFFFF, (1<<24) / ((*value) ? *value : 1));
			thigh = *value;
		}
		break;
	case 28:
		// Internal RSense
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_GCONF, TMC2209_INTERNAL_RSENSE_MASK, TMC2209_INTERNAL_RSENSE_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_GCONF, TMC2209_INTERNAL_RSENSE_MASK, TMC2209_INTERNAL_RSENSE_SHIFT, *value);
		}
		break;
	case 29:
		// Measured Speed
		if(readWrite == READ) {
			buffer = (int32_t)(((int64_t)StepDir_getFrequency(motor) * (int64_t)122) / (int64_t)TMC2209_FIELD_READ(motorToIC(motor), TMC2209_TSTEP, TMC2209_TSTEP_MASK, TMC2209_TSTEP_SHIFT));
			*value = (abs(buffer) < 20) ? 0 : buffer;
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	case 50: // StepDir internal(0)/external(1)
		if(readWrite == READ) {
			*value = StepDir_getMode(motor);
		} else if(readWrite == WRITE) {
			StepDir_setMode(motor, *value);
		}
		break;
	case 51: // StepDir interrupt frequency
		if(readWrite == READ) {
			*value = StepDir_getFrequency(motor);
		} else if(readWrite == WRITE) {
			StepDir_setFrequency(motor, *value);
		}
		break;
	case 140:
		// Microstep Resolution
		if(readWrite == READ) {
			*value = 256 >> TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_MRES_MASK, TMC2209_MRES_SHIFT);
		} else if(readWrite == WRITE) {
			switch(*value)
			{
			case 1:    *value = 8;   break;
			case 2:    *value = 7;   break;
			case 4:    *value = 6;   break;
			case 8:    *value = 5;   break;
			case 16:   *value = 4;   break;
			case 32:   *value = 3;   break;
			case 64:   *value = 2;   break;
			case 128:  *value = 1;   break;
			case 256:  *value = 0;   break;
			default:   *value = -1;  break;
			}

			if(*value != -1)
			{
				TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_MRES_MASK, TMC2209_MRES_SHIFT, *value);
			}
			else
			{
				errors |= TMC_ERROR_VALUE;
			}
		}
		break;
	case 162:
		// Chopper blank time
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_TBL_MASK, TMC2209_TBL_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_TBL_MASK, TMC2209_TBL_SHIFT, *value);
		}
		break;
	case 165:
		// Chopper hysteresis end / fast decay time
		if(readWrite == READ) {
			if(tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF) & (1<<14))
			{
				*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_HEND_MASK, TMC2209_HEND_SHIFT);
			}
			else
			{
				buffer = tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF);
				*value = (tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF) >> 4) & 0x07;
				if(buffer & (1<<11))
					*value |= 1<<3;
			}
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	case 166:
		// Chopper hysteresis start / sine wave offset
		if(readWrite == READ) {
			if(tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF) & (1<<14))
			{
				*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_HSTRT_MASK, TMC2209_HSTRT_SHIFT);
			}
			else
			{
				buffer = tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF);
				*value = (tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF) >> 7) & 0x0F;
				if(buffer & (1<<11))
					*value |= 1<<3;
			}
		} else if(readWrite == WRITE) {
			if(tmc2209_readInt(motorToIC(motor), TMC2209_CHOPCONF) & (1<<14))
			{
				TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_HSTRT_MASK, TMC2209_HSTRT_SHIFT, *value);
			}
			else
			{
				TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_HEND_MASK, TMC2209_HEND_SHIFT, *value);
			}
		}
		break;
	case 167:
		// Chopper off time
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_TOFF_MASK, TMC2209_TOFF_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_TOFF_MASK, TMC2209_TOFF_SHIFT, *value);
		}
		break;
	case 168:
		// smartEnergy current minimum (SEIMIN)
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEIMIN_MASK, TMC2209_SEIMIN_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEIMIN_MASK, TMC2209_SEIMIN_SHIFT, *value);
		}
		break;
	case 169:
		// smartEnergy current down step
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEDN_MASK, TMC2209_SEDN_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEDN_MASK, TMC2209_SEDN_SHIFT, *value);
		}
		break;
	case 170:
		// smartEnergy hysteresis
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEMAX_MASK, TMC2209_SEMAX_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEMAX_MASK, TMC2209_SEMAX_SHIFT, *value);
		}
		break;
	case 171:
		// smartEnergy current up step
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEUP_MASK, TMC2209_SEUP_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEUP_MASK, TMC2209_SEUP_SHIFT, *value);
		}
		break;
	case 172:
		// smartEnergy hysteresis start
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEMIN_MASK, TMC2209_SEMIN_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_COOLCONF, TMC2209_SEMIN_MASK, TMC2209_SEMIN_SHIFT, *value);
		}
		break;
	case 174:
		// stallGuard2 threshold
		if(readWrite == READ) {
			*value = tmc2209_readInt(motorToIC(motor), TMC2209_SGTHRS);
		} else if(readWrite == WRITE) {
			tmc2209_writeInt(motorToIC(motor), TMC2209_SGTHRS, *value);
		}
		break;
	case 179:
		// VSense
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_VSENSE_MASK, TMC2209_VSENSE_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_CHOPCONF, TMC2209_VSENSE_MASK, TMC2209_VSENSE_SHIFT, *value);
		}
		break;
	case 180:
		// smartEnergy actual current
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_DRVSTATUS, TMC2209_CS_ACTUAL_MASK, TMC2209_CS_ACTUAL_SHIFT);
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	case 181:
		// smartEnergy stall velocity
		if(readWrite == READ) {
			*value = StepDir_getStallGuardThreshold(motor);
		} else if(readWrite == WRITE) {
			// Store the threshold value in the internal StepDir generator
			StepDir_setStallGuardThreshold(motor, *value);

			// Convert the value for the TCOOLTHRS register
			// The IC only sends out Stallguard errors while TCOOLTHRS >= TSTEP >= TPWMTHRS
			// The TSTEP value is measured. To prevent measurement inaccuracies hiding
			// a stall signal, we decrease the needed velocity by roughly 12% before converting it.
			*value -= (*value) >> 3;
			if (*value)
			{
				*value = MIN(0x000FFFFF, (1<<24) / (*value));
			}
			else
			{
				*value = 0x000FFFFF;
			}
			tmc2209_writeInt(motorToIC(motor), TMC2209_TCOOLTHRS, *value);
		}
		break;
	case 182:
		// smartEnergy threshold speed
		if(readWrite == READ) {
			buffer = tmc2209_readInt(motorToIC(motor), TMC2209_TCOOLTHRS);
			*value = MIN(0xFFFFF, (1<<24) / ((buffer) ? buffer : 1));
		} else if(readWrite == WRITE) {
			*value = MIN(0xFFFFF, (1<<24) / ((*value) ? *value : 1));
			tmc2209_writeInt(motorToIC(motor), TMC2209_TCOOLTHRS, *value);
		}
		break;
	case 186:
		// PWM threshold speed
		if(readWrite == READ) {
			buffer = tmc2209_readInt(motorToIC(motor), TMC2209_TPWMTHRS);
			*value = MIN(0xFFFFF, (1<<24) / ((buffer) ? buffer : 1));
		} else if(readWrite == WRITE) {
			*value = MIN(0xFFFFF, (1<<24) / ((*value) ? *value : 1));
			tmc2209_writeInt(motorToIC(motor), TMC2209_TPWMTHRS, *value);
		}
		break;
	case 187:
		// PWM gradient
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_GRAD_MASK, TMC2209_PWM_GRAD_SHIFT);
		} else if(readWrite == WRITE) {
			// Set gradient
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_GRAD_MASK, TMC2209_PWM_GRAD_SHIFT, *value);

			// Enable/disable stealthChop accordingly
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_GCONF, TMC2209_EN_SPREADCYCLE_MASK, TMC2209_EN_SPREADCYCLE_SHIFT, (*value > 0) ? 0 : 1);
		}
		break;
	case 191:
		// PWM frequency
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_FREQ_MASK, TMC2209_PWM_FREQ_SHIFT);
		} else if(readWrite == WRITE) {
			if(*value >= 0 && *value < 4)
			{
				TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_FREQ_MASK, TMC2209_PWM_FREQ_SHIFT, *value);
			}
			else
			{
				errors |= TMC_ERROR_VALUE;
			}
		}
		break;
	case 192:
		// PWM autoscale
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_AUTOSCALE_MASK, TMC2209_PWM_AUTOSCALE_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_PWMCONF, TMC2209_PWM_AUTOSCALE_MASK, TMC2209_PWM_AUTOSCALE_SHIFT, (*value)? 1:0);
		}
		break;
	case 204:
		// Freewheeling mode
		if(readWrite == READ) {
			*value = TMC2209_FIELD_READ(motorToIC(motor), TMC2209_PWMCONF, TMC2209_FREEWHEEL_MASK, TMC2209_FREEWHEEL_SHIFT);
		} else if(readWrite == WRITE) {
			TMC2209_FIELD_UPDATE(motorToIC(motor), TMC2209_PWMCONF, TMC2209_FREEWHEEL_MASK, TMC2209_FREEWHEEL_SHIFT, *value);
		}
		break;
	case 206:
		// Load value
		if(readWrite == READ) {
			*value = tmc2209_readInt(motorToIC(motor), TMC2209_SG_RESULT);
		} else if(readWrite == WRITE) {
			errors |= TMC_ERROR_TYPE;
		}
		break;
	default:
		errors |= TMC_ERROR_TYPE;
		break;
	}

	return errors;
}

static uint32_t SAP(uint8_t type, uint8_t motor, int32_t value)
{
	return handleParameter(WRITE, motor, type, &value);
}

static uint32_t GAP(uint8_t type, uint8_t motor, int32_t *value)
{
	return handleParameter(READ, motor, type, value);
}

static void checkErrors(uint32_t tick)
{
	UNUSED(tick);
	Evalboards.ch2.errors = 0;
}

static uint32_t userFunction(uint8_t type, uint8_t motor, int32_t *value)
{
	uint32_t errors = 0;
	uint8_t state;
	IOPinTypeDef *pin;

	switch(type)
	{
	case 0:  // Read StepDir status bits
		*value = StepDir_getStatus(motor);
		break;
	case 1:
		tmc2209_set_slave(motorToIC(motor), (*value) & 0xFF);
		break;
	case 2:
		*value = tmc2209_get_slave(motorToIC(motor));
		break;
	case 3:
		*value = Timer.getDuty(timerChannel) * 100 / TIMER_MAX;
		break;
	case 4:
		Timer.setDuty(timerChannel, ((float)*value) / 100);
		break;
	case 5: // Set pin state
		state = (*value) & 0x03;
		pin = Pins.ENN;
		switch(motor) {
		case 0:
			pin = Pins.ENN;
			break;
		case 1:
			pin = Pins.SPREAD;
			break;
		case 2:
			pin = Pins.MS1_AD0;
			break;
		case 3:
			pin = Pins.MS2_AD1;
			break;
		case 4:
			pin = Pins.UC_PWM;
			break;
		case 5:
			pin = Pins.STDBY;
			break;
		}
		HAL.IOs->config->setToState(pin, state);
		break;
	case 6: // Get pin state
		pin = Pins.ENN;
		switch(motor) {
		case 0:
			pin = Pins.ENN;
			break;
		case 1:
			pin = Pins.SPREAD;
			break;
		case 2:
			pin = Pins.MS1_AD0;
			break;
		case 3:
			pin = Pins.MS2_AD1;
			break;
		case 4:
			pin = Pins.UC_PWM;
			break;
		case 5:
			pin = Pins.STDBY;
			break;
		}
		*value = (uint32_t) HAL.IOs->config->getState(pin);
		break;
	default:
		errors |= TMC_ERROR_TYPE;
		break;
	}

	return errors;
}

static void deInit(void)
{
	enableDriver(DRIVER_DISABLE);
	HAL.IOs->config->reset(Pins.ENN);
	HAL.IOs->config->reset(Pins.SPREAD);
	HAL.IOs->config->reset(Pins.STEP);
	HAL.IOs->config->reset(Pins.DIR);
	HAL.IOs->config->reset(Pins.MS1_AD0);
	HAL.IOs->config->reset(Pins.MS2_AD1);
	HAL.IOs->config->reset(Pins.DIAG);
	HAL.IOs->config->reset(Pins.INDEX);
	HAL.IOs->config->reset(Pins.STDBY);
	HAL.IOs->config->reset(Pins.UC_PWM);

	StepDir_deInit();
	Timer.deInit();
}

static uint8_t reset()
{
	StepDir_init(STEPDIR_PRECISION);
	StepDir_setPins(0, Pins.STEP, Pins.DIR, Pins.DIAG);

	return tmc2209_reset(&TMC2209);
}

static uint8_t restore()
{
	return tmc2209_restore(&TMC2209);
}

static void enableDriver(DriverState state)
{
	if(state == DRIVER_USE_GLOBAL_ENABLE)
		state = Evalboards.driverEnable;

	if(state == DRIVER_DISABLE)
		HAL.IOs->config->setHigh(Pins.ENN);
	else if((state == DRIVER_ENABLE) && (Evalboards.driverEnable == DRIVER_ENABLE))
		HAL.IOs->config->setLow(Pins.ENN);
}

static void periodicJob(uint32_t tick)
{
	tmc2209_periodicJob(&TMC2209, tick);
	StepDir_periodicJob(0);
}

void TMC2209_init(void)
{

#if defined(Landungsbruecke) || defined(LandungsbrueckeSmall)
	timerChannel = TIMER_CHANNEL_3;

#elif defined(LandungsbrueckeV3)
	timerChannel = TIMER_CHANNEL_4;
#endif
	tmc_fillCRC8Table(0x07, true, 1);
	thigh = 0;

	Pins.ENN      = &HAL.IOs->pins->DIO0;
	Pins.SPREAD   = &HAL.IOs->pins->DIO8;
	Pins.STEP     = &HAL.IOs->pins->DIO6;
	Pins.DIR      = &HAL.IOs->pins->DIO7;
	Pins.MS1_AD0  = &HAL.IOs->pins->DIO3;
	Pins.MS2_AD1  = &HAL.IOs->pins->DIO4;
	Pins.DIAG     = &HAL.IOs->pins->DIO1;
	Pins.INDEX    = &HAL.IOs->pins->DIO2;
	Pins.UC_PWM   = &HAL.IOs->pins->DIO9;
	Pins.STDBY    = &HAL.IOs->pins->DIO0;

	HAL.IOs->config->toOutput(Pins.ENN);
	HAL.IOs->config->toOutput(Pins.SPREAD);
	HAL.IOs->config->toOutput(Pins.STEP);
	HAL.IOs->config->toOutput(Pins.DIR);
	HAL.IOs->config->toOutput(Pins.MS1_AD0);
	HAL.IOs->config->toOutput(Pins.MS2_AD1);
	HAL.IOs->config->toInput(Pins.DIAG);
	HAL.IOs->config->toInput(Pins.INDEX);

	HAL.IOs->config->setLow(Pins.MS1_AD0);
	HAL.IOs->config->setLow(Pins.MS2_AD1);

	TMC2209_UARTChannel = HAL.UART;
	TMC2209_UARTChannel->pinout = UART_PINS_2;
	TMC2209_UARTChannel->rxtx.init();

	TMC2209_config = Evalboards.ch2.config;

	Evalboards.ch2.config->reset        = reset;
	Evalboards.ch2.config->restore      = restore;

	Evalboards.ch2.rotate               = rotate;
	Evalboards.ch2.right                = right;
	Evalboards.ch2.left                 = left;
	Evalboards.ch2.stop                 = stop;
	Evalboards.ch2.GAP                  = GAP;
	Evalboards.ch2.SAP                  = SAP;
	Evalboards.ch2.moveTo               = moveTo;
	Evalboards.ch2.moveBy               = moveBy;
	Evalboards.ch2.writeRegister        = tmc2209_writeRegister;
	Evalboards.ch2.readRegister         = tmc2209_readRegister;
	Evalboards.ch2.userFunction         = userFunction;
	Evalboards.ch2.enableDriver         = enableDriver;
	Evalboards.ch2.checkErrors          = checkErrors;
	Evalboards.ch2.numberOfMotors       = MOTORS;
	Evalboards.ch2.VMMin                = VM_MIN;
	Evalboards.ch2.VMMax                = VM_MAX;
	Evalboards.ch2.deInit               = deInit;
	Evalboards.ch2.periodicJob          = periodicJob;

	tmc2209_init(&TMC2209, 0, 0, TMC2209_config, &tmc2209_defaultRegisterResetState[0]);

	StepDir_init(STEPDIR_PRECISION);
	StepDir_setPins(0, Pins.STEP, Pins.DIR, Pins.DIAG);
	StepDir_setVelocityMax(0, 51200);
	StepDir_setAcceleration(0, 51200);

	HAL.IOs->config->toOutput(Pins.UC_PWM);

#if defined(Landungsbruecke) || defined(LandungsbrueckeSmall)
	Pins.UC_PWM->configuration.GPIO_Mode = GPIO_Mode_AF4;
#elif defined(LandungsbrueckeV3)
	Pins.UC_PWM->configuration.GPIO_Mode  = GPIO_MODE_AF;
	gpio_af_set(Pins.UC_PWM->port, GPIO_AF_1, Pins.UC_PWM->bitWeight);

#endif

	vref = 2000;
	HAL.IOs->config->set(Pins.UC_PWM);
	Timer.init();
	Timer.setDuty(timerChannel, ((float)vref) / VREF_FULLSCALE);

	enableDriver(DRIVER_ENABLE);
};