dw1000.c

#include "dw1000.h"
#include "dw1000_hal.h"
#include "dw1000_isr.h"
#include "deca_regs.h"
#include "dw1000_util.h"
#include "stm32f4xx_hal.h"

#ifdef DEBUG
#include "Trace.h"
#endif

/**
 * See header file dw1000.h
 */
void dw1000_init(BaseType_t (*send_callback)(),
		BaseType_t (*receive_callback)(), TaskHandle_t task_handle) {

	// Set callbacks
	dw1000local.send_callback = send_callback;
	dw1000local.receive_callback = receive_callback;
	dw1000local.task_handle = task_handle;

	// Initialize HAL
	dw1000Hal_init();

	// Soft-Reset
	dw1000Util_reset();

	// Set useful interrupts
	// Frame sent  | Frame received | Time stamp valid
	uint32_t sys_mask = 0;
	sys_mask |= ( SYS_MASK_MTXFRS |/* SYS_MASK_MRXDFR | SYS_MASK_MLDEDONE*/ SYS_MASK_MRXFCG);
	dw1000Hal_writeRegister(SYS_MASK_ID, (uint8_t *) &sys_mask, SYS_MASK_LEN);

	// Set transceiver LEDs
	// Equivalent to MSGP0 = 01, MSGP1 = 01, MSGP2 = 01, MSGP3 = 01
	// Enables LED for rec and sending
	uint32_t msgp = 0x5540;
	dw1000Hal_writeSubRegister(GPIO_CTRL_ID, GPIO_MODE_OFFSET, (uint8_t*) &msgp,
	GPIO_MODE_LEN);
	uint32_t led = 0;
	dw1000Hal_readSubRegister(PMSC_ID, PMSC_CTRL0_OFFSET, (uint8_t*) &led,
	PMSC_CTRL0_LEN);
	led |= (1 << 18) | (1 << 23); // Preparing LEDs for blink mode
	dw1000Hal_writeSubRegister(PMSC_ID, PMSC_CTRL0_OFFSET, (uint8_t*) &led,
	PMSC_CTRL0_LEN);
	dw1000Hal_readSubRegister(PMSC_ID, PMSC_LEDC_OFFSET, (uint8_t*) &led,
	PMSC_LEDC_LEN);
	// Enable blink mode
	led |= PMSC_LEDC_BLNKEN;
	// Force LEDs to blink once
	led |= 0xF0000;
	dw1000Hal_writeSubRegister(PMSC_ID, PMSC_LEDC_OFFSET, (uint8_t*) &led,
	PMSC_LEDC_LEN);

	// Reset force blink bits. Needed to make the LEDs blinking
	led &= ~0xF0000;
	dw1000Hal_writeSubRegister(PMSC_ID, PMSC_LEDC_OFFSET, (uint8_t*) &led,
	PMSC_LEDC_LEN);

	// load leading edge detect code
	dw1000Util_enableclocks(FORCE_SYS_XTI);

	uint8_t wr_buf[2];

	//set up clocks
	wr_buf[1] = 0x03; //Force TX clock off.
	wr_buf[0] = 0x01; //Force system clock to be the 19.2 MHz XTI clock.
	dw1000Hal_writeSubRegister(PMSC_ID, PMSC_CTRL0_OFFSET, wr_buf, 2); //Just first 2 byte instead of whole crtl0
	//kick off the LDE load
	uint16_t otp_ctrl_ldeload = OTP_CTRL_LDELOAD;
	dw1000Hal_writeSubRegister(OTP_IF_ID, OTP_CTRL,
			(uint8_t *) &otp_ctrl_ldeload, 2);

	osDelay(1);

	wr_buf[1] = 0x02; //Force TX clock off.
	wr_buf[0] = 0x00; //Force system clock to be the 19.2 MHz XTI clock.
	dw1000Hal_writeSubRegister(PMSC_ID, PMSC_CTRL0_OFFSET, wr_buf, 2); //Just first 2 byte instead of whole crtl0

	//Very important! Not sending on FORCE_SYS_XTI
	dw1000Util_enableclocks(ENABLE_ALL_SEQ);

#ifdef DOUBLE_BUFFER
	//Enable double buffer

	uint32_t sys_cfg = 0;

	dw1000Hal_readRegister(SYS_CFG_ID, (uint8_t *) &sys_cfg, SYS_CFG_LEN);
	sys_cfg &= (~SYS_CFG_DIS_DRXB);
	dw1000Hal_writeRegister(SYS_CFG_ID, (uint8_t *) &sys_cfg,
	SYS_CFG_LEN);
#endif

	//Enable Receiver
	dw1000Util_enableRX();
	uint32_t event_clear = 0xFFFFFFFF;
	dw1000Hal_writeRegister(SYS_STATUS_ID, (uint8_t*) &event_clear,
	SYS_STATUS_LEN);

}

/**
 * See header file dw1000.h
 *
 * The following function is taken and adapted from the original driver
 */
void dw1000_configure(dw1000_config_t *config) {
	uint8_t chan = config->channel;
	uint32_t regval;
	uint16_t reg16 = lde_replicaCoeff[config->rx_preamble_code];
	uint8_t prfIndex = config->pulse_repetition_frequency - DWT_PRF_16M;
	uint8_t bw = ((chan == 4) || (chan == 7)) ? 1 : 0; //select wide or narrow band

	uint32_t sys_cfg = 0;

	dw1000Hal_readRegister(SYS_CFG_ID, (uint8_t *) &sys_cfg, SYS_CFG_LEN);

	// DECA: for 110 kbps we need to special setup
	if (DWT_BR_110K == config->data_rate) {
		sys_cfg |= SYS_CFG_RXM110K;

		reg16 >>= 3;  //div by 8 According to Page 170
	} else {
		sys_cfg &= (~SYS_CFG_RXM110K);
	}

	sys_cfg |= (SYS_CFG_RXAUTR); //Automatic reenable of the receiver

	dw1000Hal_writeRegister(SYS_CFG_ID, (uint8_t *) &sys_cfg, SYS_CFG_LEN);

	//DECA:write/set the lde_replicaCoeff
	dw1000Hal_writeSubRegister(LDE_IF_ID, LDE_REPC_OFFSET, (uint8_t *) &reg16,
			2); //Page 169

	//configure LDE algorithm parameters
	uint8_t x = LDE_PARAM1;
	/*8-bit configuration register*/
	dw1000Hal_writeSubRegister(LDE_IF_ID, LDE_CFG1_OFFSET, &x, 1);

	uint16_t lde_param3;
	if (prfIndex) {
		lde_param3 = (uint16_t) LDE_PARAM3_64;
	} else {
		lde_param3 = (uint16_t) LDE_PARAM3_16;
	}
	dw1000Hal_writeSubRegister(LDE_IF_ID, LDE_CFG2_OFFSET,
			(uint8_t *) &lde_param3, 2);

	//    config RF pll (for a given channel)
	//    configure PLL2/RF PLL block CFG
	//Configure FS_PLLTUNE too
	dw1000Hal_writeSubRegister(FS_CTRL_ID, FS_PLLCFG_OFFSET,
			(uint8_t *) &pll2_config[chan_idx[chan]][0], FS_PLLCFG_LEN);

	//DECA: configure PLL2/RF PLL block CAL
	//Crystal Oscillator Trim
	dw1000Hal_writeSubRegister(FS_CTRL_ID, FS_XTALT_OFFSET,
			(uint8_t *) &pll2calcfg, 1);

	//DECA: Configure RF RX blocks (for specified channel/bandwidth)
	dw1000Hal_writeSubRegister(RF_CONF_ID, RF_RXCTRLH_OFFSET,
			(uint8_t *) &rx_config[bw], 1);

	//DECA: Configure RF TX blocks (for specified channel and prf)
	//DECA: Config RF TX control
	dw1000Hal_writeSubRegister(RF_CONF_ID, RF_TXCTRL_OFFSET,
			(uint8_t *) &tx_config[chan_idx[chan]], RF_TXCTRL_LEN);

	//DECA: Configure the baseband parameters (for specified prf, bit rate, pac, and SFD settings)
	//DECA:DTUNE0
	dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE0b_OFFSET,
			(uint8_t *) &sftsh[config->data_rate][0/*use standard sfd*/],
			DRX_TUNE0b_LEN);

	//DTUNE1
	dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE1a_OFFSET,
			(uint8_t *) &dtune1[prfIndex], DRX_TUNE1a_LEN);

	if (config->data_rate == DWT_BR_110K) {
		uint16_t reg = 0x64;
		dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE1b_OFFSET,
				(uint8_t *) &reg, DRX_TUNE1b_LEN);
	} else {
		if (config->tx_preamble_code == DWT_PLEN_64) //if preamble length is 64
		{
			uint8_t temp = 0x10;
			dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE1b_OFFSET,
					(uint8_t *) &temp, DRX_TUNE1b_LEN);
			dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_DRX_TUNE4HOFFSET,
					(uint8_t *) &temp, 1);
		} else {
			uint8_t temp = 0x20;
			dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE1b_OFFSET,
					(uint8_t *) &temp, DRX_TUNE1b_LEN);
			temp = 0x28;
			dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_DRX_TUNE4HOFFSET,
					(uint8_t *) &temp, 1);

		}
	}

	//DTUNE2 magic values for prf 64
	if(config->pulse_repetition_frequency == DWT_PRF_64M){
		dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_TUNE2_OFFSET, &digital_bb_config[prfIndex][config->preamble_acquisition_chunk_size], 4);
	}

	//DTUNE3 set SFD detection timeout count
	if (config->sfd_timeout != DWT_SFDTOC_DEF) { //if default value no need to program it
		dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_SFDTOC_OFFSET,
				(uint8_t *) &config->sfd_timeout, DRX_SFDTOC_LEN);
	}
	//don't allow 0 - SFD timeout will always be enabled
	if (config->sfd_timeout == 0) {
		uint16_t tmp = DWT_SFDTOC_DEF;
		dw1000Hal_writeSubRegister(DRX_CONF_ID, DRX_SFDTOC_OFFSET,
				(uint8_t *) &tmp, DRX_SFDTOC_LEN);
	}

	//Set CHAN_CTRL, Channel Number etc
	regval = (CHAN_CTRL_TX_CHAN_MASK & (chan << CHAN_CTRL_TX_CHAN_SHIFT)) | // Transmit Channel
			(CHAN_CTRL_RX_CHAN_MASK & (chan << CHAN_CTRL_RX_CHAN_SHIFT)) | // Receive Channel
			(CHAN_CTRL_RXFPRF_MASK
					& (config->pulse_repetition_frequency
							<< CHAN_CTRL_RXFPRF_SHIFT)) | // RX PRF
			((CHAN_CTRL_TNSSFD | CHAN_CTRL_RNSSFD)
					& (0 << CHAN_CTRL_TNSSFD_SHIFT)) | // nsSFD enable RX&TX
			(CHAN_CTRL_DWSFD & (0 << CHAN_CTRL_DWSFD_SHIFT)) | // use DW nsSFD
			(CHAN_CTRL_TX_PCOD_MASK
					& (config->tx_preamble_code << CHAN_CTRL_TX_PCOD_SHIFT)) | // TX Preamble Code
			(CHAN_CTRL_RX_PCOD_MASK
					& (config->rx_preamble_code << CHAN_CTRL_RX_PCOD_SHIFT)); // RX Preamble Code

	dw1000Hal_writeRegister(CHAN_CTRL_ID, (uint8_t *) &regval, CHAN_CTRL_LEN);

	// Set up TX Preamble Size and TX PRF
	// Set up TX Ranging Bit and Data Rate
	uint32_t tx_fctrl = (config->preamble_length
			| config->pulse_repetition_frequency) << 16;
	tx_fctrl = tx_fctrl | TX_FCTRL_TR
			| (config->data_rate << TX_FCTRL_TXBR_SHFT);
	dw1000Hal_writeRegister(TX_FCTRL_ID, (uint8_t *) &tx_fctrl,
	TX_FCTRL_LEN - 1); // Just first 4 byte

}

/**
 * See header file dw1000.h
 */
int dw1000_sendFrame(uint8_t *frame, uint16_t frame_length) {
	//Write payload to the buffer with no offset
	int ret = dw1000_writeFrameIntoBuffer(0, frame, frame_length);
	if (ret != DW1000_RET_OK) {
		return ret;
	}
	//Invoke transmission
	return dw1000_transmitFrame(0, frame_length);

}

/*
int dw1000_sendDelayedFrame(uint8_t *payload, uint16_t len, node_time_t tx_time) {

	//Write payload to buffer
	int ret = dw1000_writeFrameIntoBuffer(0, payload, len);

	// Write desired tx time to register
	dw1000Hal_writeSubRegister(DX_TIME_ID, 0, (uint8_t *)&tx_time, DX_TIME_LEN);

	// Force receiver off
	dw1000Util_forceTrxOff();

	// Trigger delayed sending by setting TXSTART and TXDLYS
    uint8_t temp = (uint8_t) SYS_CTRL_TXSTRT | SYS_CTRL_TXDLYS;
    dw1000Hal_writeRegister(SYS_CTRL_ID, &temp, 1);

    // Re-enable receiver
    dw1000Util_enableRX();

	return ret;

}
*/

/**
 * See header file dw1000.h
 */
int dw1000_writeFrameIntoBuffer(uint16_t tx_buffer_offset, uint8_t *frame,
		uint16_t frame_length) {
	if ((frame_length + 2) > MAXIMUM_FRAME_LENGTH) {
		// frame + CRC is longer than the allowed frame length
		return DW1000_RET_MSG_TO_LONG;
	}
	if ((tx_buffer_offset + frame_length + 2) > 1024) {
		return DW1000_RET_TX_BUFFER_OVERFLOW;
	}

	//Write payload into buffer
	dw1000Hal_writeRegister(TX_BUFFER_ID + tx_buffer_offset, frame,
			frame_length);

	return DW1000_RET_OK;
}

/**
 * See header file dw1000.h
 */
int dw1000_transmitFrame(uint16_t tx_buffer_offset, uint16_t frame_length) {
	//Set length and offset
	uint32_t tx_fctrl = 0;
	dw1000Hal_readSubRegister(TX_FCTRL_ID, 0, (uint8_t *) &tx_fctrl,
	TX_FCTRL_LEN - 1);
	tx_fctrl = ((tx_fctrl | (frame_length + 2)) | (tx_buffer_offset << 22));
	dw1000Hal_writeSubRegister(TX_FCTRL_ID, 0, (uint8_t *) &tx_fctrl,
	TX_FCTRL_LEN - 1);
	/* WITHOUT THE NEXT PART: SECOND TIME NO SENDING POSSIBLE, BUT CAN'T BE FOUND IN ORIGINAL DRIVER */
	dw1000Util_forceTrxOff();

    uint8_t temp = (uint8_t) SYS_CTRL_TXSTRT;
	HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_SET);
    dw1000Hal_writeRegister(SYS_CTRL_ID, (uint8_t *)&temp,1);
	HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_RESET);

	return DW1000_RET_OK;
}

/**
 * See header file dw1000.h
 */
void dw1000_receiveFrameFromIsr(dw1000_frame_t *frame) {
	frame->length = dw1000local.frame_length;
	frame->timestamp_valid = dw1000local.timestamp_valid;
	if(frame->timestamp_valid){
		dw1000Util_getRxTimestamp((uint64_t *)&frame->rx_timestamp);
	}
	dw1000Hal_readRegisterFromIsr(RX_BUFFER_ID, frame->buffer, frame->length); //FromIsr important!
}

int dw1000_setPanAddr(uint16_t addr) {
	return dw1000Hal_writeSubRegister(PANADR_ID, PANADR_SHORT_ADDR_OFFSET,
			(uint8_t*) &addr, PANADR_SHORT_ADDR_LEN);
}
int dw1000_setPanId(uint16_t addr) {
	return dw1000Hal_writeSubRegister(PANADR_ID, PANADR_PAN_ID_OFFSET,
			(uint8_t*) &addr, PANADR_PAN_ID_LEN);
}


/**
 * See header file dw1000.h
 */
void dw1000_generateConfig(uint16_t mode, dw1000_config_t *config, uint8_t fast) {

	uint8_t prf64Mhz = mode & 1; //0, if 16Mhz is selected

	//Set channel
	config->channel = (mode >> CHANNEL_SHIFT) & 7;

	//Set rxCode/txCode, which can be different, but should be the same
	config->rx_preamble_code = (mode >> PREAMBLE_SHIFT) & 31;
	config->tx_preamble_code = (mode >> PREAMBLE_SHIFT) & 31;

	//Set PRF
	if (prf64Mhz) {
		//64MHz PRF
		config->pulse_repetition_frequency = DWT_PRF_64M;
	} else {
		//16MHz PRF
		config->pulse_repetition_frequency = DWT_PRF_16M;
	}
	//Fixme: How to calculate SFD Timeout
	config->sfd_timeout = (129 + 8 - 8);

	if (fast == 2) {
		trace_printf("Super fast\n");
		config->preamble_acquisition_chunk_size = DWT_PAC8;
		config->data_rate = DWT_BR_6M8; //Use fasted data rate
		config->preamble_length = DWT_PLEN_128;
	} else if( fast == 1){
		trace_printf("Slow\n");
		config->preamble_acquisition_chunk_size = DWT_PAC16;
		config->data_rate = DWT_BR_850K;
		config->preamble_length = DWT_PLEN_256;
	}else{
		trace_printf("Super slow\n");
		config->preamble_acquisition_chunk_size = DWT_PAC32;
		config->data_rate = DWT_BR_110K;
		config->preamble_length = DWT_PLEN_1024;
	}

}

/**
 * See header file dw1000.h
 */
void dw1000_extiCallback(void) {
	dw1000Isr_handleInterrupt();
}