/**
 *******************************************************************************
 * @file    si5351.c
 * @brief   STM32 library/driver for the Si5351 clock chip
 *          from Skyworks Solutions, Inc. (former SiLabs)
 *******************************************************************************
 * @author: Thomas Kuschel KW4NZ
 * created 2022-05-11
 *
 * adapted, idea and some information from Petr Polasek
 * thanks also to Aleksander Alekseev alias afiskon for his stm32 drivers
 *
 * A simple description can be found in the header file stm32_si5351.h, for
 * more details read the supported README.md file
 ******************************************************************************/

/* Defines for compilation ---------------------------------------------------*/
/* if you want to automatically enable the clk output after setting synthesis */
/* you can set the AUTOMATICALLY_ENALBE_OUTPUT to 1 (not recommended)         */
#define AUTOMATICALLY_ENABLE_OUTPUT 0
/* enable some optimizing, usually set to 1 */
#define OPTIMIZED 1

/* Includes -----------------------------------------------------------------*/
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#ifdef DEBUG
#define SI5351_DEBUG 0
#include <stdio.h>
#endif

#include <assert.h>
/* including the HAL here */
#include "stm32l4xx_hal.h"
/* register map of the Si5351 */
#include "si5351_reg.h"
#include "si5351.h"

/* Private typedef -----------------------------------------------------------*/

/* @brief  Si5351 handle structure definition, private
 */
typedef struct __SI5351_HandleTypeDef {
	void *i2c_handle;			/*!< the I2C handle, must not be unique */
	struct __SI5351_HandleTypeDef *next; /*!< used for several instances */
	uint32_t xtal_frequency;	/*!< XTAL or CLKIN frequency */
#if SI5351_DEBUG
	char debug_msg[1000];		/*!< for debugging msgs, extensive tests, N/A */
#endif
	uint8_t clk_is_pllb;		/*!< assignment of PLLA or PLLB per CLK #, if bit set to 1 ...PLLB */
	uint8_t clk_is_disabled;	/*!< assignment of Output Enable Control, app. Register 3 */
	uint8_t clk_has_phase_shift;/*!< assignment of an output with a phase shift offset */
	uint8_t i2c_address;		/*!< I2C address of the datasheet */
	uint8_t interrupt_status_mask; /*!< Reg 2: Interrupt Status Mask */
	uint8_t initialized :1;		/*!< mark the driver initialized */
	uint8_t programmed :1;		/*!< mark the chip is programmed */
} si5351_HandleTypeDef;

/* @brief  SI5351 synthesis settings */
typedef struct {
	uint32_t pll_multiplier;	/*!< corresponds to feedback multisynth (N) a */
	uint32_t pll_numerator;		/*!< corresponds to feedback multisynth (N) b */
	uint32_t pll_denominator;	/*!< corresponds to feedback multisynth (N) c */
	uint32_t out_multiplier;	/*!< corresponds to multisynth (M) a */
	uint32_t out_numerator;		/*!< corresponds to multisynth (M) b */
	uint32_t out_denominator;	/*!< corresponds to multisynth (M) c */
	uint8_t out_r_divider;	/*!< R divider, log2 value bit set to 1; 2,4,8,...,128; for frequencies < 500 kHz, otherwise set to 1 i.e. bit 0 */
} synthesis_t;

typedef struct {
	char band[4];
	uint32_t qrg_min;
	uint32_t qrg_max;
	uint32_t multiplier;
	uint8_t divider_bit;
} band_t;

/* Private define ------------------------------------------------------------*/
#define SI5351_FREQUENCY_MIN 8000u
#define SI5351_FREQUENCY_MAX 160000000u

#define _80M_BAND_MIN   3000000u
#define _80M_BAND_MAX   4500000u
#define _80M_BAND_OUT_MULT  200u
#define _80M_BAND_R_DIV       0u	/* 2^0 value */

/* Private macro -------------------------------------------------------------*/
#ifdef __arm__
#ifdef __ARM_BIG_ENDIAN
#define for_endian(size) for (int i = 0; i < size; ++i)
#define last_loop_endian (i==size-1)
#else
#define for_endian(size) for (int i = size - 1; i >= 0; --i)
#define last_loop_endian (i==0)
#endif
#else
#error "Endianness not detected or another compiler"
#endif

#ifndef __GNUC__ /*__arm__*/  /* Keil ARM compiler does not support typeof */
#define snprintb(buf, n, value) __snprintb(buf, n, (uint32_t *)&value, sizeof(uint32_t))
#define snprintb16(buf, n, value) __snprintb(buf, n, (uint16_t *)&value, sizeof(uint16_t))
#define snprintb8(buf, n, value) __snprintb(buf, n, (uint8_t *)&value, sizeof(uint8_t))
#else	/* for gcc, clang compilers */
#define snprintb(buf, n, value) \
({ \
	typeof(value) _v = value; \
	__snprintb(buf, n, (typeof(_v) *) &_v, sizeof(_v)); \
})
#endif

/* Private variables ---------------------------------------------------------*/
si5351_HandleTypeDef *first_handle = NULL;	/* pointer to the first instance  */
int si5351_errno = 0;	/* error_number for functions with return == NULL	  */

/* Private function prototypes -----------------------------------------------*/
#ifdef DEBUG
int __fprintb(FILE *stream, void *value, size_t size);
#endif
static int si5351_set_synthesis(si5351_inst_t inst, synthesis_t *synth, uint8_t clk);
static int si5351_program(si5351_inst_t inst);
static int si5351_error_status_i2c(HAL_StatusTypeDef status);
static int si5351_read(si5351_inst_t inst, uint8_t regaddr, uint8_t *data, uint16_t size);
static int si5351_write(si5351_inst_t inst, uint8_t regaddr, uint8_t *data, uint16_t size);
static int band_select(uint32_t frequency, band_t *band);
static int calculation(uint32_t frequency, uint32_t xtal, synthesis_t *synth);

/* Private functions ---------------------------------------------------------*/

/** Wrapper functions for receiving/transceiving bytes from I2C bus
 *  (HAL function set)
 */

/**	@brief Give better error numbers based on the Linux error_no.h
 * @param status based on HAL library in HAL_StatusTypeDef
 *	@return error number see si5351_errno
 */
static int si5351_error_status_i2c(HAL_StatusTypeDef status) {

	switch (status) {
	case HAL_TIMEOUT:
		return -ETIMEDOUT;
		break;
	case HAL_ERROR:
		return -EIO;
		break;
	case HAL_BUSY:
		return -EBUSY;
		break;
	case HAL_OK:
	default:
		return 0;
	}
}

/**	@brief Read (blocking mode) one ore more data bytes from the I2C bus
 * 		starting at the regaddr register address
 * 	@param inst			instance (handle) of the SI5351 driver
 * 	@param regaddr		starting register address of the SI5351
 *  @param data			pointer to the first byte of data
 *	@param datasize		datasize of the reading data, sizeof (data),
 *		when reading only one register (byte), set to 1
 *	@return errno		0 on success, see si5351_errno_t
 *	@retval -EINVAL		when given a NULL handle
 *	@retval -ETIMEDOUT	when HAL_TIMEOUT
 *	@retval -EIO		when HAL_ERROR
 *	@retval -EBUSY		when HAL_BUSY
 * 	if applicable use interrupt controlled HAL functions
 */
static int si5351_read(si5351_inst_t inst, uint8_t regaddr, uint8_t *data, uint16_t size) {

	HAL_StatusTypeDef status;
	status = HAL_I2C_Mem_Read(inst->i2c_handle, inst->i2c_address,
			(uint16_t) regaddr, I2C_MEMADD_SIZE_8BIT, data, size, 0xffff);
	return si5351_error_status_i2c(status);
}

/**	@brief Write (blocking mode) one ore more data bytes to the I2C bus
 * 		starting at the regaddr register address
 * 	@param inst			instance (handle) of the SI5351 driver
 * 	@param regaddr		starting register address of the SI5351
 *  @param data			pointer to the first byte of data
 *	@param datasize		datasize of the writing data, sizeof (data),
 *		when writing to one register (byte), set to 1
 *	@return errno		0 on success, see si5351_errno_t
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 * 	if applicable use interrupt controlled HAL functions
 */
static int si5351_write(si5351_inst_t inst, uint8_t regaddr, uint8_t *data, uint16_t size) {

	HAL_StatusTypeDef status;
	status = HAL_I2C_Mem_Write(inst->i2c_handle, inst->i2c_address,
			(uint16_t) regaddr, I2C_MEMADD_SIZE_8BIT, data, size, 0xffff);
	return si5351_error_status_i2c(status);
}

/**	@brief Check if there is any I2C device ready on the bus
 * 	@param si5351_instance	Given si5351 device handle
 *	@return	1 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_i2c_ready(si5351_inst_t inst) {

	HAL_StatusTypeDef status;

	if (!inst && !(inst = first_handle))
		return -EINVAL;

	/* call HAL function for device ready check */
	status = HAL_I2C_IsDeviceReady(inst->i2c_handle, inst->i2c_address, 3,
			100 /*HAL_MAX_DELAY*/); // HAL_MAX_DELAY is blocking, use 100 ms
	/* maybe create a pointer to that function for more flexiblity using other tools as HAL */
	return (status == HAL_OK) ? 1 : si5351_error_status_i2c(status);
}

/* End of wrapper functions receiving/transceiving bytes */

/** @brief Wrapper of init with default values: xtal set to 25 MHz, i2c_address set to 0x60
 *	@param i2c_handle	the handle of the I2C bus from HAL function, e.g. hi2c1
 *	@return si5351_handle	Pointer to the si5351 handle, NULL if error, see si5351_errno
 */
si5351_HandleTypeDef* si5351_initialize(void *i2c_handle) {

	return si5351_init(i2c_handle, SI5351_I2C_ADDR_DEFAULT, SI5351_XTAL_DEFAULT);
}

/**	@brief Initialize the device Si5351 with the main parameters
 * 	@param i2c_handle	the handle of the I2C bus from HAL function, e.g. hi2c1
 *	@param i2c_address	I2C bus address of the device from datasheet typically 0x60 (or 0x61)
 * 	@param xtal_frequency	either the XTAL frequency (25/27 MHz) or CLock-In
 * 							from 10 MHz to 100 MHz entered in Hz
 *	@param datasize		reserve an extra area of data space in bytes, access with function si5351_read_data() and si5351_write_data()
 *	@return si5351_handle	Pointer to the si5351 handle, NULL if error, see si5351_errno
 */
si5351_HandleTypeDef* si5351_init(void *i2c_handle, uint8_t i2c_address,
		uint32_t xtal_frequency) {

	si5351_HandleTypeDef *si5351_handle, *handle;
	/* xtal frequency from 25000000 upto 27000000, clkin frequ range from 10000000 to 100000000 Hz */
	/* i2c_address range up to 0x7F */
	if (!i2c_handle || xtal_frequency < 10000000 || xtal_frequency > 100000000
			|| i2c_address > 0x7f) {
		si5351_errno = EINVAL;
		return NULL;
	}
	si5351_handle = calloc(1, sizeof(*si5351_handle));
	if (si5351_handle == NULL) {
		si5351_errno = ENOMEM;	// cannot allocate any memory
		return NULL;
	}
	/* shift the given address to HAL conformity (shift one bit left) */
	i2c_address <<= 1;

	/* check if there is already a handle with same i2c_handle and i2c_address */
	if (first_handle == NULL) {
		first_handle = si5351_handle;
	} else {
		handle = first_handle;
		while (handle) {
			if ((handle->i2c_address == i2c_address)
					&& (handle->i2c_handle == i2c_handle)) {
				/* the device is already set up, we have to free the memory, exit with NULL */
				free(si5351_handle);
				si5351_errno = EADDRINUSE; /* handle address already in use */
				return NULL;
			}
			if (handle->next == NULL) {
				handle->next = si5351_handle;
				break;
			}
			handle = handle->next;
		}
	}
	si5351_handle->i2c_handle = i2c_handle;
	si5351_handle->i2c_address = i2c_address;
	si5351_handle->xtal_frequency = xtal_frequency;
	/* disable all interrupts (Si5351C only) */
	si5351_handle->interrupt_status_mask = SI5351_SYS_INIT_MASK
			| SI5351_LOL_B_MASK | SI5351_LOL_A_MASK | SI5351_LOS_CLKIN_MASK
			| SI5351_LOS_XTAL_MASK;

	si5351_handle->initialized = 1;
	return si5351_handle;
}

/**	@brief Deinitialize one device Si5351
 * 	@param si5351_handle	Given si5351 handle for freeing
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ENODEV if device handle not found
 */
int si5351_deinit(si5351_HandleTypeDef *si5351_handle) {

	si5351_HandleTypeDef *handle;

	if (si5351_handle == NULL)
		return -EINVAL;

	si5351_handle->initialized = 0;
	//before freeing switch the pointer to the next item
	if (si5351_handle == first_handle) {
		first_handle = si5351_handle->next;
	} else {
		handle = first_handle;
		while (handle) {
			if (handle->next == si5351_handle) {
				handle->next = si5351_handle->next;
				break;
			}
			handle = handle->next;
		}
		if (NULL == handle)
			return -ENODEV;	// no such device
	}
	free(si5351_handle);
	return 0;
}

/** @brief Deinitialize the first Si5351 device (wrapper function), for simplification
 *  @return 0 on success
 *  @retval -ENODEV if device handle not found
 */
int si5351_deinitialize(void) {

	if (!first_handle)
		return -ENODEV;
	return si5351_deinit(first_handle);
}

/**	@brief Program the si5351 with the already set values according to Figure 10 of the datasheet
 * 	@param si5351_instance	Given si5351 device handle
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
static int si5351_program(si5351_inst_t inst) {

	uint8_t data;
	int status = 0;
#if SI5351_DEBUG
	int cx = 0;
#endif
	if (!inst && !(inst = first_handle))
		return -EINVAL;

	do {
		status = si5351_read(inst, SI5351_DEVICE_STATUS, &data, 1);
		if (status)
			return status;

	} while (data & SI5351_SYS_INIT);

	do {
		/* Disable Outputs Set CLKx_DIS high, Reg. 3 = 0xFF */
		data = SI5351_CLK7_OEB | SI5351_CLK6_OEB | SI5351_CLK5_OEB
				| SI5351_CLK4_OEB | SI5351_CLK3_OEB | SI5351_CLK2_OEB
				| SI5351_CLK1_OEB | SI5351_CLK0_OEB;
		status = si5351_write(inst, SI5351_OUTPUT_ENABLE_CONTROL, &data, 1);
		if (status)
			break;
		inst->clk_is_disabled = data;

		/* power down all output drivers reg 16 -- 23 */
		data = SI5351_CLK0_PDN;	// 0x80
#if OPTIMIZED
		status = si5351_write(inst, SI5351_CLK0_CONTROL, &data,
				SI5351_NUMBER_OF_OUTPUTS);
		if (status)
			break;
#else
		for(int i = SI5351_CLK0_CONTROL; i <= SI5351_CLK7_CONTROL; i++) {
			status = si5351_write(inst, i, &data, 1);

			if (status)
			break;
		}
#endif
		/* set interrupt masks (see register 2 description) */
		status = si5351_write(inst, SI5351_INTERRUPT_STATUS_MASK,
				&inst->interrupt_status_mask, 1);
		if (status)
			break;
		/* for debugging purpose, read out fanout enable register */
#if SI5351_DEBUG
		status = si5351_read(inst, SI5351_FANOUT_ENABLE, &data, 1);
		if (status)
			break;
		cx += snprintf(inst->debug_msg + cx,
				sizeof(inst->debug_msg) - (size_t)cx,
				"(%d) FANOUT_ENABLE 0x%x\n", cx, data);
		cx += snprintf(inst->debug_msg + cx,
				sizeof(inst->debug_msg) - (size_t)cx,
				"(%d) FANOUT_ENABLE %d\n", cx, data);
#endif
		data = SI5351_CLKIN_FANOUT_EN | SI5351_XO_FANOUT_EN
				| SI5351_MS_FANOUT_EN; // set them to 1b
		status = si5351_write(inst, SI5351_FANOUT_ENABLE, &data, 1);
		if (status)
			break;
#if SI5351_DEBUG
		status = si5351_read(inst, SI5351_FANOUT_ENABLE, &data, 1);
		if (status)
			break;
		cx += snprintf(inst->debug_msg + cx,
				sizeof(inst->debug_msg) - (size_t)cx,
				"(%d) FANOUT_ENABLE 0x%x\n", cx, data);
		cx += snprintf(inst->debug_msg + cx,
				sizeof(inst->debug_msg) - (size_t)cx,
				"(%d) FANOUT_ENABLE %d\n", cx, data);
#endif
		/* Crystal Internal Load Capacitance */

#if SI5351_DEBUG
		data = SI5351_XTAL_CL_10_PF | SI5351_XTAL_RESERVED;
		status = si5351_write(inst, SI5351_CRYSTAL_INTERNAL_LOAD_CAPACITANCE, &data, 1);
		if (status)
			break;
		cx += snprintf(inst->debug_msg + cx,
				sizeof(inst->debug_msg) - (size_t)cx,
				"(%d) XTAL int. Load Cap: 0x%x (%dd)\n", cx, data, data);
#endif
		inst->programmed = 1; /* the device is programmed */
	} while (0);

	return status;
}

/**	@brief Select the right band to calculate even divisors for the output
 * 	@param	frequency
 * 	@param	band pointer of band_t return parameters as a structure min,
 * 		max, divisor for each band
 *	@return	1 on success, when a frequency band is found
 *	@retval	0 when frequency not found
 */
static int band_select(uint32_t frequency, band_t *band) {

	static const band_t sband[] = { /* band in meters, frequ_lo, frequ_hi, multiplier, divider_bit */
		{ "80",  3000000,  4500000, 200, 0 },
		{ "40",  5625000,  7500000, 120, 0 },
		{ "30",  7500000, 11250000,  80, 0 },
		{ "20", 11250000, 15000000,  60, 0 },
		{ "15", 15000000, 22500000,  40, 0 },
		{ "10", 22500000, 32142000,  28, 0 },
		{  "8", 32142000, 45000000,  20, 0 },
		{  "6", 45000000, 64285000,  14, 0 },
		{  "4", 64285000, 76000000,  10, 0 },
		{  "3", 76000000, 11250000,   8, 0 },
		{  "2", 11250000, 15000000,   6, 0 },
		{"180",  1500000,  2250000, 400, 0 },
		{"120",  2250000,  3000000, 300, 0 },
		{ "60",  4500000,  5625000, 160, 0 },
	};
	/* possibly this could be improved with a bsearch algorithm: */
	for (uint32_t i = 0; i < (sizeof(sband) / sizeof(sband[0])); i++) {
		if (frequency > sband[i].qrg_min && frequency <= sband[i].qrg_max) {
			memcpy(band, &sband[i], sizeof(*band));
			return 1;
		}
	}
	return 0;
}

/**	@brief Calculate all parameters for the synthesis of the si5351
 * 	@param frequency selected
 * 	@param xtal	the crystal driving the Si5351
 * 	@param synth pointer to a structure snythesis_t will be set with the calculated values
 *	@return	0 on success
 */
static int calculation(uint32_t frequency, uint32_t xtal, synthesis_t *synth) {

	uint32_t t;
	band_t band;

	if (frequency < SI5351_FREQUENCY_MIN || frequency > SI5351_FREQUENCY_MAX)
		return -EINVAL;

	/* e.g. 80-m-Band 3.5 -- 4.0 MHz */
	if (band_select(frequency, &band)) {
		synth->out_r_divider = band.divider_bit;
		synth->out_multiplier = band.multiplier;
		synth->out_numerator = 0;
		synth->out_denominator = 1;
		synth->pll_multiplier = (band.multiplier * frequency) / xtal;
		// t = (xtal >> 20) + 1;
		t = xtal / 1000000; // even divider and 1 Hz grid
		synth->pll_numerator = (band.multiplier * frequency) % xtal;
		synth->pll_numerator /= t;
		synth->pll_denominator = xtal / t;
		return 0;
	}
	synth->out_r_divider = 0;
	if (frequency < 81000000) {
		// Valid for frequ in 0.5..112.5 MHz range 9000
		// However an error is > 6 Hz above 81 MHz
		// synth->pll_multiplier = 36; // PLL runs @ 900 MHz with XTAL 25 MHz, more flexible using the formular:
		// making an even integer multiplier with
		synth->pll_multiplier = (900000000 / xtal);
		synth->pll_multiplier &= ~0x01u; // make it even
		synth->pll_numerator = 0;
		synth->pll_denominator = 1;
		uint32_t Fpll = synth->pll_multiplier * xtal; // this was set to 900000000
		synth->out_multiplier = Fpll / frequency;
		t = (frequency >> 20) + 1;
		synth->out_numerator = (Fpll % frequency) / t;
		synth->out_denominator = frequency / t;
	} else {
		// Valid for Fclk in 75..160 MHz range
		if (frequency >= 150000000) {
			synth->out_multiplier = 4;
		} else if (frequency >= 100000000) {
			synth->out_multiplier = 6;
		} else {
			synth->out_multiplier = 8;
		}
		synth->out_numerator = 0;
		synth->out_denominator = 1;

		uint32_t numerator = synth->out_multiplier * frequency;
		synth->pll_multiplier = numerator / xtal;
		t = (xtal >> 20) + 1;
		synth->pll_numerator = (numerator % xtal) / t;
		synth->pll_denominator = xtal / t;
	}
	return 0;
}

/**	@brief Enables the CLK output of the si5351 (after programming and setting the synthesis)
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param clk  The CLK ouput to drive and enable 0...CLK0, 1...CLK1, 2...CLK2, ...
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_enable_output(si5351_inst_t inst, uint8_t clk) {

	int rv = 0;

	if ((!inst && !(inst = first_handle)) || clk > 7)
		return -EINVAL;

	// check if this clock is already enabled
	if (inst->clk_is_disabled & (1 << clk)) {
		inst->clk_is_disabled &= (uint8_t) ~(1 << clk);	// clear the bit
		rv = si5351_write(inst, SI5351_OUTPUT_ENABLE_CONTROL,
				&inst->clk_is_disabled, 1);
	}
	return rv;
}

/**	@brief Disables the CLK output of the si5351 (after programming and setting the synthesis)
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_disable_output(si5351_inst_t inst, uint8_t clk) {

	int rv = 0;

	if ((!inst && !(inst = first_handle)) || clk > 7)
		return -EINVAL;

	if (!(inst->clk_is_disabled & (1 << clk))) {
		inst->clk_is_disabled |= (uint8_t) (1 << clk);	// set the bit
		rv = si5351_write(inst, SI5351_OUTPUT_ENABLE_CONTROL,
				&inst->clk_is_disabled, 1);
	}
	return rv;
}

/**	@brief Sets the CLK_0 output of the si5351 (simplified API)
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param frequency
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_set_clk0(si5351_inst_t inst, uint32_t frequency) {

	int rv = 0;
	synthesis_t synth;

	if (!inst && !(inst = first_handle))
		return -EINVAL;

	do {
		if (!inst->programmed) {
			rv = si5351_program(inst);
			if (rv)
				break;
		}
		(void) calculation(frequency, inst->xtal_frequency, &synth);
		rv = si5351_set_synthesis(inst, &synth, 0);
	} while (0);
	return rv;
}

/**	@brief Sets the CLK_x output of the si5351
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 * 	@param frequency
 * 	@param pll  the used PLL, either PLLA or PLLB
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_set_clk(si5351_inst_t inst, uint8_t clk, uint32_t frequency,
		si5351_pll_t pll) {

	int rv = 0;
	synthesis_t synth;

	if ((!inst && !(inst = first_handle)) || clk > 7)
		return -EINVAL;

	if (!pll)
		inst->clk_is_pllb &= (uint8_t) ~(1u << clk); /* delete the bit */
	else
		inst->clk_is_pllb |= (uint8_t) (1u << clk); /* set the bit */

	do {
		if (!inst->programmed) {
			rv = si5351_program(inst);
			if (rv)
				break;
		}
		(void) calculation(frequency, inst->xtal_frequency, &synth);
		rv = si5351_set_synthesis(inst, &synth, clk);
	} while (0);
	return rv;
}

/**	@brief Sets the CLK_x output of the si5351 with phase
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param frequency
 * 	@param phase in degree as double value
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 * 	@param pll  the used PLL, either PLLA or PLLB
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_set_clk_phase(si5351_inst_t inst, uint8_t clk, uint32_t frequency,
		double phase, si5351_pll_t pll) {

	int rv = 0;
	synthesis_t synth;

	if ((!inst && !(inst = first_handle)) || clk > 7)
		return -EINVAL;

	if (!pll)
		inst->clk_is_pllb &= (uint8_t) ~(1u << clk); /* delete the bit */
	else
		inst->clk_is_pllb |= (uint8_t) (1u << clk); /* set the bit */

	do {

		if (!inst->programmed) {
			rv = si5351_program(inst);
			if (rv)
				break;
		}
		/* calculate the phase */
		if (phase > 0.0) {
			inst->clk_has_phase_shift |= (uint8_t) (1u << clk); /* set phase shift */
			double pll_frequency, phaseoff;
			//pll_frequency = ((double)synth.pll_multiplier + (double)(synth.pll_numerator / synth.pll_denominator)) * (double)inst->xtal_frequency;
			//phaseoff = 4.0 * pll_frequency;
			(void) phaseoff;
			(void) pll_frequency;

		} else {
			inst->clk_has_phase_shift &= (uint8_t) ~(1u << clk); /* reset phase shift */
		}
		uint8_t ph = (uint8_t) phase;
		rv = si5351_write(inst, SI5351_CLK0_INITIAL_PHASE_OFFSET + clk, &ph, 1);

		(void) calculation(frequency, inst->xtal_frequency, &synth);
		rv = si5351_set_synthesis(inst, &synth, clk);
	} while (0);
	return rv;

}

/**	@brief Resets the PLL of the si5351 (direct access to register)
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
static int si5351_reset_pll(si5351_inst_t inst, uint8_t clk) {

	/* internal function, no need to check inst nor clk */

	uint8_t reset;

#if 1
	reset = (inst->clk_is_pllb & (1 << clk)) ?
			SI5351_PLLB_RST : SI5351_PLLA_RST;
#else
	(void)clk;
	reset = SI5351_PLL_RESET_VALUE;
#endif
	return si5351_write(inst, SI5351_PLL_RESET, &reset, 1);
}

/**	@brief Sets the CLK_x output phase of the si5351 (direct access to register)
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 * 	@param phase in uint8_t type value
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
int si5351_set_phase(si5351_inst_t inst, uint8_t clk, uint8_t phase) {

	int rv = 0;

	if ((!inst && !(inst = first_handle)) || clk > 5 || phase > CLKx_PHOFF)
		return -EINVAL;

	/*	if (phase) {
	 si5351_write(inst, )
	 }
	 */
	rv = si5351_write(inst, SI5351_CLK0_INITIAL_PHASE_OFFSET + clk, &phase, 1);

	si5351_reset_pll(inst, clk);

	return rv;
}

/**	@brief Sets the MSNx and MSx parameter registers of the Si5351
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param synth  synthesis_t struct
 * 	@param clk  The CLK ouput to drive and disable 0...CLK0, 1...CLK1, 2...CLK2, ...
 *	@return	0 on success
 *	@retval	-EINVAL	when given a NULL handle
 *	@retval -ETIMEDOUT when HAL_TIMEOUT
 *	@retval -EIO when HAL_ERROR
 *	@retval -EBUSY  when HAL_BUSY
 */
static int si5351_set_synthesis(si5351_inst_t inst, synthesis_t *synth, uint8_t clk) {

	uint32_t MSNx_P1, MSNx_P2, MSNx_P3;
	uint32_t MSx_P1, MSx_P2, MSx_P3;
	int rv = 0;
	uint8_t ms_data[8];
	uint8_t regaddr;
	uint8_t divby4 = 0;
	uint8_t MSx_INT = 0;
	uint8_t MSx_SRC = 0;

	if (!inst && !(inst = first_handle))
		return -EINVAL;

	assert(clk < 8);
	assert(synth->out_r_divider < 8); /* the divider is stored in 3 bits and the value is 2^out_r_divider */
	assert(
			synth->pll_multiplier >= 4u && synth->pll_multiplier <= 2048u
					&& synth->pll_multiplier != 5u
					&& synth->pll_multiplier != 7u);
	assert(synth->pll_denominator != 0u && synth->pll_denominator < (1u << 20)); // MSNx_P3 maximum of 1048575
	assert(
			synth->out_multiplier >= 4u && synth->out_multiplier <= 2048u
					&& synth->out_multiplier != 5u
					&& synth->out_multiplier != 7u);
	assert(synth->out_denominator != 0u && synth->out_denominator < (1u << 20));

	/* PLL registers */
	MSNx_P1 = 128 * synth->pll_multiplier
			+ 128 * synth->pll_numerator / synth->pll_denominator - 512;
	// MSNx_P2 = 128 * synth->pll_numerator - synth->pll_denominator * (128 * synth->pll_numerator / synth->pll_denominator);
	MSNx_P2 = (128 * synth->pll_numerator) % synth->pll_denominator;
	MSNx_P3 = synth->pll_denominator;

	assert(MSNx_P1 < (1u << 18));
	assert(MSNx_P2 < (1u << 20));

	/* OUTPUT (M) registers */
	MSx_P1 = 128 * synth->out_multiplier
			+ 128 * synth->out_numerator / synth->out_denominator - 512;
	// MSx_P2 = 128 * synth->out_numerator - synth->out_denominator * (128 * synth->out_numerator / synth->out_denominator);
	MSx_P2 = (128 * synth->out_numerator) % synth->out_denominator;
	MSx_P3 = synth->out_denominator;
	if (synth->out_r_divider == 2) /* 2^2 = 4, see 4.1.3 in AN619 special case */
		divby4 = 0x03;

	assert(MSx_P1 < (1u << 18));
	assert(MSx_P2 < (1u << 20));

	/* distribute these registers to 8 bytes of the SI5351 */
	/* start with MSNx values */
	ms_data[0] = (uint8_t) (MSNx_P3 >> 8);
	ms_data[1] = (uint8_t) MSNx_P3;
	ms_data[2] = (uint8_t) ((MSNx_P1 >> 16) & 0x03);
	ms_data[3] = (uint8_t) (MSNx_P1 >> 8);
	ms_data[4] = (uint8_t) MSNx_P1;
	ms_data[5] = (uint8_t) ((((MSNx_P3 >> 16) & 0x0F) << 4)
			| ((MSNx_P2 >> 16) & 0x0F));
	ms_data[6] = (uint8_t) (MSNx_P2 >> 8);
	ms_data[7] = (uint8_t) MSNx_P2;
	/* write MSNx registers dependent of SI5351_PLLA or SI5351_PLLB */
	regaddr =
			(inst->clk_is_pllb & (1u << clk)) ?
					SI5351_MULTISYNTH_NB_PARAMETER_3_HI :
					SI5351_MULTISYNTH_NA_PARAMETER_3_HI;
	rv = si5351_write(inst, regaddr, ms_data,
			sizeof(ms_data) / sizeof(ms_data[0]));
	if (rv)
		return rv;
	/* write MSx registers dependent of CLK # */
	ms_data[0] = (uint8_t) (MSx_P3 >> 8);
	ms_data[1] = (uint8_t) MSx_P3;
	ms_data[2] = (uint8_t) (((MSx_P1 >> 16) & 0x03)
			| ((synth->out_r_divider & 0x07) << 4) | (divby4 << 2));
	ms_data[3] = (uint8_t) (MSx_P1 >> 8);
	ms_data[4] = (uint8_t) MSx_P1;
	ms_data[5] = (uint8_t) ((((MSx_P3 >> 16) & 0x0F) << 4)
			| ((MSx_P2 >> 16) & 0x0F));
	ms_data[6] = (uint8_t) (MSx_P2 >> 8);
	ms_data[7] = (uint8_t) MSx_P2;
	regaddr = SI5351_MULTISYNTH0_PARAMETER_3_HI
			+ (uint8_t) (clk * sizeof(ms_data) / sizeof(ms_data[0]));
	rv = si5351_write(inst, regaddr, ms_data,
			sizeof(ms_data) / sizeof(ms_data[0]));
	if (rv)
		return rv;

	MSx_INT = ((synth->out_numerator == 0)
			&& ((synth->out_multiplier & 0x01) == 0)
			&& !(inst->clk_has_phase_shift & (1 << clk)));
	MSx_INT = 0;

	MSx_SRC = !!(inst->clk_is_pllb & (1 << clk));

	ms_data[0] = (uint8_t) (MSx_INT << 6 | MSx_SRC << 5 | SI5351_CLK_SRC_MS0
			| SI5351_CLK_2_MA); //SI5351_CLK_6_MA; //SI5351_CLK_4_MA;
	regaddr = SI5351_CLK0_CONTROL + clk;
	rv = si5351_write(inst, regaddr, ms_data, 1);
	if (rv)
		return rv;

	rv = si5351_reset_pll(inst, clk);
	if (rv)
		return rv;

#if AUTOMATICALLY_ENABLE_OUTPUT
	rv = si5351_enable_output(inst, clk);
#endif

	return rv;
}

#if SI5351_DEBUG
/**	@brief With this function, you can read the debug message for test purposes
 * 	@param si5351_instance	Given si5351 device handle
 *	@return	 * char  as message for printing
 *	@retval	NULL when not found
 */
char * si5351_read_debug_msg(si5351_inst_t inst) {

	if (!inst && !(inst=first_handle))
		return NULL;

	return inst->debug_msg;
}
#endif

#ifdef DEBUG
/*!
 * @brief Output the value in binary representation and in groups of
 * bytes
 *
 * @param buf string of char * to output to
 * @param n	amount of characters to output to
 * @param value Value to output in binary representation
 * @param size
 * @return bytes written (int)
 */
int __snprintb(char *buf, size_t n, void *value, size_t size) {
	uint8_t byte;
	size_t blen = sizeof(byte) * 8;
	uint8_t bits[blen + 1];
	int cx = 0;

	bits[blen] = '\0';
	for_endian((int)size)
	{
		byte = ((uint8_t*) value)[i];
		memset(bits, '0', blen);
		for (int j = 0; byte && j < (int) blen; ++j) {
			if (byte & 0x80)
				bits[j] = '1';
			byte <<= 1;
		}
		cx += snprintf(buf + cx, n - (size_t) cx, "%s%s", bits,
				(last_loop_endian) ? "" : " ");
	}
	return cx;
}

/**	@brief Function to read any register with binary and hex representation
 * 	@param si5351_instance	Given si5351 device handle
 * 	@param buffer  for printing typically 33 bytes char
 * 	@param size of the buffer for printing typically 33 bytes
 * 	@param register address
 *	@return	 * char  as message for printing
 *	@retval	NULL when not found
 */
char* si5351_read_register_debug(si5351_inst_t inst, char *buf, size_t bufsize,
		uint8_t regaddr) {

	uint8_t data;
	int status;
	int cx;

	if (!inst)
		return NULL;

	status = si5351_read(inst, regaddr, &data, 1);
	if (status)
		return NULL;

	cx = snprintf(buf, bufsize, "R%03u[0x%02x]=%3u[0x%02x][0b", regaddr,
			regaddr, data, data);
	cx += snprintb(buf + cx, bufsize - (size_t )cx, data);
	cx += snprintf(buf + cx, bufsize - (size_t) cx, "]");
	return buf;
}
#endif //DEBUG

/* some additional functions */

/**	@brief Function to get the handle of a si5351 device instance
 * 	@param si5351_instance	Given a pointer to a si5351 device handle or
 * 	       a pointer to NULL for getting the first handle.
 *	@return	 si5351_inst_t handle
 *	@retval	NULL when not found
 *  @warning The first given *inst must be set to NULL or set to a known handle
 */
int si5351_get_instance(si5351_inst_t *inst) {

	si5351_inst_t tinst;
	int rv = 0;

	if (!inst)
		return -EINVAL;

	if (!(*inst))
		*inst = first_handle;
	else
		*inst = (*inst)->next;

	tinst = *inst;
	while (tinst) {
		rv++;
		tinst = tinst->next;
	}
	return rv;
}

int si5351_get_i2c_address(si5351_inst_t inst) {
	if (!inst && !(inst = first_handle))
		return -EINVAL;

	return inst->i2c_address;
}

void* si5351_get_i2c_handle(si5351_inst_t inst) {
	if (!inst && !(inst = first_handle))
		return NULL;

	return (void*) inst->i2c_handle;
}

