forked from 0xha0/df_datalink
308 lines
9.3 KiB
C
308 lines
9.3 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* @file time_ctrl.c
|
|
* @author FanHao ([email protected])
|
|
* @brief Time sync controller driver implementation file.
|
|
* @version 0.0
|
|
* @date 2023-12-20
|
|
* @copyright Copyright(c) 2024 Beijing Institute of Technology.\n
|
|
* Lab of Communication and Network
|
|
*
|
|
******************************************************************************/
|
|
|
|
/**
|
|
* @addtogroup TimeController_Driver
|
|
* @{
|
|
*/
|
|
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <string.h>
|
|
#include "time_ctrl.h"
|
|
#include "error.h"
|
|
#include "common.h"
|
|
#include "util.h"
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
/**
|
|
* @name TimeController_REG_Constants
|
|
* @{
|
|
*/
|
|
#define VERSION_REG 0x00000004
|
|
#define TIME_INT_REG 0x00000008
|
|
#define TIME_FRAC_REG 0x0000000C
|
|
#define TIME_RESET 0x00000010
|
|
#define PRESET_HIGH 0x00000014
|
|
#define PRESET_LOW 0x00000018
|
|
#define TIME_ADJ_INT 0x0000001C
|
|
#define TIME_ADJ_FRAC 0x00000020
|
|
#define TIME_ADJ_MODE 0x00000024
|
|
#define TIME_ADJ_TRIG 0x00000028
|
|
#define CNTSTEP_OPT 0x0000002c
|
|
|
|
#define TM_RESET_CODE 0x5253542E
|
|
|
|
/** @} TimeController_REG_Constants*/
|
|
|
|
/***************** Macros (Inline Functions) Definitions *********************/
|
|
#define TMC_Read(instPtr, addr) IO_In32(instPtr->config.baseAddr + addr)
|
|
#define TMC_Write(instPtr, addr, value) IO_Out32(instPtr->config.baseAddr + addr, value)
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Initialize Time-Controller Process
|
|
*
|
|
* @param instPtr a second rank pointer to time controller instance
|
|
* @param config time controller configurations
|
|
*
|
|
* @return SUCCESS Initialize successfully.
|
|
* FAILURE Initialization failed
|
|
*
|
|
******************************************************************************/
|
|
int32_t TimeCtrl_Init(TMC_INST **instPtr, TMC_CONFIG *config)
|
|
{
|
|
ASSERT(config != NULL);
|
|
ASSERT(instPtr != NULL);
|
|
(*instPtr) = malloc(sizeof(TMC_INST));
|
|
memcpy(&((*instPtr)->config), config, sizeof(TMC_CONFIG));
|
|
(*instPtr)->modelVer = TMC_Read((*instPtr), VERSION_REG);
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Completely destroy time control instance.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_Destroy(TMC_INST *instPtr)
|
|
{
|
|
free(instPtr);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Read time controller version register.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return timer controller version.
|
|
*
|
|
******************************************************************************/
|
|
uint32_t TimeCtrl_GetVersion(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
return TMC_Read(instPtr, VERSION_REG);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Apply a reset process to time controller
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return SUCCESS Initialize successfully.
|
|
* FAILURE Initialization failed
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_Reset(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
TMC_Write(instPtr, CNTSTEP_OPT, TM_RESET_CODE);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Assert time controller reset high
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_ResetHigh(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
// TODO:
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Assert time controller reset low
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_ResetLow(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
// TODO:
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Get current time integer value.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return integer value
|
|
*
|
|
******************************************************************************/
|
|
inline TIME_INT TimeCtrl_GetIntCnt(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
return TMC_Read(instPtr, TIME_INT_REG);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Get current time fractional value.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
*
|
|
* @return fractional value
|
|
*
|
|
******************************************************************************/
|
|
inline TIME_FRAC TimeCtrl_GetFracCnt(TMC_INST *instPtr)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
return TMC_Read(instPtr, TIME_FRAC_REG);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Get current time value.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
* @param tmWord a pointer to time data buffer
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_GetCurrentTime(TMC_INST *instPtr, TIME_WORD *tmWord)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(tmWord != NULL);
|
|
tmWord->frac_part = TMC_Read(instPtr, TIME_FRAC_REG);
|
|
tmWord->int_part = TMC_Read(instPtr, TIME_INT_REG);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Get current optimal counter step adjustment value.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
* @param config a pointer to time data buffer
|
|
*
|
|
* @return NONE.
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_GetCurrentStep(TMC_INST *instPtr, TIME_FRAC *tmFrac)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(tmFrac != NULL);
|
|
*tmFrac = TMC_Read(instPtr, CNTSTEP_OPT);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Set a new optimal adjustment value to the counter step.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
* @param tmWord a pointer to the time data
|
|
* @param type adjustment type
|
|
*
|
|
* @return NONE.
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_TimeAdjust(TMC_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(tmWord != NULL);
|
|
if (type < TM_ADJ_ADD || TM_ADJ_ADD > TM_ADJ_SET)
|
|
{
|
|
DBG_ERROR("Unsupported adjustment type: %" PRId8 ".\n", type);
|
|
return;
|
|
}
|
|
if (((int32_t)tmWord->frac_part) > TIME_FRAC_MAX || ((int32_t)tmWord->frac_part) < -TIME_FRAC_MAX)
|
|
{
|
|
DBG_WARNING("Invalid fractional value %" PRIu32 " (should between %" PRIu32 "~%" PRIu32 ")\n",
|
|
tmWord->frac_part, (uint32_t)(-TIME_FRAC_MAX), (uint32_t)TIME_FRAC_MAX);
|
|
return;
|
|
}
|
|
TMC_Write(instPtr, TIME_ADJ_FRAC, tmWord->frac_part);
|
|
TMC_Write(instPtr, TIME_ADJ_INT, tmWord->int_part);
|
|
TMC_Write(instPtr, TIME_ADJ_MODE, type);
|
|
TMC_Write(instPtr, TIME_ADJ_TRIG, type);
|
|
|
|
#ifdef EMULATION
|
|
uint32_t regFrac, regInt;
|
|
switch (type)
|
|
{
|
|
case TM_ADJ_ADD:
|
|
regFrac = TMC_Read(instPtr, TIME_FRAC_REG) + tmWord->frac_part;
|
|
regInt = TMC_Read(instPtr, TIME_INT_REG) + tmWord->int_part;
|
|
regInt += regFrac / TIME_FRAC_MAX;
|
|
regFrac %= TIME_FRAC_MAX;
|
|
|
|
TMC_Write(instPtr, TIME_FRAC_REG, regFrac);
|
|
TMC_Write(instPtr, TIME_INT_REG, regInt);
|
|
break;
|
|
case TM_ADJ_SET:
|
|
TMC_Write(instPtr, TIME_FRAC_REG, tmWord->frac_part);
|
|
TMC_Write(instPtr, TIME_INT_REG, tmWord->int_part);
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
#endif
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Set a new optimal adjustment value to the counter step.
|
|
*
|
|
* @param instPtr a pointer to time controller instance
|
|
* @param tmFrac a pointer to the time data
|
|
*
|
|
* @return NONE.
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_SetOptimizeStep(TMC_INST *instPtr, TIME_FRAC *tmFrac)
|
|
{
|
|
ASSERT(instPtr != NULL);
|
|
ASSERT(tmFrac != NULL);
|
|
TMC_Write(instPtr, CNTSTEP_OPT, *tmFrac);
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Calculate b - a.
|
|
*
|
|
* @param a time stamp A
|
|
* @param b time stamp B
|
|
* @param gap t round
|
|
*
|
|
* @return NONE
|
|
*
|
|
******************************************************************************/
|
|
void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap)
|
|
{
|
|
ASSERT(a != NULL);
|
|
ASSERT(b != NULL);
|
|
ASSERT(gap != NULL);
|
|
uint64_t timeDiff;
|
|
timeDiff = (uint64_t)b->frac_part;
|
|
timeDiff += ((uint64_t)b->int_part) * TIME_FRAC_MAX;
|
|
timeDiff -= ((uint64_t)a->int_part) * TIME_FRAC_MAX;
|
|
timeDiff -= (uint64_t)a->frac_part;
|
|
gap->frac_part = (TIME_FRAC)(timeDiff % TIME_FRAC_MAX);
|
|
gap->frac_part = (TIME_INT)(timeDiff / TIME_FRAC_MAX);
|
|
}
|
|
|
|
/** @} TimeController_Driver*/ |