0xha0/datalink_repo
2
2
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

formatted code.

Browse Source
This commit is contained in:
Gatsby Van
2023-12-26 22:04:40 +08:00
parent 5cb5ffff59
commit 83e43f791c
42 changed files with 4688 additions and 3313 deletions
Download Patch File Download Diff File Expand all files Collapse all files
datalink814_ge/src/common.c
+5 -3
View File
@@ -39,7 +39,9 @@
* @return NONE.
*
******************************************************************************/
void AppAssert(const char * file, int line){
DBG_PRINT("[assert]\tin %s:%4d.\n",file, line);
while(1);
void AppAssert(const char *file, int line)
{
DBG_PRINT("[assert]\tin %s:%4d.\n", file, line);
while (1)
;
}
datalink814_ge/src/common.h
+29 -20
View File
@@ -26,56 +26,65 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
#define __PRINT_BLOCK(block) do {\
block\
} while(0)
#define __PRINT_BLOCK(block) \
do \
{ \
block \
} while (0)
#define ASSERT(exp) ((exp) ? (void)0 : AppAssert(__FILE__, __LINE__))
#define _nop_() __asm__("nop")
#define _nop_() __asm__("nop")
/* Stringify */
#define STR(x) #x
#define STR(x) #x
// #define _814_DEBUG_LEVEL 3
#if _814_DEBUG_LEVEL > 0
#define DBG_PRINT(...) printf(__VA_ARGS__)
#define DBG_PRINT(...) printf(__VA_ARGS__)
#endif
#if _814_DEBUG_LEVEL >= 1
#define DBG_ERROR(...) __PRINT_BLOCK( \
printf("[error]\tin "__FILE__":%4d ",__LINE__); \
printf(__VA_ARGS__); \
)
printf("[error]\tin "__FILE__ \
":%4d ", \
__LINE__); \
printf(__VA_ARGS__);)
#endif
#if _814_DEBUG_LEVEL >= 2
#define DBG_WARNING(...) __PRINT_BLOCK( \
printf("[warning]\t"); \
printf(__VA_ARGS__); \
)
printf("[warning]\t"); \
printf(__VA_ARGS__);)
#endif
#if _814_DEBUG_LEVEL >= 3
#define DBG_INFO(...) __PRINT_BLOCK( \
printf("[Info]\t"); \
printf(__VA_ARGS__); \
)
printf("[Info]\t"); \
printf(__VA_ARGS__);)
#endif
#ifndef DBG_ERROR
#define DBG_ERROR(...) {}
#define DBG_ERROR(...) \
{ \
}
#endif
#ifndef DBG_WARNING
#define DBG_WARNING(...) {}
#define DBG_WARNING(...) \
{ \
}
#endif
#ifndef DBG_INFO
#define DBG_INFO(...) {}
#define DBG_INFO(...) \
{ \
}
#endif
#ifndef DBG_PRINT
#define DBG_PRINT(...) {}
#define DBG_PRINT(...) \
{ \
}
#endif
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
void AppAssert(const char * file, int line);
void AppAssert(const char *file, int line);
#endif /* __COMMON_H_ */
datalink814_ge/src/error.h
+9 -9
View File
@@ -10,21 +10,21 @@
#ifdef SUCCESS
#undef SUCCESS
#endif
#define SUCCESS 0
#define SUCCESS 0
#ifdef FAILURE
#undef FAILURE
#endif
#define FAILURE -1
#define FAILURE -1
#define EPARAM -2 /*!< Data crc check failed */
#define EPARAM -2 /*!< Data crc check failed */
#define ECRC -3 /*!< Data crc check failed */
#define EDINVAL -4 /*!< Device is invalid (sleep or power off) */
#define EDCONF -5 /*!< Device confugure error */
#define EDINVAL -4 /*!< Device is invalid (sleep or power off) */
#define EDCONF -5 /*!< Device confugure error */
#define ETIMEOUT -6 /*!< Timeout error */
#define EIGNORE -7 /*!< Received an unsupported command, and ignore it */
#define EQUEUE -8 /*!< Queue overflow */
#define EALLOC -9 /*!< Failed to allocate memory */
#define EUNEXP -10 /*!< Encounter an unexpected error */
#define EIGNORE -7 /*!< Received an unsupported command, and ignore it */
#define EQUEUE -8 /*!< Queue overflow */
#define EALLOC -9 /*!< Failed to allocate memory */
#define EUNEXP -10 /*!< Encounter an unexpected error */
// #define IS_ERR_VALUE(x) ((x) != SUCCESS)
datalink814_ge/src/linklayer/framing.c
+107 -97
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Functions for framing operations.
*
*
* Date: 2022-07-16 14:17
* LastEditors: Fan Hao
* LastEditTime: 2022-07-16 17:11
* Revisions: Rev x.x
* History:
* History:
* [YYYY-MM-DD]: Description of modification.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
*
******************************************************************************/
/***************************** Include Files *********************************/
#include <stdint.h>
@@ -25,50 +25,42 @@
/************************** Constant Definitions *****************************/
// Definition of each slot content
L2_MSG_TYPE intraBmFrameMsgs[1] = {
L2_MSG_BM
};
L2_MSG_BM};
L2_MSG_TYPE intraW2AcqFrameMsgs[6] = {
L2_MSG_SCAN_INIT,
L2_MSG_SCAN_CONT_1,
L2_MSG_SCAN_CONT_2,
L2_MSG_SCAN_CONT_3,
L2_MSG_SCAN_CONT_4,
L2_MSG_SCAN_CONT_5
};
L2_MSG_SCAN_INIT,
L2_MSG_SCAN_CONT_1,
L2_MSG_SCAN_CONT_2,
L2_MSG_SCAN_CONT_3,
L2_MSG_SCAN_CONT_4,
L2_MSG_SCAN_CONT_5};
L2_MSG_TYPE intraMgmtFrameMsgs[6] = {
L2_MSG_TK_INIT,
L2_MSG_TK_EXTD,
L2_MSG_TK_CONT_1,
L2_MSG_TK_CONT_2,
L2_MSG_TK_CONT_3,
L2_MSG_NQT_INIT
};
L2_MSG_TK_INIT,
L2_MSG_TK_EXTD,
L2_MSG_TK_CONT_1,
L2_MSG_TK_CONT_2,
L2_MSG_TK_CONT_3,
L2_MSG_NQT_INIT};
L2_MSG_TYPE taFrameMsgs[4] = {
L2_MSG_TA_INIT,
L2_MSG_TA_EXTD,
L2_MSG_TA_CONT_1,
L2_MSG_TA_CONT_2
};
L2_MSG_TA_INIT,
L2_MSG_TA_EXTD,
L2_MSG_TA_CONT_1,
L2_MSG_TA_CONT_2};
L2_MSG_TYPE interSvcFrameMsgs[2] = {
L2_MSG_LINK_INIT,
L2_MSG_LINK_CONT_1
};
L2_MSG_LINK_INIT,
L2_MSG_LINK_CONT_1};
L2_MSG_TYPE rttReqFrameMsgs[3] = {
L2_MSG_RTT_REQ_INIT,
L2_MSG_RTT_REQ_EXTD,
L2_MSG_RTT_REQ_CONT
};
L2_MSG_RTT_REQ_INIT,
L2_MSG_RTT_REQ_EXTD,
L2_MSG_RTT_REQ_CONT};
L2_MSG_TYPE rttRspFrameMsgs[3] = {
L2_MSG_RTT_RSP_INIT,
L2_MSG_RTT_RSP_EXTD,
L2_MSG_RTT_RSP_CONT
};
L2_MSG_RTT_RSP_INIT,
L2_MSG_RTT_RSP_EXTD,
L2_MSG_RTT_RSP_CONT};
/*****************************************************************************/
/**
@@ -84,9 +76,10 @@ L2_MSG_TYPE rttRspFrameMsgs[3] = {
* SUCCESS
*
******************************************************************************/
void FrameInsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk){
void FrameInsertChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk)
{
ASSERT(bitarr != NULL);
ASSERT(bitarr->num_of_bits >= offset+numBit && numBit <= 64);
ASSERT(bitarr->num_of_bits >= offset + numBit && numBit <= 64);
#ifdef CHUNK_FILL_STYLE_XJY
int32_t bitRmn = numBit;
uint64_t dataWr = 0;
@@ -94,12 +87,13 @@ void FrameInsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint6
uint32_t byteOff = 0;
uint32_t bitWr = 0;
uint32_t bitWrCnt = 0;
while (bitRmn > 0){
bitOff = offset%8;
byteOff = offset/8;
bitWr = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
dataWr = chunk>>(numBit-(bitWr + bitWrCnt));
bit_array_set_wordn(bitarr, byteOff*8 + (8-bitOff-bitWr), dataWr, bitWr);
while (bitRmn > 0)
{
bitOff = offset % 8;
byteOff = offset / 8;
bitWr = (bitRmn > (8 - bitOff)) ? (8 - bitOff) : bitRmn;
dataWr = chunk >> (numBit - (bitWr + bitWrCnt));
bit_array_set_wordn(bitarr, byteOff * 8 + (8 - bitOff - bitWr), dataWr, bitWr);
bitWrCnt += bitWr;
bitRmn -= bitWr;
offset += bitWr;
@@ -123,26 +117,28 @@ void FrameInsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint6
* SUCCESS
*
******************************************************************************/
void FrameGetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t* chunk){
void FrameGetChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t *chunk)
{
ASSERT(bitarr != NULL);
ASSERT(bitarr->num_of_bits >= offset+numBit && numBit <= 64);
ASSERT(bitarr->num_of_bits >= offset + numBit && numBit <= 64);
#ifdef CHUNK_FILL_STYLE_XJY
int32_t bitRmn = numBit;
uint64_t dataRd = 0;
uint32_t bitOff = offset%8;
uint32_t byteOff = offset/8;
uint32_t bitRd = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
uint32_t bitOff = offset % 8;
uint32_t byteOff = offset / 8;
uint32_t bitRd = (bitRmn > (8 - bitOff)) ? (8 - bitOff) : bitRmn;
uint32_t bitRdCnt = 0;
while (bitRmn > 0){
dataRd |= bit_array_get_wordn(bitarr, byteOff*8 + (8-bitOff-bitRd), bitRd);
while (bitRmn > 0)
{
dataRd |= bit_array_get_wordn(bitarr, byteOff * 8 + (8 - bitOff - bitRd), bitRd);
bitRdCnt += bitRd;
bitRmn -= bitRd;
offset += bitRd;
bitOff = offset%8;
byteOff = offset/8;
bitRd = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
dataRd = dataRd<<bitRd;
bitOff = offset % 8;
byteOff = offset / 8;
bitRd = (bitRmn > (8 - bitOff)) ? (8 - bitOff) : bitRmn;
dataRd = dataRd << bitRd;
}
*chunk = dataRd;
return SUCCESS;
@@ -164,25 +160,28 @@ void FrameGetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t
* SUCCESS
*
******************************************************************************/
static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t* crc){
// ASSERT(dataBuff != NULL);
static int32_t L2_CrcGen(uint8_t *dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t *crc)
{
// ASSERT(dataBuff != NULL);
int32_t Status = SUCCESS;
uint32_t crcVal = 0;
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t*)&crcVal);
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t *)&crcVal);
break;
case L2_CRC16_8005:
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t*)&crcVal);
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t *)&crcVal);
break;
case L2_CRC8_31:
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t*)&crcVal);
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t *)&crcVal);
break;
default:
Status = EPARAM;
break;
}
if(Status == SUCCESS){
if (Status == SUCCESS)
{
*crc = crcVal;
}
return Status;
@@ -201,24 +200,27 @@ static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcTyp
* SUCCESS
*
******************************************************************************/
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){
// ASSERT(dataBuff != NULL);
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
{
// ASSERT(dataBuff != NULL);
int32_t Status = SUCCESS;
uint32_t crcVal = 0;
uint8_t* crcPtr = dataBuff+crcPos;
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
if(Status != SUCCESS){
uint8_t *crcPtr = dataBuff + crcPos;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if (Status != SUCCESS)
{
return Status;
}
#ifdef CHUNK_FILL_STYLE_XJY
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
*crcPtr = (uint8_t)(crcVal>>8);
*(crcPtr+1) = (uint8_t)(crcVal);
*crcPtr = (uint8_t)(crcVal >> 8);
*(crcPtr + 1) = (uint8_t)(crcVal);
break;
case L2_CRC16_8005:
*crcPtr = (uint8_t)(crcVal>>8);
*(crcPtr+1) = (uint8_t)(crcVal);
*crcPtr = (uint8_t)(crcVal >> 8);
*(crcPtr + 1) = (uint8_t)(crcVal);
break;
case L2_CRC8_31:
*crcPtr = (uint8_t)crcVal;
@@ -228,13 +230,14 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
break;
}
#else
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC16_8005:
*(crcPtr+1) = (uint8_t)(crcVal>>8);
*(crcPtr + 1) = (uint8_t)(crcVal >> 8);
*crcPtr = (uint8_t)(crcVal);
break;
case L2_CRC8_31:
@@ -261,26 +264,29 @@ int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CR
* SUCCESS
*
******************************************************************************/
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType)
{
ASSERT(dataBuff != NULL);
int32_t Status = SUCCESS;
uint32_t crcVal = 0;
uint32_t crcComp = 0;
uint8_t* crcPtr = dataBuff+crcPos;
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
if(Status != SUCCESS){
uint8_t *crcPtr = dataBuff + crcPos;
Status = L2_CrcGen(dataBuff, dataLen, crcType, &crcVal);
if (Status != SUCCESS)
{
return false;
}
#ifdef CHUNK_FILL_STYLE_XJY
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
crcComp = ((uint32_t)(*crcPtr)) << 8;
crcComp |= (uint32_t)(*(crcPtr+1));
crcComp |= (uint32_t)(*(crcPtr + 1));
break;
case L2_CRC16_8005:
crcComp = ((uint32_t)(*crcPtr)) << 8;
crcComp |= (uint32_t)(*(crcPtr+1));
crcComp |= (uint32_t)(*(crcPtr + 1));
break;
case L2_CRC8_31:
crcComp = (uint32_t)(*crcPtr);
@@ -290,13 +296,14 @@ bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TY
break;
}
#else
switch (crcType){
switch (crcType)
{
case L2_CRC16_1021:
crcComp = ((uint32_t)(*(crcPtr+1))) << 8;
crcComp = ((uint32_t)(*(crcPtr + 1))) << 8;
crcComp |= (uint32_t)(*crcPtr);
break;
case L2_CRC16_8005:
crcComp = ((uint32_t)(*(crcPtr+1))) << 8;
crcComp = ((uint32_t)(*(crcPtr + 1))) << 8;
crcComp |= (uint32_t)(*crcPtr);
break;
case L2_CRC8_31:
@@ -307,21 +314,24 @@ bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TY
break;
}
#endif
if(Status == SUCCESS){
// if ( crcVal == 0 )
// {
// return false ;
// }
// else
if(crcComp == crcVal){
if (Status == SUCCESS)
{
// if ( crcVal == 0 )
// {
// return false ;
// }
// else
if (crcComp == crcVal)
{
return true;
}
else{
else
{
return false;
}
}
else{
else
{
return false;
}
}
datalink814_ge/src/linklayer/geometry/coord.c
+66 -50
View File
@@ -7,7 +7,6 @@
#include <math.h>
#include "coord.h"
const COORD COORD_NIL = {NAN, NAN, NAN};
const COORD COORD_ZERO = {0.0, 0.0, 0.0};
const COORD COORD_ONE = {1.0, 1.0, 1.0};
@@ -15,8 +14,9 @@ const COORD COORD_X_AXIS = {1.0, 0.0, 0.0};
const COORD COORD_Y_AXIS = {0.0, 1.0, 0.0};
const COORD COORD_Z_AXIS = {0.0, 0.0, 1.0};
/** @brief Subtracts two coordinate vectors. */
COORD Coord_Sub(COORD a, COORD b) {
/** @brief Subtracts two coordinate vectors. */
COORD Coord_Sub(COORD a, COORD b)
{
COORD ret;
ret.x = a.x - b.x;
ret.y = a.y - b.y;
@@ -25,7 +25,8 @@ COORD Coord_Sub(COORD a, COORD b) {
}
/** @brief Adds two coordinate vectors. */
COORD Coord_Add(COORD a, COORD b) {
COORD Coord_Add(COORD a, COORD b)
{
COORD ret;
ret.x = a.x + b.x;
ret.y = a.y + b.y;
@@ -34,14 +35,16 @@ COORD Coord_Add(COORD a, COORD b) {
}
/** @brief Multiplies a coordinate vector by a real number. */
void Coord_ScalarMult(COORD *a, double t) {
void Coord_ScalarMult(COORD *a, double t)
{
a->x *= t;
a->y *= t;
a->z *= t;
}
/** @brief Multiplies a coordinate vector by a real number. */
COORD Coord_GetScalarMult(COORD a, double t) {
COORD Coord_GetScalarMult(COORD a, double t)
{
COORD ret;
ret.x = a.x * t;
ret.y = a.y * t;
@@ -50,14 +53,16 @@ COORD Coord_GetScalarMult(COORD a, double t) {
}
/** @brief Divides a coordinate vector by a real number. */
void Coord_ScalarDiv(COORD *a, double t) {
void Coord_ScalarDiv(COORD *a, double t)
{
a->x /= t;
a->y /= t;
a->z /= t;
}
/** @brief Divides a coordinate vector by a real number. */
COORD Coord_GetScalarDiv(COORD a, double t) {
COORD Coord_GetScalarDiv(COORD a, double t)
{
COORD ret;
ret.x = a.x / t;
ret.y = a.y / t;
@@ -66,12 +71,14 @@ COORD Coord_GetScalarDiv(COORD a, double t) {
}
/** @brief Dot product. */
inline double Coord_Dot(COORD a, COORD b) {
inline double Coord_Dot(COORD a, COORD b)
{
return (a.x * b.x + a.y * b.y + a.z * b.z);
}
/** @brief Cross product. */
COORD Coord_Cross(COORD a, COORD b) {
COORD Coord_Cross(COORD a, COORD b)
{
COORD ret;
ret.x = a.y * b.z - a.z * b.y;
ret.y = a.z * b.x - a.x * b.z;
@@ -80,7 +87,8 @@ COORD Coord_Cross(COORD a, COORD b) {
}
/** @brief Multiplies two coordinate vector in elementwise. */
COORD Coord_ElementwiseMult(COORD a, COORD b) {
COORD Coord_ElementwiseMult(COORD a, COORD b)
{
COORD ret;
ret.x = a.x * b.x;
ret.y = a.y * b.y;
@@ -89,7 +97,8 @@ COORD Coord_ElementwiseMult(COORD a, COORD b) {
}
/** @brief Divides two coordinate vector in elementwise. */
COORD Coord_ElementwiseDiv(COORD a, COORD b) {
COORD Coord_ElementwiseDiv(COORD a, COORD b)
{
COORD ret;
ret.x = a.x / b.x;
ret.y = a.y / b.y;
@@ -98,45 +107,51 @@ COORD Coord_ElementwiseDiv(COORD a, COORD b) {
}
/** @brief Calculate a coordinate vector length^2 (omits calling square root). */
double Coord_SquareLength(COORD a) {
return (a.x*a.x + a.y*a.y + a.z*a.z);
double Coord_SquareLength(COORD a)
{
return (a.x * a.x + a.y * a.y + a.z * a.z);
}
/** @brief Calculate a coordinate vector length. */
double Coord_Length(COORD a) {
double Coord_Length(COORD a)
{
return sqrt(Coord_SquareLength(a));
}
/** @brief Calculate two coordinates' distance. */
double Coord_Distance(COORD a, COORD b) {
double Coord_Distance(COORD a, COORD b)
{
return Coord_Length(Coord_Sub(a, b));
}
/** @brief Normalize coordinate vector. */
void Coord_Normalize(COORD *a) {
void Coord_Normalize(COORD *a)
{
Coord_ScalarDiv(a, Coord_Length(*a));
}
/** @brief Normalize coordinate vector. */
COORD Coord_GetNormalized(COORD a) {
COORD Coord_GetNormalized(COORD a)
{
Coord_Normalize(&a);
return a;
}
/** @brief Returns the angle between the two vectors. */
double Coord_Angle(COORD a, COORD b) {
double Coord_Angle(COORD a, COORD b)
{
Coord_Normalize(&a);
Coord_Normalize(&b);
return acos(Coord_Dot(a, b));
}
// Additional Coordinates convert
// TODO: Confirm correctness
/** @brief Convert coordinate from Launch coordinate system to ECEF. */
COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) {
COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda)
{
COORD ret;
double sinA,cosA, sinB, cosB, sinL, cosL;
double sinA, cosA, sinB, cosB, sinL, cosL;
double S11, S12, S13, S21, S22, S23, S31, S32, S33;
sinA = sin(A);
cosA = cos(A);
@@ -145,51 +160,52 @@ COORD Coord_Launch2Ecef(COORD o, COORD p, double A, double B, double lambda) {
sinL = sin(lambda);
cosL = cos(lambda);
S11 = -sinA*sinL - cosA*sinB*cosL;
S12 = cosB*cosL;
S13 = -cosA*sinL + sinA*sinB*cosL;
S11 = -sinA * sinL - cosA * sinB * cosL;
S12 = cosB * cosL;
S13 = -cosA * sinL + sinA * sinB * cosL;
S21 = sinA*cosL - cosA*sinB*sinL;
S22 = cosB*sinL;
S23 = cosA*cosL + sinA*sinB*sinL;
S21 = sinA * cosL - cosA * sinB * sinL;
S22 = cosB * sinL;
S23 = cosA * cosL + sinA * sinB * sinL;
S31 = cosA*cosB;
S31 = cosA * cosB;
S32 = sinB;
S33 = -sinA*cosB;
S33 = -sinA * cosB;
ret.x = p.x*(S11 + S21 + S31) + o.x;
ret.y = p.y*(S12 + S22 + S32) + o.y;
ret.z = p.z*(S13 + S23 + S33) + o.z;
ret.x = p.x * (S11 + S21 + S31) + o.x;
ret.y = p.y * (S12 + S22 + S32) + o.y;
ret.z = p.z * (S13 + S23 + S33) + o.z;
return ret;
}
/** @brief Convert coordinate from launch coordinate system to Inertia coordinate system. */
COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t) {
COORD Coord_Launch2Inertia(COORD o, COORD p, double A, double B, double we, double t)
{
COORD ret;
double OmgX, OmgY, OmgZ, sinwet, coswet;
double M11, M12, M13, M21, M22, M23, M31, M32, M33;
OmgX = cos(A)*cos(B);
OmgX = cos(A) * cos(B);
OmgY = sin(B);
OmgZ = -sin(A)*cos(B);
sinwet = sin(we*t);
coswet = cos(we*t);
OmgZ = -sin(A) * cos(B);
sinwet = sin(we * t);
coswet = cos(we * t);
M11 = 1 - (1 - OmgX*OmgX)*(1-coswet);
M12 = OmgX*OmgY*(1-coswet) + OmgZ*sinwet;
M13 = OmgX*OmgZ*(1-coswet)-OmgY*sinwet;
M11 = 1 - (1 - OmgX * OmgX) * (1 - coswet);
M12 = OmgX * OmgY * (1 - coswet) + OmgZ * sinwet;
M13 = OmgX * OmgZ * (1 - coswet) - OmgY * sinwet;
M21 = OmgY*OmgX*(1-coswet) - OmgZ*sinwet;
M22 = 1-(1-OmgY*OmgY)*(1-coswet);
M23 = OmgY*OmgZ*(1-coswet) + OmgX*sinwet;
M21 = OmgY * OmgX * (1 - coswet) - OmgZ * sinwet;
M22 = 1 - (1 - OmgY * OmgY) * (1 - coswet);
M23 = OmgY * OmgZ * (1 - coswet) + OmgX * sinwet;
M31 = OmgZ*OmgX*(1-coswet) + OmgY*sinwet;
M32 = OmgZ*OmgY*(1-coswet) - OmgX*sinwet;
M33 = 1 - (1-OmgZ*OmgZ)*(1-coswet);
M31 = OmgZ * OmgX * (1 - coswet) + OmgY * sinwet;
M32 = OmgZ * OmgY * (1 - coswet) - OmgX * sinwet;
M33 = 1 - (1 - OmgZ * OmgZ) * (1 - coswet);
ret.x = p.x*(M11 + M21 + M31) + o.x;
ret.y = p.y*(M12 + M22 + M32) + o.y;
ret.z = p.z*(M13 + M23 + M33) + o.z;
ret.x = p.x * (M11 + M21 + M31) + o.x;
ret.y = p.y * (M12 + M22 + M32) + o.y;
ret.z = p.z * (M13 + M23 + M33) + o.z;
return ret;
}
datalink814_ge/src/linklayer/geometry/coord.h
+2 -3
View File
@@ -4,7 +4,6 @@
// SPDX-License-Identifier: LGPL-3.0-or-later
//
#ifndef _COORD_H
#define _COORD_H
@@ -16,7 +15,8 @@
* @ingroup utils
*/
typedef struct{
typedef struct
{
double x;
double y;
double z;
@@ -78,4 +78,3 @@ COORD Coord_GetNormalized(COORD a);
double Coord_Angle(COORD a, COORD b);
#endif
datalink814_ge/src/linklayer/geometry/eulerangles.c
+18 -14
View File
@@ -7,51 +7,55 @@
#include <math.h>
#include "eulerangles.h"
const EULERANGLES EULERANGLES_NIL = {NAN, NAN, NAN};
const EULERANGLES EULERANGLES_ZERO = {0.0, 0.0, 0.0};
/** @brief Add two EulerAngles. */
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2) {
EULERANGLES EulerAngles_Add(EULERANGLES e1, EULERANGLES e2)
{
EULERANGLES ret;
ret.alpha = e1.alpha + e2.alpha;
ret.beta = e1.beta + e2.beta ;
ret.beta = e1.beta + e2.beta;
ret.gamma = e1.gamma + e2.gamma;
return ret;
}
/** @brief Subtracts two EulerAngles. */
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2) {
EULERANGLES EulerAngles_Sub(EULERANGLES e1, EULERANGLES e2)
{
EULERANGLES ret;
ret.alpha = e1.alpha - e2.alpha;
ret.beta = e1.beta - e2.beta ;
ret.beta = e1.beta - e2.beta;
ret.gamma = e1.gamma - e2.gamma;
return ret;
}
/** @brief Multiplies EulerAngles by a real number. */
void EulerAngles_ScalarMult(EULERANGLES *e, double f) {
void EulerAngles_ScalarMult(EULERANGLES *e, double f)
{
e->alpha *= f;
e->beta *= f;
e->beta *= f;
e->gamma *= f;
}
/** @brief Multiplies EulerAngles by a real number. */
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f) {
EULERANGLES EulerAngles_GetScalarMult(EULERANGLES e, double f)
{
EulerAngles_ScalarMult(&e, f);
return e;
}
/** @brief Normalize EulerAngles. */
void EulerAngles_Normalize(EULERANGLES *e) {
e->alpha = fmod(e->alpha, 2*EUL_PI);
e->beta = fmod(e->beta , 2*EUL_PI);
e->gamma = fmod(e->gamma, 2*EUL_PI);
void EulerAngles_Normalize(EULERANGLES *e)
{
e->alpha = fmod(e->alpha, 2 * EUL_PI);
e->beta = fmod(e->beta, 2 * EUL_PI);
e->gamma = fmod(e->gamma, 2 * EUL_PI);
}
/** @brief Normalize EulerAngles. */
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e) {
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e)
{
EulerAngles_Normalize(&e);
return e;
}
datalink814_ge/src/linklayer/geometry/eulerangles.h
+10 -9
View File
@@ -3,8 +3,8 @@
#define EUL_PI 3.14159265358979323846
#define DEG2RAD(d) ((EUL_PI/180.0)*d)
#define RAD2DEG(r) ((180.0/EUL_PI)*r)
#define DEG2RAD(d) ((EUL_PI / 180.0) * d)
#define RAD2DEG(r) ((180.0 / EUL_PI) * r)
/**
* Orientations are represented by 3D double precision Tait-Bryan (Euler) tuples
@@ -16,18 +16,20 @@
* rotation along the Y axis rotates Z into X (leans forward), positive rotation
* along the X axis rotates Y into Z (leans right).
*/
typedef struct {
double alpha; // unit rad
double beta; // unit rad
double gamma; // unit rad
typedef struct
{
double alpha; // unit rad
double beta; // unit rad
double gamma; // unit rad
} EULERANGLES;
typedef enum {
typedef enum
{
EULERANGLES_NONE = 0,
EULERANGLES_ZYX = 1,
EULERANGLES_ZYZ = 2,
EULERANGLES_XYZ = 3,
}EULERANGLES_TYPE;
} EULERANGLES_TYPE;
extern const EULERANGLES EULERANGLES_ZERO;
extern const EULERANGLES EULERANGLES_NIL;
@@ -51,4 +53,3 @@ void EulerAngles_Normalize(EULERANGLES *e);
EULERANGLES EulerAngles_GetNormalize(EULERANGLES e);
#endif
datalink814_ge/src/linklayer/geometry/quaternion.c
+182 -143
View File
@@ -11,12 +11,14 @@
#include "eulerangles.h"
#include "quaternion.h"
typedef union {
typedef union
{
float value;
uint32_t raw;
} SOFT_FLOAT;
typedef union {
typedef union
{
double value;
uint64_t raw;
} SOFT_DOUBLE;
@@ -24,10 +26,11 @@ typedef union {
const SOFT_FLOAT EPS_FLOAT = {.raw = 0x34000000};
const SOFT_DOUBLE EPS_DOUBLE = {.raw = 0x3CB0000000000000};
const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0 ,0.0};
const QUATERNION QUATERNION_IDENTITY = {1.0, 0.0, 0.0, 0.0};
const QUATERNION QUATERNION_NIL = {NAN, NAN, NAN, NAN};
/** @brief Subtracts two quaternions. */
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) {
QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s - q2.s;
ret.i = q1.i - q2.i;
@@ -37,7 +40,8 @@ QUATERNION Quaternion_Sub(QUATERNION q1, QUATERNION q2) {
}
/** @brief Adds two quaternions. */
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) {
QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s + q2.s;
ret.i = q1.i + q2.i;
@@ -47,7 +51,8 @@ QUATERNION Quaternion_Add(QUATERNION q1, QUATERNION q2) {
}
/** @brief Multiplies a quaternion by a real number. */
void Quaternion_ScalarMult(QUATERNION *q, double t) {
void Quaternion_ScalarMult(QUATERNION *q, double t)
{
q->s *= t;
q->i *= t;
q->j *= t;
@@ -55,13 +60,15 @@ void Quaternion_ScalarMult(QUATERNION *q, double t) {
}
/** @brief Multiplies a quaternion by a real number. */
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t) {
QUATERNION Quaternion_GetScalarMult(QUATERNION a, double t)
{
Quaternion_ScalarMult(&a, t);
return a;
}
/** @brief Divides a quaternion by a real number. */
void Quaternion_ScalarDiv(QUATERNION *q, double t) {
void Quaternion_ScalarDiv(QUATERNION *q, double t)
{
q->s /= t;
q->i /= t;
q->j /= t;
@@ -69,15 +76,17 @@ void Quaternion_ScalarDiv(QUATERNION *q, double t) {
}
/** @brief Divides a quaternion by a real number. */
QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t) {
QUATERNION Quaternion_GetScalarDiv(QUATERNION q, double t)
{
Quaternion_ScalarDiv(&q, t);
return q;
}
/** @brief Multiplies two quaternions. */
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) {
QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2)
{
QUATERNION ret;
ret.s = q1.s * q2.s - (q1.i*q2.i + q1.j*q2.j + q1.k*q2.k);
ret.s = q1.s * q2.s - (q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
ret.i = q1.j * q2.k - q1.k * q2.j + q1.s * q2.i + q1.i * q2.s;
ret.j = q1.k * q2.i - q1.i * q2.k + q1.s * q2.j + q1.j * q2.s;
ret.k = q1.i * q2.j - q1.j * q2.i + q1.s * q2.k + q1.k * q2.s;
@@ -85,69 +94,80 @@ QUATERNION Quaternion_Mult(QUATERNION q1, QUATERNION q2) {
}
/** @brief Divides two quaternions. */
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2) {
QUATERNION Quaternion_Div(QUATERNION q1, QUATERNION q2)
{
Quaternion_Inverse(&q2);
return Quaternion_Mult(q1, q2);
}
/** @brief Dot product. */
inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2) {
return (q1.s*q2.s + q1.i*q2.i + q1.j*q2.j + q1.k*q2.k);
inline double Quaternion_Dot(QUATERNION q1, QUATERNION q2)
{
return (q1.s * q2.s + q1.i * q2.i + q1.j * q2.j + q1.k * q2.k);
}
/** @brief Calculate a quaternion length^2 (omits calling square root). */
inline double Quaternion_SquareLength(QUATERNION q) {
return (q.s*q.s + q.i*q.i + q.j*q.j + q.k*q.k);
inline double Quaternion_SquareLength(QUATERNION q)
{
return (q.s * q.s + q.i * q.i + q.j * q.j + q.k * q.k);
}
/** @brief Calculate a quaternion length. */
inline double Quaternion_Length(QUATERNION q) {
inline double Quaternion_Length(QUATERNION q)
{
return sqrt(Quaternion_SquareLength(q));
}
/** @brief Normalize quaternion. */
void Quaternion_Normalize(QUATERNION *q) {
void Quaternion_Normalize(QUATERNION *q)
{
Quaternion_ScalarDiv(q, Quaternion_Length(*q));
}
/** @brief Normalize quaternion. */
QUATERNION Quaternion_GetNormalized(QUATERNION q) {
QUATERNION Quaternion_GetNormalized(QUATERNION q)
{
Quaternion_Normalize(&q);
return q;
}
/** @brief Conjugated quaternion. */
void Quaternion_Conjugated(QUATERNION *q) {
void Quaternion_Conjugated(QUATERNION *q)
{
q->i = -q->i;
q->j = -q->j;
q->k = -q->k;
}
/** @brief Conjugated quaternion. */
QUATERNION Quaternion_GetConjugated(QUATERNION q) {
QUATERNION Quaternion_GetConjugated(QUATERNION q)
{
Quaternion_Conjugated(&q);
return q;
}
/** @brief Inverse quaternion. */
void Quaternion_Inverse(QUATERNION *q) {
void Quaternion_Inverse(QUATERNION *q)
{
Quaternion_Conjugated(q);
Quaternion_ScalarDiv(q, Quaternion_SquareLength(*q));
}
/** @brief Inverse quaternion. */
QUATERNION Quaternion_GeInverse(QUATERNION q) {
QUATERNION Quaternion_GeInverse(QUATERNION q)
{
Quaternion_Inverse(&q);
return q;
}
inline double Quaternion_GetRotationAngle(QUATERNION q) {
inline double Quaternion_GetRotationAngle(QUATERNION q)
{
return (acos(q.s) * 2);
}
/** @brief Get axis of quaternion. */
COORD Quaternion_GetRotationAxis(QUATERNION q) {
COORD Quaternion_GetRotationAxis(QUATERNION q)
{
double sin_theta_inv;
COORD ret;
sin_theta_inv = 1.0 / sin(acos(q.s));
@@ -158,7 +178,8 @@ COORD Quaternion_GetRotationAxis(QUATERNION q) {
}
/** @brief Get the axis and rotate angle of a unit Quaternion */
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle) {
void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle)
{
double sin_theta_inv;
*angle = acos(q.s);
@@ -173,7 +194,8 @@ void Quaternion_GetRotationAxisAndAngle(QUATERNION q, COORD *axis, double *angle
}
/** @brief Decompose the rotation on to 2 parts. */
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist) {
void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QUATERNION *twist)
{
double p;
COORD v;
v.x = q.i;
@@ -191,16 +213,17 @@ void Quaternion_GetSwingAndTwist(QUATERNION q, COORD axis, QUATERNION *swing, QU
}
/** @brief rotates v by this quaternion (quaternion must be unit) */
COORD Quaternion_Rotate(QUATERNION q, COORD v) {
COORD Quaternion_Rotate(QUATERNION q, COORD v)
{
QUATERNION temp;
COORD ret;
temp.s = 0;
temp.i = v.x;
temp.j = v.y;
temp.k = v.z;
temp = Quaternion_Mult(q,temp);
temp = Quaternion_Mult(q, temp);
Quaternion_Conjugated(&q);
temp = Quaternion_Mult(temp,q);
temp = Quaternion_Mult(temp, q);
ret.x = temp.i;
ret.y = temp.j;
ret.z = temp.k;
@@ -208,122 +231,132 @@ COORD Quaternion_Rotate(QUATERNION q, COORD v) {
}
/** @brief returns the euler angles from a rotation Quaternion */
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type) {
EULERANGLES ret;
switch (type) {
case EULERANGLES_ZYX : {
/* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex
// results
// Since the quaternion is of unit length, this should never happen,
// but some code generation configuration seem to hit this edge case
// under some circumstances.
EULERANGLES Quaternion_ToEulerAngles(QUATERNION q, EULERANGLES_TYPE type)
{
EULERANGLES ret;
switch (type)
{
case EULERANGLES_ZYX:
{
/* Covert form matlab 2023a */
// Cap all inputs to asin to 1, since values >1 produce complex
// results
// Since the quaternion is of unit length, this should never happen,
// but some code generation configuration seem to hit this edge case
// under some circumstances.
double asinValue;
bool mask1, mask2;
double asinValue;
bool mask1, mask2;
asinValue = -2.0 *(q.i*q.k - q.s*q.j);
mask1 = asinValue >= 1.0 - 10*EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10*EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
asinValue = -2.0 * (q.i * q.k - q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if(mask1 || mask2) {
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0;
}
else {
ret.alpha = atan2(2.0*(q.i*q.j + q.s*q.k), q.s*q.s + q.i*q.i - q.j*q.j - q.k*q.k);
ret.gamma = atan2(2.0*(q.j*q.k + q.s*q.i), q.s*q.s - q.i*q.i - q.j*q.j + q.k*q.k);
}
ret.beta = asin( asinValue );
break;
}
if (mask1 || mask2)
{
ret.alpha = (asinValue > 0) ? (-2.0 * atan2(q.i, q.s)) : (2.0 * atan2(q.i, q.s));
ret.gamma = 0;
}
else
{
ret.alpha = atan2(2.0 * (q.i * q.j + q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
ret.gamma = atan2(2.0 * (q.j * q.k + q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
case EULERANGLES_ZYZ : {
// TODO;
ret = EULERANGLES_NIL;
break;
}
case EULERANGLES_ZYZ:
{
// TODO;
ret = EULERANGLES_NIL;
break;
}
case EULERANGLES_XYZ : {
/* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
// operations
// sin(y) = R13 = 2 * (qx*qz + qy*qw)
// tan(x) = sin(x) / cos(x) = -R23 / R33
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
// tan(z) = sin(z) / cos(z) = -R12 / R11
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
case EULERANGLES_XYZ:
{
/* Covert form matlab 2023a */
// Prevent singularities as done in ZYX case
// Alternative to rotm2eul(quat2rotm(q), 'XYZ') with fewer
// operations
// sin(y) = R13 = 2 * (qx*qz + qy*qw)
// tan(x) = sin(x) / cos(x) = -R23 / R33
// = -2 * (qy*qz - qx*qw) / (1 - 2*(qx^2 + qy^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 - qx^2 - qy^2 + qz^2)
// tan(z) = sin(z) / cos(z) = -R12 / R11
// = -2 * (qx*qy - qz*qw) / (1 - 2*(qy^2 + qz^2))
// = -2 * (qy*qz - qx*qw) / (qw^2 + qx^2 - qy^2 - qz^2)
double asinValue;
bool mask1, mask2;
double asinValue;
bool mask1, mask2;
asinValue = 2.0 *(q.i*q.k + q.s*q.j);
mask1 = asinValue >= 1.0 - 10*EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10*EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if(mask1 || mask2) {
ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
}
else {
ret.alpha = atan2(-2.0*(q.j*q.k - q.s*q.i), q.s*q.s - q.i*q.i - q.j*q.j + q.k*q.k);
ret.gamma = atan2(-2.0*(q.i*q.j - q.s*q.k), q.s*q.s + q.i*q.i - q.j*q.j - q.k*q.k);
}
ret.beta = asin( asinValue );
break;
}
asinValue = 2.0 * (q.i * q.k + q.s * q.j);
mask1 = asinValue >= 1.0 - 10 * EPS_DOUBLE.value;
mask2 = asinValue <= -1.0 + 10 * EPS_DOUBLE.value;
if (mask1)
asinValue = 1;
if (mask2)
asinValue = -1;
if (mask1 || mask2)
{
ret.alpha = 0;
ret.gamma = (asinValue > 0) ? (2.0 * atan2(q.i, q.s)) : (-2.0 * atan2(q.i, q.s));
}
else
{
ret.alpha = atan2(-2.0 * (q.j * q.k - q.s * q.i), q.s * q.s - q.i * q.i - q.j * q.j + q.k * q.k);
ret.gamma = atan2(-2.0 * (q.i * q.j - q.s * q.k), q.s * q.s + q.i * q.i - q.j * q.j - q.k * q.k);
}
ret.beta = asin(asinValue);
break;
}
default:
ret = EULERANGLES_NIL;
}
return ret;
default:
ret = EULERANGLES_NIL;
}
return ret;
}
/** @brief returns the rotation Quaternion from a euler angles */
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type) {
QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5);
QUATERNION Quaternion_FromeEulerAngles(EULERANGLES e, EULERANGLES_TYPE type)
{
QUATERNION ret;
double cosx, sinx, cosy, siny, cosz, sinz;
cosx = cos(e.alpha * 0.5);
sinx = sin(e.alpha * 0.5);
cosy = cos(e.beta * 0.5);
siny = sin(e.beta * 0.5);
cosz = cos(e.gamma * 0.5);
sinz = sin(e.gamma * 0.5);
switch (type) {
case EULERANGLES_ZYX :
ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * cosy * sinz;
ret.k = sinx * cosy * cosz - cosx * siny * sinz;
break;
case EULERANGLES_ZYZ :
ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
ret.i = cosx * siny * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * siny * sinz;
ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
break;
case EULERANGLES_XYZ :
ret.s = cosx * cosy * cosz - sinx * siny * sinz;
ret.i = sinx * cosy * cosz + cosx * siny * sinz;
ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
ret.k = cosx * cosy * sinz + sinx * siny * cosz;
break;
default:
ret = QUATERNION_NIL;
break;
}
return ret;
switch (type)
{
case EULERANGLES_ZYX:
ret.s = cosx * cosy * cosz + sinx * siny * sinz;
ret.i = cosx * cosy * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * cosy * sinz;
ret.k = sinx * cosy * cosz - cosx * siny * sinz;
break;
case EULERANGLES_ZYZ:
ret.s = cosx * cosy * cosz - sinx * cosy * sinz;
ret.i = cosx * siny * sinz - sinx * siny * cosz;
ret.j = cosx * siny * cosz + sinx * siny * sinz;
ret.k = sinx * cosy * cosz + cosx * cosy * sinz;
break;
case EULERANGLES_XYZ:
ret.s = cosx * cosy * cosz - sinx * siny * sinz;
ret.i = sinx * cosy * cosz + cosx * siny * sinz;
ret.j = -sinx * cosy * sinz + cosx * siny * cosz;
ret.k = cosx * cosy * sinz + sinx * siny * cosz;
break;
default:
ret = QUATERNION_NIL;
break;
}
return ret;
}
/** @brief adapted from https://svn.code.sf.net/p/irrlicht/code/trunk/include/quaternion.h */
@@ -338,10 +371,12 @@ QUATERNION Quaternion_RotationFromTo(COORD from, COORD to)
c = Coord_Dot(from, to);
if (c >= 1.0) { // If dot == 1, vectors are the same
if (c >= 1.0)
{ // If dot == 1, vectors are the same
return QUATERNION_IDENTITY; // identity
}
else if (c <= -1.0) { // exactly opposite
else if (c <= -1.0)
{ // exactly opposite
axis = Coord_Cross(COORD_X_AXIS, from);
if (Coord_Length(axis) == 0)
@@ -367,38 +402,41 @@ QUATERNION Quaternion_RotationFromTo(COORD from, COORD to)
return ret;
}
/** @brief linear quaternion interpolation */
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t) {
QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t)
{
QUATERNION ret;
Quaternion_ScalarMult(&q1, 1 - t);
Quaternion_ScalarMult(&q1, t);
ret = Quaternion_Add(q1, q2);
Quaternion_Normalize(&ret);
return ret;
return ret;
}
/** @brief spherical linear interpolation */
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) {
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t)
{
double dot;
QUATERNION q3, ret;
dot = Quaternion_Dot(q1, q2);
/* dot = cos(theta)
if (dot < 0), q1 and q2 are more than 90 degrees apart,
so we can invert one to reduce spinning */
if (dot < 0) {
if (dot < 0)
{
dot = -dot;
q3.s = -q2.s;
q3.i = -q2.i;
q3.j = -q2.j;
q3.k = -q2.k;
}
else {
q3 = q2;
else
{
q3 = q2;
}
if (dot < 0.95) {
if (dot < 0.95)
{
double angle = acos(dot);
Quaternion_ScalarMult(&q1, sin(angle * (1 - t)));
Quaternion_ScalarMult(&q3, sin(angle * t));
@@ -407,7 +445,8 @@ QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t) {
return ret;
}
// if the angle is small, use linear interpolation
else {
else
{
return Quaternion_Lerp(q1, q3, t);
}
}
datalink814_ge/src/linklayer/geometry/quaternion.h
+2 -2
View File
@@ -6,7 +6,8 @@
#include "eulerangles.h"
// borrowed and adapted from https://github.com/MegaManSE/willperone/blob/master/Math/quaternion.h
typedef struct {
typedef struct
{
double s;
double i;
double j;
@@ -106,4 +107,3 @@ QUATERNION Quaternion_Lerp(QUATERNION q1, QUATERNION q2, double t);
QUATERNION Quaternion_Slerp(QUATERNION q1, QUATERNION q2, double t);
#endif
datalink814_ge/src/linklayer/linklayer814.c
+327 -212
View File
File diff suppressed because it is too large Load Diff
datalink814_ge/src/linklayer/linklayer814.h
+101 -100
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Header file for 814 linklayer functions
*
*
* Date: 2021-11-13 13:33
* LastEditors: Fan Hao
* LastEditTime: 2022-07-16 10:12
* Revisions: Rev 0.1
* History:
* History:
* [2022-07-16]: New branch for DiJian.
* [2021-11-23]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef LINKLAYER814_H_
@@ -38,12 +38,13 @@
#define NET_SEEK_STAGE 4
#define NET_ERROR_STAGE -1
#define L2_PACKET_LEN_MAX 1024 //bit
#define L2_PACKET_LEN_MAX 1024 // bit
/***************** Macros (Inline Functions) Definitions *********************/
#define AssertL2Event(instPtr, event) Linklayer_EventHandler(instPtr, event)
/**************************** Type Definitions *******************************/
typedef struct {
typedef struct
{
L2_NODE_TYPE myType;
bool isInterHead;
uint8_t clustId;
@@ -51,11 +52,12 @@ typedef struct {
uint8_t headId;
uint8_t bkpHeadId;
uint8_t netScale;
PHY_INST* phyInstPtr;
TIMECTRL_INST* tmcInstPtr;
PHY_INST *phyInstPtr;
TIMECTRL_INST *tmcInstPtr;
} L2_CONFIG;
typedef struct {
typedef struct
{
uint32_t frameRx;
uint32_t frameTx;
uint32_t frameRecv;
@@ -87,130 +89,129 @@ typedef struct {
uint32_t svcCorrectCnt;
} L2_LOG;
typedef struct {
typedef struct
{
L2_CONFIG config;
L2_NODE_INFO myInfo;
NODE_LIST* nodeList;
L2_SCHD_POOL* schdEventPool;
NODE_LIST *nodeList;
L2_SCHD_POOL *schdEventPool;
uint32_t netStartTime;
bool isEnable; // if the node is connected to the network
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNum; // stores the last current and next frame number.
L2_RCD slotNum; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD tor; // stores the last current and next link layer tx/rx state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
bool isEnable; // if the node is connected to the network
bool dataAvail; // if there is a valid received frame data in rx buffer
bool cxn2Net; // if the node is connected to the network
L2_RCD frameNum; // stores the last current and next frame number.
L2_RCD slotNum; // stores the last current and next slot number.
L2_RCD frame; // stores the last current and next link layer frame type.
L2_RCD tor; // stores the last current and next link layer tx/rx state.
L2_NODE_STATE mainState; // the main state of link layer state
L2_NODE_STATE nextState; // next state of link layer state, which will be applied to mainState every second.
L2_RTNE_CONFIG rtneConfig; // current routine table configuration and status.
L2_SCAN_FSM* scanNetFSM; // FSM of scan net (link set up)
L2_MGMT_FSM* mgmtNetFSM; // FSM of management net (link management and time sync)
L2_SVC_FSM* serviceNetFSM; // FSM of service net (data service)
L2_RTT_FSM* rttFSM; // FSM for RTT (time sync algorithm)
uint8_t* sendPktData;
uint8_t* recvPktData;
L2_SCAN_FSM *scanNetFSM; // FSM of scan net (link set up)
L2_MGMT_FSM *mgmtNetFSM; // FSM of management net (link management and time sync)
L2_SVC_FSM *serviceNetFSM; // FSM of service net (data service)
L2_RTT_FSM *rttFSM; // FSM for RTT (time sync algorithm)
uint8_t *sendPktData;
uint8_t *recvPktData;
L2_LOG log;
// for MPC
uint8_t buffSwitch;
uint8_t mgmtMsgRecvCnt0;
uint8_t svcMsgRecvCnt0;
uint8_t* mgmtMsgRecvBuff0; // mgmtMsg receive data buffer
uint8_t* svcMsgRecvBuff0; // svc receive data buffer
uint8_t *mgmtMsgRecvBuff0; // mgmtMsg receive data buffer
uint8_t *svcMsgRecvBuff0; // svc receive data buffer
uint8_t mgmtMsgRecvCnt1;
uint8_t svcMsgRecvCnt1;
uint8_t* mgmtMsgRecvBuff1; // mgmtMsg receive data buffer
uint8_t* svcMsgRecvBuff1; // svc receive data buffer
uint8_t *mgmtMsgRecvBuff1; // mgmtMsg receive data buffer
uint8_t *svcMsgRecvBuff1; // svc receive data buffer
} L2_INST;
/************************** Variable Definitions *****************************/
extern L2_RTNE_TABLE_ENTRY* initTable[L2_INIT_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY* seekTable[L2_SEEK_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY* interSeekTable[L2_NORMAL_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY* intraSeekTable[L2_NORMAL_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY* normalTable[L2_NORMAL_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *initTable[L2_INIT_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *seekTable[L2_SEEK_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *interSeekTable[L2_NORMAL_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *intraSeekTable[L2_NORMAL_RTNE_PERIOD];
extern L2_RTNE_TABLE_ENTRY *normalTable[L2_NORMAL_RTNE_PERIOD];
/************************** Function Prototypes ******************************/
// Main
int32_t Linklayer_Init(L2_INST** instPtr, const L2_CONFIG *config);
int32_t Linklayer_Reconfig(L2_INST* instPtr, const L2_CONFIG *config);
void Linklayer_Destroy(L2_INST* instPtr);
int32_t Linklayer_Start(L2_INST* instPtr);
int32_t Linklayer_Halt(L2_INST* instPtr);
int32_t Linklayer_Proc(L2_INST* instPtr);
int32_t Linklayer_EventHandler(L2_INST* instPtr, const L2_EVENT* l2Event);
int32_t Linklayer_PktHandler(L2_INST* instPtr, L2_FRAME_TYPE frameType);
int32_t L2_SetupNextSlot(L2_INST* instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
int32_t Linklayer_Init(L2_INST **instPtr, const L2_CONFIG *config);
int32_t Linklayer_Reconfig(L2_INST *instPtr, const L2_CONFIG *config);
void Linklayer_Destroy(L2_INST *instPtr);
int32_t Linklayer_Start(L2_INST *instPtr);
int32_t Linklayer_Halt(L2_INST *instPtr);
int32_t Linklayer_Proc(L2_INST *instPtr);
int32_t Linklayer_EventHandler(L2_INST *instPtr, const L2_EVENT *l2Event);
int32_t Linklayer_PktHandler(L2_INST *instPtr, L2_FRAME_TYPE frameType);
int32_t L2_SetupNextSlot(L2_INST *instPtr, L2_TOR_TYPE txrxMode, L2_FRAME_TYPE frameType);
// Scheduling Event
void L2_EventPoolHandler(L2_INST* instPtr);
int32_t L2_ScheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
int32_t L2_RescheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RescheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots);
void L2_EventPoolHandler(L2_INST *instPtr);
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
int32_t L2_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNo, uint32_t slotNo);
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots);
// Framing
void L2_FrameStateUpdate(L2_INST* instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST* instPtr);
void L2_FrameStateUpdate(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastRcvFrameType(L2_INST *instPtr);
L1_FRAME_TYPE L2_L2frame2L1frame(L2_FRAME_TYPE l2Frame);
void FrameGetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t* chunk);
void FrameInsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t chunck);
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
void FrameGetChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t *chunk);
void FrameInsertChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t chunck);
int32_t L2_CrcAppend(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
bool L2_CrcCheck(uint8_t *dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType);
// Scan Net
int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr);
void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr);
void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr);
int32_t ScanNet_Proc(L2_INST* instPtr);
int32_t ScanNet_PktHandler(L2_INST* instPtr);
int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry);
int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr);
void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr);
void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr);
int32_t ScanNet_Proc(L2_INST *instPtr);
int32_t ScanNet_PktHandler(L2_INST *instPtr);
int32_t ScanNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
// Management Net
int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM);
void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM);
void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM);
int32_t MgmtNet_PktHandler(L2_INST* instPtr);
int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry);
int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM);
void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM);
void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM);
int32_t MgmtNet_PktHandler(L2_INST *instPtr);
int32_t MgmtNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry);
// Service net
int32_t ServiceNet_Init(L2_SVC_FSM** serviceFSM);
void ServiceNet_Reset(L2_SVC_FSM* serviceFSM);
void ServiceNet_Destroy(L2_SVC_FSM* serviceFSM);
int32_t ServiceNet_PktHandler(L2_INST* instPtr);
int32_t ServiceNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* schedEntry);
int32_t ServiceNet_Init(L2_SVC_FSM **serviceFSM);
void ServiceNet_Reset(L2_SVC_FSM *serviceFSM);
void ServiceNet_Destroy(L2_SVC_FSM *serviceFSM);
int32_t ServiceNet_PktHandler(L2_INST *instPtr);
int32_t ServiceNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *schedEntry);
// Miscellaneous
uint8_t L2_GetNodeId(L2_INST* instPtr);
uint8_t L2_GetClusterId(L2_INST* instPtr);
uint8_t L2_GetHeadId(L2_INST* instPtr);
L2_NODE_TYPE L2_GetNodeType(L2_INST* instPtr);
L2_NODE_STATE L2_GetMainState(L2_INST* instPtr);
L2_RTT_FSM* L2_GetRttFsm(L2_INST* instPtr);
bool L2_isDataAvail(L2_INST* instPtr);
bool L2_isCxn2Net(L2_INST* instPtr);
void L2_RcdShift(L2_RCD* rcd, int32_t netxVal);
uint8_t L2_GetNodeId(L2_INST *instPtr);
uint8_t L2_GetClusterId(L2_INST *instPtr);
uint8_t L2_GetHeadId(L2_INST *instPtr);
L2_NODE_TYPE L2_GetNodeType(L2_INST *instPtr);
L2_NODE_STATE L2_GetMainState(L2_INST *instPtr);
L2_RTT_FSM *L2_GetRttFsm(L2_INST *instPtr);
bool L2_isDataAvail(L2_INST *instPtr);
bool L2_isCxn2Net(L2_INST *instPtr);
void L2_RcdShift(L2_RCD *rcd, int32_t netxVal);
uint32_t L2_GetFrameLen(L2_FRAME_TYPE l2FrameType);
uint32_t L2_GetLastFrameNum(L2_INST* instPtr);
uint32_t L2_GetLastSlotNum(L2_INST* instPtr);
L2_FRAME_TYPE L2_GetLastFrameType(L2_INST* instPtr);
L2_TOR_TYPE L2_GetLastTor(L2_INST* instPtr);
uint32_t L2_GetCurrentFrameNum(L2_INST* instPtr);
uint32_t L2_GetCurrentSlotNum(L2_INST* instPtr);
L2_FRAME_TYPE L2_GetCurrentFrameType(L2_INST* instPtr);
L2_TOR_TYPE L2_GetCurrentTor(L2_INST* instPtr);
uint32_t L2_GetNextFrameNum(L2_INST* instPtr);
uint32_t L2_GetNextSlotNum(L2_INST* instPtr);
L2_FRAME_TYPE L2_GetNextFrameType(L2_INST* instPtr);
L2_TOR_TYPE L2_GetNextTor(L2_INST* instPtr);
uint32_t L2_GetLastFrameNum(L2_INST *instPtr);
uint32_t L2_GetLastSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetLastFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetLastTor(L2_INST *instPtr);
uint32_t L2_GetCurrentFrameNum(L2_INST *instPtr);
uint32_t L2_GetCurrentSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetCurrentFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetCurrentTor(L2_INST *instPtr);
uint32_t L2_GetNextFrameNum(L2_INST *instPtr);
uint32_t L2_GetNextSlotNum(L2_INST *instPtr);
L2_FRAME_TYPE L2_GetNextFrameType(L2_INST *instPtr);
L2_TOR_TYPE L2_GetNextTor(L2_INST *instPtr);
// Time syncronization
int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum, bool relative);
int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative);
// RTT
int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr);
void RTT_Reset(L2_RTT_FSM* rttFsmPtr);
void RTT_Destroy(L2_RTT_FSM* rttFsmPtr);
int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType);
int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr);
void RTT_Reset(L2_RTT_FSM *rttFsmPtr);
void RTT_Destroy(L2_RTT_FSM *rttFsmPtr);
int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType);
#endif /* LINKLAYER814_H_ */
datalink814_ge/src/linklayer/linklayer814_types.h
+84 -64
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: A brief description of this file
*
*
* Date: 2022-08-28 22:53
* LastEditors: Fan Hao
* LastEditTime: 2022-08-29 00:45
* Revisions: Rev x.x
* History:
* History:
* [YYYY-MM-DD]: Description of modification.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
******************************************************************************/
*
******************************************************************************/
#ifndef LINKLAYER814_DEF_H_
#define LINKLAYER814_DEF_H_
@@ -24,8 +24,8 @@
#include "../phy_ctrl/phy_ctrl.h"
/************************** Constant Definitions *****************************/
// Common node ID definition
#define L2_BROADCAST_ID 0x1F
#define L2_NONE_ID 0
#define L2_BROADCAST_ID 0x1F
#define L2_NONE_ID 0
// for MPC Configure
#define L2_MSG_STROAGE_BUFF_MAX 16384 // 16K Bytes
@@ -42,18 +42,19 @@
#define L2_FRAMENO_NONE 0
// Common number of slots definition
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define L2_SLOTS_PER_FRAME 200
#define L2_SLOTS_PER_SUBFRAME 20
#define L2_NUM_SLOTS_NONE UINT32_MAX
#define L2_NUM_SLOTS_20MS 4
#define L2_NUM_SLOTS_10MS 2
#define L2_NUM_SLOTS_5MS 1
#define L2_NUM_SLOTS_SKIP 0
#define L2_NUM_SLOTS_20MS 4
#define L2_NUM_SLOTS_10MS 2
#define L2_NUM_SLOTS_5MS 1
#define L2_NUM_SLOTS_SKIP 0
/**************************** Type Definitions *******************************/
typedef struct {
typedef struct
{
uint32_t last;
uint32_t curr;
uint32_t next;
@@ -61,7 +62,8 @@ typedef struct {
/** @name Tx/Rx mode code
*/
typedef enum {
typedef enum
{
L2_TOR_SUSPEND = 0,
L2_TOR_TX = 1,
L2_TOR_RX = 2
@@ -69,7 +71,8 @@ typedef enum {
/** @name Prior information type definition
*/
typedef enum {
typedef enum
{
L2_NODE_NONE = 0,
L2_NODE_HEAD = 1,
L2_NODE_MEMBER = 2,
@@ -78,16 +81,18 @@ typedef enum {
/** @name Node state type definition
*/
typedef enum {
L2_NONE_STATE = 0,
L2_INIT_STATE = 1,
L2_NORMAL_STATE = 2,
L2_INTRA_SEEK_STATE = 3,
typedef enum
{
L2_NONE_STATE = 0,
L2_INIT_STATE = 1,
L2_NORMAL_STATE = 2,
L2_INTRA_SEEK_STATE = 3,
L2_INTER_SEEK_STATE = 4,
L2_SEEK_STATE = 5
} L2_NODE_STATE;
typedef struct {
typedef struct
{
uint8_t nodeId;
uint8_t clustId;
uint8_t headId;
@@ -100,7 +105,8 @@ typedef struct {
/** @name L2 event enum
* A enum of L2 event types.
*/
typedef enum {
typedef enum
{
L2_EV_NONE = 0,
L2_EV_SCAN_PROC_DONE = 1,
L2_EV_MGMT_PROC_DONE = 2,
@@ -120,15 +126,17 @@ typedef enum {
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
L2_EVENT_TYPE event;
void* msg;
void *msg;
} L2_EVENT;
/** @name Slot types
* A definition of different slots.
*/
typedef enum {
typedef enum
{
L2_SLOT_NONE = 0,
L2_SLOT_IDLE = 1,
L2_SLOT_INTER_SCAN_ACQ = 2,
@@ -148,7 +156,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_CRC_NONE = 0,
L2_CRC16_1021 = 1,
L2_CRC16_8005 = 2,
@@ -158,7 +167,8 @@ typedef enum {
/** @name Frame types
* A definition of different frame types.
*/
typedef enum {
typedef enum
{
L2_NONE_FRAME = 0,
L2_INTER_SCAN_FRAME = 1,
L2_INTRA_SCAN_FRAME = 2,
@@ -177,7 +187,8 @@ typedef enum {
/** @name Schedule subtable entry
* A definition of Schedule subtable entry.
*/
typedef struct {
typedef struct
{
L2_SLOT_TYPE slotType : 8;
uint8_t slotSeq;
uint8_t src0;
@@ -186,8 +197,9 @@ typedef struct {
uint8_t dst1;
} L2_RTNE_TABLE_ENTRY;
// Scan net
typedef struct {
// Scan net
typedef struct
{
uint8_t NodeId;
uint8_t ClusterId;
uint32_t frameNum;
@@ -200,73 +212,79 @@ typedef struct {
uint32_t Latitude;
} L2_BM_LOG;
typedef struct {
uint8_t NodeId;
uint8_t ClusterId;
typedef struct
{
uint8_t NodeId;
uint8_t ClusterId;
uint16_t EastSpeed;
uint16_t NorthSpeed;
uint16_t UpperSpeed;
uint32_t Height;
uint32_t Longitude;
uint32_t Latitude;
L2_NODE_STATE NetStage;
L2_NODE_STATE NetStage;
uint32_t NetScanTime;
} L2_W2ACQ_LOG;
typedef enum {
typedef enum
{
L2_SCAN_NONE_STAGE = 0,
L2_SCAN_INIT_STAGE = 1,
L2_SCAN_BMADVT_STAGE = 2, // Benchmark advertisement
L2_SCANT_2WAYACQ_STAGE = 3
} SCAN_STAGE;
typedef enum {
typedef enum
{
L2_SCAN_NONE_MODE = 0,
L2_SCAN_INTRA_MODE = 1,
L2_SCAN_INTER_MODE = 2
} L2_SCAN_MODE;
typedef struct {
bool isLocked; // init acq done.
bool isBmRcved;// bm received.
bool isAcked; // 2way acq done.
typedef struct
{
bool isLocked; // init acq done.
bool isBmRcved; // bm received.
bool isAcked; // 2way acq done.
uint32_t scanCnt;
uint32_t bmTimeout;
uint32_t w2acqTimeout;
SCAN_STAGE scanStage; // only for member node
L2_SCAN_MODE scanMode; // inter sanc or intra scan
SCAN_STAGE scanStage; // only for member node
L2_SCAN_MODE scanMode; // inter sanc or intra scan
L2_BM_LOG BMrcvLog;
L2_W2ACQ_LOG W2ACQrcvLog;
} L2_SCAN_FSM;
// Service Net
typedef struct {
typedef struct
{
bool isBisar;
bool isBisarEven; // Send frame at even slot at bisar mode.
uint8_t bisarDst;
uint32_t bisarEndframe;
} L2_SVC_FSM;
// Management net
typedef struct {
uint8_t wordTag ;
uint8_t msgTag ;
typedef struct
{
uint8_t wordTag;
uint8_t msgTag;
uint8_t msgSubTag;
uint8_t srcNodeId ;
uint8_t quitNodeId ;
uint8_t quitCtrlWord ;
uint8_t srcNodeId;
uint8_t quitNodeId;
uint8_t quitCtrlWord;
uint8_t nextNetState;
uint16_t EastSpeed ;
uint16_t EastSpeed;
uint16_t NorthSpeed;
uint16_t UpperSpeed;
uint32_t Height ;
uint32_t Longitude ;
uint32_t Latitude ;
uint32_t Height;
uint32_t Longitude;
uint32_t Latitude;
bool isValid;
} L2_MGMT_MSG_RECORD;
typedef struct {
typedef struct
{
uint16_t wordTag;
uint16_t msgTag;
uint16_t msgSubTag;
@@ -275,10 +293,11 @@ typedef struct {
uint16_t dstAddr;
uint32_t timeWordL;
uint32_t timeWordH;
int32_t doppler;
int32_t doppler;
} L2_RTT_MSG_RECORD;
typedef struct {
typedef struct
{
bool isBisarProc;
bool isPendTimeAlloc;
bool isPendSlotAlloc;
@@ -287,13 +306,14 @@ typedef struct {
L2_RTT_MSG_RECORD rttMsgReqLog;
} L2_MGMT_FSM;
typedef struct{
L2_RTNE_TABLE_ENTRY** rtneTable;
typedef struct
{
L2_RTNE_TABLE_ENTRY **rtneTable;
uint32_t rtnePrd;
L2_RTNE_TABLE_ENTRY* last;
L2_RTNE_TABLE_ENTRY* curr;
L2_RTNE_TABLE_ENTRY* next;
}L2_RTNE_CONFIG;
L2_RTNE_TABLE_ENTRY *last;
L2_RTNE_TABLE_ENTRY *curr;
L2_RTNE_TABLE_ENTRY *next;
} L2_RTNE_CONFIG;
/************************** Variable Definitions *****************************/
#endif /* LINKLAYER814_H_ */
datalink814_ge/src/linklayer/linklayer_routine.c
+1271 -686
View File
File diff suppressed because it is too large Load Diff
datalink814_ge/src/linklayer/msg_def.h
+32 -33
View File
@@ -16,7 +16,7 @@
#ifndef MSG_DEF_H_
#define MSG_DEF_H_
#define L2_MSG_LEN 72 // bit
#define L2_MSG_LEN 72 // bit
#define L2_MSG_MAX 512 // bit
#define PDU_CRC_LEN 16
// ** 77 Basic Header 1 Format ** //
@@ -141,19 +141,19 @@
// ** 83 Net Scan Message - Continue 1 ** //
#define NS_MSG_CONT_1_RESV_LEN 2
#define NS_MSG_CONT_1_RESV_OFFSET 0
#define NS_MSG_CONT_1_RESV_OFFSET 0
#define NS_MSG_CONT_1_WTAG_LEN 2
#define NS_MSG_CONT_1_WTAG_OFFSET 2
#define NS_MSG_CONT_1_CONTID_LEN 4
#define NS_MSG_CONT_1_CONTID_OFFSET 4
#define NS_MSG_CONT_1_TIME_TAG_LEN 48
#define NS_MSG_CONT_1_TIME_TAG_OFFSET 8
#define NS_MSG_CONT_1_TIME_TAG_OFFSET 8
#define NS_MSG_CONT_1_EAST_SPEED_LEN 16
#define NS_MSG_CONT_1_EAST_SPEED_OFFSET 56
// ** 84 Net Scan Message - Continue 2 ** //
#define NS_MSG_CONT_2_RESV_1_LEN 2
#define NS_MSG_CONT_2_RESV_1_OFFSET 0
#define NS_MSG_CONT_2_RESV_1_OFFSET 0
#define NS_MSG_CONT_2_WTAG_LEN 2
#define NS_MSG_CONT_2_WTAG_OFFSET 2
#define NS_MSG_CONT_2_CONTID_LEN 4
@@ -169,7 +169,7 @@
// ** 85 Net Scan Message - Continue 3 ** //
#define NS_MSG_CONT_3_RESV_LEN 2
#define NS_MSG_CONT_3_RESV_OFFSET 0
#define NS_MSG_CONT_3_RESV_OFFSET 0
#define NS_MSG_CONT_3_WTAG_LEN 2
#define NS_MSG_CONT_3_WTAG_OFFSET 2
#define NS_MSG_CONT_3_CONTID_LEN 4
@@ -181,7 +181,7 @@
// ** 86 Net Scan Message - Continue 4 ** //
#define NS_MSG_CONT_4_RESV_LEN 2
#define NS_MSG_CONT_4_RESV_OFFSET 0
#define NS_MSG_CONT_4_RESV_OFFSET 0
#define NS_MSG_CONT_4_WTAG_LEN 2
#define NS_MSG_CONT_4_WTAG_OFFSET 2
#define NS_MSG_CONT_4_CONTID_LEN 4
@@ -193,7 +193,7 @@
// ** 87 Net Scan Message - Continue 5 ** //
#define NS_MSG_CONT_5_RESV_1_LEN 2
#define NS_MSG_CONT_5_RESV_1_OFFSET 0
#define NS_MSG_CONT_5_RESV_1_OFFSET 0
#define NS_MSG_CONT_5_WTAG_LEN 2
#define NS_MSG_CONT_5_WTAG_OFFSET 2
#define NS_MSG_CONT_5_CONTID_LEN 4
@@ -220,13 +220,13 @@
#define TK_MSG_INI_TQLT_LEN 4
#define TK_MSG_INI_TQLT_OFFSET 19
#define TK_MSG_INI_TBASE_ID_INDICATE_LEN 1
#define TK_MSG_INI_TBASE_ID_INDICATE_OFFSET 23
#define TK_MSG_INI_TBASE_ID_INDICATE_OFFSET 23
#define TK_MSG_INI_CSCALE_NUM_LEN 4
#define TK_MSG_INI_CSCALE_NUM_OFFSET 24
#define TK_MSG_INI_MEMBER_NUM_LEN 4
#define TK_MSG_INI_MEMBER_NUM_OFFSET 28
#define TK_MSG_INI_MUTE_TAG_LEN 1
#define TK_MSG_INI_MUTE_TAG_OFFSET 32
#define TK_MSG_INI_MUTE_TAG_OFFSET 32
#define TK_MSG_INI_NET_STATE_LEN 4
#define TK_MSG_INI_NET_STATE_OFFSET 33
#define TK_MSG_INI_RESV_LEN 35
@@ -290,7 +290,6 @@
#define TK_MSG_CONT_3_UPPER_ACC_LEN 16
#define TK_MSG_CONT_3_UPPER_ACC_OFFSET 56
// ** 94 Time Announcements Initial ** //
#define TA_MSG_SER_TAG_LEN 2
#define TA_MSG_SER_TAG_OFFSET 0
@@ -339,7 +338,6 @@
#define TA_MSG_CONT_1_RESV1_LEN 10
#define TA_MSG_CONT_1_RESV1_OFFSET 62
// ** 97 Time Announcements Continue 2 ** //
#define TA_MSG_CONT_2_RESV0_LEN 2
#define TA_MSG_CONT_2_RESV0_OFFSET 0
@@ -351,8 +349,8 @@
#define TA_MSG_CONT_2_EAST_OFFSET 8
#define TA_MSG_CONT_2_NORTH_LEN 16
#define TA_MSG_CONT_2_NORTH_OFFSET 24
#define TA_MSG_CONT_2_UPPER_LEN 16
#define TA_MSG_CONT_2_UPPER_OFFSET 40
#define TA_MSG_CONT_2_UPPER_LEN 16
#define TA_MSG_CONT_2_UPPER_OFFSET 40
#define TA_MSG_CONT_2_RESV1_LEN 16
#define TA_MSG_CONT_2_RESV1_OFFSET 56
@@ -443,7 +441,6 @@
#define RTT_REQ_CONT_RESV1_LEN 40
#define RTT_REQ_CONT_RESV1_OFFSET 32
// ** 101 RTT Response Initial Word ** //
#define RTT_RSP_SER_TAG_LEN 2
#define RTT_RSP_SER_TAG_OFFSET 0
@@ -546,7 +543,8 @@
#define BCU_MSG_RESV_LEN 19
#define BCU_MSG_RESV_OFFSET 53
typedef enum{
typedef enum
{
L2_MSG_NONE = 0,
L2_MSG_BM = 1,
L2_MSG_SCAN_INIT = 2,
@@ -577,32 +575,33 @@ typedef enum{
L2_MSG_BCU_INIT = 27,
L2_MSG_LINK_INIT = 28,
L2_MSG_LINK_CONT_1 = 29
}L2_MSG_TYPE;
} L2_MSG_TYPE;
typedef enum{
typedef enum
{
L2_PDU_HEADER_NONE = 0,
L2_PDU_HEADER_0 = 1,
L2_PDU_HEADER_1 = 2
}L2_PDU_HEADER_TYPE;
} L2_PDU_HEADER_TYPE;
// Intra-cluster frames
#define INTRA_BM_FRAME_LENGTH_MAX (232/8) // byte
#define INTRA_W2ACQ_FRAME_LENGTH_MAX (2040/8) // byte
#define INTRA_MGMT_FRAME_LENGTH_MAX (2040/8) // byte
#define INTRA_TA_FRAME_LENGTH_MAX (2040/8) // byte
#define INTRA_SVC_FRAME_LENGTH_MAX (2040/8) // byte
#define INTRA_RTT_FRAME_LENGTH_MAX (2040/8) // byte
#define INTRA_BM_FRAME_LENGTH_MAX (232 / 8) // byte
#define INTRA_W2ACQ_FRAME_LENGTH_MAX (2040 / 8) // byte
#define INTRA_MGMT_FRAME_LENGTH_MAX (2040 / 8) // byte
#define INTRA_TA_FRAME_LENGTH_MAX (2040 / 8) // byte
#define INTRA_SVC_FRAME_LENGTH_MAX (2040 / 8) // byte
#define INTRA_RTT_FRAME_LENGTH_MAX (2040 / 8) // byte
// Inter-cluster frames
#define INTER_BM_FRAME_LENGTH_MAX (312/8) // byte
#define INTER_W2ACQ_FRAME_LENGTH_MAX (712/8) // byte
#define W2ACQ_FRAME_CRC_DLEN ((NS_MSG_LEN*4)/8) //byte
#define W2ACQ_FRAME_CRC_POS ((NS_MSG_LEN*4 + NS_MSG_CONT_4_CRC_OFFSET)/8)// byte
#define INTER_BM_FRAME_LENGTH_MAX (312 / 8) // byte
#define INTER_W2ACQ_FRAME_LENGTH_MAX (712 / 8) // byte
#define W2ACQ_FRAME_CRC_DLEN ((NS_MSG_LEN * 4) / 8) // byte
#define W2ACQ_FRAME_CRC_POS ((NS_MSG_LEN * 4 + NS_MSG_CONT_4_CRC_OFFSET) / 8) // byte
#define INTER_MGMT_FRAME_LENGTH_MAX (2152/8) // byte
#define INTER_TA_FRAME_LENGTH_MAX (2152/8) // byte
#define INTER_SVC_FRAME_LENGTH_MAX (2152/8) // byte
#define INTER_RTT_FRAME_LENGTH_MAX (2152/8) // byte
#define INTER_MGMT_FRAME_LENGTH_MAX (2152 / 8) // byte
#define INTER_TA_FRAME_LENGTH_MAX (2152 / 8) // byte
#define INTER_SVC_FRAME_LENGTH_MAX (2152 / 8) // byte
#define INTER_RTT_FRAME_LENGTH_MAX (2152 / 8) // byte
extern L2_MSG_TYPE intraBmFrameMsgs[1];
extern L2_MSG_TYPE intraW2AcqFrameMsgs[6];
@@ -611,4 +610,4 @@ extern L2_MSG_TYPE taFrameMsgs[4];
extern L2_MSG_TYPE interSvcFrameMsgs[2];
extern L2_MSG_TYPE rttReqFrameMsgs[3];
extern L2_MSG_TYPE rttRspFrameMsgs[3];
#endif //MSG_DEF_H_
#endif // MSG_DEF_H_
datalink814_ge/src/linklayer/net_mgmt.c
+254 -191
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Implementations of 814 linklayer management net.
*
*
* Date: 2021-11-18 17:27
* LastEditors: Fan Hao
* LastEditTime: 2022-01-16 15:52
* Revisions: Rev 0.1
* History:
* History:
* [2022-07-16]: New branch for DiJian.
* [2021-11-18]: Initial version.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
/***************************** Include Files *********************************/
@@ -25,19 +25,20 @@
#include "../common.h"
#include "node_list.h"
/************************** Constant Definitions *****************************/
#define SQUENCE_MGMT_SLOTS 0
#define INTER_MGMT_SLOTS 2
#define INTER_TSYNC_SLOTS 2
#define INTRA_MGMT_SLOTS 1
#define INTRA_TSYNC_SLOTS 1
#define NONE_MGMT_SLOTS -1
#define CONX_LOST_TH 10 // 10 count of mgmt msg no response to delete
#define TIME2QUIT 80 // time to send quit message
#define SQUENCE_MGMT_SLOTS 0
#define INTER_MGMT_SLOTS 2
#define INTER_TSYNC_SLOTS 2
#define INTRA_MGMT_SLOTS 1
#define INTRA_TSYNC_SLOTS 1
#define NONE_MGMT_SLOTS -1
#define CONX_LOST_TH 10 // 10 count of mgmt msg no response to delete
#define TIME2QUIT 80 // time to send quit message
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
MGMTPKT_NONE = 0,
MGMTPKT_INTER_MGMT = 1,
MGMTPKT_INTRA_MGMT = 2,
@@ -49,8 +50,8 @@ typedef enum {
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType);
static int32_t MgmtMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType);
static int32_t MgmtPkt_Packing(L2_INST *instPtr, MGMTPKT_TYPE pktType);
/*****************************************************************************/
/**
* @brief Packing management message to a bit array.
@@ -63,25 +64,32 @@ static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType);
* SUCCESS
*
******************************************************************************/
static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
static int32_t MgmtMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType)
{
ASSERT(instPtr != NULL);
ASSERT(bitarr != NULL);
uint64_t dataChunk;
L2_RTT_FSM* rttFsmPtr = L2_GetRttFsm(instPtr);
if (msgType < L2_MSG_TK_INIT || msgType > L2_MSG_BCU_INIT) {
L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
if (msgType < L2_MSG_TK_INIT || msgType > L2_MSG_BCU_INIT)
{
return EPARAM;
}
if(msgType >= L2_MSG_TK_INIT && msgType <= L2_MSG_MADJ_EXTD) {
if (bitarr->num_of_bits < MGMT_MSG_LEN) {
if (msgType >= L2_MSG_TK_INIT && msgType <= L2_MSG_MADJ_EXTD)
{
if (bitarr->num_of_bits < MGMT_MSG_LEN)
{
return EPARAM;
}
}
if(msgType >= L2_MSG_RTT_REQ_INIT && msgType <= L2_MSG_RTT_RSP_CONT) {
if (bitarr->num_of_bits < RTT_MSG_LEN) {
if (msgType >= L2_MSG_RTT_REQ_INIT && msgType <= L2_MSG_RTT_RSP_CONT)
{
if (bitarr->num_of_bits < RTT_MSG_LEN)
{
return EPARAM;
}
}
switch (msgType) {
switch (msgType)
{
case L2_MSG_TK_INIT:
// Series tag
dataChunk = 0;
@@ -104,8 +112,10 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, TK_MSG_INI_OFFSET, TK_MSG_INI_LEN, dataChunk);
// Next net stage
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
switch (instPtr->nextState) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
switch (instPtr->nextState)
{
case L2_INIT_STATE:
dataChunk = NET_INI_STAGE;
break;
@@ -159,7 +169,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// Upper speed
dataChunk = 0xDDDD;
FrameInsertChunk(bitarr, TK_MSG_CONT_1_UPPER_SPEED_OFFSET,
TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
TK_MSG_CONT_1_UPPER_SPEED_LEN, dataChunk);
// Height
dataChunk = 0xEEEEEE;
@@ -226,7 +236,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, TA_MSG_MSG_STAG_OFFSET, TA_MSG_MSG_STAG_LEN, dataChunk);
// Source node id
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, TA_MSG_SRC_ADDR_OFFSET, TA_MSG_SRC_ADDR_LEN, dataChunk);
// Height
@@ -299,7 +309,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, dataChunk);
// destination node address.
dataChunk = (0x02) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (0x02) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, dataChunk);
break;
@@ -335,7 +345,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// message identification.
dataChunk = 29;
FrameInsertChunk(bitarr, RTT_RSP_MSG_TAG_OFFSET,
RTT_RSP_MSG_TAG_LEN, dataChunk);
RTT_RSP_MSG_TAG_LEN, dataChunk);
// message identification.
dataChunk = 2; // response
@@ -350,7 +360,7 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, dataChunk);
// destination node address.
dataChunk = (0x01) | (((uint16_t) (L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (0x01) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, dataChunk);
break;
@@ -404,13 +414,13 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
// dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
// FrameInsertChunk(bitarr, NQT_MSG_OBJ_OFFSET, NQT_MSG_OBJ_LEN, dataChunk);
// L2_NODE_STATE newState = L2_INIT_STATE;
// L2_EVENT stateEvent = {
// .event = L2_EV_MAIN_STATE_UPDATE,
// .msg = &newState
// L2_EVENT stateEvent = {
// .event = L2_EV_MAIN_STATE_UPDATE,
// .msg = &newState
// };
// AssertL2Event(instPtr, &stateEvent);
// L2_EVENT lostCxnEvent = {
// .event = L2_EV_LOST_CXN,
// L2_EVENT lostCxnEvent = {
// .event = L2_EV_LOST_CXN,
// .msg = NULL
// };
// AssertL2Event(instPtr, &lostCxnEvent);
@@ -430,7 +440,6 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Packing management message to a bit array.
@@ -442,64 +451,79 @@ static int32_t MgmtMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
* error code
*
******************************************************************************/
static int32_t MgmtPkt_Packing(L2_INST* instPtr, MGMTPKT_TYPE pktType) {
static int32_t MgmtPkt_Packing(L2_INST *instPtr, MGMTPKT_TYPE pktType)
{
ASSERT(instPtr != NULL);
uint8_t* buff = instPtr->sendPktData;
uint8_t *buff = instPtr->sendPktData;
uint32_t filedSize = 0;
BIT_ARRAY* mgmtPkt = bit_array_create(L2_MSG_MAX);
if (mgmtPkt == NULL) {
BIT_ARRAY *mgmtPkt = bit_array_create(L2_MSG_MAX);
if (mgmtPkt == NULL)
{
DBG_ERROR("Failed to allocate memory for scanMsg.\n");
return FAILURE;
}
switch (pktType) {
switch (pktType)
{
case MGMTPKT_INTRA_MGMT:
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, intraMgmtFrameMsgs[i]) == SUCCESS) {
memcpy((uint8_t*)buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words, (size_t) (MGMT_MSG_LEN / 8));
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, intraMgmtFrameMsgs[i]) == SUCCESS)
{
memcpy((uint8_t *)buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words, (size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing intraMgmtFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
}
break;
case MGMTPKT_INTRA_RTTREQ:
for(int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttReqFrameMsgs[i]) == SUCCESS) {
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttReqFrameMsgs[i]) == SUCCESS)
{
memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing rttReqFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
}
break;
case MGMTPKT_INTRA_RTTRSP:
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttRspFrameMsgs[i]) == SUCCESS) {
memcpy(buff + (size_t) (i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t) (MGMT_MSG_LEN / 8));
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++)
{
if (MgmtMsg_Packing(instPtr, mgmtPkt, rttRspFrameMsgs[i]) == SUCCESS)
{
memcpy(buff + (size_t)(i * (MGMT_MSG_LEN / 8)),
mgmtPkt->words,
(size_t)(MGMT_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing rttRspFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
@@ -519,16 +543,18 @@ failed:
/*****************************************************************************/
/**
* @brief initialize the management FSM instance.
*
*
* @param mgmtFSM A second rank pointer to management FSM instance.
*
*
* @return SUCCESS Initialize successfully
* EALLOC Memory allocation error.
*
*
******************************************************************************/
int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM) {
(*mgmtFSM) = (L2_MGMT_FSM*) malloc(sizeof(L2_MGMT_FSM));
if (*mgmtFSM == NULL) {
int32_t MgmtNet_Init(L2_MGMT_FSM **mgmtFSM)
{
(*mgmtFSM) = (L2_MGMT_FSM *)malloc(sizeof(L2_MGMT_FSM));
if (*mgmtFSM == NULL)
{
return EALLOC;
}
MgmtNet_Reset(*mgmtFSM);
@@ -544,7 +570,8 @@ int32_t MgmtNet_Init(L2_MGMT_FSM** mgmtFSM) {
* @return NONE
*
******************************************************************************/
void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
void MgmtNet_Reset(L2_MGMT_FSM *mgmtFSM)
{
mgmtFSM->isBisarProc = false;
mgmtFSM->isPendSlotAlloc = false;
mgmtFSM->isPendTimeAlloc = false;
@@ -553,25 +580,24 @@ void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
mgmtFSM->mgmtMsgLog.isValid = false;
mgmtFSM->mgmtMsgLog.quitCtrlWord = 0;
mgmtFSM->mgmtMsgLog.quitNodeId = 0;
mgmtFSM->mgmtMsgLog.msgSubTag = 0 ;
mgmtFSM->mgmtMsgLog.msgSubTag = 0;
mgmtFSM->mgmtMsgLog.msgTag = 0;
mgmtFSM->mgmtMsgLog.wordTag = 0;
mgmtFSM->mgmtMsgLog.srcNodeId = 0;
// Position value
mgmtFSM->mgmtMsgLog.EastSpeed = 0 ;
mgmtFSM->mgmtMsgLog.NorthSpeed = 0 ;
mgmtFSM->mgmtMsgLog.UpperSpeed = 0 ;
mgmtFSM->mgmtMsgLog.Height = 0 ;
mgmtFSM->mgmtMsgLog.Longitude = 0 ;
mgmtFSM->mgmtMsgLog.Latitude = 0 ;
mgmtFSM->mgmtMsgLog.EastSpeed = 0;
mgmtFSM->mgmtMsgLog.NorthSpeed = 0;
mgmtFSM->mgmtMsgLog.UpperSpeed = 0;
mgmtFSM->mgmtMsgLog.Height = 0;
mgmtFSM->mgmtMsgLog.Longitude = 0;
mgmtFSM->mgmtMsgLog.Latitude = 0;
// clear buffer.
memset(&(mgmtFSM->rttMsgRspLog), 0x00, sizeof(L2_RTT_MSG_RECORD));
memset(&(mgmtFSM->rttMsgReqLog), 0x00, sizeof(L2_RTT_MSG_RECORD));
}
/*****************************************************************************/
/**
* @brief Destroy management net FSM, free all allocated memory.
@@ -581,7 +607,8 @@ void MgmtNet_Reset(L2_MGMT_FSM* mgmtFSM) {
* @return NONE
*
******************************************************************************/
void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM){
void MgmtNet_Destroy(L2_MGMT_FSM *mgmtFSM)
{
free(mgmtFSM);
}
@@ -589,14 +616,15 @@ void MgmtNet_Destroy(L2_MGMT_FSM* mgmtFSM){
/**
* @brief Handles management net packet, apply time sync and other management
* net functions.
*
*
* @param instPtr a pointer to l2 instance.
*
*
* @return SUCCESS Packet was successfully handled.
* Other Packed was dumped(not addressed to me, unrecognized frame type)
*
*
******************************************************************************/
int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
int32_t MgmtNet_PktHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
int32_t Status;
uint32_t filedSize = 0;
@@ -607,85 +635,93 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
DBG_INFO("Management net handling packet...\n");
BIT_ARRAY* mgmtMsg = bit_array_create(L2_MSG_MAX);
if (mgmtMsg == NULL) {
BIT_ARRAY *mgmtMsg = bit_array_create(L2_MSG_MAX);
if (mgmtMsg == NULL)
{
DBG_INFO("Failed to start handling process...\n");
return EALLOC;
}
switch (L2_GetLastFrameType(instPtr)) {
switch (L2_GetLastFrameType(instPtr))
{
case L2_INTER_MGMT_FRAME:
case L2_INTRA_MGMT_FRAME: {
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++) {
case L2_INTRA_MGMT_FRAME:
{
for (int i = 0; i < ARRAY_SIZE(intraMgmtFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (MGMT_MSG_LEN / 8)),
(size_t) (MGMT_MSG_LEN / 8));
switch (intraMgmtFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (MGMT_MSG_LEN / 8)),
(size_t)(MGMT_MSG_LEN / 8));
switch (intraMgmtFrameMsgs[i])
{
case L2_MSG_TK_INIT:
FrameGetChunk(mgmtMsg, TK_MSG_INI_NET_STATE_OFFSET, TK_MSG_INI_NET_STATE_LEN, &dataChunk);
mgmtMsgLog->nextNetState = (uint8_t) (dataChunk);
mgmtMsgLog->nextNetState = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_WTAG_OFFSET, TK_MSG_INI_WTAG_LEN, &dataChunk);
mgmtMsgLog->wordTag = (uint8_t) (dataChunk);
mgmtMsgLog->wordTag = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_TAG_OFFSET, TK_MSG_INI_TAG_LEN, &dataChunk);
mgmtMsgLog->msgTag = (uint8_t) (dataChunk);
mgmtMsgLog->msgTag = (uint8_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_INI_STAG_OFFSET, TK_MSG_INI_STAG_LEN, &dataChunk);
mgmtMsgLog->msgSubTag = (uint8_t) (dataChunk);
mgmtMsgLog->msgSubTag = (uint8_t)(dataChunk);
// Preventing the error response caused by unexpected bit error.
if ( mgmtMsgLog->wordTag == 3
&& mgmtMsgLog->msgTag == 28
&& mgmtMsgLog->msgSubTag == 1 ) {
if (mgmtMsgLog->wordTag == 3 && mgmtMsgLog->msgTag == 28 && mgmtMsgLog->msgSubTag == 1)
{
mgmtMsgLog->isValid = true;
}
else {
mgmtMsgLog->isValid = false ;
else
{
mgmtMsgLog->isValid = false;
}
break;
case L2_MSG_TK_EXTD :
case L2_MSG_TK_EXTD:
FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t)(dataChunk);
break;
case L2_MSG_TK_CONT_1:
FrameGetChunk(mgmtMsg, TK_MSG_EXTEND_EAST_SPEED_OFFSET, TK_MSG_EXTEND_EAST_SPEED_LEN, &dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->EastSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_NORTH_SPEED_OFFSET, TK_MSG_CONT_1_NORTH_SPEED_LEN, &dataChunk);
mgmtMsgLog->NorthSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->NorthSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_UPPER_SPEED_OFFSET, TK_MSG_CONT_1_UPPER_SPEED_LEN, &dataChunk);
mgmtMsgLog->UpperSpeed = (uint16_t) (dataChunk);
mgmtMsgLog->UpperSpeed = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_1_HEIGHT_OFFSET, TK_MSG_CONT_1_HEIGHT_LEN, &dataChunk);
mgmtMsgLog->Height = (uint32_t) (dataChunk);
mgmtMsgLog->Height = (uint32_t)(dataChunk);
break;
case L2_MSG_TK_CONT_2:
FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LONGITUDE_OFFSET, TK_MSG_CONT_2_LONGITUDE_LEN, &dataChunk);
mgmtMsgLog->Longitude = (uint32_t) (dataChunk);
mgmtMsgLog->Longitude = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, TK_MSG_CONT_2_LATITUDE_OFFSET, TK_MSG_CONT_2_LATITUDE_LEN, &dataChunk);
mgmtMsgLog->Latitude = (uint32_t) (dataChunk);
mgmtMsgLog->Latitude = (uint32_t)(dataChunk);
break;
case L2_MSG_TK_CONT_3:
FrameGetChunk(mgmtMsg, TK_MSG_CONT_3_MEMBER_ADDR_OFFSET, TK_MSG_CONT_3_MEMBER_ADDR_LEN, &dataChunk);
mgmtMsgLog->srcNodeId = (uint8_t) (dataChunk & 0x1F);
mgmtMsgLog->srcNodeId = (uint8_t)(dataChunk & 0x1F);
break;
case L2_MSG_NQT_INIT:
FrameGetChunk(mgmtMsg, NQT_MSG_OBJ_OFFSET, NQT_MSG_OBJ_LEN, &dataChunk);
mgmtMsgLog->quitNodeId = (uint8_t) (dataChunk & 0x1F);
if (mgmtMsgLog->quitNodeId != L2_GetNodeId(instPtr)) {
mgmtMsgLog->quitNodeId = (uint8_t)(dataChunk & 0x1F);
if (mgmtMsgLog->quitNodeId != L2_GetNodeId(instPtr))
{
FrameGetChunk(mgmtMsg, NQT_MSG_CTRL_MUTE_OFFSET, NQT_MSG_CTRL_MUTE_LEN, &dataChunk);
mgmtMsgLog->quitCtrlWord = (uint8_t)(dataChunk & 0x7F);
if (mgmtMsgLog->quitCtrlWord == 0x01) {
if (mgmtMsgLog->quitCtrlWord == 0x01)
{
L2_DeleteNodeById(instPtr->nodeList, mgmtMsgLog->quitNodeId);
}
}
else {
else
{
DBG_WARNING("Ignore node quit request(ID not mine), target node id is [%d]\n", mgmtMsgLog->quitNodeId);
}
break;
@@ -695,16 +731,19 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += MGMT_MSG_LEN;
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_MGMT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
}
}
// Member Node update state with received message
if(L2_GetNodeId(instPtr) != L2_NODE_HEAD && mgmtMsgLog->isValid == true ){
if (L2_GetNodeId(instPtr) != L2_NODE_HEAD && mgmtMsgLog->isValid == true)
{
L2_NODE_STATE newState = L2_NONE_STATE;
switch (mgmtMsgLog->nextNetState) {
switch (mgmtMsgLog->nextNetState)
{
case NET_INI_STAGE:
newState = L2_INIT_STATE;
break;
@@ -727,57 +766,61 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
}
L2_EVENT stateUpEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateUpEvent);
}
L2_RstNodeTTL(instPtr->nodeList, mgmtMsgLog->srcNodeId);
// TODO: Check
if(mgmtMsgLog->srcNodeId == L2_GetHeadId(instPtr)){
if (mgmtMsgLog->srcNodeId == L2_GetHeadId(instPtr))
{
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
}
break;
}
case L2_INTER_TSYNC_FRAME:
case L2_INTRA_TSYNC_FRAME: {
if(L2_GetNodeType(instPtr) == L2_NODE_HEAD){
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++) {
case L2_INTRA_TSYNC_FRAME:
{
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
for (int i = 0; i < ARRAY_SIZE(rttReqFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
(size_t) (RTT_MSG_LEN / 8));
switch (rttReqFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (RTT_MSG_LEN / 8)),
(size_t)(RTT_MSG_LEN / 8));
switch (rttReqFrameMsgs[i])
{
case L2_MSG_RTT_REQ_INIT:
FrameGetChunk(mgmtMsg, RTT_REQ_WTAG_OFFSET, RTT_REQ_WTAG_LEN, &dataChunk);
rttMsgReqLog->wordTag = (uint16_t) (dataChunk);
rttMsgReqLog->wordTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_TAG_OFFSET, RTT_REQ_MSG_TAG_LEN, &dataChunk);
rttMsgReqLog->msgTag = (uint16_t) (dataChunk);
rttMsgReqLog->msgTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_STAG_OFFSET, RTT_REQ_MSG_STAG_LEN, &dataChunk);
rttMsgReqLog->msgSubTag = (uint16_t) (dataChunk);
rttMsgReqLog->msgSubTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_MSG_OFFSET, RTT_REQ_MSG_LEN, &dataChunk);
rttMsgReqLog->msgLen = (uint16_t) (dataChunk);
rttMsgReqLog->msgLen = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_SRC_ADDR_OFFSET, RTT_REQ_SRC_ADDR_LEN, &dataChunk);
rttMsgReqLog->srcAddr = (uint16_t) (dataChunk);
rttMsgReqLog->srcAddr = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_DST_ADDR_OFFSET, RTT_REQ_DST_ADDR_LEN, &dataChunk);
rttMsgReqLog->dstAddr = (uint16_t) (dataChunk);
rttMsgReqLog->dstAddr = (uint16_t)(dataChunk);
break;
case L2_MSG_RTT_REQ_EXTD:
FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGL_OFFSET, RTT_REQ_EX_TTAGL_LEN, &dataChunk);
rttMsgReqLog->timeWordL = (uint32_t) (dataChunk);
rttMsgReqLog->timeWordL = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_REQ_EX_TTAGH_OFFSET, RTT_REQ_EX_TTAGH_LEN, &dataChunk);
rttMsgReqLog->timeWordH = (uint32_t) (dataChunk);
rttMsgReqLog->timeWordH = (uint32_t)(dataChunk);
break;
case L2_MSG_RTT_REQ_CONT:
FrameGetChunk(mgmtMsg, RTT_REQ_CONT_DOP_OFFSET, RTT_REQ_CONT_DOP_LEN, &dataChunk);
rttMsgReqLog->doppler = (int32_t) (dataChunk);
rttMsgReqLog->doppler = (int32_t)(dataChunk);
break;
default:
@@ -786,51 +829,55 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += RTT_MSG_LEN;
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
}
}
}
else {
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++) {
else
{
for (int i = 0; i < ARRAY_SIZE(rttRspFrameMsgs); i++)
{
memcpy(mgmtMsg->words,
instPtr->recvPktData + (size_t) (i * (RTT_MSG_LEN / 8)),
(size_t) (RTT_MSG_LEN / 8));
switch (rttRspFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (RTT_MSG_LEN / 8)),
(size_t)(RTT_MSG_LEN / 8));
switch (rttRspFrameMsgs[i])
{
case L2_MSG_RTT_RSP_INIT:
FrameGetChunk(mgmtMsg, RTT_RSP_WTAG_OFFSET, RTT_RSP_WTAG_LEN, &dataChunk);
rttMsgRspLog->wordTag = (uint16_t) (dataChunk);
rttMsgRspLog->wordTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_TAG_OFFSET, RTT_RSP_MSG_TAG_LEN, &dataChunk);
rttMsgRspLog->msgTag = (uint16_t) (dataChunk);
rttMsgRspLog->msgTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_STAG_OFFSET, RTT_RSP_MSG_STAG_LEN, &dataChunk);
rttMsgRspLog->msgSubTag = (uint16_t) (dataChunk);
rttMsgRspLog->msgSubTag = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_MSG_OFFSET, RTT_RSP_MSG_LEN, &dataChunk);
rttMsgRspLog->msgLen = (uint16_t) (dataChunk);
rttMsgRspLog->msgLen = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_SRC_ADDR_OFFSET, RTT_RSP_SRC_ADDR_LEN, &dataChunk);
rttMsgRspLog->srcAddr = (uint16_t) (dataChunk);
rttMsgRspLog->srcAddr = (uint16_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_DST_ADDR_OFFSET, RTT_RSP_DST_ADDR_LEN, &dataChunk);
rttMsgRspLog->dstAddr = (uint16_t) (dataChunk);
rttMsgRspLog->dstAddr = (uint16_t)(dataChunk);
break;
case L2_MSG_RTT_RSP_EXTD:
FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGL_OFFSET, RTT_RSP_EX_TTAGL_LEN, &dataChunk);
rttMsgRspLog->timeWordL = (uint32_t) (dataChunk);
rttMsgRspLog->timeWordL = (uint32_t)(dataChunk);
FrameGetChunk(mgmtMsg, RTT_RSP_EX_TTAGH_OFFSET, RTT_RSP_EX_TTAGH_LEN, &dataChunk);
rttMsgRspLog->timeWordH = (uint32_t) (dataChunk);
rttMsgRspLog->timeWordH = (uint32_t)(dataChunk);
break;
case L2_MSG_RTT_RSP_CONT:
FrameGetChunk(mgmtMsg, RTT_RSP_CONT_DOP_OFFSET,
RTT_RSP_CONT_DOP_LEN, &dataChunk);
rttMsgRspLog->doppler = (int32_t) (dataChunk);
RTT_RSP_CONT_DOP_LEN, &dataChunk);
rttMsgRspLog->doppler = (int32_t)(dataChunk);
break;
default:
@@ -839,7 +886,8 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
filedSize += RTT_MSG_LEN;
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_RTT_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message decapsulate exceeds frame length.\n");
Status = EPARAM;
goto failed;
@@ -849,7 +897,8 @@ int32_t MgmtNet_PktHandler(L2_INST* instPtr) {
L2_RstNodeTTL(instPtr->nodeList, rttMsgRspLog->srcAddr & 0x001F);
// TODO: Check
if((rttMsgRspLog->srcAddr&0x001F) == L2_GetHeadId(instPtr)){
if ((rttMsgRspLog->srcAddr & 0x001F) == L2_GetHeadId(instPtr))
{
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
}
break;
@@ -866,34 +915,35 @@ failed:
return Status;
}
/**
* @brief Helper function for check node TTL.
* @param entry
* @return trur/false
*/
static bool IsTTLValid(NODE_ENTRY* entry) {
static bool IsTTLValid(NODE_ENTRY *entry)
{
ASSERT(entry != NULL);
return (entry->ttlCnt <= CONX_LOST_TH);
}
/*****************************************************************************/
/**
* @brief Management net FSM routine.
*
* @param mgmtFSM
*
* @param mgmtFSM
* mainState
*
*
* @return Slot has be used.
*
*
******************************************************************************/
int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
int32_t MgmtNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
int32_t Status;
DBG_INFO("-------- MgmtNet Routine --------\n");
if(rtneEntry->slotSeq != 0){
if (rtneEntry->slotSeq != 0)
{
return SUCCESS;
}
@@ -902,27 +952,31 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
// Determine next slot TOR state.
L2_TOR_TYPE torState = L2_TOR_SUSPEND;
if ((rtneEntry->src0 == L2_GetNodeId(instPtr))
|| (rtneEntry->src1 == L2_GetNodeId(instPtr))) {
if ((rtneEntry->src0 == L2_GetNodeId(instPtr)) || (rtneEntry->src1 == L2_GetNodeId(instPtr)))
{
torState = L2_TOR_TX;
}
else if ((rtneEntry->dst0 == L2_GetNodeId(instPtr))
|| (rtneEntry->dst1 == L2_GetNodeId(instPtr))) {
else if ((rtneEntry->dst0 == L2_GetNodeId(instPtr)) || (rtneEntry->dst1 == L2_GetNodeId(instPtr)))
{
torState = L2_TOR_RX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
// Update RTT FSM if needed.
if (rtneEntry->slotType == L2_SLOT_INTRA_TSYNC) {
if(torState == L2_TOR_TX) {
// Update RTT FSM if needed.
if (rtneEntry->slotType == L2_SLOT_INTRA_TSYNC)
{
if (torState == L2_TOR_TX)
{
RTT_FsmUpdate(instPtr, L2_TOR_TX);
}
else if (torState == L2_TOR_RX)
{
L2_SCHD_EVENT* rttRxEvent = L2_CreatNewSchdEvent(instPtr->schdEventPool, L2_EV_RTT_RECV_UPDATE, true);
if(rttRxEvent == NULL) {
L2_SCHD_EVENT *rttRxEvent = L2_CreatNewSchdEvent(instPtr->schdEventPool, L2_EV_RTT_RECV_UPDATE, true);
if (rttRxEvent == NULL)
{
DBG_ERROR("Failed to initialize rtt event.\n");
}
// Schedule rtt update after receive process end.
@@ -930,15 +984,18 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
}
}
switch (rtneEntry->slotType) {
switch (rtneEntry->slotType)
{
case L2_SLOT_INTER_MGMT:
// TODO:
// TODO:
L2_SetupNextSlot(instPtr, torState, L2_INTER_MGMT_FRAME);
break;
case L2_SLOT_INTRA_MGMT:
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if (torState == L2_TOR_TX) {
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_MGMT) != SUCCESS) {
if (torState == L2_TOR_TX)
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_MGMT) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -946,20 +1003,25 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
L2_SetupNextSlot(instPtr, torState, L2_INTRA_MGMT_FRAME);
break;
case L2_SLOT_INTER_TSYNC:
// TODO:
// TODO:
L2_SetupNextSlot(instPtr, torState, L2_INTER_TSYNC_FRAME);
break;
case L2_SLOT_INTRA_TSYNC:
if (torState == L2_TOR_TX) {
if (torState == L2_TOR_TX)
{
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if(L2_GetNodeType(instPtr) == L2_NODE_HEAD){
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTREQ) != SUCCESS) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTREQ) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
}
else {
if(MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS) {
else
{
if (MgmtPkt_Packing(instPtr, MGMTPKT_INTRA_RTTRSP) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -975,35 +1037,36 @@ int32_t MgmtNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry){
/*************************************************/
// Update link layer state...
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
L2_NODE_STATE newState = L2_NONE_STATE;
L2_EVENT stateEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale
&& L2_GetMainState(instPtr) != L2_INTRA_SEEK_STATE) {
.msg = &newState};
if (L2_GetNodeListLen(instPtr->nodeList) < instPtr->config.netScale && L2_GetMainState(instPtr) != L2_INTRA_SEEK_STATE)
{
newState = L2_INTRA_SEEK_STATE;
AssertL2Event(instPtr, &stateEvent);
}
else if(L2_GetNodeListLen(instPtr->nodeList) >= instPtr->config.netScale &&
L2_GetMainState(instPtr) != L2_NORMAL_STATE){
else if (L2_GetNodeListLen(instPtr->nodeList) >= instPtr->config.netScale &&
L2_GetMainState(instPtr) != L2_NORMAL_STATE)
{
newState = L2_NORMAL_STATE;
AssertL2Event(instPtr, &stateEvent);
}
}
else{ // L2_NODE_MEMBER or BACKUP_HEAD
if(L2_isNodeInList(instPtr->nodeList, L2_GetHeadId(instPtr)) == false){
else
{ // L2_NODE_MEMBER or BACKUP_HEAD
if (L2_isNodeInList(instPtr->nodeList, L2_GetHeadId(instPtr)) == false)
{
L2_NODE_STATE newState = L2_INIT_STATE;
L2_EVENT stateEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateEvent);
L2_EVENT lostCxnEvent = {
.event = L2_EV_LOST_CXN,
.msg = NULL
};
.msg = NULL};
AssertL2Event(instPtr, &lostCxnEvent);
DBG_WARNING("Lost connection with head node [%d].\n", L2_GetHeadId(instPtr));
}
datalink814_ge/src/linklayer/net_scan.c
+259 -172
View File
@@ -30,28 +30,29 @@
#define INIT_BEAM_NO 0
#define BEAM_PER_ANT 37
#define INIT_ANT_NO 0
#define ANT_NUM 6
#define ANT_NUM 6
#define BM_LSTEN_TIMEOUT 40
#define SQUENCE_SCAN_SLOTS 0
#define SQUENCE_SCAN_SLOTS 0
#define INTRA_INIT_SCAN_SLOTS 2
#define INTRA_BM_ADVT_SLOTS 4
#define INTRA_WAY2ACQ_SLOTS 1
#define INTRA_INIT_SCAN_SLOTS 2
#define INTRA_BM_ADVT_SLOTS 4
#define INTRA_WAY2ACQ_SLOTS 1
#define INTER_INIT_SCAN_SLOTS 4
#define INTER_BM_ADVT_SLOTS 20
#define INTER_WAY2ACQ_SLOTS 2
#define INTER_INIT_SCAN_SLOTS 4
#define INTER_BM_ADVT_SLOTS 20
#define INTER_WAY2ACQ_SLOTS 2
//#define INIT_SCAN_SLOT_MAX 37
//#define BM_ADVT_SLOT_MAX 2
#define WAY2_ACQ_SLOT_MAX 120
// #define INIT_SCAN_SLOT_MAX 37
// #define BM_ADVT_SLOT_MAX 2
#define WAY2_ACQ_SLOT_MAX 120
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
SCANPKT_NONE = 0,
SCANPKT_INTER_BM = 1,
SCANPKT_INTRA_BM = 2,
@@ -61,25 +62,26 @@ typedef enum {
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state);
static void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void BmrecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void W2ackStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state);
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage);
//static int32_t Check_W2AcqAck(NODE_ENTRY* nodeEntry);
static int32_t Scan_InitScanHandler(L2_INST* instPtr);
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr);
static int32_t Scan_2WAcqHandler(L2_INST* instPtr);
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType);
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType);
static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage);
// static int32_t Check_W2AcqAck(NODE_ENTRY* nodeEntry);
static int32_t Scan_InitScanHandler(L2_INST *instPtr);
static int32_t Scan_BmAdvtHandler(L2_INST *instPtr);
static int32_t Scan_2WAcqHandler(L2_INST *instPtr);
static int32_t ScanMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType);
static int32_t ScanPkt_Packing(L2_INST *instPtr, SCANPKT_TYPE pktType);
/**
* @brief Update Init-Scan lock state.
*/
static inline void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
static inline void LockStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update BeamLockState from [%d] to [%d].\n",
scanFsmPtr->isLocked, state);
scanFsmPtr->isLocked, state);
scanFsmPtr->isLocked = state;
return;
}
@@ -87,10 +89,11 @@ static inline void LockStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
/**
* @brief Update Base-info Advertisement state.
*/
static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
static void BmrecvStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update BmRecvState from [%d] to [%d].\n",
scanFsmPtr->isBmRcved, state);
scanFsmPtr->isBmRcved, state);
scanFsmPtr->isBmRcved = state;
return;
}
@@ -98,10 +101,11 @@ static void BmrecvStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
/**
* @brief Update 2-Way Acquisition ack state.
*/
static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
static void W2ackStateUpdate(L2_SCAN_FSM *scanFsmPtr, bool state)
{
ASSERT(scanFsmPtr != NULL);
DBG_INFO("Update W2acqAckState from [%d] to [%d].\n",
scanFsmPtr->isAcked, state);
scanFsmPtr->isAcked, state);
scanFsmPtr->isAcked = state;
return;
}
@@ -117,9 +121,10 @@ static void W2ackStateUpdate(L2_SCAN_FSM* scanFsmPtr, bool state){
* @return NONE.
*
******************************************************************************/
static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
static void StageUpdate(L2_SCAN_FSM *scanFsmPtr, SCAN_STAGE newStage)
{
DBG_INFO("ScanNet current stage: %u -> ScanNet new stage: %u \n",
scanFsmPtr->scanStage, newStage);
scanFsmPtr->scanStage, newStage);
scanFsmPtr->scanStage = newStage;
}
/*****************************************************************************/
@@ -133,17 +138,20 @@ static void StageUpdate(L2_SCAN_FSM* scanFsmPtr, SCAN_STAGE newStage) {
* SUCCESS
*
******************************************************************************/
static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE msgType) {
static int32_t ScanMsg_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_MSG_TYPE msgType)
{
ASSERT(bitarr != NULL);
int32_t Status = SUCCESS;
if (bitarr->num_of_bits < NS_MSG_LEN || msgType < L2_MSG_BM
|| msgType > L2_MSG_SCAN_CONT_5) {
if (bitarr->num_of_bits < NS_MSG_LEN || msgType < L2_MSG_BM || msgType > L2_MSG_SCAN_CONT_5)
{
return (EPARAM);
}
uint64_t dataChunk = 0;
switch (msgType) {
switch (msgType)
{
case L2_MSG_BM:
if (bitarr->num_of_bits < BM_MSG_LEN) {
if (bitarr->num_of_bits < BM_MSG_LEN)
{
return EPARAM;
}
/* message type all zero */
@@ -196,11 +204,12 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, dataChunk);
/* Append CRC */
L2_CrcAppend((uint8_t *) bitarr->words, BM_MSG_CRC_OFFSET / 8, BM_MSG_CRC_OFFSET / 8, L2_CRC8_31);
L2_CrcAppend((uint8_t *)bitarr->words, BM_MSG_CRC_OFFSET / 8, BM_MSG_CRC_OFFSET / 8, L2_CRC8_31);
break;
case L2_MSG_SCAN_INIT:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* Sequence tag */
@@ -228,18 +237,18 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, NS_MSG_INI_TYPE_OFFSET, NS_MSG_INI_TYPE_LEN, dataChunk);
/* source node id */
dataChunk = (L2_GetNodeId(instPtr) & 0x1F)
| (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, dataChunk);
/* destination node id */
dataChunk = (L2_GetNodeId(instPtr) == L2_NODE_HEAD) ?
(L2_BROADCAST_ID | (L2_BROADCAST_ID << 5))
: (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
dataChunk = (L2_GetNodeId(instPtr) == L2_NODE_HEAD) ? (L2_BROADCAST_ID | (L2_BROADCAST_ID << 5))
: (L2_GetHeadId(instPtr) & 0x1F) | (((uint16_t)(L2_GetClusterId(instPtr) & 0x1F)) << 5);
FrameInsertChunk(bitarr, NS_MSG_INI_LEN_OFFSET, NS_MSG_INI_LEN_LEN, dataChunk);
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
/* current net stage */
switch (instPtr->mainState) {
switch (instPtr->mainState)
{
case L2_INIT_STATE:
dataChunk = NET_INI_STAGE;
break;
@@ -255,13 +264,14 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
}
FrameInsertChunk(bitarr, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, dataChunk);
// Begin Scan Time Word
// Begin Scan Time Word
dataChunk = instPtr->netStartTime;
FrameInsertChunk(bitarr, NS_MSG_INI_SCAN_TIME_OFFSET, NS_MSG_INI_SCAN_TIME_LEN, dataChunk);
}
break;
case L2_MSG_SCAN_CONT_1:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -282,7 +292,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_2:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -307,7 +318,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_3:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -328,7 +340,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
break;
case L2_MSG_SCAN_CONT_4:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -344,7 +357,8 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
FrameInsertChunk(bitarr, NS_MSG_CONT_4_KEY_OFFSET, NS_MSG_CONT_4_KEY_LEN, dataChunk);
break;
case L2_MSG_SCAN_CONT_5:
if (bitarr->num_of_bits < NS_MSG_LEN) {
if (bitarr->num_of_bits < NS_MSG_LEN)
{
return EPARAM;
}
/* word tag */
@@ -366,34 +380,42 @@ static int32_t ScanMsg_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_MSG_TYPE
return Status;
}
static int32_t ScanPkt_Packing(L2_INST* instPtr, SCANPKT_TYPE pktType){
static int32_t ScanPkt_Packing(L2_INST *instPtr, SCANPKT_TYPE pktType)
{
uint32_t filedSize = 0;
uint8_t* buff = instPtr->sendPktData;
BIT_ARRAY* scanPkt = bit_array_create(L2_MSG_MAX);
if (scanPkt == NULL) {
uint8_t *buff = instPtr->sendPktData;
BIT_ARRAY *scanPkt = bit_array_create(L2_MSG_MAX);
if (scanPkt == NULL)
{
DBG_ERROR("Failed to allocate memory for scanMsg.\n");
return FAILURE;
}
switch (pktType) {
switch (pktType)
{
case SCANPKT_INTRA_BM:
if (ScanMsg_Packing(instPtr, scanPkt, L2_MSG_BM) != SUCCESS) {
if (ScanMsg_Packing(instPtr, scanPkt, L2_MSG_BM) != SUCCESS)
{
DBG_ERROR("Failed to packing L2_MSG_BM frame.\n");
goto failed;
}
memcpy(buff, scanPkt->words, (size_t)(BM_MSG_LEN / 8));
break;
case SCANPKT_INTRA_2WACQ:
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
if (ScanMsg_Packing(instPtr, scanPkt, intraW2AcqFrameMsgs[i]) == SUCCESS) {
memcpy((uint8_t*)buff + (size_t)(i * (NS_MSG_LEN / 8)),
scanPkt->words, (size_t)(NS_MSG_LEN / 8));
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++)
{
if (ScanMsg_Packing(instPtr, scanPkt, intraW2AcqFrameMsgs[i]) == SUCCESS)
{
memcpy((uint8_t *)buff + (size_t)(i * (NS_MSG_LEN / 8)),
scanPkt->words, (size_t)(NS_MSG_LEN / 8));
}
else {
else
{
DBG_ERROR("Failed to packing intraW2AcqFrameMsgs[%d].\n", i);
goto failed;
}
filedSize += NS_MSG_LEN;
if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX) {
if (filedSize / 8 >= INTRA_W2ACQ_FRAME_LENGTH_MAX)
{
DBG_ERROR("Message is too long to be properly packed.\n");
goto failed;
}
@@ -420,36 +442,44 @@ failed:
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
static int32_t Scan_InitScanHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
DBG_INFO("** Netsetup - InitScan Procedure (Attempts: %u)**\n", scanFsmPtr->scanCnt);
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->isLocked == true) {
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
if (scanFsmPtr->isLocked == true)
{
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
} /* Otherwise remain L2_SCAN_INIT_STAGE */
else {
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
scanFsmPtr->scanCnt++;
}
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
if (scanFsmPtr->isLocked == true) {
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
if (scanFsmPtr->isLocked == true)
{
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
} /* Otherwise remain L2_SCAN_INIT_STAGE */
else {
else
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
scanFsmPtr->scanCnt++;
}
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -467,12 +497,14 @@ static int32_t Scan_InitScanHandler(L2_INST* instPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
static int32_t Scan_BmAdvtHandler(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
DBG_INFO("** Netsetup - BmAdvt Procedure (Timeout: %u) **\n", scanFsmPtr->bmTimeout);
// if received a valid bm frame.
if (scanFsmPtr->isBmRcved == true) {
if (scanFsmPtr->isBmRcved == true)
{
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
DBG_INFO("Received Bm frame, skipping 10ms for turn around...\n");
// TODO: check functionality.
@@ -480,18 +512,23 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
return SUCCESS;
}
// no valid bm frame was received.
else {
else
{
/* if time out reached, go back to init scan stage. */
if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT) {
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->bmTimeout > BM_LSTEN_TIMEOUT)
{
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_SCAN_FRAME);
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_SCAN_FRAME);
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -499,16 +536,20 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
scanFsmPtr->bmTimeout = 0; /* reset listen counter. */
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
}
else {
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
else
{
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTRA_BM_FRAME);
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, L2_TOR_RX, L2_INTER_BM_FRAME);
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
@@ -527,33 +568,39 @@ static int32_t Scan_BmAdvtHandler(L2_INST* instPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
static int32_t Scan_2WAcqHandler(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
DBG_INFO("** Netsetup - 2WAcq Procedure (Timeout: %u) **\n", scanFsmPtr->w2acqTimeout);
L2_FRAME_TYPE frameType = L2_NONE_FRAME;
if(scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE){
if (scanFsmPtr->scanMode == L2_SCAN_INTRA_MODE)
{
frameType = L2_INTRA_W2_ACQ_FRAME;
}
else if(scanFsmPtr->scanMode == L2_SCAN_INTER_MODE){
else if (scanFsmPtr->scanMode == L2_SCAN_INTER_MODE)
{
frameType = L2_INTER_W2_ACQ_FRAME;
}
else{
else
{
DBG_ERROR("UNKNOWN Scan Mode: %d.\n", scanFsmPtr->scanMode);
return FAILURE;
}
// 2 way acq failed, back to init stage.
if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX) {
if (scanFsmPtr->w2acqTimeout >= WAY2_ACQ_SLOT_MAX)
{
DBG_WARNING("2WAcq timeout, restart net scan from L2_SCAN_INIT_STAGE.\n");
scanFsmPtr->w2acqTimeout = 0; // reset counter for next scan.
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
}
else {
else
{
scanFsmPtr->w2acqTimeout++; // increase counter.
}
// using pull mode to receive 2wacq frame
// since first time sync procedure is accomplished in BM stage.
// since first time sync procedure is accomplished in BM stage.
Phy_SetMode(instPtr->config.phyInstPtr, PHY_MODE_PULL);
L2_SetupNextSlot(instPtr, L2_TOR_RX, frameType);
return SUCCESS;
@@ -570,20 +617,23 @@ static int32_t Scan_2WAcqHandler (L2_INST* instPtr){
* error code
*
******************************************************************************/
int32_t ScanNet_PktHandler(L2_INST* instPtr) {
int32_t ScanNet_PktHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t Status;
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
uint64_t dataChunk;
DBG_INFO("Scan net handling packet...\n");
BIT_ARRAY* scanMsg = bit_array_create(L2_MSG_MAX);
if (scanMsg == NULL) {
BIT_ARRAY *scanMsg = bit_array_create(L2_MSG_MAX);
if (scanMsg == NULL)
{
DBG_INFO("Failed to start handling process...\n");
return EALLOC;
}
switch (L2_GetLastFrameType(instPtr)) {
switch (L2_GetLastFrameType(instPtr))
{
case L2_INTER_SCAN_FRAME:
case L2_INTRA_SCAN_FRAME:
// INTRA_BM_FRAME could be received if current frame type is L2_INTRA_SCAN_FRAME
@@ -592,41 +642,44 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
LockStateUpdate(scanFsmPtr, true);
StageUpdate(scanFsmPtr, L2_SCAN_BMADVT_STAGE);
case L2_INTER_BM_FRAME:
case L2_INTRA_BM_FRAME: {
for (int i = 0; i < ARRAY_SIZE(intraBmFrameMsgs); i++) {
case L2_INTRA_BM_FRAME:
{
for (int i = 0; i < ARRAY_SIZE(intraBmFrameMsgs); i++)
{
memcpy(scanMsg->words,
instPtr->recvPktData + (size_t)(i * (BM_MSG_LEN / 8)),
(size_t)(BM_MSG_LEN / 8));
switch (intraBmFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (BM_MSG_LEN / 8)),
(size_t)(BM_MSG_LEN / 8));
switch (intraBmFrameMsgs[i])
{
case L2_MSG_BM:
FrameGetChunk(scanMsg, BM_MSG_SRC_ADDR_OFFSET, BM_MSG_SRC_ADDR_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.NodeId = dataChunk & 0x000000000000001F;
scanFsmPtr->BMrcvLog.ClusterId = (dataChunk>>5) & 0x000000000000001F;
scanFsmPtr->BMrcvLog.ClusterId = (dataChunk >> 5) & 0x000000000000001F;
FrameGetChunk(scanMsg, BM_MSG_TUNIT_NUM_OFFSET, BM_MSG_TUNIT_NUM_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.frameNum = (uint32_t) dataChunk;
scanFsmPtr->BMrcvLog.frameNum = (uint32_t)dataChunk;
FrameGetChunk(scanMsg, BM_MSG_TFRAME_NUM_OFFSET, BM_MSG_TFRAME_NUM_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.slotNum = (uint32_t)(dataChunk * L2_SLOTS_PER_SUBFRAME);
FrameGetChunk(scanMsg, BM_MSG_TSLOT_NUM_OFFSET, BM_MSG_TSLOT_NUM_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.slotNum = scanFsmPtr->BMrcvLog.slotNum + (uint32_t) dataChunk;
scanFsmPtr->BMrcvLog.slotNum = scanFsmPtr->BMrcvLog.slotNum + (uint32_t)dataChunk;
FrameGetChunk(scanMsg, BM_MSG_LONGITUDE_OFFSET, BM_MSG_LONGITUDE_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.Longitude = (uint32_t)(dataChunk);
FrameGetChunk(scanMsg, BM_MSG_LATITUDE_OFFSET, BM_MSG_LATITUDE_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.Latitude = (uint32_t)(dataChunk);
FrameGetChunk(scanMsg, BM_MSG_HEIGHT_OFFSET, BM_MSG_HEIGHT_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.Height = (uint32_t)(dataChunk);
FrameGetChunk(scanMsg, BM_MSG_EAST_SPEED_OFFSET, BM_MSG_EAST_SPEED_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.EastSpeed = (uint32_t)(dataChunk);
FrameGetChunk(scanMsg, BM_MSG_NORTH_SPEED_OFFSET, BM_MSG_NORTH_SPEED_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.NorthSpeed = (uint32_t)(dataChunk);
FrameGetChunk(scanMsg, BM_MSG_UPPER_SPEED_OFFSET, BM_MSG_UPPER_SPEED_LEN, &dataChunk);
scanFsmPtr->BMrcvLog.UpperSpeed = (uint32_t)(dataChunk);
break;
@@ -636,45 +689,54 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
goto failed;
}
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
NODE_ENTRY* nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId);
if(nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId);
if (nodeEntry == NULL)
{
nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->BMrcvLog.NodeId, L2_NODE_HEAD);
// TODO: Insert other node information.
}
else {
else
{
// TODO: Update node information.
}
if ( L2_isCxn2Net(instPtr) == false ) {
if (L2_isCxn2Net(instPtr) == false)
{
// TODO: Check
L2_TimeAlign(instPtr, scanFsmPtr->BMrcvLog.frameNum, scanFsmPtr->BMrcvLog.slotNum, true);
}
// trigger flag
BmrecvStateUpdate(scanFsmPtr, true);
}
else{
else
{
DBG_ERROR("Node type [%d] is not supposed to receive BM frame", L2_GetNodeType(instPtr));
Status = EUNEXP;
goto failed;
}
break;
}
case L2_INTER_W2_ACQ_FRAME:
case L2_INTRA_W2_ACQ_FRAME: {
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++) {
case L2_INTER_W2_ACQ_FRAME:
case L2_INTRA_W2_ACQ_FRAME:
{
for (int i = 0; i < ARRAY_SIZE(intraW2AcqFrameMsgs); i++)
{
memcpy(scanMsg->words,
instPtr->recvPktData + (size_t)(i * (NS_MSG_LEN / 8)),
(size_t)(NS_MSG_LEN / 8));
switch (intraW2AcqFrameMsgs[i]) {
instPtr->recvPktData + (size_t)(i * (NS_MSG_LEN / 8)),
(size_t)(NS_MSG_LEN / 8));
switch (intraW2AcqFrameMsgs[i])
{
case L2_MSG_SCAN_INIT:
FrameGetChunk(scanMsg, NS_MSG_INI_SRC_ADDR_OFFSET, NS_MSG_INI_SRC_ADDR_LEN, &dataChunk);
scanFsmPtr->W2ACQrcvLog.NodeId = dataChunk & 0x001F;
scanFsmPtr->W2ACQrcvLog.ClusterId = (dataChunk>>5) & 0x0001F;
scanFsmPtr->W2ACQrcvLog.ClusterId = (dataChunk >> 5) & 0x0001F;
FrameGetChunk(scanMsg, NS_MSG_INI_NET_STAGE_OFFSET, NS_MSG_INI_NET_STAGE_LEN, &dataChunk);
switch (dataChunk) {
switch (dataChunk)
{
case NET_INI_STAGE:
scanFsmPtr->W2ACQrcvLog.NetStage = L2_INIT_STATE;
break;
@@ -709,7 +771,7 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
FrameGetChunk(scanMsg, NS_MSG_CONT_2_UPPER_SPEED_OFFSET, NS_MSG_CONT_2_UPPER_SPEED_LEN, &dataChunk);
scanFsmPtr->W2ACQrcvLog.UpperSpeed = (uint16_t)(dataChunk);
FrameGetChunk(scanMsg, NS_MSG_CONT_2_HEIGHT_OFFSET, NS_MSG_CONT_2_HEIGHT_LEN, &dataChunk);
scanFsmPtr->W2ACQrcvLog.Height = (uint32_t)(dataChunk);
break;
@@ -731,27 +793,30 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
}
}
NODE_ENTRY* nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId);
if(nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId);
if (nodeEntry == NULL)
{
nodeEntry = L2_AppendNode(instPtr->nodeList, scanFsmPtr->W2ACQrcvLog.NodeId, L2_NODE_MEMBER);
// TODO: Insert other node information.
}
else {
else
{
// TODO: Update node information.
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
// check if net setup finished.
if(L2_GetMainState(instPtr) == L2_INIT_STATE && L2_isCxn2Net(instPtr) == false){
if (L2_GetMainState(instPtr) == L2_INIT_STATE && L2_isCxn2Net(instPtr) == false)
{
// Assert net setup done event...
uint32_t netStartTime = scanFsmPtr->W2ACQrcvLog.NetScanTime;
L2_EVENT netSetupDoneEvent = {
.event = L2_EV_NET_SETUP_DONE,
.msg = &netStartTime
};
.msg = &netStartTime};
AssertL2Event(instPtr, &netSetupDoneEvent);
// Update time sync since netStartTime updated.
L2_TimeAlign(instPtr, L2_GetLastFrameNum(instPtr), L2_GetLastSlotNum(instPtr), true);
@@ -759,14 +824,15 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
L2_RTNE_CONFIG newRtneConfig = {0};
L2_EVENT rtneUpEvent = {
.event = L2_ROUTINE_UPDATE,
.msg = &newRtneConfig
};
if(scanFsmPtr->W2ACQrcvLog.NetStage == L2_INIT_STATE){
.msg = &newRtneConfig};
if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INIT_STATE)
{
newRtneConfig.rtneTable = initTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(initTable);
AssertL2Event(instPtr, &rtneUpEvent);
}
else if(scanFsmPtr->W2ACQrcvLog.NetStage == L2_INTRA_SEEK_STATE){
else if (scanFsmPtr->W2ACQrcvLog.NetStage == L2_INTRA_SEEK_STATE)
{
newRtneConfig.rtneTable = intraSeekTable;
newRtneConfig.rtnePrd = ARRAY_SIZE(intraSeekTable);
AssertL2Event(instPtr, &rtneUpEvent);
@@ -777,15 +843,13 @@ int32_t ScanNet_PktHandler(L2_INST* instPtr) {
L2_NODE_STATE newState = scanFsmPtr->W2ACQrcvLog.NetStage;
L2_EVENT stateUpEvent = {
.event = L2_EV_MAIN_STATE_UPDATE,
.msg = &newState
};
.msg = &newState};
AssertL2Event(instPtr, &stateUpEvent);
}
break;
}
default: {
default:
{
DBG_ERROR("Cannot handler message type [%d] properly.\n", L2_GetLastFrameType(instPtr));
Status = EIGNORE;
goto failed;
@@ -808,9 +872,11 @@ failed:
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
*
******************************************************************************/
int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
*scanFSMPtr = (L2_SCAN_FSM *) malloc(sizeof(L2_SCAN_FSM));
if (*scanFSMPtr == NULL) {
int32_t ScanNet_Init(L2_SCAN_FSM **scanFSMPtr)
{
*scanFSMPtr = (L2_SCAN_FSM *)malloc(sizeof(L2_SCAN_FSM));
if (*scanFSMPtr == NULL)
{
DBG_ERROR("Failed to allocate memory for scanFsmPtr.\n");
return EALLOC;
}
@@ -818,7 +884,6 @@ int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
return (SUCCESS);
}
/*****************************************************************************/
/**
* @brief Reset ScanNet FSM
@@ -828,7 +893,8 @@ int32_t ScanNet_Init(L2_SCAN_FSM** scanFSMPtr) {
* @return NONE
*
******************************************************************************/
void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
void ScanNet_Reset(L2_SCAN_FSM *scanFSMPtr)
{
ASSERT(scanFSMPtr != NULL);
scanFSMPtr->isLocked = false;
scanFSMPtr->isBmRcved = false;
@@ -865,7 +931,8 @@ void ScanNet_Reset(L2_SCAN_FSM* scanFSMPtr) {
* @return NONE
*
******************************************************************************/
void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
void ScanNet_Destroy(L2_SCAN_FSM *scanFSMPtr)
{
free(scanFSMPtr);
}
@@ -878,15 +945,18 @@ void ScanNet_Destroy(L2_SCAN_FSM* scanFSMPtr) {
* @return used slot (5ms slot).
*
******************************************************************************/
int32_t ScanNet_Proc(L2_INST* instPtr) {
L2_SCAN_FSM* scanFsmPtr = instPtr->scanNetFSM;
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD) {
int32_t ScanNet_Proc(L2_INST *instPtr)
{
L2_SCAN_FSM *scanFsmPtr = instPtr->scanNetFSM;
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
DBG_ERROR("Only member node needs to execute ScanNet_Proc. \n");
return FAILURE;
}
int32_t Status;
DBG_INFO("-------- ScanNet Process --------\n");
switch (scanFsmPtr->scanStage) {
switch (scanFsmPtr->scanStage)
{
case L2_SCAN_NONE_STAGE:
/* Start Scan. */
StageUpdate(scanFsmPtr, L2_SCAN_INIT_STAGE);
@@ -920,47 +990,59 @@ int32_t ScanNet_Proc(L2_INST* instPtr) {
*
******************************************************************************/
int acqSend = 0;
int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
int32_t ScanNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
L2_TOR_TYPE torState;
int32_t Status;
DBG_INFO("-------- ScanNet Routine --------\n");
if(rtneEntry->slotSeq != 0){
if (rtneEntry->slotSeq != 0)
{
return SQUENCE_SCAN_SLOTS;
}
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD){
if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == L2_BROADCAST_ID){
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr) || rtneEntry->dst0 == L2_BROADCAST_ID)
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == L2_BROADCAST_ID){
else if (rtneEntry->src0 == L2_GetNodeId(instPtr) || rtneEntry->src0 == L2_BROADCAST_ID)
{
torState = L2_TOR_TX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
}
else{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr)){
else
{
if (rtneEntry->dst0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_RX;
}
else if (rtneEntry->src0 == L2_GetNodeId(instPtr)){
else if (rtneEntry->src0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_TX;
}
else {
else
{
torState = L2_TOR_SUSPEND;
}
}
switch (rtneEntry->slotType) {
switch (rtneEntry->slotType)
{
case L2_SLOT_INTER_SCAN_ACQ:
// TODO:
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_SCAN_FRAME);
break;
case L2_SLOT_INTRA_SCAN_ACQ:
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX) {
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && torState == L2_TOR_TX)
{
DBG_ERROR("Only head node can send init-scan Frame.\n");
Status = FAILURE;
goto routine_err;
@@ -974,14 +1056,17 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
break;
case L2_SLOT_INTRA_SCAN_BM:
if (torState == L2_TOR_TX) {
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD) {
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD)
{
DBG_ERROR("Only head node can send BM Frame.\n");
Status = FAILURE;
goto routine_err;
}
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS) {
if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_BM) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
@@ -994,20 +1079,23 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
L2_SetupNextSlot(instPtr, L2_TOR_SUSPEND, L2_INTER_W2_ACQ_FRAME);
break;
case L2_SLOT_INTRA_SCAN_2W:
if (torState == L2_TOR_TX) {
if(L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false) {
if (torState == L2_TOR_TX)
{
if (L2_GetNodeType(instPtr) != L2_NODE_HEAD && L2_isDataAvail(instPtr) == false)
{
// skip...
break;
}
/* Prepare Request/ACK data. */
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
/* Pack message words */
if(ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS) {
if (ScanPkt_Packing(instPtr, SCANPKT_INTRA_2WACQ) != SUCCESS)
{
Status = FAILURE;
goto routine_err;
}
W2ackStateUpdate(instPtr->scanNetFSM, true);
acqSend ++;
acqSend++;
}
/* Antenna setting (reserved.) */
L2_SetupNextSlot(instPtr, torState, L2_INTRA_W2_ACQ_FRAME);
@@ -1021,4 +1109,3 @@ int32_t ScanNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry)
routine_err:
return Status;
}
datalink814_ge/src/linklayer/net_service.c
+75 -55
View File
@@ -22,18 +22,17 @@
#include "../error.h"
#include "../common.h"
#ifdef _MPC_CTRL
#endif
/************************** Constant Definitions *****************************/
#define SQUENCE_SVC_SLOTS 0
#define INER_SVC_SLOTS 2
#define INRA_SVC_SLOTS 1
#define NONE_SVC_SLOTS 0
#define TEST_UDU_PDLEN 16
#define TEST_UDU_LEN (UDUH_LEN/8 + TEST_UDU_PDLEN + PDU_CRC_LEN/8 )
#define INER_SVC_SLOTS 2
#define INRA_SVC_SLOTS 1
#define NONE_SVC_SLOTS 0
#define TEST_UDU_PDLEN 16
#define TEST_UDU_LEN (UDUH_LEN / 8 + TEST_UDU_PDLEN + PDU_CRC_LEN / 8)
/***************** Macros (Inline Functions) Definitions *********************/
/************************** Function Prototypes ******************************/
@@ -59,9 +58,11 @@
* SUCCESS
*
******************************************************************************/
static int UduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, uint8_t dst, uint32_t dataLen){
static int UduHeader_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, uint8_t dst, uint32_t dataLen)
{
ASSERT(bitarr != NULL);
if(bitarr->num_of_bits < UDUH_LEN){
if (bitarr->num_of_bits < UDUH_LEN)
{
return EPARAM;
}
uint64_t dataChunk = 0;
@@ -75,10 +76,10 @@ static int UduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, uint8_t dst, u
dataChunk = dataLen;
FrameInsertChunk(bitarr, UDUH_LENGTH_OFFSET, UDUH_LENGTH_LEN, dataChunk);
// source node id
dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, UDUH_SRC_ADDR_OFFSET, UDUH_SRC_ADDR_LEN, dataChunk);
// destination node id
dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (dst & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, UDUH_DST_ADDR_OFFSET, UDUH_DST_ADDR_LEN, dataChunk);
// UDU Sequence number
dataChunk = 0;
@@ -107,14 +108,18 @@ static int UduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, uint8_t dst, u
* SUCCESS
*
******************************************************************************/
static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_TYPE headerType, uint8_t dst){
static int PduHeader_Packing(L2_INST *instPtr, BIT_ARRAY *bitarr, L2_PDU_HEADER_TYPE headerType, uint8_t dst)
{
ASSERT(bitarr != NULL);
if(headerType < L2_PDU_HEADER_0 || headerType > L2_PDU_HEADER_1){
if (headerType < L2_PDU_HEADER_0 || headerType > L2_PDU_HEADER_1)
{
return EPARAM;
}
uint64_t dataChunk = 0;
if(headerType == L2_PDU_HEADER_0){
if(bitarr->num_of_bits < PDUH0_LEN){
if (headerType == L2_PDU_HEADER_0)
{
if (bitarr->num_of_bits < PDUH0_LEN)
{
return EPARAM;
}
// PDU type
@@ -124,19 +129,21 @@ static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_
dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
FrameInsertChunk(bitarr, PDUH0_LENGTH_OFFSET, PDUH0_LENGTH_LEN, dataChunk);
// source node id
dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, PDUH0_SRC_ADDR_OFFSET, PDUH0_SRC_ADDR_LEN, dataChunk);
// destination node id
dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (dst & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, PDUH0_DST_ADDR_OFFSET, PDUH0_DST_ADDR_LEN, dataChunk);
// Sequence number
dataChunk = 0;
FrameInsertChunk(bitarr, PDUH0_SEQ_NUM_OFFSET, PDUH0_SEQ_NUM_LEN, dataChunk);
// Append CRC
L2_CrcAppend((uint8_t*)bitarr->words, PDUH0_HEADER_CRC_OFFSET/8, PDUH0_HEADER_CRC_OFFSET/8, L2_CRC8_31);
L2_CrcAppend((uint8_t *)bitarr->words, PDUH0_HEADER_CRC_OFFSET / 8, PDUH0_HEADER_CRC_OFFSET / 8, L2_CRC8_31);
}
else if(headerType == L2_PDU_HEADER_1){
if(bitarr->num_of_bits < PDUH1_LEN){
else if (headerType == L2_PDU_HEADER_1)
{
if (bitarr->num_of_bits < PDUH1_LEN)
{
return EPARAM;
}
// PDU type
@@ -146,16 +153,17 @@ static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_
dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
FrameInsertChunk(bitarr, PDUH1_LENGTH_OFFSET, PDUH1_LENGTH_LEN, dataChunk);
// source node id
dataChunk = (L2_GetNodeId(instPtr) & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (L2_GetNodeId(instPtr) & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, PDUH1_SRC_ADDR_OFFSET, PDUH1_SRC_ADDR_LEN, dataChunk);
// destination node id
dataChunk = (dst & 0x1F)|((L2_GetClusterId(instPtr) & 0x1F) << 5);
dataChunk = (dst & 0x1F) | ((L2_GetClusterId(instPtr) & 0x1F) << 5);
FrameInsertChunk(bitarr, PDUH1_DST_ADDR_OFFSET, PDUH1_DST_ADDR_LEN, dataChunk);
// Slice tage...
// Append CRC
L2_CrcAppend((uint8_t*)bitarr->words, PDUH1_HEADER_CRC_OFFSET/8, PDUH1_HEADER_CRC_OFFSET/8, L2_CRC8_31);
L2_CrcAppend((uint8_t *)bitarr->words, PDUH1_HEADER_CRC_OFFSET / 8, PDUH1_HEADER_CRC_OFFSET / 8, L2_CRC8_31);
}
else{
else
{
return FAILURE;
}
return SUCCESS;
@@ -171,23 +179,26 @@ static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_
* Other Initialize failed.
*
******************************************************************************/
int32_t ServiceNet_Init(L2_SVC_FSM** serviceFSM) {
(*serviceFSM) = (L2_SVC_FSM*) malloc(sizeof(L2_SVC_FSM));
if (*serviceFSM == NULL) {
int32_t ServiceNet_Init(L2_SVC_FSM **serviceFSM)
{
(*serviceFSM) = (L2_SVC_FSM *)malloc(sizeof(L2_SVC_FSM));
if (*serviceFSM == NULL)
{
return EALLOC;
}
ServiceNet_Reset(*serviceFSM);
return SUCCESS;
}
void ServiceNet_Reset(L2_SVC_FSM* serviceFSM) {
void ServiceNet_Reset(L2_SVC_FSM *serviceFSM)
{
serviceFSM->isBisar = false;
serviceFSM->isBisarEven = false;
serviceFSM->bisarDst = L2_NONE_ID;
}
void ServiceNet_Destroy(L2_SVC_FSM* serviceFSM){
void ServiceNet_Destroy(L2_SVC_FSM *serviceFSM)
{
free(serviceFSM);
}
/*****************************************************************************/
@@ -200,7 +211,8 @@ void ServiceNet_Destroy(L2_SVC_FSM* serviceFSM){
* other error occurred.
*
******************************************************************************/
int32_t ServiceNet_PktHandler(L2_INST* instPtr) {
int32_t ServiceNet_PktHandler(L2_INST *instPtr)
{
// TODO:
DBG_INFO("Scan net handling packet...\n");
@@ -212,70 +224,78 @@ int32_t ServiceNet_PktHandler(L2_INST* instPtr) {
* @brief Service net FSM procedure.
*
* @param instPtr a pointer to linklayer layer instance
* @param rtneEntry
* @param rtneEntry
*
* @return Slot has been used.
*
******************************************************************************/
int32_t ServiceNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
int32_t ServiceNet_Routine(L2_INST *instPtr, const L2_RTNE_TABLE_ENTRY *rtneEntry)
{
ASSERT(instPtr != NULL);
int32_t Status;
L2_TOR_TYPE torState;
DBG_INFO("-------- ServiceNet Routine --------\n");
if(rtneEntry->slotSeq != 0){
if (rtneEntry->slotSeq != 0)
{
return SQUENCE_SVC_SLOTS;
}
// Determine TOR State.
if(rtneEntry->src0 == L2_GetNodeId(instPtr)){
if (rtneEntry->src0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_TX;
}
else if(rtneEntry->dst0 == L2_GetNodeId(instPtr)){
else if (rtneEntry->dst0 == L2_GetNodeId(instPtr))
{
torState = L2_TOR_RX;
}
else{
else
{
torState = L2_TOR_SUSPEND;
}
BIT_ARRAY* svcMsg = bit_array_create(L2_MSG_MAX);
if(svcMsg == NULL){
BIT_ARRAY *svcMsg = bit_array_create(L2_MSG_MAX);
if (svcMsg == NULL)
{
DBG_ERROR("Failed to allocate memory for svcMsg.\n");
return FAILURE;
}
switch(rtneEntry->slotType){
switch (rtneEntry->slotType)
{
case L2_SLOT_INTER_SVC:
L2_SetupNextSlot(instPtr, torState, L2_INTER_SVC_FRAME);
break;
case L2_SLOT_INTRA_SVC:
if(torState == L2_TOR_TX){
//TODO:
if (torState == L2_TOR_TX)
{
// TODO:
memset(instPtr->sendPktData, 0xAA, L2_PACKET_LEN_MAX);
// Pack PDU header
if(PduHeader_Packing(instPtr, svcMsg, L2_PDU_HEADER_0, rtneEntry->dst0) != SUCCESS){
if (PduHeader_Packing(instPtr, svcMsg, L2_PDU_HEADER_0, rtneEntry->dst0) != SUCCESS)
{
DBG_ERROR("Failed to packing PduHeader.\n");
Status = FAILURE;
goto routine_err;
}
memcpy(instPtr->sendPktData, svcMsg->words, (size_t)(PDUH0_LEN/8));
memcpy(instPtr->sendPktData, svcMsg->words, (size_t)(PDUH0_LEN / 8));
// Pack UDU Header
if(UduHeader_Packing(instPtr, svcMsg, rtneEntry->dst0,
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8)
!= SUCCESS){
if (UduHeader_Packing(instPtr, svcMsg, rtneEntry->dst0,
INTRA_SVC_FRAME_LENGTH_MAX - (PDUH0_LEN + MSG_KEY_LENGTH_LEN) / 8) != SUCCESS)
{
DBG_ERROR("Failed to packing UduHeader.\n");
Status = FAILURE;
goto routine_err;
}
memcpy(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8), svcMsg->words, (size_t)(UDUH_LEN/8));
memcpy(instPtr->sendPktData + ((PDUH0_LEN + MSG_KEY_LENGTH_LEN) / 8), svcMsg->words, (size_t)(UDUH_LEN / 8));
// For ZhiQiang
// Payload Byte3, bit0~2: message class, bit3~7 message subclass
// FIXME: message class tag 0x01, only for test.
memset(instPtr->sendPktData+((PDUH0_LEN+MSG_KEY_LENGTH_LEN+UDUH_LEN)/8) + 3, 0x01, 1);
L2_CrcAppend(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8),
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
L2_CRC16_8005
);
memset(instPtr->sendPktData + ((PDUH0_LEN + MSG_KEY_LENGTH_LEN + UDUH_LEN) / 8) + 3, 0x01, 1);
L2_CrcAppend(instPtr->sendPktData + ((PDUH0_LEN + MSG_KEY_LENGTH_LEN) / 8),
INTRA_SVC_FRAME_LENGTH_MAX - (PDUH0_LEN + MSG_KEY_LENGTH_LEN + PDU_CRC_LEN) / 8,
INTRA_SVC_FRAME_LENGTH_MAX - (PDUH0_LEN + MSG_KEY_LENGTH_LEN + PDU_CRC_LEN) / 8,
L2_CRC16_8005);
}
L2_SetupNextSlot(instPtr, torState, L2_INTRA_SVC_FRAME);
break;
@@ -285,9 +305,9 @@ int32_t ServiceNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntr
Status = FAILURE;
goto routine_err;
}
bit_array_free(svcMsg);// free memory
bit_array_free(svcMsg); // free memory
return SUCCESS;
routine_err:
bit_array_free(svcMsg);// free memory
bit_array_free(svcMsg); // free memory
return Status;
}
datalink814_ge/src/linklayer/node_list.c
+124 -79
View File
@@ -26,7 +26,8 @@
#include "../error.h"
/***************** Macros (Inline Functions) Definitions *********************/
inline uint32_t L2_GetNodeListLen(NODE_LIST *list) {
inline uint32_t L2_GetNodeListLen(NODE_LIST *list)
{
ASSERT(list != NULL);
return list->len;
}
@@ -38,30 +39,33 @@ static NODE_ENTRY util_entry;
/**
* @brief helper function to free node entry.
*/
static void entry_free(void* val){
static void entry_free(void *val)
{
free(val);
}
/**
* @brief helper function to match node entries by nodeIds
*/
static int8_t entry_compare(void* a, void* b){
static int8_t entry_compare(void *a, void *b)
{
ASSERT(a != NULL);
ASSERT(b != NULL);
register uint8_t idA, idB;
idA = ((NODE_ENTRY*)a)->nodeInfo.nodeId;
idB = ((NODE_ENTRY*)b)->nodeInfo.nodeId;
idA = ((NODE_ENTRY *)a)->nodeInfo.nodeId;
idB = ((NODE_ENTRY *)b)->nodeInfo.nodeId;
if (idA == idB)
{
return 0;
}
else if(idA < idB) {
else if (idA < idB)
{
return -1;
}
else {
else
{
return 1;
}
}
/*****************************************************************************/
/**
@@ -73,9 +77,11 @@ static int8_t entry_compare(void* a, void* b){
* ENOMEM - not enough memory.
*
******************************************************************************/
int32_t L2_NodeList_Init(NODE_LIST **list) {
int32_t L2_NodeList_Init(NODE_LIST **list)
{
(*list) = list_new();
if (*list == NULL) {
if (*list == NULL)
{
return ENOMEM;
}
(*list)->free = entry_free;
@@ -91,7 +97,8 @@ int32_t L2_NodeList_Init(NODE_LIST **list) {
*NONE
*
******************************************************************************/
void L2_NodeList_Clear(NODE_LIST *list){
void L2_NodeList_Clear(NODE_LIST *list)
{
ASSERT(list != NULL);
list_clear(list);
}
@@ -105,7 +112,8 @@ void L2_NodeList_Clear(NODE_LIST *list){
* @return NONE
*
******************************************************************************/
void L2_NodeList_Destroy(NODE_LIST *list){
void L2_NodeList_Destroy(NODE_LIST *list)
{
list_destroy(list);
}
/*****************************************************************************/
@@ -119,20 +127,21 @@ void L2_NodeList_Destroy(NODE_LIST *list){
* NULL if node dose not exist.
*
******************************************************************************/
NODE_ENTRY* L2_GetNodeById(NODE_LIST *list, uint8_t nodeId) {
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
util_entry.nodeInfo.nodeId = nodeId;
list_node_t* node = list_find(list, &util_entry);
if(node == NULL) {
list_node_t *node = list_find(list, &util_entry);
if (node == NULL)
{
return NULL;
}
else {
return ((NODE_ENTRY*) node->val);
else
{
return ((NODE_ENTRY *)node->val);
}
}
/*****************************************************************************/
/**
* @brief Append a new node entry to the node list
@@ -145,21 +154,24 @@ NODE_ENTRY* L2_GetNodeById(NODE_LIST *list, uint8_t nodeId) {
* Other Error occurred.(Node already exist, memory alloc error ect.)
*
******************************************************************************/
NODE_ENTRY* L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType)
{
ASSERT(list != NULL);
if(L2_isNodeInList(list, nodeId)){
if (L2_isNodeInList(list, nodeId))
{
DBG_WARNING("Node [%d] already exist.\n", nodeId);
return NULL;
}
DBG_INFO("Link layer appending node [%d] to node list.\n", nodeId);
NODE_ENTRY* nodeEntry = (NODE_ENTRY*) malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = (NODE_ENTRY *)malloc(sizeof(NODE_ENTRY));
if (nodeEntry == NULL)
{
DBG_ERROR("Failed to allocated memory for nodeEntry.\n");
return NULL;
}
list_node_t* node = list_node_new(nodeEntry);
if(node == NULL) {
list_node_t *node = list_node_new(nodeEntry);
if (node == NULL)
{
DBG_ERROR("Failed to allocated memory for list node.\n");
free(nodeEntry);
return NULL;
@@ -189,11 +201,13 @@ NODE_ENTRY* L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType
* Other Node not exist.
*
******************************************************************************/
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId) {
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
util_entry.nodeInfo.nodeId = nodeId;
list_node_t* node = list_find(list, &util_entry);
if(node == NULL) {
list_node_t *node = list_find(list, &util_entry);
if (node == NULL)
{
DBG_WARNING("Node [%d] in not in node list, skip deleting process.\n", nodeId);
return SUCCESS;
}
@@ -213,7 +227,8 @@ int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId) {
* false node not in the list
*
******************************************************************************/
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId) {
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
return (L2_GetNodeById(list, nodeId) != NULL);
}
@@ -231,13 +246,16 @@ bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId) {
*
******************************************************************************/
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst) {
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*dst = Coord_Distance(myCoord, nodeEntry->nodeInfo.pos);
return SUCCESS;
}
@@ -255,19 +273,21 @@ int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* d
* FAILURE node not exist.
*
******************************************************************************/
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) {
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.pos), newPos, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get node positions information.
@@ -280,13 +300,16 @@ int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos) {
* FAILURE node not exist.
*
******************************************************************************/
int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(pos, &(nodeEntry->nodeInfo.pos), sizeof(COORD));
}
return SUCCESS;
@@ -304,19 +327,21 @@ int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *pos) {
* FAILURE node not exist.
*
******************************************************************************/
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed) {
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(&(nodeEntry->nodeInfo.speed), newSpeed, sizeof(COORD));
}
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get node speed information.
@@ -329,13 +354,16 @@ int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpee
* FAILURE node not exist.
*
******************************************************************************/
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
memcpy(speed, &(nodeEntry->nodeInfo.speed), sizeof(COORD));
}
return SUCCESS;
@@ -352,13 +380,16 @@ int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed) {
* FAILURE Node not exist.
*
******************************************************************************/
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi) {
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->timeDevi = devi;
}
return SUCCESS;
@@ -375,13 +406,16 @@ int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi
* @return NONE
*
******************************************************************************/
int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt += inc;
}
return SUCCESS;
@@ -398,13 +432,16 @@ int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc) {
* FAILURE Node not exist.
*
******************************************************************************/
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
nodeEntry->ttlCnt = 0;
}
return SUCCESS;
@@ -421,13 +458,16 @@ int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId) {
* @return NONE
*
******************************************************************************/
int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl)
{
ASSERT(list != NULL);
NODE_ENTRY* nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL) {
NODE_ENTRY *nodeEntry = L2_GetNodeById(list, nodeId);
if (nodeEntry == NULL)
{
return FAILURE;
}
else {
else
{
*ttl = nodeEntry->ttlCnt;
}
return SUCCESS;
@@ -446,19 +486,22 @@ int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl) {
* Other Error occurred.
*
******************************************************************************/
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *)) {
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *))
{
ASSERT(list != NULL);
int32_t Status;
list_node_t* node;
list_iterator_t* iter = list_iterator_new(list, LIST_HEAD);
if(iter == NULL) {
list_node_t *node;
list_iterator_t *iter = list_iterator_new(list, LIST_HEAD);
if (iter == NULL)
{
DBG_ERROR("Failed to initialize list iteration process.\n");
return EALLOC;
}
while ((node = list_iterator_next(iter)))
{
Status = operation((NODE_ENTRY*)node->val, param);
if(Status != SUCCESS){
Status = operation((NODE_ENTRY *)node->val, param);
if (Status != SUCCESS)
{
DBG_ERROR("Error occurred when iteratively edit node entry.\n");
break;
}
@@ -478,21 +521,23 @@ int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)
* @return SUCCESS
*
******************************************************************************/
int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *)) {
int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *))
{
ASSERT(list != NULL);
list_node_t* node;
list_iterator_t* iter = list_iterator_new(list, LIST_HEAD);
if(iter == NULL) {
list_node_t *node;
list_iterator_t *iter = list_iterator_new(list, LIST_HEAD);
if (iter == NULL)
{
DBG_ERROR("Failed to initialize list iteration process.\n");
return EALLOC;
}
while ((node = list_iterator_next(iter)))
{
if (condition((NODE_ENTRY*) node->val) == false) {
if (condition((NODE_ENTRY *)node->val) == false)
{
list_remove(list, node);
}
}
list_iterator_destroy(iter);
return SUCCESS;
}
datalink814_ge/src/linklayer/node_list.h
+6 -5
View File
@@ -29,7 +29,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef struct NODE_LIST_ENTRY {
typedef struct NODE_LIST_ENTRY
{
L2_NODE_INFO nodeInfo;
uint32_t ttlCnt;
int32_t timeDevi;
@@ -44,8 +45,8 @@ int32_t L2_NodeList_Init(NODE_LIST **list);
void L2_NodeList_Clear(NODE_LIST *list);
void L2_NodeList_Destroy(NODE_LIST *list);
uint32_t L2_GetNodeListLen(NODE_LIST *list);
NODE_ENTRY* L2_GetNodeById(NODE_LIST *list, uint8_t nodeId);
NODE_ENTRY* L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
NODE_ENTRY *L2_GetNodeById(NODE_LIST *list, uint8_t nodeId);
NODE_ENTRY *L2_AppendNode(NODE_LIST *list, uint8_t nodeId, L2_NODE_TYPE nodeType);
int32_t L2_DeleteNodeById(NODE_LIST *list, uint8_t nodeId);
int32_t L2_UpdateNodePos(NODE_LIST *list, uint8_t nodeId, const COORD *newPos);
bool L2_isNodeInList(NODE_LIST *list, uint8_t nodeId);
@@ -53,11 +54,11 @@ int32_t L2_GetNodePos(NODE_LIST *list, uint8_t nodeId, COORD *newPos);
int32_t L2_UpdateNodeSpeed(NODE_LIST *list, uint8_t nodeId, const COORD *newSpeed);
int32_t L2_GetNodeSpeed(NODE_LIST *list, uint8_t nodeId, COORD *speed);
int32_t L2_UpdateNodeTimeDeviation(NODE_LIST *list, uint8_t nodeId, int32_t devi);
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double* dst);
int32_t L2_GetDistance(NODE_LIST *list, COORD myCoord, uint8_t nodeId, double *dst);
int32_t L2_RstNodeTTL(NODE_LIST *list, uint8_t nodeId);
int32_t L2_IncNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t inc);
int32_t L2_GetNodeTTL(NODE_LIST *list, uint8_t nodeId, uint32_t *ttl);
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY*, const void*));
int32_t L2_IterEditNode(NODE_LIST *list, const void *param, int32_t (*operation)(NODE_ENTRY *, const void *));
int32_t L2_IterDeleteNode(NODE_LIST *list, bool (*condition)(NODE_ENTRY *));
#endif /* NODE_LIST_H_ */
datalink814_ge/src/linklayer/rtt.h
+10 -7
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: A simplified version of timesync for datalink 814 (Dijian)
*
*
* Date: 2021-12-04 16:32
* LastEditors: Fan Hao
* LastEditTime: 2022-07-06 16:32
* Revisions: Rev 0.0
* History:
* History:
* [2022-07-06]: Initial version.
* [2021-12-04]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef RTT_H_
@@ -29,7 +29,8 @@
/**************************** Type Definitions *******************************/
// TODO: Multi-node RTT
typedef struct {
typedef struct
{
TIME_WORD Tt1;
TIME_WORD Tr1;
TIME_WORD Tt2;
@@ -48,7 +49,8 @@ typedef struct {
TIME_WORD TreplyB;
} RTT_RECORD;
typedef enum {
typedef enum
{
RTT_NONE_STAGE = 0,
RTT_REQ1 = 1,
RTT_RSP1 = 2,
@@ -56,7 +58,8 @@ typedef enum {
RTT_RSP2 = 4, // Rtt Stages
} L2_RTT_STGE;
typedef struct {
typedef struct
{
L2_RTT_STGE rttStage;
bool isR1Valid;
bool isR2Valid;
datalink814_ge/src/linklayer/schdevent.c
+96 -60
View File
@@ -8,7 +8,7 @@
* Revisions: Rev 0.0
* History:
* [2023-12-21]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
@@ -29,30 +29,36 @@ static L2_SCHD_EVENT util_schdevent;
/**
* @brief helper function to free schedule events.
*/
static void schdevent_free(void* val){
if(val == NULL) {
static void schdevent_free(void *val)
{
if (val == NULL)
{
return;
}
free(((L2_SCHD_EVENT*)val)->l2Event.msg);
free(((L2_SCHD_EVENT *)val)->l2Event.msg);
free(val);
}
/**
* @brief helper function to match events by ids.
*/
static int8_t schdevent_compare(void* a, void* b){
static int8_t schdevent_compare(void *a, void *b)
{
ASSERT(a == NULL);
ASSERT(b != NULL);
register uint32_t idA, idB;
idA = ((L2_SCHD_EVENT*)a)->eventId;
idB = ((L2_SCHD_EVENT*)b)->eventId;
if( idA== idB) {
idA = ((L2_SCHD_EVENT *)a)->eventId;
idB = ((L2_SCHD_EVENT *)b)->eventId;
if (idA == idB)
{
return 0;
}
else if (idA < idB) {
else if (idA < idB)
{
return -1;
}
else {
else
{
return 1;
}
}
@@ -66,14 +72,17 @@ static int8_t schdevent_compare(void* a, void* b){
* @return SUCCESS
*
******************************************************************************/
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL** schdPool) {
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool)
{
(*schdPool) = malloc(sizeof(L2_SCHD_POOL));
if(*schdPool == NULL) {
if (*schdPool == NULL)
{
DBG_ERROR("Failed to allocated memory for schdPool.\n");
return EALLOC;
}
(*schdPool)->schdList = list_new();
if((*schdPool)->schdList == NULL) {
if ((*schdPool)->schdList == NULL)
{
DBG_ERROR("Failed to allocated memory for schdPool->schdList.\n");
free(*schdPool);
return EALLOC;
@@ -86,7 +95,7 @@ int32_t L2_CreateSchedEventPool(L2_SCHD_POOL** schdPool) {
/*****************************************************************************/
/**
* @brief Clear schedule event pool, delete all events in pool and
* @brief Clear schedule event pool, delete all events in pool and
* free allocated memory.
*
* @param schdPool a pointer to schedule pool instance.
@@ -94,7 +103,8 @@ int32_t L2_CreateSchedEventPool(L2_SCHD_POOL** schdPool) {
* @return SUCCESS
*
******************************************************************************/
void L2_ClearSchedEventPool(L2_SCHD_POOL* schdPool) {
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool)
{
ASSERT(schdPool != NULL);
list_clear(schdPool->schdList);
schdPool->envCnt = 0;
@@ -109,8 +119,10 @@ void L2_ClearSchedEventPool(L2_SCHD_POOL* schdPool) {
* @return SUCCESS
*
******************************************************************************/
void L2_DestroySchedEventPool(L2_SCHD_POOL* schdPool) {
if(schdPool == NULL) {
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool)
{
if (schdPool == NULL)
{
return;
}
list_destroy(schdPool->schdList);
@@ -129,19 +141,23 @@ void L2_DestroySchedEventPool(L2_SCHD_POOL* schdPool) {
* Pointer to schedule event.
*
******************************************************************************/
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_SCHD_POOL* schdPool, L2_EVENT_TYPE event, bool isVolatile){
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile)
{
ASSERT(schdPool != NULL);
if(event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE){
if (event < L2_EV_SCAN_PROC_DONE || event > L2_ROUTINE_UPDATE)
{
DBG_ERROR("Unknown event type: %d.\n", event);
return NULL;
}
L2_SCHD_EVENT* schdEvent = (L2_SCHD_EVENT*)malloc(sizeof(L2_SCHD_EVENT));
if(schdEvent == NULL){
L2_SCHD_EVENT *schdEvent = (L2_SCHD_EVENT *)malloc(sizeof(L2_SCHD_EVENT));
if (schdEvent == NULL)
{
DBG_ERROR("Failed to allocate schedule event.\n");
return NULL;
}
list_node_t *node = list_node_new(schdEvent);
if(node == NULL) {
if (node == NULL)
{
DBG_ERROR("Failed to allocate schedule event list node.\n");
free(schdEvent);
return NULL;
@@ -169,20 +185,22 @@ L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_SCHD_POOL* schdPool, L2_EVENT_TYPE event,
* Pointer to the schedule event.
*
******************************************************************************/
L2_SCHD_EVENT* L2_GetEventById(L2_SCHD_POOL* schdPool, uint32_t id) {
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id)
{
ASSERT(schdPool != NULL);
util_schdevent.eventId = id;
list_node_t* node = list_find(schdPool->schdList, &util_schdevent);
if(node != NULL) {
list_node_t *node = list_find(schdPool->schdList, &util_schdevent);
if (node != NULL)
{
DBG_WARNING("Event with id=%d, dose not exist.\n", id);
return NULL;
}
return ((L2_SCHD_EVENT*) node->val);
return ((L2_SCHD_EVENT *)node->val);
}
/*****************************************************************************/
/**
* @brief Delete a schedule event, remove the event from event pool,
* @brief Delete a schedule event, remove the event from event pool,
* release memory.
*
* @param schdPool a pointer to schedule pool instance.
@@ -192,11 +210,13 @@ L2_SCHD_EVENT* L2_GetEventById(L2_SCHD_POOL* schdPool, uint32_t id) {
* error code
*
******************************************************************************/
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL* schdPool, uint32_t id) {
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id)
{
ASSERT(schdPool != NULL);
util_schdevent.eventId = id;
list_node_t* node = list_find(schdPool->schdList, &util_schdevent);
if(node != NULL) {
list_node_t *node = list_find(schdPool->schdList, &util_schdevent);
if (node != NULL)
{
DBG_ERROR("Event with id=%d, dose not exist.\n", id);
return EIGNORE;
}
@@ -217,20 +237,23 @@ int32_t L2_DeleteSchdEventById(L2_SCHD_POOL* schdPool, uint32_t id) {
* SUCCESS
*
******************************************************************************/
int32_t L2_ScheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNum, uint32_t slotNum){
int32_t L2_ScheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
{
ASSERT(schdEvent != NULL);
if(L2_isEventScheduled(schdEvent) == true){
if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE;
}
if(frameNum <= L2_GetCurrentFrameNum(instPtr) && slotNum < L2_GetCurrentSlotNum(instPtr)){
if (frameNum <= L2_GetCurrentFrameNum(instPtr) && slotNum < L2_GetCurrentSlotNum(instPtr))
{
DBG_ERROR("Cannot schedule event %d to the past(current: [%u-%u], schedule: [%u-%u]).\n",
schdEvent->eventId, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr), frameNum, slotNum);
schdEvent->eventId, L2_GetCurrentFrameNum(instPtr), L2_GetCurrentSlotNum(instPtr), frameNum, slotNum);
return FAILURE;
}
DBG_INFO("Schedule event (type:%d) [%u] at %u-%u.\n",
schdEvent->l2Event.event, schdEvent->eventId,
frameNum, slotNum);
DBG_INFO("Schedule event (type:%d) [%u] at %u-%u.\n",
schdEvent->l2Event.event, schdEvent->eventId,
frameNum, slotNum);
schdEvent->isSchd = true;
schdEvent->schdFrame = frameNum;
schdEvent->schdSlot = slotNum;
@@ -249,14 +272,16 @@ int32_t L2_ScheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t
* SUCCESS
*
******************************************************************************/
int32_t L2_ScheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){
int32_t L2_ScheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL);
if(L2_isEventScheduled(schdEvent) == true){
if (L2_isEventScheduled(schdEvent) == true)
{
DBG_ERROR("Event %d has already been scheduled.\n", schdEvent->eventId);
return FAILURE;
}
uint32_t frameNum = L2_GetCurrentFrameNum(instPtr) + (numSlots/L2_SLOTS_PER_FRAME);
uint32_t slotNum = L2_GetCurrentSlotNum(instPtr) + (numSlots%L2_SLOTS_PER_FRAME);
uint32_t frameNum = L2_GetCurrentFrameNum(instPtr) + (numSlots / L2_SLOTS_PER_FRAME);
uint32_t slotNum = L2_GetCurrentSlotNum(instPtr) + (numSlots % L2_SLOTS_PER_FRAME);
return L2_ScheduleEventAt(instPtr, schdEvent, frameNum, slotNum);
}
@@ -273,9 +298,11 @@ int32_t L2_ScheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32
* SUCCESS
*
******************************************************************************/
int32_t L2_RescheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t frameNum, uint32_t slotNum){
int32_t L2_RescheduleEventAt(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t frameNum, uint32_t slotNum)
{
ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
schdEvent->isSchd = false;
@@ -294,15 +321,16 @@ int32_t L2_RescheduleEventAt(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_
* SUCCESS
*
******************************************************************************/
int32_t L2_RescheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint32_t numSlots){
int32_t L2_RescheduleEventAfter(L2_INST *instPtr, L2_SCHD_EVENT *schdEvent, uint32_t numSlots)
{
ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
return L2_ScheduleEventAfter(instPtr, schdEvent, numSlots);
}
/*****************************************************************************/
/**
* @brief Check and excute schedule event in event pool illiterate.
@@ -312,12 +340,13 @@ int32_t L2_RescheduleEventAfter(L2_INST* instPtr, L2_SCHD_EVENT* schdEvent, uint
* @return NONE.
*
******************************************************************************/
void L2_EventPoolHandler(L2_INST* instPtr){
void L2_EventPoolHandler(L2_INST *instPtr)
{
ASSERT(instPtr != NULL);
list_node_t* node;
L2_SCHD_EVENT* event;
list_node_t *node;
L2_SCHD_EVENT *event;
L2_SCHD_POOL *pool = instPtr->schdEventPool;
list_iterator_t* iter = list_iterator_new(pool->schdList, LIST_HEAD);
list_iterator_t *iter = list_iterator_new(pool->schdList, LIST_HEAD);
if (iter == NULL)
{
DBG_ERROR("Failed to initialize schedule event pool iteration.\n");
@@ -325,17 +354,20 @@ void L2_EventPoolHandler(L2_INST* instPtr){
}
while ((node = list_iterator_next(iter)))
{
event = (L2_SCHD_EVENT*)node->val;
if(L2_isEventScheduled(event)){
if(event->schdFrame == L2_GetCurrentFrameNum(instPtr)
&& event->schdSlot == L2_GetCurrentSlotNum(instPtr)){
event = (L2_SCHD_EVENT *)node->val;
if (L2_isEventScheduled(event))
{
if (event->schdFrame == L2_GetCurrentFrameNum(instPtr) && event->schdSlot == L2_GetCurrentSlotNum(instPtr))
{
DBG_INFO("Asserting scheduled event [%d].\n", event->eventId);
AssertL2Event(instPtr, &(event->l2Event));
if(event->isVolatile == true) {
if (event->isVolatile == true)
{
DBG_INFO("Deleting scheduled event [%d], since it is volatile.\n", event->eventId);
list_remove(pool->schdList, node);
}
else {
else
{
event->occurCnt++;
}
}
@@ -355,7 +387,8 @@ void L2_EventPoolHandler(L2_INST* instPtr){
* error code.
*
******************************************************************************/
inline void L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg){
inline void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg)
{
ASSERT(schdEvent != NULL);
schdEvent->l2Event.msg = msg;
}
@@ -369,9 +402,11 @@ inline void L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg){
* @return SUCCESS.
*
******************************************************************************/
inline int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent){
inline int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL);
if(schdEvent->isSchd == false){
if (schdEvent->isSchd == false)
{
DBG_WARNING("Event %d has not been scheduled yet.\n", schdEvent->eventId);
}
schdEvent->isSchd = false;
@@ -388,7 +423,8 @@ inline int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent){
* false.
*
******************************************************************************/
inline bool L2_isEventScheduled(const L2_SCHD_EVENT* schdEvent){
inline bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent)
{
ASSERT(schdEvent != NULL);
return schdEvent->isSchd;
}
datalink814_ge/src/linklayer/schdevent.h
+16 -14
View File
@@ -8,7 +8,7 @@
* Revisions: Rev 0.0
* History:
* [2023-12-21]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
@@ -27,7 +27,8 @@
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
typedef struct
{
bool isSchd;
bool isVolatile;
L2_EVENT l2Event;
@@ -40,24 +41,25 @@ typedef struct {
/** @name L2 event
* A definition of L2 event instance.
*/
typedef struct {
list_t* schdList;
typedef struct
{
list_t *schdList;
uint32_t envCnt;
} L2_SCHD_POOL;
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
// Scheduling Event
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL** schdPool);
void L2_ClearSchedEventPool(L2_SCHD_POOL* schdPool);
void L2_DestroySchedEventPool(L2_SCHD_POOL* schdPool);
int32_t L2_CreateSchedEventPool(L2_SCHD_POOL **schdPool);
void L2_ClearSchedEventPool(L2_SCHD_POOL *schdPool);
void L2_DestroySchedEventPool(L2_SCHD_POOL *schdPool);
L2_SCHD_EVENT* L2_CreatNewSchdEvent(L2_SCHD_POOL* schdPool, L2_EVENT_TYPE event, bool isVolatile);
L2_SCHD_EVENT* L2_GetEventById(L2_SCHD_POOL* schdPool, uint32_t id);
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL* schdPool, uint32_t id);
L2_SCHD_EVENT *L2_CreatNewSchdEvent(L2_SCHD_POOL *schdPool, L2_EVENT_TYPE event, bool isVolatile);
L2_SCHD_EVENT *L2_GetEventById(L2_SCHD_POOL *schdPool, uint32_t id);
int32_t L2_DeleteSchdEventById(L2_SCHD_POOL *schdPool, uint32_t id);
void L2_SchdEventAppendMsg(L2_SCHD_EVENT* schdEvent, void* msg);
bool L2_isEventScheduled(const L2_SCHD_EVENT* schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT* schdEvent);
void L2_SchdEventAppendMsg(L2_SCHD_EVENT *schdEvent, void *msg);
bool L2_isEventScheduled(const L2_SCHD_EVENT *schdEvent);
int32_t L2_CancelEvent(L2_SCHD_EVENT *schdEvent);
#endif //SCHDEVENT_H_
#endif // SCHDEVENT_H_
datalink814_ge/src/linklayer/time_sync.c
+128 -83
View File
@@ -1,19 +1,19 @@
/*****************************************************************************/
/**
* Description: Time sync implementation file.
*
*
* Author: Author's name
* Date: 2021-12-04 16:32
* LastEditors: FanHao
* LastEditTime: 2022-07-10 20:43
* Revisions: Rev 0.0
* History:
* History:
* [2022-07-10]: New branch for Dijian.
* [2021-12-04]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
/***************************** Include Files *********************************/
@@ -26,30 +26,28 @@
#include "../common.h"
/************************** Constant Definitions *****************************/
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
#define RTT_DOPPLER_NONE UINT32_MAX
#define LIGHT_SPPED_AIR 299702547.0 // m/s
#define LIGHT_SPPED_VAC 299792458.0 // m/s
#define RTT_DOPPLER_NONE UINT32_MAX
const TIME_WORD RTT_TIME_NONE = {
.int_part = UINT32_MAX,
.frac_part = UINT32_MAX
};
.frac_part = UINT32_MAX};
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
/************************** Variable Definitions *****************************/
/************************** Function Prototypes ******************************/
static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_STGE newStage);
static void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage);
static void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType);
/*****************************************************************************/
/**
* @brief a slot alignment, base on physical layer information.
*
* @param instPtr a pointer to linklayer layer instance
* @param frameNum
* @param slotNum
* @param frameNum
* @param slotNum
*
* @return SUCCESS Initialize successfully.
* FAILURE Initialization failed, allocate memory fail or initial
@@ -57,10 +55,12 @@ static void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE
*
******************************************************************************/
// TODO: Check
int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum, bool relative){
int32_t L2_TimeAlign(L2_INST *instPtr, uint32_t frameNum, uint32_t slotNum, bool relative)
{
L1_FRAME_TYPE lastRecvdFrame;
lastRecvdFrame = Phy_GetLastRecvFrameType(instPtr->config.phyInstPtr);
if(lastRecvdFrame == L1_NONE_FRAME){
if (lastRecvdFrame == L1_NONE_FRAME)
{
return EIGNORE;
}
@@ -71,7 +71,8 @@ int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum, bool
expctTime = ((uint64_t)(frameNum * L2_SLOTS_PER_FRAME + slotNum)) * TIME_FRAC_MAX;
expctTime += Phy_GetTimeComp(lastRecvdFrame);
if(relative) {
if (relative)
{
expctTime += (uint64_t)instPtr->netStartTime * L2_SLOTS_PER_FRAME * TIME_FRAC_MAX;
}
@@ -97,7 +98,8 @@ int32_t L2_TimeAlign(L2_INST* instPtr, uint32_t frameNum, uint32_t slotNum, bool
* @return NONE.
*
******************************************************************************/
static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_STGE newStage) {
static void RTT_StageUpdate(L2_RTT_FSM *rttFsmPtr, L2_RTT_STGE newStage)
{
ASSERT(rttFsmPtr != NULL);
ASSERT(newStage >= RTT_NONE_STAGE && newStage <= RTT_RSP2);
DBG_INFO("RTT Stage Update: %d -> %d.\n", rttFsmPtr->rttStage, newStage);
@@ -113,7 +115,8 @@ static void RTT_StageUpdate(L2_RTT_FSM* rttFsmPtr, L2_RTT_STGE newStage) {
* @return NONE
*
******************************************************************************/
void RTT_Reset(L2_RTT_FSM* rttFsmPtr){
void RTT_Reset(L2_RTT_FSM *rttFsmPtr)
{
ASSERT(rttFsmPtr != NULL);
memset(&(rttFsmPtr->rttRecord), 0x00, sizeof(RTT_RECORD));
rttFsmPtr->rttStage = RTT_REQ1;
@@ -135,15 +138,15 @@ void RTT_Reset(L2_RTT_FSM* rttFsmPtr){
* @return NONE
*
******************************************************************************/
void RTT_Destroy(L2_RTT_FSM* rttFsmPtr){
void RTT_Destroy(L2_RTT_FSM *rttFsmPtr)
{
free(rttFsmPtr);
}
/*****************************************************************************/
/**
* @brief Get the frame start time. If torType == L2_TOR_TX, get the next
* slot start time, if torType == L2_TOR_RX get last frame start
* slot start time, if torType == L2_TOR_RX get last frame start
* time.
*
* @param instPtr a pointer to linklayer layer instance
@@ -153,34 +156,39 @@ void RTT_Destroy(L2_RTT_FSM* rttFsmPtr){
* @return NONE
*
******************************************************************************/
void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType){
void RTT_GetFrameStart(L2_INST *instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType)
{
ASSERT(instPtr != NULL);
ASSERT(tmStamp != NULL);
// Get the next frame start time
if(torType == L2_TOR_TX) {
tmStamp->int_part = L2_GetNextFrameNum(instPtr)*L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
tmStamp->frac_part = 0 ;
if (torType == L2_TOR_TX)
{
tmStamp->int_part = L2_GetNextFrameNum(instPtr) * L2_SLOTS_PER_FRAME + L2_GetNextSlotNum(instPtr);
tmStamp->frac_part = 0;
}
// Get the last reiceive frame start time
else if(torType == L2_TOR_RX) {
PHY_INST* phyRef = instPtr->config.phyInstPtr;
else if (torType == L2_TOR_RX)
{
PHY_INST *phyRef = instPtr->config.phyInstPtr;
TIME_WORD timeStamp;
int64_t timeDiff;
Phy_GetLastReceivedFrameSyncTimeStamp(phyRef, &timeStamp);
timeDiff = (uint64_t) timeStamp.frac_part;
timeDiff = (uint64_t)timeStamp.frac_part;
timeDiff += ((uint64_t)timeStamp.int_part) * TIME_FRAC_MAX;
Phy_GetLastReceivedSlotStartTimeStamp(phyRef, tmStamp);
timeDiff -= ((uint64_t) tmStamp->int_part) * TIME_FRAC_MAX;
timeDiff -= (uint64_t) tmStamp->frac_part;
timeDiff -= ((uint64_t)tmStamp->int_part) * TIME_FRAC_MAX;
timeDiff -= (uint64_t)tmStamp->frac_part;
timeDiff = (int64_t)Phy_GetTimeComp(L2_L2frame2L1frame(L2_GetLastFrameType(instPtr))) - timeDiff;
if (timeDiff < 0) {
if (timeDiff < 0)
{
timeDiff += TIME_FRAC_MAX;
tmStamp->int_part--;
_nop_();
}
tmStamp->frac_part = (TIME_FRAC)timeDiff;
}
else {
else
{
*tmStamp = RTT_TIME_NONE;
DBG_ERROR("Unexpected tx/rx mode, setting dummy values.\n");
}
@@ -196,9 +204,11 @@ void RTT_GetFrameStart(L2_INST* instPtr, TIME_WORD *tmStamp, L2_TOR_TYPE torType
* EMAC_MEMORY_ALLOC_ERROR Memory allocate error.
*
******************************************************************************/
int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr){
(*rttFsmPtr) = (L2_RTT_FSM*) malloc(sizeof(L2_RTT_FSM));
if (*rttFsmPtr == NULL) {
int32_t RTT_Init(L2_RTT_FSM **rttFsmPtr)
{
(*rttFsmPtr) = (L2_RTT_FSM *)malloc(sizeof(L2_RTT_FSM));
if (*rttFsmPtr == NULL)
{
free(*rttFsmPtr);
return EALLOC;
}
@@ -213,23 +223,27 @@ int32_t RTT_Init(L2_RTT_FSM** rttFsmPtr){
* @param instPtr a pointer to linklayer layer instance
* @param torType tx/rx mode type
*
* @return SUCCESS
* @return SUCCESS
* error code
*
******************************************************************************/
int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
int32_t RTT_FsmUpdate(L2_INST *instPtr, L2_TOR_TYPE torType)
{
ASSERT(instPtr != NULL);
L2_RTT_FSM* rttFsmPtr = L2_GetRttFsm(instPtr);
RTT_RECORD* rttRecord = &(rttFsmPtr->rttRecord);
L2_RTT_FSM *rttFsmPtr = L2_GetRttFsm(instPtr);
RTT_RECORD *rttRecord = &(rttFsmPtr->rttRecord);
PHY_INST *phyRef = instPtr->config.phyInstPtr;
DBG_INFO("-------- RTT FSM Update --------\n");
// Stage trans
// For Head Node: Tx->Rx->Tx->Rx
if(L2_GetNodeType(instPtr) == L2_NODE_HEAD){
switch(rttFsmPtr->rttStage) {
case RTT_REQ1 :
if(torType == L2_TOR_TX){
if (L2_GetNodeType(instPtr) == L2_NODE_HEAD)
{
switch (rttFsmPtr->rttStage)
{
case RTT_REQ1:
if (torType == L2_TOR_TX)
{
DBG_INFO("Prepare data for RTT REQ-1\n");
// Update Tt1
RTT_GetFrameStart(instPtr, &(rttRecord->Tt1), torType);
@@ -241,13 +255,16 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
rttFsmPtr->doppler = RTT_DOPPLER_NONE;
RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_RSP1 :
if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){
case RTT_RSP1:
if (torType == L2_TOR_RX)
{
if (L2_isDataAvail(instPtr) == true)
{
DBG_INFO("RTT RSP-1 have been received.\n");
DBG_INFO("Preparing data for RTT REQ-2.\n");
rttFsmPtr->isR2Valid = true;
@@ -258,7 +275,8 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
// Calculate TroundA
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt1), &(rttRecord->Tr2), &(rttRecord->TroundA));
}
else{
else
{
DBG_WARNING("RTT RSP-1 had not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set Tr2 dopplerR2 and TroundA to dummy value.\n");
rttFsmPtr->isR2Valid = false;
@@ -273,21 +291,25 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
DBG_INFO("TroundA-timeH:%u.\n", rttRecord->TroundA.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_REQ2 :
if(torType == L2_TOR_TX){
case RTT_REQ2:
if (torType == L2_TOR_TX)
{
DBG_INFO("Prepare data for RTT REQ-2\n");
// Update Tt3
RTT_GetFrameStart(instPtr, &(rttRecord->Tt3), torType);
if ( rttFsmPtr->isR2Valid ) {
if (rttFsmPtr->isR2Valid)
{
// Calculate reply A
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr2), &(rttRecord->TreplyA));
}
else {
else
{
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set TreplyA to dummy value.\n");
rttRecord->TreplyA = RTT_TIME_NONE;
@@ -302,13 +324,16 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
rttFsmPtr->doppler = rttRecord->dopplerR2;
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_RSP2 :
if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){
case RTT_RSP2:
if (torType == L2_TOR_RX)
{
if (L2_isDataAvail(instPtr) == true)
{
DBG_INFO("RTT RSP-2 have been received.\n");
rttFsmPtr->isR4Valid = true;
// Update doppler
@@ -319,7 +344,8 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
// Calculate TroundA
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt3), &(rttRecord->Tr4), &(rttRecord->TroundA));
}
else{
else
{
DBG_WARNING("RTT RSP-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set Tr4 dopplerR4 and TroundA to dummy value.\n");
rttFsmPtr->isR4Valid = false;
@@ -335,21 +361,26 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
DBG_INFO("RTT procedure end...\n");
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
default :
default:
DBG_ERROR("Unexpected RTT FSM stage [%d].\n", rttFsmPtr->rttStage);
break;
}
}
// For Member Node: Rx->Tx->Rx->Tx
else{
switch(rttFsmPtr->rttStage) {
case RTT_REQ1 :
if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){
else
{
switch (rttFsmPtr->rttStage)
{
case RTT_REQ1:
if (torType == L2_TOR_RX)
{
if (L2_isDataAvail(instPtr) == true)
{
DBG_INFO("RTT REQ-1 have been received.\n");
DBG_INFO("Preparing data for RTT RSP-1.\n");
rttFsmPtr->isR2Valid = true;
@@ -358,7 +389,8 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
// Update Tr1
RTT_GetFrameStart(instPtr, &(rttRecord->Tr1), torType);
}
else{
else
{
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set dopplerR1 and Tr1 to dummy value.\n");
rttFsmPtr->isR1Valid = false;
@@ -370,20 +402,24 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
DBG_INFO("Tr1-timeH:%u.\n", rttRecord->Tr1.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_RSP1);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_RSP1 :
if(torType == L2_TOR_TX){
case RTT_RSP1:
if (torType == L2_TOR_TX)
{
DBG_INFO("Prepare data for RTT RSP-1\n");
// Update Tt2
RTT_GetFrameStart(instPtr, &(rttRecord->Tt2), torType);
if ( rttFsmPtr->isR1Valid ) {
if (rttFsmPtr->isR1Valid)
{
// Calculate reply B
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr1), &(rttRecord->Tt2), &(rttRecord->TreplyB));
}
else {
else
{
rttRecord->TreplyB = RTT_TIME_NONE;
DBG_WARNING("RTT REQ-1 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set ReplayB to dummy value.\n");
@@ -398,13 +434,16 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
rttFsmPtr->doppler = rttRecord->dopplerR1;
RTT_StageUpdate(rttFsmPtr, RTT_REQ2);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_REQ2 :
if(torType == L2_TOR_RX){
if(L2_isDataAvail(instPtr) == true){
case RTT_REQ2:
if (torType == L2_TOR_RX)
{
if (L2_isDataAvail(instPtr) == true)
{
DBG_INFO("RTT REQ-2 have been received.\n");
DBG_INFO("Preparing data for RTT RSP-2.\n");
rttFsmPtr->isR3Valid = true;
@@ -415,7 +454,8 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
// Calculate TroundB
TimeCtrl_CalculateTimeGap(&(rttRecord->Tt2), &(rttRecord->Tr3), &(rttRecord->TroundB));
}
else{
else
{
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set dopplerR3 and ReplayB to dummy value.\n");
rttFsmPtr->isR3Valid = false;
@@ -429,21 +469,25 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
DBG_INFO("TroundB-timeH:%u.\n", rttRecord->TroundB.int_part);
RTT_StageUpdate(rttFsmPtr, RTT_RSP2);
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
case RTT_RSP2 :
if(torType == L2_TOR_TX){
case RTT_RSP2:
if (torType == L2_TOR_TX)
{
DBG_INFO("Prepare data for RTT RSP-2\n");
// Update Tt4
RTT_GetFrameStart(instPtr, &(rttRecord->Tt4), torType);
if ( rttFsmPtr->isR3Valid ) {
if (rttFsmPtr->isR3Valid)
{
// Calculate reply B
TimeCtrl_CalculateTimeGap(&(rttRecord->Tr3), &(rttRecord->Tt4), &(rttRecord->TreplyB));
// TODO: implement a time adjustment.
}
else {
else
{
DBG_WARNING("RTT REQ-2 have not been received [Stage: %d].\n", rttFsmPtr->rttStage);
DBG_WARNING("Set ReplayB to dummy value.\n");
rttRecord->TreplyB = RTT_TIME_NONE;
@@ -459,7 +503,8 @@ int32_t RTT_FsmUpdate(L2_INST* instPtr, L2_TOR_TYPE torType) {
RTT_StageUpdate(rttFsmPtr, RTT_REQ1);
DBG_INFO("RTT procedure end...\n");
}
else{
else
{
DBG_INFO("RTT FSM remain current state [%d].\n", rttFsmPtr->rttStage);
}
break;
datalink814_ge/src/main.c
+111 -81
View File
@@ -20,10 +20,10 @@
/***************************** Include Files *********************************/
#include <stdint.h>
#include "config/app_config.h"
#include "xscugic.h" // The generic interrupt controller driver component
#include "xil_exception.h" // ARM Cortex A9 specific exception related APIs
#include "xscugic.h" // The generic interrupt controller driver component
#include "xil_exception.h" // ARM Cortex A9 specific exception related APIs
#include "xgpio.h"
#include "sleep.h" // Sleep APIs for Cortex A9
#include "sleep.h" // Sleep APIs for Cortex A9
#include "xil_cache.h"
#include "linklayer/linklayer814.h"
#include "linklayer/msg_def.h"
@@ -47,39 +47,36 @@ L2_CONFIG l2Config = {
.bkpHeadId = 1,
.netScale = 2,
.phyInstPtr = NULL,
.tmcInstPtr = NULL
};
.tmcInstPtr = NULL};
TIMECTRL_CONFIG tmcConfig = {
.baseAddr = TIME_CONTROLLER_BASEADDR,
.reset = 10
};
.reset = 10};
PHY_CONFIG l1Config = {
.devId = PHY_DEVICE_ID,
.txBramId = TXBUFF_BRAM_ID,
.rxBramId = RXBUFF_BRAM_ID,
.antBramId = ANTCONFIG_BRAM_ID,
.baseAddr = PHY_CTRL_BASEADDR
};
.baseAddr = PHY_CTRL_BASEADDR};
MPC_CONFIG mpcConfig = {
.bramDevId = MPC_PCIE_BRAM_ID,
.msgLenMax = MPC_BUFFER_LENGTH //byte
.msgLenMax = MPC_BUFFER_LENGTH // byte
};
static bool mpcStatusUpAssert = false;
/************************** Function Prototypes ******************************/
int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId);
int SetupInterruptSystem(XScuGic *instPtr, uint32_t devId);
void PhysicalIntr_Handler(void *callBackRef);
void PeriodicPull_Handler(void *callBackRef);
#ifdef _MPC_CTRL
void MPC_IntrHandler(void* CallBackRef);
void MPC_IntrHandler(void *CallBackRef);
#endif
/*********************** Driver Instance Definitions **************************/
TIMECTRL_INST* tmcInstPtr = NULL;
PHY_INST* phyInstPtr = NULL;
L2_INST* linkInstPtr = NULL;
static XScuGic gicInst; //General interrupt controller intance
TIMECTRL_INST *tmcInstPtr = NULL;
PHY_INST *phyInstPtr = NULL;
L2_INST *linkInstPtr = NULL;
static XScuGic gicInst; // General interrupt controller intance
// FIXME: Only for test...
XGpio configGpio; /* The Instance of the GPIO Driver */
/****************************************************************************/
@@ -96,21 +93,24 @@ XGpio configGpio; /* The Instance of the GPIO Driver */
* FAILURE if initialization failed.
*
*****************************************************************************/
int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
int SetupInterruptSystem(XScuGic *instPtr, uint32_t devId)
{
XScuGic_Config *configPtr;
int Status;
uint32_t intrState;
// Initialize the GIC instance.
configPtr = XScuGic_LookupConfig(devId);
Status = XScuGic_CfgInitialize(instPtr, configPtr,
configPtr->CpuBaseAddress);
if (Status != SUCCESS) {
configPtr->CpuBaseAddress);
if (Status != SUCCESS)
{
return Status;
}
// Perform a self-test to ensure that the hardware was built correctly.
Status = XScuGic_SelfTest(instPtr);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
return Status;
}
// Clear any pending interrupt flags
@@ -140,8 +140,9 @@ int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
// Connect the interrupt service routine isr1 to the interrupt controller
Status = XScuGic_Connect(instPtr, PHY_INTR_ID,
(Xil_ExceptionHandler) PhysicalIntr_Handler, (void *) phyInstPtr);
if (Status != SUCCESS) {
(Xil_ExceptionHandler)PhysicalIntr_Handler, (void *)phyInstPtr);
if (Status != SUCCESS)
{
return Status;
}
@@ -150,8 +151,9 @@ int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
// Connect the interrupt service routine isr1 to the interrupt controller
Status = XScuGic_Connect(instPtr, PERIODIC_5MS_INTR_ID,
(Xil_ExceptionHandler) PeriodicPull_Handler, (void *) linkInstPtr);
if (Status != SUCCESS) {
(Xil_ExceptionHandler)PeriodicPull_Handler, (void *)linkInstPtr);
if (Status != SUCCESS)
{
return Status;
}
@@ -160,8 +162,9 @@ int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
XScuGic_SetPriorityTriggerType(instPtr, MPC_RX_DONE_INTR_ID, 0x00, 0x03);
// Connect the interrupt service routine isr1 to the interrupt controller
Status = XScuGic_Connect(instPtr, MPC_RX_DONE_INTR_ID,
(Xil_ExceptionHandler) MPC_IntrHandler, (void *) mpcInstPtr);
if (Status != XST_SUCCESS) {
(Xil_ExceptionHandler)MPC_IntrHandler, (void *)mpcInstPtr);
if (Status != XST_SUCCESS)
{
return Status;
}
XScuGic_Enable(instPtr, MPC_RX_DONE_INTR_ID);
@@ -176,8 +179,8 @@ int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
Xil_ExceptionInit();
Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
(Xil_ExceptionHandler) XScuGic_InterruptHandler,
(void *) instPtr);
(Xil_ExceptionHandler)XScuGic_InterruptHandler,
(void *)instPtr);
// Enable non-critical exceptions
Xil_ExceptionEnable();
@@ -185,10 +188,9 @@ int SetupInterruptSystem(XScuGic* instPtr, uint32_t devId){
return Status;
} // SetupInterruptSystem
#ifdef _MPC_CTRL
void MPC_PCIESentStart(){
void MPC_PCIESentStart()
{
uint32_t GpioConfig = XGpio_DiscreteRead(&configGpio, 2);
GpioConfig ^= 0x00000002;
XGpio_DiscreteWrite(&configGpio, 2, GpioConfig);
@@ -203,7 +205,8 @@ void MPC_PCIESentStart(){
* @return None.
*
******************************************************************************/
void MPC_IntrHandler(void* CallBackRef) {
void MPC_IntrHandler(void *CallBackRef)
{
MPC_MsgHandler(mpcInstPtr);
}
#endif
@@ -217,67 +220,73 @@ void MPC_IntrHandler(void* CallBackRef) {
* @return None.
*
******************************************************************************/
void PhysicalIntr_Handler(void *callBackRef) {
void PhysicalIntr_Handler(void *callBackRef)
{
uint32_t GpioConfig = XGpio_DiscreteRead(&configGpio, 2);
if(L2_isDataAvail(linkInstPtr) == true ){
GpioConfig^=0x00000001;
if (L2_isDataAvail(linkInstPtr) == true)
{
GpioConfig ^= 0x00000001;
}
Phy_UpdateState(phyInstPtr);
if(phyInstPtr->TOR.curr == PHY_TOR_TX){
switch(phyInstPtr->frameType.curr){
if (phyInstPtr->TOR.curr == PHY_TOR_TX)
{
switch (phyInstPtr->frameType.curr)
{
case L1_1M2BPS_5MS_FRAME:
mpcRecord->frame1Sent ++ ;
mpcRecord->frame1Sent++;
break;
case L1_200KBPS_5MS_FRAME:
mpcRecord->frame2Sent ++ ;
mpcRecord->frame2Sent++;
break;
case L1_40KBPS_10MS_FRAME:
mpcRecord->frame3Sent ++ ;
mpcRecord->frame3Sent++;
break;
case L1_40KBPS_20MS_FRAME:
mpcRecord->frame4Sent ++ ;
mpcRecord->frame4Sent++;
break;
case L1_10KBPS_20MS_FRAME:
mpcRecord->frame5Sent ++ ;
mpcRecord->frame5Sent++;
break;
case L1_2KBPS_100MS_FRAME:
mpcRecord->frame6Sent ++ ;
mpcRecord->frame6Sent++;
break;
case L1_10KBPS_10MS_FRAME:
mpcRecord->frame7Sent ++ ;
mpcRecord->frame7Sent++;
break;
case L1_2KBPS_20MS_FRAME:
mpcRecord->frame8Sent ++ ;
mpcRecord->frame8Sent++;
break;
default:
break;
}
}
if(phyInstPtr->TOR.last == PHY_TOR_RX && phyInstPtr->isFrameSync){
switch(phyInstPtr->frameType.last){
if (phyInstPtr->TOR.last == PHY_TOR_RX && phyInstPtr->isFrameSync)
{
switch (phyInstPtr->frameType.last)
{
case L1_1M2BPS_5MS_FRAME:
mpcRecord->frame1Recv ++ ;
mpcRecord->frame1Recv++;
break;
case L1_200KBPS_5MS_FRAME:
mpcRecord->frame2Recv ++ ;
mpcRecord->frame2Recv++;
break;
case L1_40KBPS_10MS_FRAME:
mpcRecord->frame3Recv ++ ;
mpcRecord->frame3Recv++;
break;
case L1_40KBPS_20MS_FRAME:
mpcRecord->frame4Recv ++ ;
mpcRecord->frame4Recv++;
break;
case L1_10KBPS_20MS_FRAME:
mpcRecord->frame5Recv ++ ;
mpcRecord->frame5Recv++;
break;
case L1_2KBPS_100MS_FRAME:
mpcRecord->frame6Recv ++ ;
mpcRecord->frame6Recv++;
break;
case L1_10KBPS_10MS_FRAME:
mpcRecord->frame7Recv ++ ;
mpcRecord->frame7Recv++;
break;
case L1_2KBPS_20MS_FRAME:
mpcRecord->frame8Recv ++ ;
mpcRecord->frame8Recv++;
break;
default:
break;
@@ -295,16 +304,19 @@ void PhysicalIntr_Handler(void *callBackRef) {
* @return None.
*
*****************************************************************************/
void PeriodicPull_Handler(void *callBackRef) {
void PeriodicPull_Handler(void *callBackRef)
{
int32_t Status;
Status = Linklayer_Proc(linkInstPtr);
if(Status != SUCCESS){
if (Status != SUCCESS)
{
DBG_ERROR("Link layer runs failed, with error code: %ld\n", Status);
_nop_();
}
if (linkInstPtr->slotNum.next % DATA_STORE_INTERVAL == 1) {
if (linkInstPtr->slotNum.next % DATA_STORE_INTERVAL == 1)
{
mpcStatusUpAssert = true;
mpcRecord->frame1Cnt = mpcRecord->frame1Recv - mpcRecord->frame1CntLast;
mpcRecord->frame2Cnt = mpcRecord->frame2Recv - mpcRecord->frame2CntLast;
@@ -325,7 +337,7 @@ void PeriodicPull_Handler(void *callBackRef) {
mpcRecord->frame7CntLast = mpcRecord->frame7Recv;
mpcRecord->frame8CntLast = mpcRecord->frame8Recv;
}
} // PhyIntr_Handler
} // PhyIntr_Handler
/*****************************************************************************/
/**
@@ -337,19 +349,23 @@ void PeriodicPull_Handler(void *callBackRef) {
* @return
*
******************************************************************************/
int main(void) {
int main(void)
{
int Status;
Status = HARDWARE_initialize();
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("Hardware Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Hardware Init FAILED! \n");
return Status;
}
Status = XGpio_Initialize(&configGpio, XPAR_CONFIG_GPIO_DEVICE_ID);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_PRINT("Config Gpio Initialization Failed\n");
return FAILURE;
}
@@ -358,20 +374,24 @@ int main(void) {
// Init time controller
Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("TmcCtrl Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("TmcCtrl Init FAILED! \n");
return Status;
}
// Init physical layer
Status = Phy_InstInit(&phyInstPtr, &l1Config);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("PhyCtrl Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("PhyCtrl Init FAILED! \n");
return Status;
}
@@ -381,30 +401,36 @@ int main(void) {
l2Config.tmcInstPtr = tmcInstPtr;
Status = Linklayer_Init(&linkInstPtr, &l2Config);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("LinkLayer Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Linklayer Init FAILED! \n");
return Status;
}
// Init MPC
Status = MPC_Init(&mpcInstPtr, &mpcConfig);
if (Status == SUCCESS) {
if (Status == SUCCESS)
{
DBG_PRINT("MPC Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("MPC Init FAILED! \n");
return Status;
}
// Initialize GIC.
Status = SetupInterruptSystem(&gicInst ,SCUGIC_DEVICE_ID);
if (Status == SUCCESS) {
Status = SetupInterruptSystem(&gicInst, SCUGIC_DEVICE_ID);
if (Status == SUCCESS)
{
DBG_PRINT("Intc Init SUCCESS! \n");
}
else {
else
{
DBG_PRINT("Intc Init FAILED! \n");
return Status;
}
@@ -415,16 +441,20 @@ int main(void) {
// FIXME: only for debug, linklayer should be start by MPC
Linklayer_Start(linkInstPtr);
while(true) {
if (mpcStatusUpAssert == true) {
while (true)
{
if (mpcStatusUpAssert == true)
{
mpcStatusUpAssert = false;
if (linkInstPtr->buffSwitch == 0) {
if (linkInstPtr->buffSwitch == 0)
{
MPC_StateUpload(mpcInstPtr, linkInstPtr->mgmtMsgRecvBuff1, linkInstPtr->mgmtMsgRecvCnt1,
linkInstPtr->svcMsgRecvBuff1, linkInstPtr->svcMsgRecvCnt1, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
linkInstPtr->svcMsgRecvBuff1, linkInstPtr->svcMsgRecvCnt1, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
}
else {
else
{
MPC_StateUpload(mpcInstPtr, linkInstPtr->mgmtMsgRecvBuff0, linkInstPtr->mgmtMsgRecvCnt0,
linkInstPtr->svcMsgRecvBuff0, linkInstPtr->svcMsgRecvCnt0, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
linkInstPtr->svcMsgRecvBuff0, linkInstPtr->svcMsgRecvCnt0, &(linkInstPtr->mgmtNetFSM->mgmtMsgLog));
}
}
}
datalink814_ge/src/mpc/mpc_ctrl.c
+150 -122
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: A brief description of this file
*
*
* Date: 2022-01-19 00:54
* LastEditors: Fan Hao
* LastEditTime: 2022-03-28 20:24
* Revisions: Rev 0.0
* History:
* History:
* [2022-01-19]: Initial version.
* [2022-03-28]: Add general information print function.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#include <inttypes.h>
@@ -31,8 +31,8 @@
#define STATE_STROAGE_BUFF_MAX 32768 // 32K Bytes
static int32_t MPC_InsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk);
static int32_t MPC_GetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t* chunk);
static int32_t MPC_InsertChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk);
static int32_t MPC_GetChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t *chunk);
#ifdef _MPC_UART
#include <frame_uart.h>
@@ -44,19 +44,18 @@ static int32_t MPC_GetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit,
#include "../phy_ctrl/phy_ctrl.h"
extern L2_CONFIG l2Config;
extern PHY_INST* phyInstPtr;
extern L2_INST* linkInstPtr;
extern PHY_INST *phyInstPtr;
extern L2_INST *linkInstPtr;
MPC_INST* mpcInstPtr = NULL;
MPC_RECORD* mpcRecord = NULL;
BIT_ARRAY* mpcTxMsgBuff = NULL;
BIT_ARRAY* mpcRxMsgBuff = NULL;
MPC_INST *mpcInstPtr = NULL;
MPC_RECORD *mpcRecord = NULL;
BIT_ARRAY *mpcTxMsgBuff = NULL;
BIT_ARRAY *mpcRxMsgBuff = NULL;
static MPC_MSG_HEADER mpcHeader = {
.head = 0x5302,
};
/*****************************************************************************/
/**
* @brief Insert a data chunk to a bit array.
@@ -71,9 +70,11 @@ static MPC_MSG_HEADER mpcHeader = {
* SUCCESS
*
******************************************************************************/
static int32_t MPC_InsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk){
static int32_t MPC_InsertChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk)
{
ASSERT(bitarr != NULL);
if(bitarr->num_of_bits < offset+numBit){
if (bitarr->num_of_bits < offset + numBit)
{
return FAILURE;
}
bit_array_set_wordn(bitarr, offset, chunk, numBit);
@@ -94,21 +95,25 @@ static int32_t MPC_InsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numB
* SUCCESS
*
******************************************************************************/
static int32_t MPC_GetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t* chunk){
static int32_t MPC_GetChunk(BIT_ARRAY *bitarr, uint32_t offset, uint32_t numBit, uint64_t *chunk)
{
ASSERT(bitarr != NULL);
if(bitarr->num_of_bits < offset+numBit || numBit > 64){
if (bitarr->num_of_bits < offset + numBit || numBit > 64)
{
return FAILURE;
}
*chunk = bit_array_get_wordn(bitarr, offset, numBit);
return SUCCESS;
}
static uint32_t MPC_GetMsgLen(MPC_MSG_TYPE type){
switch(type){
static uint32_t MPC_GetMsgLen(MPC_MSG_TYPE type)
{
switch (type)
{
case MPC_MSG_TEST:
return (MPC_MSG_TEST_LEN/8);
return (MPC_MSG_TEST_LEN / 8);
case MPC_MSG_STATE:
return (MPC_MSG_STATE_LEN/8);
return (MPC_MSG_STATE_LEN / 8);
default:
return MPC_MSG_LEN_NONE;
}
@@ -124,13 +129,15 @@ static uint32_t MPC_GetMsgLen(MPC_MSG_TYPE type){
* error code.
*
******************************************************************************/
int MPC_Init(MPC_INST** instPtr, MPC_CONFIG* configPtr){
int MPC_Init(MPC_INST **instPtr, MPC_CONFIG *configPtr)
{
ASSERT(instPtr != NULL);
ASSERT(configPtr != NULL);
int Status = SUCCESS;
XBram_Config *bramConfigPtr;
(*instPtr) = (MPC_INST*)malloc(sizeof(MPC_INST));
if((*instPtr) == NULL){
(*instPtr) = (MPC_INST *)malloc(sizeof(MPC_INST));
if ((*instPtr) == NULL)
{
Status = EALLOC;
goto init_failed;
}
@@ -138,41 +145,47 @@ int MPC_Init(MPC_INST** instPtr, MPC_CONFIG* configPtr){
(*instPtr)->configs.msgLenMax = configPtr->msgLenMax;
bramConfigPtr = XBram_LookupConfig(configPtr->bramDevId);
if (bramConfigPtr == NULL) {
if (bramConfigPtr == NULL)
{
Status = EDINVAL;
goto init_failed;
}
Status = XBram_CfgInitialize(&((*instPtr)->pcieBram), bramConfigPtr,
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
bramConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto init_failed;
}
/* Execute the BRAM driver selftest. */
Status = XBram_SelfTest(&((*instPtr)->pcieBram), 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto init_failed;
}
(*instPtr)->txBaseAddr = (uint8_t*)((*instPtr)->pcieBram.Config.MemBaseAddress);
(*instPtr)->rxBaseAddr = (uint8_t*)((*instPtr)->pcieBram.Config.MemBaseAddress);
(*instPtr)->txBaseAddr = (uint8_t *)((*instPtr)->pcieBram.Config.MemBaseAddress);
(*instPtr)->rxBaseAddr = (uint8_t *)((*instPtr)->pcieBram.Config.MemBaseAddress);
mpcRecord = (MPC_RECORD*) malloc(sizeof(MPC_RECORD));
if(mpcRecord == NULL){
mpcRecord = (MPC_RECORD *)malloc(sizeof(MPC_RECORD));
if (mpcRecord == NULL)
{
Status = EALLOC;
goto init_failed;
}
mpcTxMsgBuff = bit_array_create(STATE_STROAGE_BUFF_MAX*8);
if(mpcTxMsgBuff == NULL){
mpcTxMsgBuff = bit_array_create(STATE_STROAGE_BUFF_MAX * 8);
if (mpcTxMsgBuff == NULL)
{
goto init_failed;
}
mpcRxMsgBuff = bit_array_create(MPC_MSG_LEN_MAX);
if(mpcRxMsgBuff == NULL){
if (mpcRxMsgBuff == NULL)
{
goto init_failed;
}
return Status;
memset(mpcRecord, 0x00, sizeof(MPC_RECORD));
memset(mpcTxMsgBuff, 0x00, sizeof(STATE_STROAGE_BUFF_MAX*8));
memset(mpcTxMsgBuff, 0x00, sizeof(STATE_STROAGE_BUFF_MAX * 8));
memset(mpcRxMsgBuff, 0x00, sizeof(MPC_MSG_LEN_MAX));
init_failed:
@@ -193,69 +206,75 @@ init_failed:
* error code.
*
******************************************************************************/
int MPC_MsgHandler(MPC_INST* instPtr){
int MPC_MsgHandler(MPC_INST *instPtr)
{
ASSERT(instPtr != NULL);
MPC_MSG_HEADER* pktHeader = (MPC_MSG_HEADER*)(instPtr->rxBaseAddr);
MPC_MSG_HEADER *pktHeader = (MPC_MSG_HEADER *)(instPtr->rxBaseAddr);
// instPtr->txBaseAddr + MPC_MSG_HEAD_LEN;
uint64_t dataChunk = 0;
switch(pktHeader->type){
case MPC_MSG_TEST_CODE:
memcpy(mpcRxMsgBuff->words,
instPtr->rxBaseAddr + (MPC_MSG_HEAD_LEN/8),
DIV_ROUND_UP(MPC_MSG_TEST_LEN,8));
MPC_GetChunk(mpcRxMsgBuff, TEST_REQ_CMD_OFFSET, TEST_REQ_CMD_LEN, &dataChunk);
if((uint8_t)dataChunk == TEST_REQ_CMD_START){
MPC_GetChunk(mpcRxMsgBuff, TEST_MODE_SET_OFFSET, TEST_MODE_SET_LEN, &dataChunk);
// TODO: check.
if((uint8_t)dataChunk == TEST_MODE_HEAD){
l2Config.myType = L2_NODE_HEAD;
l2Config.myId = 1;
}
else{
l2Config.myType = L2_NODE_MEMBER;
l2Config.myId = 2;
}
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_FPGA_ST_OFFSET, TEST_REQ_FPGA_ST_LEN, 0x01);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_AD_ST_OFFSET, TEST_REQ_AD_ST_LEN, 0x01);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_RF_ST_OFFSET, TEST_REQ_RF_ST_LEN, 0x01);
MPC_SendMsg(instPtr, MPC_MSG_TEST, (uint8_t*)mpcTxMsgBuff->words, MPC_MSG_TEST_LEN);
Linklayer_Reconfig(linkInstPtr, &l2Config);
Linklayer_Start(linkInstPtr);
switch (pktHeader->type)
{
case MPC_MSG_TEST_CODE:
memcpy(mpcRxMsgBuff->words,
instPtr->rxBaseAddr + (MPC_MSG_HEAD_LEN / 8),
DIV_ROUND_UP(MPC_MSG_TEST_LEN, 8));
MPC_GetChunk(mpcRxMsgBuff, TEST_REQ_CMD_OFFSET, TEST_REQ_CMD_LEN, &dataChunk);
if ((uint8_t)dataChunk == TEST_REQ_CMD_START)
{
MPC_GetChunk(mpcRxMsgBuff, TEST_MODE_SET_OFFSET, TEST_MODE_SET_LEN, &dataChunk);
// TODO: check.
if ((uint8_t)dataChunk == TEST_MODE_HEAD)
{
l2Config.myType = L2_NODE_HEAD;
l2Config.myId = 1;
}
else if((uint8_t)dataChunk == TEST_REQ_CMD_END){
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_FPGA_ST_OFFSET, TEST_REQ_FPGA_ST_LEN, 0x00);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_AD_ST_OFFSET, TEST_REQ_AD_ST_LEN, 0x00);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_RF_ST_OFFSET, TEST_REQ_RF_ST_LEN, 0x00);
MPC_SendMsg(instPtr, MPC_MSG_TEST, (uint8_t*)mpcTxMsgBuff->words, MPC_MSG_TEST_LEN);
memset(mpcRecord, 0x00, sizeof(MPC_RECORD));
memset((uint8_t*)mpcTxMsgBuff->words,0x00,STATE_STROAGE_BUFF_MAX);
Linklayer_Halt(linkInstPtr);
MPC_SendMsg(instPtr, MPC_MSG_STATE, (uint8_t*)mpcTxMsgBuff->words, STATE_STROAGE_BUFF_MAX);
Phy_SetupNextSlot(phyInstPtr, PHY_TOR_SUSPEND, L1_200KBPS_5MS_FRAME, NULL); // any frame type is okay
else
{
l2Config.myType = L2_NODE_MEMBER;
l2Config.myId = 2;
}
else{
DBG_WARNING("Unexpected self-test command.\n");
}
break;
case MPC_MSG_TSYNC_CODE:
memcpy(mpcRxMsgBuff->words,
instPtr->rxBaseAddr + ((MPC_MSG_HEAD_LEN/8)/2),
DIV_ROUND_UP(MPC_MSG_TSYNC_LEN,8));
//FIXME:
// MPC_GetChunk(mpcRxMsgBuff, RTT_ADJ_OFFSET, RTT_ADJ_LEN, &dataChunk);
// rttRecord->rttAdj = (double)dataChunk ;
// MPC_GetChunk(mpcRxMsgBuff, TSYNC_ADJ_OFFSET, TSYNC_ADJ_LEN, &dataChunk);
// double tsyncAdj = (double)dataChunk * 0.192;
//FIXME:
// PhyWrite(TSYNC_PULS_ADJ, (uint32_t)tsyncAdj);
// MPC_GetChunk(mpcRxMsgBuff, DOPPLER_OFFSET, DOPPLER_LEN, &dataChunk);
// double dopplerAdj = (double)((dataChunk << 32)/192000000) ;
// PhyWrite(DOPPLER_ADJ, (uint32_t)dopplerAdj);
break;
default:
DBG_ERROR("Unknown message type code: %04X.\n\r", pktHeader->type);
return EPARAM;
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_FPGA_ST_OFFSET, TEST_REQ_FPGA_ST_LEN, 0x01);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_AD_ST_OFFSET, TEST_REQ_AD_ST_LEN, 0x01);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_RF_ST_OFFSET, TEST_REQ_RF_ST_LEN, 0x01);
MPC_SendMsg(instPtr, MPC_MSG_TEST, (uint8_t *)mpcTxMsgBuff->words, MPC_MSG_TEST_LEN);
Linklayer_Reconfig(linkInstPtr, &l2Config);
Linklayer_Start(linkInstPtr);
}
else if ((uint8_t)dataChunk == TEST_REQ_CMD_END)
{
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_FPGA_ST_OFFSET, TEST_REQ_FPGA_ST_LEN, 0x00);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_AD_ST_OFFSET, TEST_REQ_AD_ST_LEN, 0x00);
MPC_InsertChunk(mpcTxMsgBuff, TEST_REQ_RF_ST_OFFSET, TEST_REQ_RF_ST_LEN, 0x00);
MPC_SendMsg(instPtr, MPC_MSG_TEST, (uint8_t *)mpcTxMsgBuff->words, MPC_MSG_TEST_LEN);
memset(mpcRecord, 0x00, sizeof(MPC_RECORD));
memset((uint8_t *)mpcTxMsgBuff->words, 0x00, STATE_STROAGE_BUFF_MAX);
Linklayer_Halt(linkInstPtr);
MPC_SendMsg(instPtr, MPC_MSG_STATE, (uint8_t *)mpcTxMsgBuff->words, STATE_STROAGE_BUFF_MAX);
Phy_SetupNextSlot(phyInstPtr, PHY_TOR_SUSPEND, L1_200KBPS_5MS_FRAME, NULL); // any frame type is okay
}
else
{
DBG_WARNING("Unexpected self-test command.\n");
}
break;
case MPC_MSG_TSYNC_CODE:
memcpy(mpcRxMsgBuff->words,
instPtr->rxBaseAddr + ((MPC_MSG_HEAD_LEN / 8) / 2),
DIV_ROUND_UP(MPC_MSG_TSYNC_LEN, 8));
// // FIXME:
// MPC_GetChunk(mpcRxMsgBuff, RTT_ADJ_OFFSET, RTT_ADJ_LEN, &dataChunk);
// rttRecord->rttAdj = (double)dataChunk;
// MPC_GetChunk(mpcRxMsgBuff, TSYNC_ADJ_OFFSET, TSYNC_ADJ_LEN, &dataChunk);
// double tsyncAdj = (double)dataChunk * 0.192;
// // FIXME:
// PhyWrite(TSYNC_PULS_ADJ, (uint32_t)tsyncAdj);
// MPC_GetChunk(mpcRxMsgBuff, DOPPLER_OFFSET, DOPPLER_LEN, &dataChunk);
// double dopplerAdj = (double)((dataChunk << 32) / 192000000);
// PhyWrite(DOPPLER_ADJ, (uint32_t)dopplerAdj);
break;
default:
DBG_ERROR("Unknown message type code: %04X.\n\r", pktHeader->type);
return EPARAM;
}
return SUCCESS;
}
@@ -274,18 +293,22 @@ int MPC_MsgHandler(MPC_INST* instPtr){
* error code.
*
******************************************************************************/
int MPC_SendMsg(MPC_INST* instPtr, MPC_MSG_TYPE type, uint8_t* buffer, uint32_t dlen){
int MPC_SendMsg(MPC_INST *instPtr, MPC_MSG_TYPE type, uint8_t *buffer, uint32_t dlen)
{
ASSERT(instPtr != NULL);
if(buffer == NULL ||
if (buffer == NULL ||
dlen >= instPtr->configs.msgLenMax ||
(type <= MPC_MSG_NONE || type > MPC_MSG_STATE)){
(type <= MPC_MSG_NONE || type > MPC_MSG_STATE))
{
return EPARAM;
}
if(instPtr->pcieBram.IsReady == false){
if (instPtr->pcieBram.IsReady == false)
{
return EDINVAL;
}
int Status = SUCCESS;
switch(type){
switch (type)
{
case MPC_MSG_TEST:
mpcHeader.type = MPC_MSG_TEST_CODE;
break;
@@ -296,13 +319,12 @@ int MPC_SendMsg(MPC_INST* instPtr, MPC_MSG_TYPE type, uint8_t* buffer, uint32_t
DBG_ERROR("Unknown mpc message type: %d.\n", type);
break;
}
memcpy(instPtr->txBaseAddr, &mpcHeader, MPC_MSG_HEAD_LEN/8);
memcpy(instPtr->txBaseAddr + (MPC_MSG_HEAD_LEN/8), buffer, dlen);
memcpy(instPtr->txBaseAddr, &mpcHeader, MPC_MSG_HEAD_LEN / 8);
memcpy(instPtr->txBaseAddr + (MPC_MSG_HEAD_LEN / 8), buffer, dlen);
MPC_PCIESentStart();
return Status;
}
/*****************************************************************************/
/**
* @brief Upload current state to master pc.(content detail defined in mpc
@@ -314,24 +336,27 @@ int MPC_SendMsg(MPC_INST* instPtr, MPC_MSG_TYPE type, uint8_t* buffer, uint32_t
* error code.
*
******************************************************************************/
int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgCnt ,
uint8_t* svcBuffer , uint32_t svcMsgCnt , L2_MGMT_MSG_RECORD* mgmtMsg){
int MPC_StateUpload(MPC_INST *instPtr, uint8_t *mgmtBuffer, uint32_t mgmtMsgCnt,
uint8_t *svcBuffer, uint32_t svcMsgCnt, L2_MGMT_MSG_RECORD *mgmtMsg)
{
ASSERT(instPtr != NULL);
int32_t Status = SUCCESS;
uint64_t dataChunk = 0;
if(instPtr->pcieBram.IsReady == false){
if (instPtr->pcieBram.IsReady == false)
{
return EDINVAL;
}
// Stage
switch (linkInstPtr->mainState) {
case L2_INIT_STATE :
switch (linkInstPtr->mainState)
{
case L2_INIT_STATE:
dataChunk = STATE_STAGE_SETUP;
break;
case L2_INTRA_SEEK_STATE:
dataChunk = STATE_STAGE_RTNE;
break;
case L2_NORMAL_STATE :
case L2_NORMAL_STATE:
dataChunk = STATE_STAGE_SVC;
break;
default:
@@ -340,7 +365,7 @@ int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgC
MPC_InsertChunk(mpcTxMsgBuff, STATE_STAGE_OFFSET, STATE_STAGE_LEN, dataChunk);
// Net setup time
dataChunk = (L2_GetNextFrameNum(linkInstPtr) + linkInstPtr->netStartTime) * 1000 + L2_GetNextSlotNum(linkInstPtr) * 5 ;
dataChunk = (L2_GetNextFrameNum(linkInstPtr) + linkInstPtr->netStartTime) * 1000 + L2_GetNextSlotNum(linkInstPtr) * 5;
MPC_InsertChunk(mpcTxMsgBuff, STATE_SETUP_TIME_OFFSET, STATE_SETUP_TIME_LEN, dataChunk);
// number of nodes
dataChunk = L2_GetNodeListLen(linkInstPtr->nodeList);
@@ -422,10 +447,10 @@ int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgC
MPC_InsertChunk(mpcTxMsgBuff, STATE_FRAME_8_RECV_OFFSET, STATE_FRAME_8_RECV_LEN, dataChunk);
// Trajectory pos X
dataChunk = mgmtMsg->Longitude % 1000 ;
dataChunk = mgmtMsg->Longitude % 1000;
MPC_InsertChunk(mpcTxMsgBuff, STATE_TRAJ_X_OFFSET, STATE_TRAJ_X_LEN, dataChunk);
// Trajectory pos Y
dataChunk = mgmtMsg->Latitude % 1000 ;
dataChunk = mgmtMsg->Latitude % 1000;
MPC_InsertChunk(mpcTxMsgBuff, STATE_TRAJ_Y_OFFSET, STATE_TRAJ_Y_LEN, dataChunk);
// Trajectory alpha
dataChunk = 45;
@@ -434,10 +459,13 @@ int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgC
// TODO : Currently DJ is node 1 , only Byte 1 is activate , others is useless
// Topo node 1
dataChunk = BIT(L2_GetNodeId(linkInstPtr) - 1);
if (L2_GetNodeListLen(linkInstPtr->nodeList) > 1 ) {
for(int i = 1; i <= linkInstPtr->config.netScale; i++){
if (L2_isNodeInList(linkInstPtr->nodeList, i)) {
dataChunk |= BIT(i-1);
if (L2_GetNodeListLen(linkInstPtr->nodeList) > 1)
{
for (int i = 1; i <= linkInstPtr->config.netScale; i++)
{
if (L2_isNodeInList(linkInstPtr->nodeList, i))
{
dataChunk |= BIT(i - 1);
}
}
}
@@ -461,14 +489,14 @@ int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgC
MPC_InsertChunk(mpcTxMsgBuff, SVC_MSG_NUM_OFFSET, SVC_MSG_NUM_LEN, dataChunk);
// Copy msg data
size_t svcLen = mgmtMsgCnt*(72/8*6);
size_t mgmtLen = svcMsgCnt*(2040/8);
memcpy(((uint8_t*)mpcTxMsgBuff->words) + MPC_MSG_STATE_LEN/8, mgmtBuffer, svcLen);
memcpy(((uint8_t*)mpcTxMsgBuff->words) + MPC_MSG_STATE_LEN/8 + mgmtMsgCnt*(72/8*6),
svcBuffer, mgmtLen);
size_t svcLen = mgmtMsgCnt * (72 / 8 * 6);
size_t mgmtLen = svcMsgCnt * (2040 / 8);
memcpy(((uint8_t *)mpcTxMsgBuff->words) + MPC_MSG_STATE_LEN / 8, mgmtBuffer, svcLen);
memcpy(((uint8_t *)mpcTxMsgBuff->words) + MPC_MSG_STATE_LEN / 8 + mgmtMsgCnt * (72 / 8 * 6),
svcBuffer, mgmtLen);
Status = MPC_SendMsg(instPtr, MPC_MSG_STATE,(uint8_t*)mpcTxMsgBuff->words, MPC_MSG_STATE_LEN/8 + svcLen + mgmtLen);
// Status = MPC_SendMsg(instPtr, MPC_MSG_STATE,(uint8_t*)stateMsg->words, MPC_MSG_STATE_LEN/8);
Status = MPC_SendMsg(instPtr, MPC_MSG_STATE, (uint8_t *)mpcTxMsgBuff->words, MPC_MSG_STATE_LEN / 8 + svcLen + mgmtLen);
// Status = MPC_SendMsg(instPtr, MPC_MSG_STATE,(uint8_t*)stateMsg->words, MPC_MSG_STATE_LEN/8);
return Status;
}
datalink814_ge/src/mpc/mpc_ctrl.h
+23 -20
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: A brief description of this file
*
*
* Date: 2022-01-19 00:54
* LastEditors: Fan Hao
* LastEditTime: 2022-03-28 20:24
* Revisions: Rev 0.0
* History:
* History:
* [2022-01-19]: Initial version.
* [2022-03-28]: Add general information print function.
*
*
* Copyright(c) 2022 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef MPC_CTRL_H_
@@ -30,32 +30,37 @@
#include "../phy_ctrl/phy_ctrl.h"
#include "../linklayer/linklayer814.h"
typedef struct {
typedef struct
{
const uint16_t head;
uint16_t type;
uint8_t reserve[4];
} MPC_MSG_HEADER;
typedef struct{
typedef struct
{
uint32_t bramDevId;
uint32_t msgLenMax;
} MPC_CONFIG;
typedef struct{
typedef struct
{
MPC_CONFIG configs;
XBram pcieBram;
uint8_t* txBaseAddr;
uint8_t* rxBaseAddr;
uint8_t *txBaseAddr;
uint8_t *rxBaseAddr;
} MPC_INST;
typedef enum{
typedef enum
{
MPC_MSG_NONE = 0,
MPC_MSG_TEST = 1,
MPC_MSG_STATE = 2,
} MPC_MSG_TYPE;
// FIXME: Simplify this part.
typedef struct {
typedef struct
{
uint8_t frame1Cnt;
uint8_t frame2Cnt;
uint8_t frame3Cnt;
@@ -90,17 +95,15 @@ typedef struct {
uint16_t frame8Recv;
} MPC_RECORD;
extern void MPC_PCIESentStart();
extern MPC_RECORD* mpcRecord;
extern MPC_INST* mpcInstPtr;
extern MPC_RECORD *mpcRecord;
extern MPC_INST *mpcInstPtr;
int MPC_Init(MPC_INST** instPtr, MPC_CONFIG* configPtr);
int MPC_MsgHandler(MPC_INST* instPtr);
int MPC_StateUpload(MPC_INST* instPtr , uint8_t* mgmtBuffer , uint32_t mgmtMsgCnt
, uint8_t* svcBuffer , uint32_t svcMsgCnt , L2_MGMT_MSG_RECORD* mgmtmsg);
int MPC_SendMsg(MPC_INST* instPtr, MPC_MSG_TYPE type, uint8_t* buffer, uint32_t dlen);
int MPC_Init(MPC_INST **instPtr, MPC_CONFIG *configPtr);
int MPC_MsgHandler(MPC_INST *instPtr);
int MPC_StateUpload(MPC_INST *instPtr, uint8_t *mgmtBuffer, uint32_t mgmtMsgCnt, uint8_t *svcBuffer, uint32_t svcMsgCnt, L2_MGMT_MSG_RECORD *mgmtmsg);
int MPC_SendMsg(MPC_INST *instPtr, MPC_MSG_TYPE type, uint8_t *buffer, uint32_t dlen);
#endif //_MPC_PCIE
#endif //_MPC_CTRL
#endif //MPC_CTRL_H_
#endif // MPC_CTRL_H_
datalink814_ge/src/mpc/mpc_msg_def.h
+2 -3
View File
@@ -6,8 +6,8 @@
#ifndef _MPC_MSG_DEF
#define _MPC_MSG_DEF
#define MPC_MSG_HEAD_LEN 64 //bit
#define MPC_MSG_LEN_MAX 1024 //bit
#define MPC_MSG_HEAD_LEN 64 // bit
#define MPC_MSG_LEN_MAX 1024 // bit
#define MPC_MSG_LEN_NONE UINT32_MAX
// ** Self Test Message ** //
@@ -119,7 +119,6 @@
#define STORE_ADDR_START_OFFSET 544
// ** Time SyncAdj Message ** //
// Type code
#define MPC_MSG_TSYNC_CODE 0x00FF
datalink814_ge/src/phy_ctrl/antenna.c
+67 -32
View File
@@ -36,7 +36,7 @@
#define ANT_MODE_LOWAPOWER 0x04
#define ANT_MODE_STANDBY 0x0F
#define ATN_MODE_DEFAULT (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_LOWAPOWER)
#define ATN_MODE_HIGH (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_HIGH_POWER)
#define ATN_MODE_HIGH (ANT_MODE_WIDE_BEAM_1 | ANT_MODE_HIGH_POWER)
#define DEFAULT_CENTRTAL_FREQ 0x06D7 // 26.25GHz
@@ -48,39 +48,67 @@
#define MODECTRL_DEFAULT 0x00
uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS] = {0};
uint16_t hopPattern_Config[L1_FRAME_MAX_HOPS] =
{
0
};
uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] = {1500};
uint16_t hopPattern_2KBPS_20MS[L1_FRAME_2KBPS_20MS_HOPS] =
{
1500
};
uint16_t hopPattern_2KBPS_100MS[L1_FRAME_2KBPS_100MS_HOPS] = {
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400
};
uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] = {1500};
uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] = { 1500, 1600, 1700,
1800, 1900, 2000, 1500, 1600, 1700, 1800, 1900 };
uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] = {
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400
uint16_t hopPattern_10KBPS_10MS[L1_FRAME_10KBPS_10MS_HOPS] =
{
1500
};
uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] = {
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400
uint16_t hopPattern_10KBPS_20MS[L1_FRAME_10KBPS_20MS_HOPS] =
{
1500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
1700, 1800, 1900
};
uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS] ={ 1500, 1600, 1700, 1800, 1900, 2000, 1500, 1600 , 1700,
1800, 1900, 2000, 1500, 1600 , 1700, 1800, 1900, 2000, 1500, 1500};
uint16_t hopPattern_40KBPS_10MS[L1_FRAME_40KBPS_10MS_HOPS] =
{
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400
};
uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] = { 500, 600, 700, 800, 900, 1000, 500, 600 , 700,
800, 900, 1000, 500, 600 , 700, 800, 900, 1000, 500, 500 };
uint16_t hopPattern_40KBPS_20MS[L1_FRAME_40KBPS_20MS_HOPS] =
{
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400, 2400, 2400, 2400, 2400, 2400,
2400, 2400, 2400
};
uint16_t hopPattern_200KBPS_5MS[L1_FRAME_200KBPS_5MS_HOPS] =
{
1500, 1600, 1700, 1800, 1900, 2000, 1500, 1600,
1700, 1800, 1900, 2000, 1500, 1600, 1700, 1800,
1900, 2000, 1500, 1500
};
uint16_t hopPattern_1M2BPS_5MS[L1_FRAME_1M2BPS_5MS_HOPS] =
{
500 , 600 , 700 , 800 , 900 , 1000, 500 , 600 ,
700 , 800 , 900 , 1000, 500 , 600 , 700 , 800 ,
900 , 1000, 500 , 500
};
// Pair by regex
// Regex: ((\{\.oaxis = [0-9]+[ ,]+\.raxis = [0-9]+[ ]*\}, ){2})
@@ -195,8 +223,14 @@ ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN] = {
{.oaxis = 600, .raxis =2520}
};
#else
PHY_ANT_ANGLE antSubnrrwPattern[NUM_ANT_SUBNRRW_PATTERN] = {0};
PHY_ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN] = {0};
PHY_ANT_ANGLE antSubnrrwPattern[NUM_ANT_SUBNRRW_PATTERN] =
{
0
};
PHY_ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN] =
{
0
};
#endif
/*****************************************************************************/
/**
@@ -222,7 +256,6 @@ int32_t Phy_AntCoinfoInit(PHY_ANT_CONFIG *antCoinfoPtr)
antCoinfoPtr->antMode = ATN_MODE_DEFAULT;
#endif
// FIXME: Dummy default value.
// off axis angle setting
antCoinfoPtr->oaxis[0] = 0;
@@ -277,7 +310,8 @@ int32_t Phy_AntCoinfoInit(PHY_ANT_CONFIG *antCoinfoPtr)
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
{
if(instPtr->beamWd.next == ANT_BEAM_8){
if (instPtr->beamWd.next == ANT_BEAM_8)
{
if (beamNo >= NUM_ANT_NRRW_PATTERN)
{
return FAILURE;
@@ -292,7 +326,8 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
return SUCCESS;
}
}
else if(instPtr->beamWd.next == ANT_BEAM_25){
else if (instPtr->beamWd.next == ANT_BEAM_25)
{
if (beamNo >= NUM_ANT_SUBNRRW_PATTERN)
{
return FAILURE;
@@ -307,7 +342,8 @@ int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo)
return SUCCESS;
}
}
else{
else
{
// Beam number assign not support.
// Assign default angle (0,0)
instPtr->beamNo.next = 0;
@@ -357,7 +393,7 @@ int32_t Phy_SetBeamAngle(PHY_INST *instPtr, double offAx, double rndAx)
if (offAxDeg > 60)
{
DBG_ERROR("Angle out of range, off axis angle: %u(should be 0~60), round axis angle: %u(should be 0~360).",
offAxDeg, rndAxDeg);
offAxDeg, rndAxDeg);
return EPARAM;
}
offAxDeg = offAxDeg * 10;
@@ -368,4 +404,3 @@ int32_t Phy_SetBeamAngle(PHY_INST *instPtr, double offAx, double rndAx)
instPtr->antCoinfoPtr.raxis[0] = (uint8_t)(rndAxDeg >> 8);
return SUCCESS;
}
datalink814_ge/src/phy_ctrl/phy_ctrl.c
+198 -141
View File
@@ -33,8 +33,8 @@
/************************** Constant Definitions *****************************/
/***************** Macros (Inline Functions) Definitions *********************/
/************************** Function Prototypes ******************************/
static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal);
static int32_t Phy_SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t* dataBuff);
static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal);
static int32_t Phy_SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *dataBuff);
/**************************** Type Definitions *******************************/
@@ -50,20 +50,22 @@ static int32_t Phy_SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8
* SUCCESS Setting data successful.
*
******************************************************************************/
static int32_t Phy_SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8_t* dataBuff) {
static int32_t Phy_SetSendData(PHY_INST *instPtr, L1_FRAME_TYPE frameType, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
uint32_t frameLen, sendCmd;
frameLen = Phy_GetFrameLen(frameType);
if (frameLen == L1_FRAME_NONE_LEN) {
if (frameLen == L1_FRAME_NONE_LEN)
{
DBG_ERROR("Invalid frame length.\n");
return EPARAM;
}
instPtr->sendDataOffset = (TXRXDATA_MAX_LEN >> 1) - instPtr->sendDataOffset;
memcpy((uint8_t*)(instPtr->sendDataAddr + instPtr->sendDataOffset), dataBuff, frameLen);
instPtr->sendDataOffset = (TXRXDATA_MAX_LEN >> 1) - instPtr->sendDataOffset;
memcpy((uint8_t *)(instPtr->sendDataAddr + instPtr->sendDataOffset), dataBuff, frameLen);
sendCmd = frameLen & SEND_DATA_CMD_LEN_MASK;
sendCmd |= (instPtr->sendDataOffset << SEND_DATA_CMD_OFFSET_SHIFT) & SEND_DATA_CMD_OFFSET_MASK;
@@ -72,7 +74,6 @@ static int32_t Phy_SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Check whether a frame was detected. (Do not check if the frame was
@@ -84,12 +85,15 @@ static int32_t Phy_SetSendData(PHY_INST* instPtr, L1_FRAME_TYPE frameType, uint8
* false
*
******************************************************************************/
inline bool Phy_IsFrameDetected(PHY_INST* instPtr) {
inline bool Phy_IsFrameDetected(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if(instPtr->isAcq && instPtr->isFrameSync) {
if (instPtr->isAcq && instPtr->isFrameSync)
{
return true;
}
else {
else
{
return false;
}
}
@@ -98,19 +102,22 @@ inline bool Phy_IsFrameDetected(PHY_INST* instPtr) {
/**
* @brief Check whether a frame was received. (Do not check if the frame was
* crashed.)
*
*
* @param instPtr A pointer to physical layer control instance.
*
*
* @return true
* false
*
*
******************************************************************************/
inline bool Phy_IsFrameReceived(PHY_INST* instPtr) {
inline bool Phy_IsFrameReceived(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if(instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode) {
if (instPtr->isAcq && instPtr->isFrameSync && instPtr->isDecode)
{
return true;
}
else {
else
{
return false;
}
}
@@ -126,7 +133,8 @@ inline bool Phy_IsFrameReceived(PHY_INST* instPtr) {
* false
*
******************************************************************************/
inline bool Phy_IsFrameValid(PHY_INST* instPtr) {
inline bool Phy_IsFrameValid(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
return instPtr->isValid;
}
@@ -140,14 +148,15 @@ inline bool Phy_IsFrameValid(PHY_INST* instPtr) {
* @return Layer2 frame type. Refer "frame_def.h"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr) {
inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (Phy_IsFrameReceived(instPtr) == false) {
if (Phy_IsFrameReceived(instPtr) == false)
{
return L1_NONE_FRAME;
}
int32_t frame_type = (PhyRead(instPtr, PHY_MODE_CTRL) & PHY_MODE_CTRL_FRAME_MASK)
>> PHY_MODE_CTRL_FRAME_SHIFT;
if(frame_type < L1_1M2BPS_5MS_FRAME)
int32_t frame_type = (PhyRead(instPtr, PHY_MODE_CTRL) & PHY_MODE_CTRL_FRAME_MASK) >> PHY_MODE_CTRL_FRAME_SHIFT;
if (frame_type < L1_1M2BPS_5MS_FRAME)
return frame_type;
else
return L1_NONE_FRAME;
@@ -162,9 +171,11 @@ inline L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr) {
* @return Layer2 frame type. Refer "frame_def.h"
*
******************************************************************************/
inline L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST* instPtr) {
inline L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
if (instPtr->TOR.last != PHY_TOR_TX) {
if (instPtr->TOR.last != PHY_TOR_TX)
{
return L1_NONE_FRAME;
}
return instPtr->frameType.last;
@@ -179,7 +190,8 @@ inline L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST* instPtr) {
* @return NONE.
*
******************************************************************************/
static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal){
static void Phy_RcdShift(PHY_RCD *rcd, int32_t netxVal)
{
ASSERT(rcd != NULL);
rcd->last = rcd->curr;
rcd->curr = rcd->next;
@@ -196,8 +208,10 @@ static void Phy_RcdShift(PHY_RCD* rcd, int32_t netxVal){
* L1_FRAME_NONE_LEN if the frame type code cannot be recognized.
*
******************************************************************************/
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
switch (frameType) {
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_LEN;
case L1_2KBPS_100MS_FRAME:
@@ -228,7 +242,8 @@ uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType) {
* @return NONE.
*
******************************************************************************/
void Phy_UpdateState(PHY_INST* instPtr) {
void Phy_UpdateState(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
/* Update Frame Sate. */
@@ -240,10 +255,12 @@ void Phy_UpdateState(PHY_INST* instPtr) {
instPtr->isAcq = true;
instPtr->isFrameSync = true;
instPtr->isDecode = ((txrxSate & TXRX_STATE_DECODE_MASK) == TXRX_STATE_DECODE_SUCCESS) ? true : false;
if (Phy_IsFrameReceived(instPtr)) {
if (Phy_IsFrameReceived(instPtr))
{
instPtr->isValid = ((txrxSate & TXRX_STATE_CRC_MASK) == TXRX_STATE_CRC_OK) ? true : false;
}
else {
else
{
instPtr->isValid = false;
}
instPtr->recvSlotTime.int_part = PhyRead(instPtr, TM_TARGET_H);
@@ -252,24 +269,25 @@ void Phy_UpdateState(PHY_INST* instPtr) {
instPtr->recvFsTime.frac_part = PhyRead(instPtr, FSTIME_L);
}
/*****************************************************************************/
/**
* @brief Initialize physical layer instance accordingly.
*
*
* @param instPtr a pointer to physical instance
* @param phyConfig a pointer to configuration structure.
*
*
* @return SUCCESS
*
*
******************************************************************************/
int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig)
{
ASSERT(instPtr != NULL);
ASSERT(phyConfig != NULL);
int32_t Status;
*instPtr = (PHY_INST*) malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL) {
*instPtr = (PHY_INST *)malloc(sizeof(PHY_INST));
if ((*instPtr) == NULL)
{
return EALLOC;
}
memcpy(&(*instPtr)->phyCofig, phyConfig, sizeof(PHY_CONFIG));
@@ -301,56 +319,65 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
XBram_Config *ConfigPtr;
ConfigPtr = XBram_LookupConfig(phyConfig->antBramId);
if (ConfigPtr == NULL) {
if (ConfigPtr == NULL)
{
Status = EDINVAL;
goto error;
}
Status = XBram_CfgInitialize(&antInfoBram, ConfigPtr,
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&antInfoBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto error;
}
ConfigPtr = XBram_LookupConfig(phyConfig->txBramId);
if (ConfigPtr == NULL) {
Status = EDINVAL;
if (ConfigPtr == NULL)
{
Status = EDINVAL;
goto error;
}
Status = XBram_CfgInitialize(&txFrameBram, ConfigPtr,
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&txFrameBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto error;
}
ConfigPtr = XBram_LookupConfig(phyConfig->rxBramId);
if (ConfigPtr == NULL) {
Status = EDINVAL;
if (ConfigPtr == NULL)
{
Status = EDINVAL;
goto error;
}
Status = XBram_CfgInitialize(&rxFrameBram, ConfigPtr,
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS) {
ConfigPtr->CtrlBaseAddress);
if (Status != SUCCESS)
{
goto error;
}
/*
* Execute the BRAM driver selftest.
*/
Status = XBram_SelfTest(&rxFrameBram, 0);
if (Status != SUCCESS) {
if (Status != SUCCESS)
{
goto error;
}
@@ -358,18 +385,21 @@ int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig) {
(*instPtr)->recvDataAddr = rxFrameBram.Config.MemBaseAddress;
(*instPtr)->sendDataAddr = txFrameBram.Config.MemBaseAddress;
#else
(*instPtr)->antConfigAddr = (uintptr_t)calloc(ANTCONFIG_MAX_LEN/sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->antConfigAddr == (uintptr_t)NULL) {
(*instPtr)->antConfigAddr = (uintptr_t)calloc(ANTCONFIG_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->antConfigAddr == (uintptr_t)NULL)
{
Status = EALLOC;
goto error;
}
(*instPtr)->recvDataAddr = (uintptr_t)calloc(TXRXDATA_MAX_LEN/sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->recvDataAddr == (uintptr_t)NULL) {
(*instPtr)->recvDataAddr = (uintptr_t)calloc(TXRXDATA_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->recvDataAddr == (uintptr_t)NULL)
{
Status = EALLOC;
goto error;
}
(*instPtr)->sendDataAddr = (uintptr_t)calloc(TXRXDATA_MAX_LEN/sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->sendDataAddr == (uintptr_t)NULL) {
(*instPtr)->sendDataAddr = (uintptr_t)calloc(TXRXDATA_MAX_LEN / sizeof(uint64_t), sizeof(uint64_t));
if ((*instPtr)->sendDataAddr == (uintptr_t)NULL)
{
Status = EALLOC;
goto error;
}
@@ -383,20 +413,22 @@ error:
/*****************************************************************************/
/**
* @brief Destroy physical layer inctance, free allocated memory.
*
*
* @param instPtr a pointer to physical instance
*
*
* @return NONE
*
*
******************************************************************************/
void Phy_Destroy(PHY_INST* instPtr) {
if (instPtr == NULL){
void Phy_Destroy(PHY_INST *instPtr)
{
if (instPtr == NULL)
{
return;
}
#ifdef EMULATION
free((void*)instPtr->antConfigAddr);
free((void*)instPtr->recvDataAddr);
free((void*)instPtr->sendDataAddr);
free((void *)instPtr->antConfigAddr);
free((void *)instPtr->recvDataAddr);
free((void *)instPtr->sendDataAddr);
#endif
free(instPtr);
}
@@ -404,16 +436,18 @@ void Phy_Destroy(PHY_INST* instPtr) {
/*****************************************************************************/
/**
* @brief Start physical layer
*
*
* @param instPtr a pointer to physical instance
*
*
* @return NONE
*
*
******************************************************************************/
void Phy_Start(PHY_INST* instPtr) {
void Phy_Start(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
if((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_EN) {
if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_EN)
{
DBG_WARNING("Physical layer was allready enabled.\n");
}
instPtr->isPhyEn = true;
@@ -424,16 +458,18 @@ void Phy_Start(PHY_INST* instPtr) {
/*****************************************************************************/
/**
* @brief Halt physical layer operation
*
*
* @param instPtr a pointer to physical instance
*
*
* @return NONE
*
*
******************************************************************************/
void Phy_Halt(PHY_INST* instPtr) {
void Phy_Halt(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL);
if((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_DSBL) {
if ((phyMainCtrl & PHY_MAIN_CTRL_EN_MASK) == PHY_MAIN_CTRL_EN_DSBL)
{
DBG_WARNING("Physical layer was allready halted.\n");
}
instPtr->isPhyEn = false;
@@ -443,19 +479,21 @@ void Phy_Halt(PHY_INST* instPtr) {
/*****************************************************************************/
/**
* @brief Set physical layer working mode.
*
*
* @param instPtr a pointer to physical instance
* @param modeCod intened mode code
*
*
* @return SUCCESS
* other
*
*
******************************************************************************/
int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) {
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod)
{
ASSERT(instPtr != NULL);
uint32_t phyMainCtrl = PhyRead(instPtr, PHY_MAIN_CTRL) & ~PHY_MAIN_CTRL_MODE_MASK;
switch (modeCod) {
switch (modeCod)
{
case PHY_MODE_NONE:
phyMainCtrl |= PHY_MAIN_CTRL_MODE_NONE;
break;
@@ -479,21 +517,22 @@ int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod) {
/*****************************************************************************/
/**
* @brief Setup next slot operation.
*
*
* @param instPtr a pointer to physical instance
* @param txrxMode tor type of next slot
* @param frameType frame type of next slot
*
*
* @return SUCCESS
* other
*
*
******************************************************************************/
int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t* dataBuff) {
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode,
L1_FRAME_TYPE frameType, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
int32_t Status;
uint32_t phyModeCtrl, hopNum;
uint16_t* hopPattern = NULL;
uint16_t *hopPattern = NULL;
// Implement a ping-pong flow, offset always is 0 or half of buffer depth.
instPtr->antConfigOffset = (ANTCONFIG_MAX_LEN >> 1) - instPtr->antConfigOffset;
uint32_t antConfigAddr = instPtr->antConfigAddr + instPtr->antConfigOffset;
@@ -501,7 +540,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
instPtr->TOR.next = txrxMode;
instPtr->frameType.next = frameType;
switch (txrxMode) {
switch (txrxMode)
{
case PHY_TOR_SUSPEND:
phyModeCtrl = PHY_MODE_CTRL_TOR_SUSPEND;
break;
@@ -514,7 +554,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
default:
return EPARAM;
}
switch (frameType) {
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
phyModeCtrl |= PHY_MODE_CTRL_FRAME_2KBPS_20MS;
hopNum = L1_FRAME_2KBPS_20MS_HOPS;
@@ -561,48 +602,46 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
}
hopPattern = (instPtr->isPresetHop) ? hopPattern : hopPattern_Config;
uint8_t* dataSrcPtr = (uint8_t*)(&(instPtr->antCoinfoPtr));
for (int32_t i = 0; i < hopNum; i++) {
uint8_t *dataSrcPtr = (uint8_t *)(&(instPtr->antCoinfoPtr));
for (int32_t i = 0; i < hopNum; i++)
{
instPtr->antCoinfoPtr.antFreq[1] = (uint8_t)(hopPattern[i] & 0x00FF);
instPtr->antCoinfoPtr.antFreq[0] = (uint8_t)(
(hopPattern[i] >> 8) & 0x00FF);
memcpy((uint8_t*) (antConfigAddr + i * sizeof(PHY_ANT_CONFIG)),
dataSrcPtr, sizeof(PHY_ANT_CONFIG));
instPtr->antCoinfoPtr.antFreq[0] = (uint8_t)((hopPattern[i] >> 8) & 0x00FF);
memcpy((uint8_t *)(antConfigAddr + i * sizeof(PHY_ANT_CONFIG)),
dataSrcPtr, sizeof(PHY_ANT_CONFIG));
}
uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_OFFSET_MASK)
| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
| ((sizeof(PHY_ANT_CONFIG) << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
uint32_t antCtrl = (instPtr->antConfigOffset & ANT_CONFIG_OFFSET_MASK) | ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK) | ((sizeof(PHY_ANT_CONFIG) << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
/*
memcpy((uint8_t*)antConfigDataAddr, hopPattern, hopNum*sizeof(uint16_t));
uint32_t antCtrl = (instPtr->antCoinfigOffset & ANT_CONFIG_OFFSET_MASK)
| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
| ((sizeof(uint16_t) << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
*/
PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl|PHY_MODE_CTRL_CRC_EN);
/*
memcpy((uint8_t*)antConfigDataAddr, hopPattern, hopNum*sizeof(uint16_t));
uint32_t antCtrl = (instPtr->antCoinfigOffset & ANT_CONFIG_OFFSET_MASK)
| ((hopNum << ANT_CONFIG_NUM_SHIFT) & ANT_CONFIG_NUM_MASK)
| ((sizeof(uint16_t) << ANT_CONFIG_LEN_SHIFT) & ANT_CONFIG_LEN_MASK);
*/
PhyWrite(instPtr, PHY_MODE_CTRL, phyModeCtrl | PHY_MODE_CTRL_CRC_EN);
PhyWrite(instPtr, ANT_CONFIG, antCtrl);
// Trigger physical configuration.
if(txrxMode == PHY_TOR_TX) {
if (txrxMode == PHY_TOR_TX)
{
Status = Phy_SetSendData(instPtr, frameType, dataBuff);
if(Status != SUCCESS) {
if (Status != SUCCESS)
{
DBG_ERROR("Failed to apply send data.\n");
return Status;
}
}
PhyWrite(instPtr, SLOT_CMD, SLOT_CMD_KEY); // Phy statr fetching data.
Phy_RcdShift(&(instPtr->TOR),txrxMode);
Phy_RcdShift(&(instPtr->frameType),frameType);
// TODO:
Phy_RcdShift(&(instPtr->TOR), txrxMode);
Phy_RcdShift(&(instPtr->frameType), frameType);
// TODO:
Phy_RcdShift(&(instPtr->antNo), INIT_ANT_NO);
Phy_RcdShift(&(instPtr->beamNo), INIT_BEAM_NO);
Phy_RcdShift(&(instPtr->beamWd), INIT_ANT_WD);
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get coarse frequency estimation of last valid frame.
@@ -612,7 +651,8 @@ int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode,
* @return frequency estimation
*
******************************************************************************/
double Phy_GetCoarseFreqEst(PHY_INST* instPtr) {
double Phy_GetCoarseFreqEst(PHY_INST *instPtr)
{
ASSERT(instPtr != NULL);
uint32_t regValue;
uint8_t acqFreqIdx;
@@ -621,7 +661,8 @@ double Phy_GetCoarseFreqEst(PHY_INST* instPtr) {
double freqEst;
frameType = Phy_GetLastRecvFrameType(instPtr);
if(frameType > L1_1M2BPS_5MS_FRAME || frameType == L1_NONE_FRAME){
if (frameType > L1_1M2BPS_5MS_FRAME || frameType == L1_NONE_FRAME)
{
return INFINITY; // FIXME
}
regValue = PhyRead(instPtr, COASE_FREQEST);
@@ -629,15 +670,17 @@ double Phy_GetCoarseFreqEst(PHY_INST* instPtr) {
acqFreqIdx = 80 - acqFreqIdx;
syncFreqIdx = (regValue >> COASE_FREQEST_SYNC_SHIFT) && COASE_FREQEST_SYNC_MASK;
syncFreqIdx = 8 - syncFreqIdx;
if(frameType >= L1_200KBPS_5MS_FRAME) {
freqEst = (double)acqFreqIdx*(250e3/4);
freqEst += (double)syncFreqIdx*1.5e6/64/128;
if (frameType >= L1_200KBPS_5MS_FRAME)
{
freqEst = (double)acqFreqIdx * (250e3 / 4);
freqEst += (double)syncFreqIdx * 1.5e6 / 64 / 128;
}
else {
freqEst = acqFreqIdx*(250e3/4);
freqEst += (double)syncFreqIdx*12e6/64/128;
else
{
freqEst = acqFreqIdx * (250e3 / 4);
freqEst += (double)syncFreqIdx * 12e6 / 64 / 128;
}
return freqEst;
}
@@ -652,8 +695,10 @@ double Phy_GetCoarseFreqEst(PHY_INST* instPtr) {
* FAILURE frame type cannot be recognized
*
******************************************************************************/
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
switch (frameType) {
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_HOPS;
case L1_2KBPS_100MS_FRAME:
@@ -677,7 +722,7 @@ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
/*****************************************************************************/
/**
* @brief Get the corresponding time compensation value (time interval from
* @brief Get the corresponding time compensation value (time interval from
* frame start to frame sync)
*
* @param frameType a L1 frame type enumeration.
@@ -686,8 +731,10 @@ uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType){
* FAILURE frame type cannot be recognized
*
******************************************************************************/
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){
switch (frameType) {
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType)
{
switch (frameType)
{
case L1_2KBPS_20MS_FRAME:
return L1_FRAME_2KBPS_20MS_COMP;
case L1_2KBPS_100MS_FRAME:
@@ -720,12 +767,14 @@ uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType){
* FAILURE pattern length exceeds maximum length.
*
******************************************************************************/
int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen){
int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen)
{
ASSERT(patternBuff != NULL);
if(patternLen > L1_FRAME_MAX_HOPS){
if (patternLen > L1_FRAME_MAX_HOPS)
{
return FAILURE;
}
memcpy(hopPattern_Config, patternBuff, patternLen*sizeof(uint16_t));
memcpy(hopPattern_Config, patternBuff, patternLen * sizeof(uint16_t));
return SUCCESS;
}
@@ -740,12 +789,15 @@ int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen){
* FAILURE pattern length exceeds maximum length.
*
******************************************************************************/
int32_t Phy_SetTimeWord(PHY_INST* instPtr, const TIME_WORD* tmWord, PHY_TIME_WORD_TYPE tmType){
int32_t Phy_SetTimeWord(PHY_INST *instPtr, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType)
{
ASSERT(instPtr != NULL);
if(tmWord->frac_part>=PHY_TIME_WORD_L_MAX){
if (tmWord->frac_part >= PHY_TIME_WORD_L_MAX)
{
return EPARAM;
}
switch(tmType){
switch (tmType)
{
case TIME_WORD_POLL:
PhyWrite(instPtr, POLLTIME_L, tmWord->frac_part);
PhyWrite(instPtr, POLLTIME_H, tmWord->int_part);
@@ -760,7 +812,6 @@ int32_t Phy_SetTimeWord(PHY_INST* instPtr, const TIME_WORD* tmWord, PHY_TIME_WOR
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief Get the frame sync time of last frame.
@@ -771,7 +822,8 @@ int32_t Phy_SetTimeWord(PHY_INST* instPtr, const TIME_WORD* tmWord, PHY_TIME_WOR
* @return NONE
*
******************************************************************************/
void Phy_GetFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSyncT) {
void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT)
{
ASSERT(instPtr != NULL);
ASSERT(frameSyncT != NULL);
frameSyncT->int_part = PhyRead(instPtr, FSTIME_H);
@@ -788,7 +840,8 @@ void Phy_GetFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSyncT) {
* @return NONE
*
******************************************************************************/
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSyncT) {
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT)
{
ASSERT(instPtr != NULL);
ASSERT(frameSyncT != NULL);
memcpy(frameSyncT, &(instPtr->recvFsTime), sizeof(TIME_WORD));
@@ -804,7 +857,8 @@ void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSy
* @return NONE
*
******************************************************************************/
void Phy_GetSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotStart) {
void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart)
{
ASSERT(instPtr != NULL);
ASSERT(slotStart != NULL);
slotStart->int_part = PhyRead(instPtr, TM_TARGET_H);
@@ -821,7 +875,8 @@ void Phy_GetSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotStart) {
* @return NONE
*
******************************************************************************/
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotStart) {
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart)
{
ASSERT(instPtr != NULL);
ASSERT(slotStart != NULL);
memcpy(slotStart, &(instPtr->recvSlotTime), sizeof(TIME_WORD));
@@ -836,7 +891,8 @@ void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotSta
* @return NONE
*
******************************************************************************/
void Phy_PeekRecvData(PHY_INST* instPtr, uint8_t* dataBuff) {
void Phy_PeekRecvData(PHY_INST *instPtr, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
if (instPtr->isValid == false)
@@ -847,8 +903,8 @@ void Phy_PeekRecvData(PHY_INST* instPtr, uint8_t* dataBuff) {
frameLen = (PhyRead(instPtr, RECVDATA_STATE) & RECVDATA_STATE_LEN_MASK) >> RECVDATA_STATE_LEN_SHIFT;
offset = (PhyRead(instPtr, RECVDATA_STATE) & RECVDATA_STATE_OFFSET_MASK) >> RECVDATA_STATE_OFFSET_SHIFT;
memcpy(dataBuff, (uint8_t*)(instPtr->recvDataAddr + offset), frameLen);
memcpy(dataBuff, (uint8_t *)(instPtr->recvDataAddr + offset), frameLen);
}
/*****************************************************************************/
@@ -861,7 +917,8 @@ void Phy_PeekRecvData(PHY_INST* instPtr, uint8_t* dataBuff) {
* @return NONE
*
******************************************************************************/
void Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* dataBuff) {
void Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *dataBuff)
{
ASSERT(instPtr != NULL);
ASSERT(dataBuff != NULL);
if (instPtr->isValid == false)
datalink814_ge/src/phy_ctrl/phy_ctrl.h
+100 -91
View File
@@ -1,11 +1,11 @@
/*****************************************************************************/
/**
* Description: Physical layer control definitions and function prototypes.
*
*
* Date: 2021-12-04 16:32
* LastEditTime: 2023-04-24 17:41
* Revisions: Rev 0.1
* History:
* History:
* [2021-12-08]: Initial version.
* [2022-06-18]: New branch for glink control purpose:
* -Add poll mode, physical layer will start Tx/Rx procedure
@@ -16,7 +16,7 @@
* - Read frame type from PL status register.
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef PHY_CTRL_H_
@@ -28,55 +28,55 @@
#include "../common.h"
#include "../time_ctrl/time_ctrl.h"
/************************** Constant Definitions *****************************/
#define TMWORD_FRAC_MAX 500000
#define ANTCONFIG_MAX_LEN 8192
#define TXRXDATA_MAX_LEN 2048
#define NUM_ANT_SUBNRRW_PATTERN 37
#define NUM_ANT_NRRW_PATTERN 61
#define TMWORD_FRAC_MAX 500000
#define ANTCONFIG_MAX_LEN 8192
#define TXRXDATA_MAX_LEN 2048
#define NUM_ANT_SUBNRRW_PATTERN 37
#define NUM_ANT_NRRW_PATTERN 61
#define INIT_BEAM_NO 0
#define INIT_ANT_WD ANT_BEAM_120
#define INIT_ANT_NO 0
#define L1_FRAME_NONE_HOPS UINT32_MAX
#define L1_FRAME_MAX_HOPS 81
#define L1_FRAME_2KBPS_20MS_HOPS 1
#define L1_FRAME_2KBPS_100MS_HOPS 81
#define L1_FRAME_10KBPS_10MS_HOPS 1
#define L1_FRAME_10KBPS_20MS_HOPS 11
#define L1_FRAME_40KBPS_20MS_HOPS 19
#define L1_FRAME_40KBPS_10MS_HOPS 9
#define L1_FRAME_200KBPS_5MS_HOPS 20
#define L1_FRAME_1M2BPS_5MS_HOPS 20
#define L1_FRAME_NONE_HOPS UINT32_MAX
#define L1_FRAME_MAX_HOPS 81
#define L1_FRAME_2KBPS_20MS_HOPS 1
#define L1_FRAME_2KBPS_100MS_HOPS 81
#define L1_FRAME_10KBPS_10MS_HOPS 1
#define L1_FRAME_10KBPS_20MS_HOPS 11
#define L1_FRAME_40KBPS_20MS_HOPS 19
#define L1_FRAME_40KBPS_10MS_HOPS 9
#define L1_FRAME_200KBPS_5MS_HOPS 20
#define L1_FRAME_1M2BPS_5MS_HOPS 20
#define L1_FRAME_NONE_LEN UINT32_MAX// byte
#define L1_FRAME_LEN_MAX (8192/8) // byte
#define L1_FRAME_2KBPS_20MS_LEN 1 // byte
#define L1_FRAME_2KBPS_100MS_LEN 312/8 // byte
#define L1_FRAME_10KBPS_10MS_LEN 1 // byte
#define L1_FRAME_10KBPS_20MS_LEN (232/8) // byte
#define L1_FRAME_40KBPS_20MS_LEN (2152/8) // byte
#define L1_FRAME_40KBPS_10MS_LEN (712/8) // byte
#define L1_FRAME_200KBPS_5MS_LEN (2040/8) // byte
#define L1_FRAME_1M2BPS_5MS_LEN (8176/8) // byte
#define L1_FRAME_NONE_LEN UINT32_MAX // byte
#define L1_FRAME_LEN_MAX (8192 / 8) // byte
#define L1_FRAME_2KBPS_20MS_LEN 1 // byte
#define L1_FRAME_2KBPS_100MS_LEN 312 / 8 // byte
#define L1_FRAME_10KBPS_10MS_LEN 1 // byte
#define L1_FRAME_10KBPS_20MS_LEN (232 / 8) // byte
#define L1_FRAME_40KBPS_20MS_LEN (2152 / 8) // byte
#define L1_FRAME_40KBPS_10MS_LEN (712 / 8) // byte
#define L1_FRAME_200KBPS_5MS_LEN (2040 / 8) // byte
#define L1_FRAME_1M2BPS_5MS_LEN (8176 / 8) // byte
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns(Timer_L)
#define L1_FRAME_2KBPS_20MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_2KBPS_100MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_10MS_COMP 847733 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_20MS_COMP 847733 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_20MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_10MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_200KBPS_5MS_COMP 19725 // *10ns(Timer_L)
#define L1_FRAME_1M2BPS_5MS_COMP 19725 // *10ns(Timer_L)
#define L1_FRAME_NONE_COMP UINT32_MAX // *10ns(Timer_L)
#define L1_FRAME_2KBPS_20MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_2KBPS_100MS_COMP 1841158 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_10MS_COMP 847733 // *10ns(Timer_L)
#define L1_FRAME_10KBPS_20MS_COMP 847733 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_20MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_40KBPS_10MS_COMP 110667 // *10ns(Timer_L)
#define L1_FRAME_200KBPS_5MS_COMP 19725 // *10ns(Timer_L)
#define L1_FRAME_1M2BPS_5MS_COMP 19725 // *10ns(Timer_L)
#define PHY_TIME_WORD_H_MAX UINT32_MAX
#define PHY_TIME_WORD_L_MAX 0x0007A120 // 5ms/10ns
#define PHY_TIME_WORD_H_MAX UINT32_MAX
#define PHY_TIME_WORD_L_MAX 0x0007A120 // 5ms/10ns
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
L1_NONE_FRAME = 0,
L1_2KBPS_20MS_FRAME = 1,
L1_2KBPS_100MS_FRAME = 2,
@@ -88,7 +88,8 @@ typedef enum {
L1_1M2BPS_5MS_FRAME = 8
} L1_FRAME_TYPE;
typedef struct {
typedef struct
{
/* Antenna control frame head 0xAA */
uint8_t head;
/* default 0, reserved for further use */
@@ -146,7 +147,8 @@ typedef struct {
uint8_t dummy[6];
} PHY_ANT_CONFIG;
typedef struct {
typedef struct
{
/* off-axis angle range is 0~60,
* 0.1 degree each LSB */
uint16_t oaxis;
@@ -155,13 +157,15 @@ typedef struct {
uint16_t raxis;
} PHY_ANT_ANGLE;
typedef enum {
typedef enum
{
TIME_WORD_NONE = 0,
TIME_WORD_POLL = 1,
TIME_WORD_LISTEN_TIMEOUT = 2
} PHY_TIME_WORD_TYPE;
typedef enum {
typedef enum
{
ANT_BEAM_NONE = 0,
ANT_BEAM_120 = 1,
ANT_BEAM_60 = 2,
@@ -169,20 +173,23 @@ typedef enum {
ANT_BEAM_8 = 4
} PHY_ANT_BEAM_WIDTH;
typedef enum {
typedef enum
{
PHY_TOR_SUSPEND = 0,
PHY_TOR_TX = 1,
PHY_TOR_RX = 2
} PHY_TOR_TYPE;
typedef enum {
typedef enum
{
PHY_MODE_NONE = 0,
PHY_MODE_LISTEN = 1,
PHY_MODE_SCHEDULE = 2,
PHY_MODE_PULL = 3
} PHY_MODE;
typedef struct {
typedef struct
{
uint32_t devId;
uint32_t txBramId;
uint32_t rxBramId;
@@ -190,33 +197,35 @@ typedef struct {
uintptr_t baseAddr;
} PHY_CONFIG;
typedef struct {
typedef struct
{
int32_t last;
int32_t curr;
int32_t next;
} PHY_RCD;
typedef struct {
typedef struct
{
PHY_CONFIG phyCofig; // Device address configs
uintptr_t recvDataAddr;
uintptr_t sendDataAddr;
uintptr_t antConfigAddr;
bool isPhyEn; // if physical layer running
bool isAcq; // if last reception acquisition success
bool isFrameSync; // if last reception frame sync success
bool isDecode; // if last reception decode success
bool isValid; // if last reception frame data valid (crc check pass)
TIME_WORD recvSlotTime; // last slot start time stamp
TIME_WORD recvFsTime; // last frame sync time stamp
bool isPresetHop; // if using preset frequency hop
PHY_RCD antNo; // antenna number record
PHY_RCD beamNo; // beam number record
PHY_RCD beamWd; // beam width record
PHY_RCD TOR; // tx rx state record
PHY_RCD frameType; // frame type record
uint32_t sendDataOffset; // antenna configuration bram offset (to implement ping-pong data flow)
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
PHY_ANT_CONFIG antCoinfoPtr;// antenna configuration pointer
bool isPhyEn; // if physical layer running
bool isAcq; // if last reception acquisition success
bool isFrameSync; // if last reception frame sync success
bool isDecode; // if last reception decode success
bool isValid; // if last reception frame data valid (crc check pass)
TIME_WORD recvSlotTime; // last slot start time stamp
TIME_WORD recvFsTime; // last frame sync time stamp
bool isPresetHop; // if using preset frequency hop
PHY_RCD antNo; // antenna number record
PHY_RCD beamNo; // beam number record
PHY_RCD beamWd; // beam width record
PHY_RCD TOR; // tx rx state record
PHY_RCD frameType; // frame type record
uint32_t sendDataOffset; // antenna configuration bram offset (to implement ping-pong data flow)
uint32_t antConfigOffset; // antenna configuration bram offset (to implement ping-pong data flow)
PHY_ANT_CONFIG antCoinfoPtr; // antenna configuration pointer
} PHY_INST;
/************************** Variable Definitions *****************************/
@@ -234,40 +243,40 @@ extern PHY_ANT_ANGLE antSubnrrwPattern[NUM_ANT_SUBNRRW_PATTERN];
extern PHY_ANT_ANGLE antNrrwPattern[NUM_ANT_NRRW_PATTERN];
/************************** Function Prototypes ******************************/
int32_t Phy_InstInit(PHY_INST** instPtr, PHY_CONFIG *phyConfig);
void Phy_Destroy(PHY_INST* instPtr);
int32_t Phy_InstInit(PHY_INST **instPtr, PHY_CONFIG *phyConfig);
void Phy_Destroy(PHY_INST *instPtr);
void Phy_UpdateState(PHY_INST* instPtr);
void Phy_UpdateState(PHY_INST *instPtr);
void Phy_Start(PHY_INST* instPtr);
void Phy_Halt(PHY_INST* instPtr);
int32_t Phy_SetMode(PHY_INST* instPtr, PHY_MODE modeCod);
void Phy_Start(PHY_INST *instPtr);
void Phy_Halt(PHY_INST *instPtr);
int32_t Phy_SetMode(PHY_INST *instPtr, PHY_MODE modeCod);
int32_t Phy_SetupNextSlot(PHY_INST* instPtr, PHY_TOR_TYPE txrxMode, L1_FRAME_TYPE frameType, uint8_t* dataBuff);
void Phy_PeekRecvData(PHY_INST* instPtr, uint8_t* dataBuff);
void Phy_RetrieveRecvData(PHY_INST* instPtr, uint8_t* dataBuff);
int32_t Phy_SetupNextSlot(PHY_INST *instPtr, PHY_TOR_TYPE txrxMode, L1_FRAME_TYPE frameType, uint8_t *dataBuff);
void Phy_PeekRecvData(PHY_INST *instPtr, uint8_t *dataBuff);
void Phy_RetrieveRecvData(PHY_INST *instPtr, uint8_t *dataBuff);
bool Phy_IsFrameDetected(PHY_INST* instPtr);
bool Phy_IsFrameReceived(PHY_INST* instPtr);
bool Phy_IsFrameValid(PHY_INST* instPtr);
bool Phy_IsFrameDetected(PHY_INST *instPtr);
bool Phy_IsFrameReceived(PHY_INST *instPtr);
bool Phy_IsFrameValid(PHY_INST *instPtr);
uint32_t Phy_GetFrameLen(L1_FRAME_TYPE frameType);
L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST* instPtr);
L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST* instPtr);
L1_FRAME_TYPE Phy_GetLastRecvFrameType(PHY_INST *instPtr);
L1_FRAME_TYPE Phy_GetLastSendFrameType(PHY_INST *instPtr);
int32_t Phy_SetHopPattern(void* patternBuff, uint32_t patternLen);
int32_t Phy_SetHopPattern(void *patternBuff, uint32_t patternLen);
uint32_t Phy_GetHopLength(L1_FRAME_TYPE frameType);
uint32_t Phy_GetTimeComp(L1_FRAME_TYPE frameType);
int32_t Phy_AntCoinfoInit(PHY_ANT_CONFIG* antCoinfoPtr);
int32_t Phy_SetBeamWidth(PHY_INST* instPtr, PHY_ANT_BEAM_WIDTH beamWidth);
int32_t Phy_SetBeamNo(PHY_INST* instPtr, int32_t beamNo);
int32_t Phy_SetBeamAngle(PHY_INST* phyInstPtr, double offAx, double rndAx);
int32_t Phy_SetTimeWord(PHY_INST* instPtr, const TIME_WORD* tmWord, PHY_TIME_WORD_TYPE tmType);
int32_t Phy_AntCoinfoInit(PHY_ANT_CONFIG *antCoinfoPtr);
int32_t Phy_SetBeamWidth(PHY_INST *instPtr, PHY_ANT_BEAM_WIDTH beamWidth);
int32_t Phy_SetBeamNo(PHY_INST *instPtr, int32_t beamNo);
int32_t Phy_SetBeamAngle(PHY_INST *phyInstPtr, double offAx, double rndAx);
int32_t Phy_SetTimeWord(PHY_INST *instPtr, const TIME_WORD *tmWord, PHY_TIME_WORD_TYPE tmType);
double Phy_GetCoarseFreqEst(PHY_INST* instPtr);
void Phy_GetFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSyncT);
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST* instPtr, TIME_WORD* frameSyncT);
void Phy_GetSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotStart);
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST* instPtr, TIME_WORD* slotStart);
double Phy_GetCoarseFreqEst(PHY_INST *instPtr);
void Phy_GetFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
void Phy_GetLastReceivedFrameSyncTimeStamp(PHY_INST *instPtr, TIME_WORD *frameSyncT);
void Phy_GetSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
void Phy_GetLastReceivedSlotStartTimeStamp(PHY_INST *instPtr, TIME_WORD *slotStart);
#endif /* PHY_CTRL_H_ */
datalink814_ge/src/phy_ctrl/phy_ctrlreg.h
+147 -149
View File
@@ -12,7 +12,7 @@
* [2022-06-18]: New branch for glink control purpose:
* -Add poll modephysical layer will start Tx/Rx procedure
* at specific time stamp.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
@@ -25,238 +25,236 @@
#include "../util/util.h"
/***************** Macros (Inline Functions) Definitions *********************/
#define PhyRead(instPtr, addr) IO_In32(instPtr->phyCofig.baseAddr + addr)
#define PhyWrite(instPtr, addr, value) IO_Out32(instPtr->phyCofig.baseAddr + addr, value)
#define PhyRead(instPtr, addr) IO_In32(instPtr->phyCofig.baseAddr + addr)
#define PhyWrite(instPtr, addr, value) IO_Out32(instPtr->phyCofig.baseAddr + addr, value)
/************************** Constant Definitions *****************************/
/**
* Register: PHY_MAIN_CTRL
*/
#define PHY_MAIN_CTRL 0X00000000U
#define PHY_MAIN_CTRL_EN_SHIFT 0
#define PHY_MAIN_CTRL_EN_WIDTH 1
#define PHY_MAIN_CTRL_EN_MASK 0x00000001U
#define PHY_MAIN_CTRL_MODE_SHIFT 8
#define PHY_MAIN_CTRL_MODE_WIDTH 8
#define PHY_MAIN_CTRL_MODE_MASK 0x0000FF00U
#define PHY_MAIN_CTRL 0X00000000U
#define PHY_MAIN_CTRL_EN_SHIFT 0
#define PHY_MAIN_CTRL_EN_WIDTH 1
#define PHY_MAIN_CTRL_EN_MASK 0x00000001U
#define PHY_MAIN_CTRL_MODE_SHIFT 8
#define PHY_MAIN_CTRL_MODE_WIDTH 8
#define PHY_MAIN_CTRL_MODE_MASK 0x0000FF00U
#define PHY_MAIN_CTRL_EN_EN 0x00000001U
#define PHY_MAIN_CTRL_EN_DSBL 0x00000000U
#define PHY_MAIN_CTRL_MODE_NONE 0x00000000U
#define PHY_MAIN_CTRL_MODE_LISTEN 0x00000100U
#define PHY_MAIN_CTRL_MODE_SCHEDULE 0x00000200U
#define PHY_MAIN_CTRL_MODE_PULL 0x00000300U
#define PHY_MAIN_CTRL_EN_EN 0x00000001U
#define PHY_MAIN_CTRL_EN_DSBL 0x00000000U
#define PHY_MAIN_CTRL_MODE_NONE 0x00000000U
#define PHY_MAIN_CTRL_MODE_LISTEN 0x00000100U
#define PHY_MAIN_CTRL_MODE_SCHEDULE 0x00000200U
#define PHY_MAIN_CTRL_MODE_PULL 0x00000300U
/**
* Register: TXRX_CTRL
*/
#define PHY_MODE_CTRL 0x00000004U
#define PHY_MODE_CTRL_TOR_SHIFT 0
#define PHY_MODE_CTRL_TOR_WIDTH 8
#define PHY_MODE_CTRL_TOR_MASK 0x000000FFU
#define PHY_MODE_CTRL_FRAME_SHIFT 8
#define PHY_MODE_CTRL_FRAME_WIDTH 8
#define PHY_MODE_CTRL_FRAME_MASK 0x0000FF00U
#define PHY_MODE_CTRL_CRC_SHIFT 8
#define PHY_MODE_CTRL_CRC_WIDTH 16
#define PHY_MODE_CTRL_CRC_MASK 0x00FF0000U
#define PHY_MODE_CTRL 0x00000004U
#define PHY_MODE_CTRL_TOR_SHIFT 0
#define PHY_MODE_CTRL_TOR_WIDTH 8
#define PHY_MODE_CTRL_TOR_MASK 0x000000FFU
#define PHY_MODE_CTRL_FRAME_SHIFT 8
#define PHY_MODE_CTRL_FRAME_WIDTH 8
#define PHY_MODE_CTRL_FRAME_MASK 0x0000FF00U
#define PHY_MODE_CTRL_CRC_SHIFT 8
#define PHY_MODE_CTRL_CRC_WIDTH 16
#define PHY_MODE_CTRL_CRC_MASK 0x00FF0000U
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
#define PHY_MODE_CTRL_TOR_RX 0x00000002U
#define PHY_MODE_CTRL_TOR_SUSPEND 0x00000000U
#define PHY_MODE_CTRL_TOR_TX 0x00000001U
#define PHY_MODE_CTRL_TOR_RX 0x00000002U
#define PHY_MODE_CTRL_FRAME_NONE 0x00000000U
#define PHY_MODE_CTRL_FRAME_2KBPS_20MS 0x00000100U
#define PHY_MODE_CTRL_FRAME_2KBPS_100MS 0x00000200U
#define PHY_MODE_CTRL_FRAME_10KBPS_10MS 0x00000300U // Initial Scan Frame
#define PHY_MODE_CTRL_FRAME_10KBPS_20MS 0x00000400U // Benchmark Frame
#define PHY_MODE_CTRL_FRAME_40KBPS_20MS 0x00000500U
#define PHY_MODE_CTRL_FRAME_40KBPS_10MS 0x00000600U
#define PHY_MODE_CTRL_FRAME_200KBPS_5MS 0x00000700U // 2-Way ACQ Frame, Management Frame, Service Frame
#define PHY_MODE_CTRL_FRAME_1M2BPS_5MS 0x00000800U // Bistatic Synthetic Aperture Radar Frame
#define PHY_MODE_CTRL_FRAME_NONE 0x00000000U
#define PHY_MODE_CTRL_FRAME_2KBPS_20MS 0x00000100U
#define PHY_MODE_CTRL_FRAME_2KBPS_100MS 0x00000200U
#define PHY_MODE_CTRL_FRAME_10KBPS_10MS 0x00000300U // Initial Scan Frame
#define PHY_MODE_CTRL_FRAME_10KBPS_20MS 0x00000400U // Benchmark Frame
#define PHY_MODE_CTRL_FRAME_40KBPS_20MS 0x00000500U
#define PHY_MODE_CTRL_FRAME_40KBPS_10MS 0x00000600U
#define PHY_MODE_CTRL_FRAME_200KBPS_5MS 0x00000700U // 2-Way ACQ Frame, Management Frame, Service Frame
#define PHY_MODE_CTRL_FRAME_1M2BPS_5MS 0x00000800U // Bistatic Synthetic Aperture Radar Frame
#define PHY_MODE_CTRL_CRC_EN 0x00010000U
#define PHY_MODE_CTRL_CRC_DSBL 0x00000000U
#define PHY_MODE_CTRL_CRC_EN 0x00010000U
#define PHY_MODE_CTRL_CRC_DSBL 0x00000000U
/**
* Register: POLLTIME_L
*/
#define POLLTIME_L 0x00000008U
#define POLLTIME_L_SHIFT 0
#define POLLTIME_L_WIDTH 32
#define POLLTIME_L_MASK 0xFFFFFFFFU
#define POLLTIME_L 0x00000008U
#define POLLTIME_L_SHIFT 0
#define POLLTIME_L_WIDTH 32
#define POLLTIME_L_MASK 0xFFFFFFFFU
/**
* Register: POLLTIME_H
*/
#define POLLTIME_H 0x0000000CU
#define POLLTIME_H_SHIFT 0
#define POLLTIME_H_WIDTH 32
#define POLLTIME_H_MASK 0xFFFFFFFFU
#define POLLTIME_H 0x0000000CU
#define POLLTIME_H_SHIFT 0
#define POLLTIME_H_WIDTH 32
#define POLLTIME_H_MASK 0xFFFFFFFFU
/**
* Register: TIMEOUT_L
*/
#define LISTENTIMEOUT_L 0x00000010U
#define LISTENTIMEOUT_L_SHIFT 0
#define LISTENTIMEOUT_L_WIDTH 32
#define LISTENTIMEOUT_L_MASK 0xFFFFFFFFU
#define LISTENTIMEOUT_L 0x00000010U
#define LISTENTIMEOUT_L_SHIFT 0
#define LISTENTIMEOUT_L_WIDTH 32
#define LISTENTIMEOUT_L_MASK 0xFFFFFFFFU
/**
* Register: TIMEOUT_H
*/
#define LISTENTIMEOUT_H 0x00000014U
#define LISTENTIMEOUT_H_SHIFT 0
#define LISTENTIMEOUT_H_WIDTH 32
#define LISTENTIMEOUT_H_MASK 0xFFFFFFFFU
#define LISTENTIMEOUT_H 0x00000014U
#define LISTENTIMEOUT_H_SHIFT 0
#define LISTENTIMEOUT_H_WIDTH 32
#define LISTENTIMEOUT_H_MASK 0xFFFFFFFFU
/**
* Register: ANT_CTRL
*/
#define ANT_CONFIG 0x00000018U
#define ANT_CONFIG_OFFSET_SHIFT 0
#define ANT_CONFIG_OFFSET_WIDTH 16
#define ANT_CONFIG_OFFSET_MASK 0x0000FFFFU
#define ANT_CONFIG_LEN_SHIFT 16
#define ANT_CONFIG_LEN_WIDTH 8
#define ANT_CONFIG_LEN_MASK 0x00FF0000U
#define ANT_CONFIG_NUM_SHIFT 24
#define ANT_CONFIG_NUM_WIDTH 8
#define ANT_CONFIG_NUM_MASK 0xFF000000U
#define ANT_CONFIG 0x00000018U
#define ANT_CONFIG_OFFSET_SHIFT 0
#define ANT_CONFIG_OFFSET_WIDTH 16
#define ANT_CONFIG_OFFSET_MASK 0x0000FFFFU
#define ANT_CONFIG_LEN_SHIFT 16
#define ANT_CONFIG_LEN_WIDTH 8
#define ANT_CONFIG_LEN_MASK 0x00FF0000U
#define ANT_CONFIG_NUM_SHIFT 24
#define ANT_CONFIG_NUM_WIDTH 8
#define ANT_CONFIG_NUM_MASK 0xFF000000U
/**
* Register: SEND_DATA_CMD
*/
#define SEND_DATA_CMD 0x0000001CU
#define SEND_DATA_CMD_SHIFT 0
#define SEND_DATA_CMD_WIDTH 32
#define SEND_DATA_CMD_MASK 0xFFFFFFFFU
#define SEND_DATA_CMD 0x0000001CU
#define SEND_DATA_CMD_SHIFT 0
#define SEND_DATA_CMD_WIDTH 32
#define SEND_DATA_CMD_MASK 0xFFFFFFFFU
#define SEND_DATA_CMD_LEN_SHIFT 0
#define SEND_DATA_CMD_LEN_WIDTH 16
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
#define SEND_DATA_CMD_OFFSET_SHIFT 16
#define SEND_DATA_CMD_OFFSET_WIDTH 16
#define SEND_DATA_CMD_OFFSET_MASK 0xFFFF0000U
#define SEND_DATA_CMD_LEN_SHIFT 0
#define SEND_DATA_CMD_LEN_WIDTH 16
#define SEND_DATA_CMD_LEN_MASK 0x0000FFFFU
#define SEND_DATA_CMD_OFFSET_SHIFT 16
#define SEND_DATA_CMD_OFFSET_WIDTH 16
#define SEND_DATA_CMD_OFFSET_MASK 0xFFFF0000U
/**
* Register: SEND_CONFIG_CMD
*/
#define SLOT_CMD 0x00000020U
#define SLOT_CMD_SHIFT 0
#define SLOT_CMD_WIDTH 32
#define SLOT_CMD_MASK 0xFFFFFFFFU
#define SLOT_CMD_KEY 0x434D442EU
#define SLOT_CMD 0x00000020U
#define SLOT_CMD_SHIFT 0
#define SLOT_CMD_WIDTH 32
#define SLOT_CMD_MASK 0xFFFFFFFFU
#define SLOT_CMD_KEY 0x434D442EU
/**
* Register: TXRX_STATE
*/
#define TXRX_STATE 0x00000024U
#define TXRX_STATE_ACQ_SHIFT 0
#define TXRX_STATE_ACQ_WIDTH 8
#define TXRX_STATE_ACQ_MASK 0x000000FFU
#define TXRX_STATE_ACQ_SUCCESS 0x00000001U
#define TXRX_STATE_ACQ_FAIL 0x00000002U
#define TXRX_STATE 0x00000024U
#define TXRX_STATE_ACQ_SHIFT 0
#define TXRX_STATE_ACQ_WIDTH 8
#define TXRX_STATE_ACQ_MASK 0x000000FFU
#define TXRX_STATE_ACQ_SUCCESS 0x00000001U
#define TXRX_STATE_ACQ_FAIL 0x00000002U
#define TXRX_STATE_FSYNC_SHIFT 8
#define TXRX_STATE_FSYNC_WIDTH 8
#define TXRX_STATE_FSYNC_MASK 0x0000FF00U
#define TXRX_STATE_FSYNC_SUCCESS 0x00000100U
#define TXRX_STATE_FSYNC_FAIL 0x00000200U
#define TXRX_STATE_FSYNC_SHIFT 8
#define TXRX_STATE_FSYNC_WIDTH 8
#define TXRX_STATE_FSYNC_MASK 0x0000FF00U
#define TXRX_STATE_FSYNC_SUCCESS 0x00000100U
#define TXRX_STATE_FSYNC_FAIL 0x00000200U
#define TXRX_STATE_DECODE_SHIFT 16
#define TXRX_STATE_DECODE_WIDTH 8
#define TXRX_STATE_DECODE_MASK 0x00FF0000U
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
#define TXRX_STATE_DECODE_FAIL 0x00020000U
#define TXRX_STATE_CRC_SHIFT 24
#define TXRX_STATE_CRC_WIDTH 8
#define TXRX_STATE_CRC_MASK 0xFF000000U
#define TXRX_STATE_CRC_OK 0x01000000U
#define TXRX_STATE_CRC_ERROR 0x02000000U
#define TXRX_STATE_DECODE_SHIFT 16
#define TXRX_STATE_DECODE_WIDTH 8
#define TXRX_STATE_DECODE_MASK 0x00FF0000U
#define TXRX_STATE_DECODE_SUCCESS 0x00010000U
#define TXRX_STATE_DECODE_FAIL 0x00020000U
#define TXRX_STATE_CRC_SHIFT 24
#define TXRX_STATE_CRC_WIDTH 8
#define TXRX_STATE_CRC_MASK 0xFF000000U
#define TXRX_STATE_CRC_OK 0x01000000U
#define TXRX_STATE_CRC_ERROR 0x02000000U
/**
* Register: RECVDATA_STATE
*/
#define RECVDATA_STATE 0x00000028U
#define RECVDATA_STATE_LEN_SHIFT 0
#define RECVDATA_STATE_LEN_WIDTH 16
#define RECVDATA_STATE_LEN_MASK 0x0000FFFFU
#define RECVDATA_STATE 0x00000028U
#define RECVDATA_STATE_LEN_SHIFT 0
#define RECVDATA_STATE_LEN_WIDTH 16
#define RECVDATA_STATE_LEN_MASK 0x0000FFFFU
#define RECVDATA_STATE_OFFSET_SHIFT 16
#define RECVDATA_STATE_OFFSET_WIDTH 16
#define RECVDATA_STATE_OFFSET_MASK 0xFFFF0000U
#define RECVDATA_STATE_OFFSET_SHIFT 16
#define RECVDATA_STATE_OFFSET_WIDTH 16
#define RECVDATA_STATE_OFFSET_MASK 0xFFFF0000U
/**
* Register: FSTIME_L
*/
#define FSTIME_L 0x000002CU
#define FSTIME_L_SHIFT 0
#define FSTIME_L_WIDTH 32
#define FSTIME_L_MASK 0xFFFFFFFFU
#define FSTIME_L 0x000002CU
#define FSTIME_L_SHIFT 0
#define FSTIME_L_WIDTH 32
#define FSTIME_L_MASK 0xFFFFFFFFU
/**
* Register: FSTIME_INT1
*/
#define FSTIME_H 0x0000030U
#define FSTIME_H_SHIFT 0
#define FSTIME_H_WIDTH 32
#define FSTIME_H_MASK 0xFFFFFFFFU
#define FSTIME_H 0x0000030U
#define FSTIME_H_SHIFT 0
#define FSTIME_H_WIDTH 32
#define FSTIME_H_MASK 0xFFFFFFFFU
/**
* Register: TM_TARGET_L
*/
#define TM_TARGET_L 0x0000064U
#define TM_TARGET_L_SHIFT 0
#define TM_TARGET_L_WIDTH 32
#define TM_TARGET_L_MASK 0xFFFFFFFFU
#define TM_TARGET_L 0x0000064U
#define TM_TARGET_L_SHIFT 0
#define TM_TARGET_L_WIDTH 32
#define TM_TARGET_L_MASK 0xFFFFFFFFU
/**
* Register: TM_TARGET_H
*/
#define TM_TARGET_H 0x0000038U
#define TM_TARGET_H_SHIFT 0
#define TM_TARGET_H_WIDTH 32
#define TM_TARGET_H_MASK 0xFFFFFFFFU
#define TM_TARGET_H 0x0000038U
#define TM_TARGET_H_SHIFT 0
#define TM_TARGET_H_WIDTH 32
#define TM_TARGET_H_MASK 0xFFFFFFFFU
/**
* Register: COASEFREQEST
*/
#define COASE_FREQEST 0x0000003CU
#define COASE_FREQEST_ACQ_SHIFT 0
#define COASE_FREQEST_ACQ_WIDTH 8
#define COASE_FREQEST_ACQ_MASK 0x000000FFU
#define COASE_FREQEST_SYNC_SHIFT 0
#define COASE_FREQEST_SYNC_WIDTH 16
#define COASE_FREQEST_SYNC_MASK 0x000FF00U
#define COASE_FREQEST 0x0000003CU
#define COASE_FREQEST_ACQ_SHIFT 0
#define COASE_FREQEST_ACQ_WIDTH 8
#define COASE_FREQEST_ACQ_MASK 0x000000FFU
#define COASE_FREQEST_SYNC_SHIFT 0
#define COASE_FREQEST_SYNC_WIDTH 16
#define COASE_FREQEST_SYNC_MASK 0x000FF00U
/**
* Register: FINE_FREQEST
*/
#define FINE_FREQEST 0x00000040U
#define FINE_FREQEST_SHIFT 0
#define FINE_FREQEST_WIDTH 32
#define FINE_FREQEST_MASK 0xFFFFFFFFU
#define FINE_FREQEST 0x00000040U
#define FINE_FREQEST_SHIFT 0
#define FINE_FREQEST_WIDTH 32
#define FINE_FREQEST_MASK 0xFFFFFFFFU
/**
* Register: BEAM_SELECT
*/
#define BEAM_SELECT_INDEX 0x00000044U
#define BEAM_SELECT_INDEX_SHIFT 0
#define BEAM_SELECT_INDEX_WIDTH 32
#define BEAM_SELECT_INDEX_MASK 0xFFFFFFFFU
#define BEAM_SELECT_INDEX 0x00000044U
#define BEAM_SELECT_INDEX_SHIFT 0
#define BEAM_SELECT_INDEX_WIDTH 32
#define BEAM_SELECT_INDEX_MASK 0xFFFFFFFFU
/**
* Register: BEAM_SELECT
*/
#define BEAM_AXIS 0x00000044U
#define BEAM_AXIS_EL_SHIFT 0
#define BEAM_AXIS_EL_WIDTH 16
#define BEAM_AXIS_EL_MASK 0x0000FFFFU
#define BEAM_AXIS_AZ_SHIFT 16
#define BEAM_AXIS_AZ_WIDTH 16
#define BEAM_AXIS_AZ_MASK 0xFFFF0000U
#define BEAM_AXIS 0x00000044U
#define BEAM_AXIS_EL_SHIFT 0
#define BEAM_AXIS_EL_WIDTH 16
#define BEAM_AXIS_EL_MASK 0x0000FFFFU
#define BEAM_AXIS_AZ_SHIFT 16
#define BEAM_AXIS_AZ_WIDTH 16
#define BEAM_AXIS_AZ_MASK 0xFFFF0000U
// Reserved.
#endif /* PHY_CTRLREG_H_ */
datalink814_ge/src/time_ctrl/time_ctrl.c
+53 -45
View File
@@ -1,18 +1,18 @@
/*****************************************************************************/
/**
* Description: Time sync implementation file.
*
*
* Author: Author's name
* Date: 2023-12-20 16:32
* LastEditors: FanHao
* LastEditTime: 2023-12-22 20:43
* Revisions: Rev 0.0
* History:
* History:
* [2023-12-20]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
/***************************** Include Files *********************************/
@@ -23,23 +23,22 @@
#include "../util/util.h"
/************************** Constant Definitions *****************************/
#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 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
#define TM_RESET_CODE 0x5253542E
/***************** 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)
#define TMC_Read(instPtr, addr) IO_In32(instPtr->config.baseAddr + addr)
#define TMC_Write(instPtr, addr, value) IO_Out32(instPtr->config.baseAddr + addr, value)
/*****************************************************************************/
/**
@@ -52,7 +51,8 @@
* FAILURE Initialization failed
*
******************************************************************************/
int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config){
int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config)
{
ASSERT(config != NULL);
ASSERT(instPtr != NULL);
(*instPtr) = malloc(sizeof(TIMECTRL_INST));
@@ -61,11 +61,13 @@ int32_t TimeCtrl_Init(TIMECTRL_INST **instPtr, TIMECTRL_CONFIG *config){
return SUCCESS;
}
void TimeCtrl_Destroy(TIMECTRL_INST *instPtr) {
void TimeCtrl_Destroy(TIMECTRL_INST *instPtr)
{
free(instPtr);
}
uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr){
uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr)
{
return TMC_Read(instPtr, VERSION_REG);
}
/*****************************************************************************/
@@ -78,12 +80,12 @@ uint32_t TimeCtrl_GetVersion(TIMECTRL_INST *instPtr){
* FAILURE Initialization failed
*
******************************************************************************/
void TimeCtrl_Reset(TIMECTRL_INST *instPtr){
void TimeCtrl_Reset(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
TMC_Write(instPtr, CNTSTEP_OPT, TM_RESET_CODE);
}
/*****************************************************************************/
/**
* @brief Assert time controller reset high
@@ -93,12 +95,12 @@ void TimeCtrl_Reset(TIMECTRL_INST *instPtr){
* @return NONE
*
******************************************************************************/
void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr){
void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
// TODO:
}
/*****************************************************************************/
/**
* @brief Assert time controller reset low
@@ -108,12 +110,12 @@ void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr){
* @return NONE
*
******************************************************************************/
void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr){
void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
// TODO:
}
/*****************************************************************************/
/**
* @brief Get current time integer value.
@@ -123,7 +125,8 @@ void TimeCtrl_ResetLow(TIMECTRL_INST *instPtr){
* @return integer value
*
******************************************************************************/
inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr){
inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
return TMC_Read(instPtr, TIME_INT_REG);
}
@@ -137,7 +140,8 @@ inline TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr){
* @return fractional value
*
******************************************************************************/
inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr){
inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr)
{
ASSERT(instPtr != NULL);
return TMC_Read(instPtr, TIME_FRAC_REG);
}
@@ -152,7 +156,8 @@ inline TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr){
* @return NONE
*
******************************************************************************/
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD* tmWord){
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord)
{
ASSERT(instPtr != NULL);
ASSERT(tmWord != NULL);
tmWord->frac_part = TMC_Read(instPtr, TIME_FRAC_REG);
@@ -169,13 +174,13 @@ void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD* tmWord){
* @return NONE.
*
******************************************************************************/
void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC* tmFrac) {
void TimeCtrl_GetCurrentStep(TIMECTRL_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.
@@ -187,17 +192,19 @@ void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC* tmFrac) {
* @return NONE.
*
******************************************************************************/
void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD* tmWord, TIMECTRL_ADJ_TYPE type){
void TimeCtrl_TimeAdjust(TIMECTRL_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){
if (type < TM_ADJ_ADD || TM_ADJ_ADD > TM_ADJ_SET)
{
DBG_ERROR("Unsupported adjustment type: %d", type);
return;
}
if(((int32_t)tmWord->frac_part) > TIME_FRAC_MAX
|| ((int32_t)tmWord->frac_part) < -TIME_FRAC_MAX) {
DBG_WARNING("Invalid fractional value %d (should between %d~%d)\n",
tmWord->frac_part, -TIME_FRAC_MAX, TIME_FRAC_MAX);
if (((int32_t)tmWord->frac_part) > TIME_FRAC_MAX || ((int32_t)tmWord->frac_part) < -TIME_FRAC_MAX)
{
DBG_WARNING("Invalid fractional value %d (should between %d~%d)\n",
tmWord->frac_part, -TIME_FRAC_MAX, TIME_FRAC_MAX);
return;
}
TMC_Write(instPtr, TIME_ADJ_FRAC, tmWord->frac_part);
@@ -216,33 +223,34 @@ void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD* tmWord, TIMECT
* @return NONE.
*
******************************************************************************/
void TimeCtrl_SetOptimizeStep(TIMECTRL_INST* instPtr, TIME_FRAC* tmFrac) {
void TimeCtrl_SetOptimizeStep(TIMECTRL_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
* @param gap t round
*
* @return NONE
*
******************************************************************************/
void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap){
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->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;
timeDiff -= (uint64_t)a->frac_part;
gap->frac_part = (TIME_FRAC)(timeDiff % TIME_FRAC_MAX);
gap->frac_part = (TIME_INT)(timeDiff / TIME_FRAC_MAX);
}
datalink814_ge/src/time_ctrl/time_ctrl.h
+18 -15
View File
@@ -1,17 +1,17 @@
/*****************************************************************************/
/**
* Description: A simplified version of timesync for datalink 814 (Dijian)
*
*
* Date: 2023-12-20-04 16:32
* LastEditors: Fan Hao
* LastEditTime: 2023-12-20 16:32
* Revisions: Rev 0.0
* History:
* History:
* [2023-12-20]: Initial version.
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#ifndef TIME_CTRL_H_
@@ -20,10 +20,11 @@
/***************************** Include Files *********************************/
#include <stdint.h>
/************************** Constant Definitions *****************************/
#define TIME_FRAC_MAX 500000 // how many 10ns in a 5ms
#define TIME_FRAC_MAX 500000 // how many 10ns in a 5ms
/***************** Macros (Inline Functions) Definitions *********************/
/**************************** Type Definitions *******************************/
typedef enum {
typedef enum
{
TM_ADJ_NONE = 0,
TM_ADJ_ADD = 1,
TM_ADJ_SET = 2
@@ -32,19 +33,22 @@ typedef enum {
typedef uint32_t TIME_FRAC;
typedef uint32_t TIME_INT;
typedef struct {
typedef struct
{
/* 5ms each LSB */
TIME_INT int_part;
/* 10ns each LSB */
TIME_FRAC frac_part;
} TIME_WORD;
typedef struct {
typedef struct
{
uintptr_t baseAddr;
uint32_t reset; //time for reset (unit: us)
uint32_t reset; // time for reset (unit: us)
} TIMECTRL_CONFIG;
typedef struct {
typedef struct
{
TIMECTRL_CONFIG config;
uint32_t modelVer;
} TIMECTRL_INST;
@@ -60,12 +64,11 @@ void TimeCtrl_ResetHigh(TIMECTRL_INST *instPtr);
TIME_INT TimeCtrl_GetIntCnt(TIMECTRL_INST *instPtr);
TIME_FRAC TimeCtrl_GetFracCnt(TIMECTRL_INST *instPtr);
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD* tmWord);
void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC* tmFrac);
void TimeCtrl_GetCurrentTime(TIMECTRL_INST *instPtr, TIME_WORD *tmWord);
void TimeCtrl_GetCurrentStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC* tmFrac);
void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD* tmWord, TIMECTRL_ADJ_TYPE type);
void TimeCtrl_SetOptimizeStep(TIMECTRL_INST *instPtr, TIME_FRAC *tmFrac);
void TimeCtrl_TimeAdjust(TIMECTRL_INST *instPtr, const TIME_WORD *tmWord, TIMECTRL_ADJ_TYPE type);
void TimeCtrl_CalculateTimeGap(const TIME_WORD *a, const TIME_WORD *b, TIME_WORD *gap);
#endif /* TIME_SYNC_H_ */
datalink814_ge/src/util/crc.c
+102 -80
View File
@@ -1,19 +1,19 @@
/*****************************************************************************/
/**
* Description: CRC implementation file
*
*
* Author: Hao Fan
* Date: 2021-11-16 00:50
* LastEditors: Hao Fan
* LastEditTime: 2022-07-17 20:53
* Revisions: Rev 0.0
* History:
* History:
* [2022-07-17]: Add CRC16_0x8005 CRC8_0x31.
* [2022-01-06]: Initial verion (Basic CRC-16).
*
*
* Copyright(c) 2021 Beijing Institute of Technology.
* Lab of Communication and Networking
*
*
******************************************************************************/
#include "util.h"
@@ -22,7 +22,8 @@
#ifdef __CRC_LSB
// CRC-16-CCITT G(X)=X16+X12+X5+1
// Reversed
static const uint16_t crc16_1021_lsb_lut[256] = {
static const uint16_t crc16_1021_lsb_lut[256] =
{
0x0000, 0x1189, 0x2312, 0x329B, 0x4624, 0x57AD, 0x6536, 0x74BF,
0x8C48, 0x9DC1, 0xAF5A, 0xBED3, 0xCA6C, 0xDBE5, 0xE97E, 0xF8F7,
0x1081, 0x0108, 0x3393, 0x221A, 0x56A5, 0x472C, 0x75B7, 0x643E,
@@ -58,7 +59,8 @@ static const uint16_t crc16_1021_lsb_lut[256] = {
};
// CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_lsb_lut[256] = {
const uint16_t crc16_8005_lsb_lut[256] =
{
0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
@@ -94,7 +96,8 @@ const uint16_t crc16_8005_lsb_lut[256] = {
};
// CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_lsb_lut[256] = {
const uint8_t crc8_31_lsb_lut[256] =
{
0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83,
0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41,
0x9D, 0xC3, 0x21, 0x7F, 0xFC, 0xA2, 0x40, 0x1E,
@@ -130,7 +133,8 @@ const uint8_t crc8_31_lsb_lut[256] = {
};
#else
// CRC-16-CCITT G(X)=X16+X12+X5+1
static const uint16_t crc16_1021_msb_lut[256] = {
static const uint16_t crc16_1021_msb_lut[256] =
{
0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7,
0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6,
@@ -166,7 +170,8 @@ static const uint16_t crc16_1021_msb_lut[256] = {
};
// CRC-16-MAXIM G(X)=X16+X15+X2+1
const uint16_t crc16_8005_msb_lut[256] = {
const uint16_t crc16_8005_msb_lut[256] =
{
0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
@@ -202,7 +207,8 @@ const uint16_t crc16_8005_msb_lut[256] = {
};
// CRC-8-MAXIM G(X)=X8+X5+X4+1
const uint8_t crc8_31_msb_lut[256] = {
const uint8_t crc8_31_msb_lut[256] =
{
0x00, 0x31, 0x62, 0x53, 0xC4, 0xF5, 0xA6, 0x97,
0xB9, 0x88, 0xDB, 0xEA, 0x7D, 0x4C, 0x1F, 0x2E,
0x43, 0x72, 0x21, 0x10, 0x87, 0xB6, 0xE5, 0xD4,
@@ -258,86 +264,91 @@ for(int i = 0; i < sizeof(dataBuff); i++)
/*****************************************************************************/
/**
* @brief CRC8 generator
*
*
* @param buffPtr data buffer pointer.
* @param dataLen data length
* @param poly polynomials of crc
* @param crcVal a pointer to store generated crc value
*
*
* @return SUCCESS
* FAILURE
*
*
******************************************************************************/
int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal) {
int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal)
{
uint8_t crc = CRC8_INIT;
const uint8_t* crc_lut = NULL;
const uint8_t *crc_lut = NULL;
switch(poly){
switch (poly)
{
#ifdef __CRC_LSB
case 0x31:
crc_lut = crc8_31_lsb_lut;
break;
default:
return FAILURE;
case 0x31:
crc_lut = crc8_31_lsb_lut;
break;
default:
return FAILURE;
#else
case 0x31:
crc_lut = crc8_31_msb_lut;
break;
default:
return FAILURE;
case 0x31:
crc_lut = crc8_31_msb_lut;
break;
default:
return FAILURE;
#endif
}
for(int i = 0; i < dataLen; i++){
crc = crc_lut[(uint8_t)(crc^buffPtr[i])];
for (int i = 0; i < dataLen; i++)
{
crc = crc_lut[(uint8_t)(crc ^ buffPtr[i])];
}
*crcVal = crc;
return SUCCESS;
}
/*****************************************************************************/
/**
* @brief CRC16 generator
*
*
* @param buffPtr data buffer pointer.
* @param dataLen data length
* @param poly polynomials of crc
* @param crcVal a pointer to store generated crc value
*
*
* @return SUCCESS
* FAILURE
*
*
******************************************************************************/
int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVal) {
int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVal)
{
uint16_t crc = CRC16_INIT;
const uint16_t* crc_lut = NULL;
switch(poly){
const uint16_t *crc_lut = NULL;
switch (poly)
{
#ifdef __CRC_LSB
case 0x8005:
crc_lut = crc16_8005_lsb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_lsb_lut;
break;
default:
return FAILURE;
case 0x8005:
crc_lut = crc16_8005_lsb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_lsb_lut;
break;
default:
return FAILURE;
#else
case 0x8005:
crc_lut = crc16_8005_msb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_msb_lut;
break;
default:
return FAILURE;
case 0x8005:
crc_lut = crc16_8005_msb_lut;
break;
case 0x1021:
crc_lut = crc16_1021_msb_lut;
break;
default:
return FAILURE;
#endif
}
for(int i = 0; i < dataLen; i++){
for (int i = 0; i < dataLen; i++)
{
#ifdef __CRC_LSB
crc = (crc>>8) ^ crc_lut[(uint8_t)(crc^buffPtr[i])];
crc = (crc >> 8) ^ crc_lut[(uint8_t)(crc ^ buffPtr[i])];
#else
crc = (crc<<8) ^ crc_lut[(uint8_t)((crc >> 8)^buffPtr[i])];
crc = (crc << 8) ^ crc_lut[(uint8_t)((crc >> 8) ^ buffPtr[i])];
#endif
}
*crcVal = crc;
@@ -347,71 +358,82 @@ int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVa
/*****************************************************************************/
/**
* @brief CRC8 compare cheker
*
*
* @param buffPtr data buffer pointer.
* @param dataLen data length
* @param crcOffset byte offset of crc stored position.
* @param poly polynomials of crc
*
*
* @return true
* false
*
******************************************************************************/
bool crc8_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) {
*
******************************************************************************/
bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{
int Status = SUCCESS;
uint8_t crc_comp;
uint8_t crc;
if (dataLen == 0) {
return false;
}
if (dataLen == 0)
{
return false;
}
crc_comp = (uint8_t)(*(buffPtr + crcOffset));
Status = crc8_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){
if (crc == crc_comp) {
if (Status == SUCCESS)
{
if (crc == crc_comp)
{
return true;
}
else {
else
{
return false;
}
}
else{
else
{
return false;
}
}
/*****************************************************************************/
/**
* @brief CRC16 compare cheker
*
*
* @param buffPtr data buffer pointer.
* @param dataLen data length
* @param crcOffset byte offset of crc stored position.
* @param poly polynomials of crc
*
*
* @return true
* fasle
*
******************************************************************************/
bool crc16_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly) {
*
******************************************************************************/
bool crc16_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly)
{
int Status = SUCCESS;
uint16_t crc_comp;
uint16_t crc;
if (dataLen == 0) {
return false;
}
if (dataLen == 0)
{
return false;
}
crc_comp = (uint16_t)(*(buffPtr + crcOffset));
crc_comp |= (uint16_t)((*(buffPtr + crcOffset + 1)) << 8);
Status = crc16_gen(buffPtr, dataLen, poly, &crc);
if(Status == SUCCESS){
if (crc == crc_comp) {
if (Status == SUCCESS)
{
if (crc == crc_comp)
{
return true;
}
else {
else
{
return false;
}
}
else{
else
{
return false;
}
}
datalink814_ge/src/util/list.c
+96 -56
View File
@@ -20,7 +20,8 @@
*/
list_node_t *
list_node_new(void *val) {
list_node_new(void *val)
{
list_node_t *self;
if (!(self = LIST_MALLOC(sizeof(list_node_t))))
return NULL;
@@ -35,7 +36,8 @@ list_node_new(void *val) {
*/
list_t *
list_new(void) {
list_new(void)
{
list_t *self;
if (!(self = LIST_MALLOC(sizeof(list_t))))
return NULL;
@@ -49,18 +51,20 @@ list_new(void) {
/*
* Remove all the node in list.
* @self: Pointer to the list
* @self: Pointer to the list
*/
void
list_clear(list_t *self) {
void list_clear(list_t *self)
{
uint32_t len = self->len;
list_node_t *next;
list_node_t *curr = self->head;
while (len) {
while (len)
{
next = curr->next;
if (self->free) self->free(curr->val);
if (self->free)
self->free(curr->val);
LIST_FREE(curr);
curr = next;
len--;
@@ -69,11 +73,11 @@ list_clear(list_t *self) {
/*
* Free the list.
* @self: Pointer to the list
* @self: Pointer to the list
*/
void
list_destroy(list_t *self) {
void list_destroy(list_t *self)
{
list_clear(self);
LIST_FREE(self);
}
@@ -86,15 +90,20 @@ list_destroy(list_t *self) {
*/
list_node_t *
list_rpush(list_t *self, list_node_t *node) {
if (!node) return NULL;
list_rpush(list_t *self, list_node_t *node)
{
if (!node)
return NULL;
if (self->len) {
if (self->len)
{
node->prev = self->tail;
node->next = NULL;
self->tail->next = node;
self->tail = node;
} else {
}
else
{
self->head = self->tail = node;
node->prev = node->next = NULL;
}
@@ -109,14 +118,19 @@ list_rpush(list_t *self, list_node_t *node) {
*/
list_node_t *
list_rpop(list_t *self) {
if (!self->len) return NULL;
list_rpop(list_t *self)
{
if (!self->len)
return NULL;
list_node_t *node = self->tail;
if (--self->len) {
if (--self->len)
{
(self->tail = node->prev)->next = NULL;
} else {
}
else
{
self->tail = self->head = NULL;
}
@@ -130,14 +144,19 @@ list_rpop(list_t *self) {
*/
list_node_t *
list_lpop(list_t *self) {
if (!self->len) return NULL;
list_lpop(list_t *self)
{
if (!self->len)
return NULL;
list_node_t *node = self->head;
if (--self->len) {
if (--self->len)
{
(self->head = node->next)->prev = NULL;
} else {
}
else
{
self->head = self->tail = NULL;
}
@@ -153,15 +172,20 @@ list_lpop(list_t *self) {
*/
list_node_t *
list_lpush(list_t *self, list_node_t *node) {
if (!node) return NULL;
list_lpush(list_t *self, list_node_t *node)
{
if (!node)
return NULL;
if (self->len) {
if (self->len)
{
node->next = self->head;
node->prev = NULL;
self->head->prev = node;
self->head = node;
} else {
}
else
{
self->head = self->tail = node;
node->prev = node->next = NULL;
}
@@ -173,22 +197,29 @@ list_lpush(list_t *self, list_node_t *node) {
/*
* Return the node associated to val or NULL.
* @self: Pointer to the list for finding given value
* @val: Value to find
* @val: Value to find
*/
list_node_t *
list_find(list_t *self, void *val) {
list_find(list_t *self, void *val)
{
list_iterator_t *it = list_iterator_new(self, LIST_HEAD);
list_node_t *node;
while ((node = list_iterator_next(it))) {
if (self->compare) {
if (self->compare(val, node->val) == 0) {
while ((node = list_iterator_next(it)))
{
if (self->compare)
{
if (self->compare(val, node->val) == 0)
{
list_iterator_destroy(it);
return node;
}
} else {
if (val == node->val) {
}
else
{
if (val == node->val)
{
list_iterator_destroy(it);
return node;
}
@@ -201,23 +232,27 @@ list_find(list_t *self, void *val) {
/*
* Return the node at the given index or NULL.
* @self: Pointer to the list for finding given index
* @self: Pointer to the list for finding given index
* @index: the index of node in the list
*/
list_node_t *
list_at(list_t *self, int index) {
list_at(list_t *self, int index)
{
list_direction_t direction = LIST_HEAD;
if (index < 0) {
if (index < 0)
{
direction = LIST_TAIL;
index = ~index;
}
if ((unsigned)index < self->len) {
if ((unsigned)index < self->len)
{
list_iterator_t *it = list_iterator_new(self, direction);
list_node_t *node = list_iterator_next(it);
while (index--) node = list_iterator_next(it);
while (index--)
node = list_iterator_next(it);
list_iterator_destroy(it);
return node;
}
@@ -227,21 +262,22 @@ list_at(list_t *self, int index) {
/*
* Remove the given node from the list, freeing it and it's value.
* @self: Pointer to the list to delete a node
* @self: Pointer to the list to delete a node
* @node: Pointer the node to be deleted
*/
void
list_remove(list_t *self, list_node_t *node) {
void list_remove(list_t *self, list_node_t *node)
{
node->prev
? (node->prev->next = node->next)
: (self->head = node->next);
? (node->prev->next = node->next)
: (self->head = node->next);
node->next
? (node->next->prev = node->prev)
: (self->tail = node->prev);
? (node->next->prev = node->prev)
: (self->tail = node->prev);
if (self->free) self->free(node->val);
if (self->free)
self->free(node->val);
LIST_FREE(node);
--self->len;
@@ -253,10 +289,11 @@ list_remove(list_t *self, list_node_t *node) {
*/
list_iterator_t *
list_iterator_new(list_t *list, list_direction_t direction) {
list_iterator_new(list_t *list, list_direction_t direction)
{
list_node_t *node = direction == LIST_HEAD
? list->head
: list->tail;
? list->head
: list->tail;
return list_iterator_new_from_node(node, direction);
}
@@ -266,7 +303,8 @@ list_iterator_new(list_t *list, list_direction_t direction) {
*/
list_iterator_t *
list_iterator_new_from_node(list_node_t *node, list_direction_t direction) {
list_iterator_new_from_node(list_node_t *node, list_direction_t direction)
{
list_iterator_t *self;
if (!(self = LIST_MALLOC(sizeof(list_iterator_t))))
return NULL;
@@ -281,12 +319,14 @@ list_iterator_new_from_node(list_node_t *node, list_direction_t direction) {
*/
list_node_t *
list_iterator_next(list_iterator_t *self) {
list_iterator_next(list_iterator_t *self)
{
list_node_t *curr = self->next;
if (curr) {
if (curr)
{
self->next = self->direction == LIST_HEAD
? curr->next
: curr->prev;
? curr->next
: curr->prev;
}
return curr;
}
@@ -295,8 +335,8 @@ list_iterator_next(list_iterator_t *self) {
* Free the list iterator.
*/
void
list_iterator_destroy(list_iterator_t *self) {
void list_iterator_destroy(list_iterator_t *self)
{
LIST_FREE(self);
self = NULL;
}
datalink814_ge/src/util/list.h
+71 -66
View File
@@ -9,102 +9,107 @@
#define __LIST_H__
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
#include <stdlib.h>
#include <stdint.h>
/*
* list_t iterator direction.
*/
/*
* list_t iterator direction.
*/
typedef enum {
LIST_HEAD,
LIST_TAIL
} list_direction_t;
typedef enum
{
LIST_HEAD,
LIST_TAIL
} list_direction_t;
/*
* list_t node struct.
*/
/*
* list_t node struct.
*/
typedef struct list_node {
struct list_node *prev;
struct list_node *next;
void *val;
} list_node_t;
typedef struct list_node
{
struct list_node *prev;
struct list_node *next;
void *val;
} list_node_t;
/*
* list_t struct.
*/
/*
* list_t struct.
*/
typedef struct {
list_node_t *head;
list_node_t *tail;
uint32_t len;
void (*free)(void *val);
int8_t (*compare)(void *a, void *b);
} list_t;
typedef struct
{
list_node_t *head;
list_node_t *tail;
uint32_t len;
void (*free)(void *val);
int8_t (*compare)(void *a, void *b);
} list_t;
/*
* list_t iterator struct.
*/
/*
* list_t iterator struct.
*/
typedef struct {
list_node_t *next;
list_direction_t direction;
} list_iterator_t;
typedef struct
{
list_node_t *next;
list_direction_t direction;
} list_iterator_t;
// Node prototypes.
// Node prototypes.
list_node_t *
list_node_new(void *val);
list_node_t *
list_node_new(void *val);
// list_t prototypes.
// list_t prototypes.
list_t *
list_new(void);
list_t *
list_new(void);
list_node_t *
list_rpush(list_t *self, list_node_t *node);
list_node_t *
list_rpush(list_t *self, list_node_t *node);
list_node_t *
list_lpush(list_t *self, list_node_t *node);
list_node_t *
list_lpush(list_t *self, list_node_t *node);
list_node_t *
list_find(list_t *self, void *val);
list_node_t *
list_find(list_t *self, void *val);
list_node_t *
list_at(list_t *self, int index);
list_node_t *
list_at(list_t *self, int index);
list_node_t *
list_rpop(list_t *self);
list_node_t *
list_rpop(list_t *self);
list_node_t *
list_lpop(list_t *self);
list_node_t *
list_lpop(list_t *self);
void
list_remove(list_t *self, list_node_t *node);
void
list_remove(list_t *self, list_node_t *node);
void
list_clear(list_t *self);
void
list_clear(list_t *self);
void
list_destroy(list_t *self);
void
list_destroy(list_t *self);
// list_t iterator prototypes.
// list_t iterator prototypes.
list_iterator_t *
list_iterator_new(list_t *list, list_direction_t direction);
list_iterator_t *
list_iterator_new(list_t *list, list_direction_t direction);
list_iterator_t *
list_iterator_new_from_node(list_node_t *node, list_direction_t direction);
list_iterator_t *
list_iterator_new_from_node(list_node_t *node, list_direction_t direction);
list_node_t *
list_iterator_next(list_iterator_t *self);
list_node_t *
list_iterator_next(list_iterator_t *self);
void
list_iterator_destroy(list_iterator_t *self);
void
list_iterator_destroy(list_iterator_t *self);
#ifdef __cplusplus
}
datalink814_ge/src/util/util.c
+56 -39
View File
@@ -11,7 +11,8 @@
/************************** Functions Implementation **************************/
/******************************************************************************/
#ifdef __BITSWAP_TABLE
static uint8_t byte_flipLR[256] = {
static uint8_t byte_flipLR[256] =
{
0x00, 0x80, 0x40, 0xC0, 0x20, 0xA0, 0x60, 0xE0,
0x10, 0x90, 0x50, 0xD0, 0x30, 0xB0, 0x70, 0xF0,
0x08, 0x88, 0x48, 0xC8, 0x28, 0xA8, 0x68, 0xE8,
@@ -47,16 +48,19 @@ static uint8_t byte_flipLR[256] = {
};
#endif
inline void IO_Out32(uintptr_t addr, uint32_t value){
inline void IO_Out32(uintptr_t addr, uint32_t value)
{
volatile uint32_t *localAddr = (volatile uint32_t *)addr;
*localAddr = value;
}
inline uint32_t IO_In32(uintptr_t addr){
return *(volatile uint32_t*) addr;
inline uint32_t IO_In32(uintptr_t addr)
{
return *(volatile uint32_t *)addr;
}
uint8_t byte_bit_swap(uint8_t byte){
uint8_t byte_bit_swap(uint8_t byte)
{
#ifdef __BITSWAP_TABLE
return byte_flipLR[byte];
#else
@@ -64,38 +68,42 @@ uint8_t byte_bit_swap(uint8_t byte){
#endif
}
uint32_t word_bit_swap(uint32_t word){
uint32_t word_bit_swap(uint32_t word)
{
#ifdef __BITSWAP_TABLE
uint32_t rv_word = (byte_flipLR[word & 0xff] << 24) |
(byte_flipLR[(word >> 8) & 0xff] << 16) |
(byte_flipLR[(word >> 16) & 0xff] << 8) |
(byte_flipLR[(word >> 24) & 0xff]);
(byte_flipLR[(word >> 8) & 0xff] << 16) |
(byte_flipLR[(word >> 16) & 0xff] << 8) |
(byte_flipLR[(word >> 24) & 0xff]);
return rv_word;
#else
word = (((word & 0xaaaaaaaa) >> 1) | ((word & 0x55555555) << 1));
word = (((word & 0xcccccccc) >> 2) | ((word & 0x33333333) << 2));
word = (((word & 0xf0f0f0f0) >> 4) | ((word & 0x0f0f0f0f) << 4));
word = (((word & 0xff00ff00) >> 8) | ((word & 0x00ff00ff) << 8));
return((word >> 16) | (word << 16));
return ((word >> 16) | (word << 16));
#endif
}
uint32_t count_ones(uint32_t word){
uint32_t count_ones(uint32_t word)
{
#ifdef __GNUC__
return __builtin_popcount(word);
#else
uint32_t one_count = 0;
while (word) {
while (word)
{
if (word & 0x00000001)
one_count ++;
one_count++;
word >>= 1;
}
return one_count;
#endif
}
double fix2double(int32_t val, uint8_t frac_bits){
return (((double)val)/(BIT(frac_bits)));
double fix2double(int32_t val, uint8_t frac_bits)
{
return (((double)val) / (BIT(frac_bits)));
}
/**
@@ -104,20 +112,23 @@ double fix2double(int32_t val, uint8_t frac_bits){
uint32_t find_first_set_bit(uint32_t word)
{
#ifdef __GNUC__
if(word != 0){
if (word != 0)
{
return __builtin_ctz(word);
}
else{
else
{
return 32;
}
#else
uint32_t first_set_bit = 0;
while (word) {
while (word)
{
if (word & 0x1)
return first_set_bit;
word >>= 1;
first_set_bit ++;
first_set_bit++;
}
return 32;
@@ -130,21 +141,24 @@ uint32_t find_first_set_bit(uint32_t word)
uint32_t find_last_set_bit(uint32_t word)
{
#ifdef __GNUC__
if(word != 0){
return (31-__builtin_clz(word));
if (word != 0)
{
return (31 - __builtin_clz(word));
}
else{
else
{
return 32;
}
#else
uint32_t bit = 0;
uint32_t last_set_bit = 32;
while (word) {
while (word)
{
if (word & 0x1)
last_set_bit = bit;
word >>= 1;
bit ++;
bit++;
}
return last_set_bit;
@@ -155,15 +169,17 @@ uint32_t find_last_set_bit(uint32_t word)
* Locate the closest element in an array.
*/
uint32_t find_closest(int32_t val,
const int32_t *array,
uint32_t size)
const int32_t *array,
uint32_t size)
{
int32_t diff = abs(array[0] - val);
uint32_t ret = 0;
uint32_t i;
for (i = 1; i < size; i++) {
if (abs(array[i] - val) < diff) {
for (i = 1; i < size; i++)
{
if (abs(array[i] - val) < diff)
{
diff = abs(array[i] - val);
ret = i;
}
@@ -200,7 +216,7 @@ int32_t log_base_2(uint32_t x)
* Find greatest common divisor of the given two numbers.
*/
uint32_t greatest_common_divisor(uint32_t a,
uint32_t b)
uint32_t b)
{
uint32_t div;
uint32_t common_div = 1;
@@ -219,11 +235,11 @@ uint32_t greatest_common_divisor(uint32_t a,
* Calculate best rational approximation for a given fraction.
*/
void rational_best_approximation(uint32_t given_numerator,
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator)
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator)
{
uint32_t gcd;
@@ -233,7 +249,8 @@ void rational_best_approximation(uint32_t given_numerator,
*best_denominator = given_denominator / gcd;
if ((*best_numerator > max_numerator) ||
(*best_denominator > max_denominator)) {
(*best_denominator > max_denominator))
{
*best_numerator = 0;
*best_denominator = 0;
}
@@ -246,7 +263,8 @@ uint32_t hweight8(uint32_t word)
{
uint32_t count = 0;
while (word) {
while (word)
{
if (word & 0x1)
count++;
word >>= 1;
@@ -258,8 +276,8 @@ uint32_t hweight8(uint32_t word)
/**
* Calculate the quotient and the remainder of an integer division.
*/
uint64_t do_div(uint64_t* n,
uint64_t base)
uint64_t do_div(uint64_t *n,
uint64_t base)
{
uint64_t mod = 0;
@@ -348,4 +366,3 @@ uint32_t srt_to_uint32(const char *str)
else
return value;
}
datalink814_ge/src/util/util.h
+50 -46
View File
@@ -1,4 +1,5 @@
/***************************************************************************//**
/***************************************************************************
//**
* @file util.h
* @brief Implementation of utility functions.
* @author DBogdan (dragos.bogdan@analog.com)
@@ -48,19 +49,19 @@
/******************************************************************************/
/********************** Macros and Constants Definitions **********************/
/******************************************************************************/
//#define __CRC_LSB
// #define __CRC_LSB
#define __BITSWAP_TABLE
#define __CRC_LSB
#define CRC16_INIT 0x0000
#define CRC8_INIT 0x00
#define BIT(x) (1 << (x))
#define BIT(x) (1 << (x))
#define ARRAY_SIZE(x) \
(sizeof(x) / sizeof((x)[0]))
#define DIV_ROUND_UP(x,y) \
(((x) + (y) - 1) / (y))
#define DIV_ROUND_UP(x, y) \
(((x) + (y)-1) / (y))
#define DIV_ROUND_CLOSEST(x, y) \
(((x) + (y) / 2) / (y))
#define DIV_ROUND_CLOSEST_ULL(x, y) \
@@ -69,52 +70,56 @@
#define min(x, y) \
(((x) < (y)) ? (x) : (y))
#define min_t(type, x, y) \
(type)min((type)(x), (type)(y))
(type) min((type)(x), (type)(y))
#define max(x, y) \
(((x) > (y)) ? (x) : (y))
#define max_t(type, x, y) \
(type)max((type)(x), (type)(y))
(type) max((type)(x), (type)(y))
#define clamp(val, min_val, max_val) \
(max(min((val), (max_val)), (min_val)))
#define clamp_t(type, val, min_val, max_val) \
(type)clamp((type)(val), (type)(min_val), (type)(max_val))
(type) clamp((type)(val), (type)(min_val), (type)(max_val))
#define swap(x, y) \
{typeof(x) _tmp_ = (x); (x) = (y); (y) = _tmp_;}
#define swap(x, y) \
{ \
typeof(x) _tmp_ = (x); \
(x) = (y); \
(y) = _tmp_; \
}
#define round_up(x,y) \
(((x)+(y)-1)/(y))
#define round_up(x, y) \
(((x) + (y)-1) / (y))
#define BITS_PER_LONG 32
#define GENMASK(h, l) ({ \
uint32_t t = (uint32_t)(~0UL); \
t = t << (BITS_PER_LONG - (h - l + 1)); \
t = t >> (BITS_PER_LONG - (h + 1)); \
t; \
#define GENMASK(h, l) ({ \
uint32_t t = (uint32_t)(~0UL); \
t = t << (BITS_PER_LONG - (h - l + 1)); \
t = t >> (BITS_PER_LONG - (h + 1)); \
t; \
})
#define bswap_constant_32(x) \
((((x) & 0xff000000) >> 24) | (((x) & 0x00ff0000) >> 8) | \
(((x) & 0x0000ff00) << 8) | (((x) & 0x000000ff) << 24))
#define bswap_constant_32(x) \
((((x) & 0xff000000) >> 24) | (((x) & 0x00ff0000) >> 8) | \
(((x) & 0x0000ff00) << 8) | (((x) & 0x000000ff) << 24))
#define bswap_constant_16(x) ((((x) & (uint16_t)0xff00) >> 8) | \
(((x) & (uint16_t)0x00ff) << 8))
(((x) & (uint16_t)0x00ff) << 8))
#define bit_swap_constant_8(x) \
((((x) & 0x80) >> 7) | \
(((x) & 0x40) >> 5) | \
(((x) & 0x20) >> 3) | \
(((x) & 0x10) >> 1) | \
(((x) & 0x08) << 1) | \
(((x) & 0x04) << 3) | \
(((x) & 0x02) << 5) | \
((((x) & 0x80) >> 7) | \
(((x) & 0x40) >> 5) | \
(((x) & 0x20) >> 3) | \
(((x) & 0x10) >> 1) | \
(((x) & 0x08) << 1) | \
(((x) & 0x04) << 3) | \
(((x) & 0x02) << 5) | \
(((x) & 0x01) << 7))
#define U16_MAX ((uint16_t)~0U)
#define S16_MAX ((int16_t)(U16_MAX>>1))
#define U16_MAX ((uint16_t)~0U)
#define S16_MAX ((int16_t)(U16_MAX >> 1))
#define DIV_U64(x, y) (x / y)
@@ -147,8 +152,8 @@ uint32_t find_first_set_bit(uint32_t word);
uint32_t find_last_set_bit(uint32_t word);
/* Locate the closest element in an array. */
uint32_t find_closest(int32_t val,
const int32_t *array,
uint32_t size);
const int32_t *array,
uint32_t size);
/* Shift the value and apply the specified mask. */
uint32_t field_prep(uint32_t mask, uint32_t val);
/* Get a field specified by a mask from a word. */
@@ -157,22 +162,22 @@ uint32_t field_get(uint32_t mask, uint32_t word);
int32_t log_base_2(uint32_t x);
/* Find greatest common divisor of the given two numbers. */
uint32_t greatest_common_divisor(uint32_t a,
uint32_t b);
uint32_t b);
/* Calculate best rational approximation for a given fraction. */
void rational_best_approximation(uint32_t given_numerator,
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator);
uint32_t given_denominator,
uint32_t max_numerator,
uint32_t max_denominator,
uint32_t *best_numerator,
uint32_t *best_denominator);
/* Calculate the number of set bits. */
uint32_t hweight8(uint32_t word);
/* Calculate the quotient and the remainder of an integer division. */
uint64_t do_div(uint64_t* n,
uint64_t base);
uint64_t do_div(uint64_t *n,
uint64_t base);
/* Unsigned 64bit divide with 64bit divisor and remainder */
uint64_t div64_u64_rem(uint64_t dividend, uint64_t divisor,
uint64_t *remainder);
uint64_t *remainder);
/* Unsigned 64bit divide with 32bit divisor with remainder */
uint64_t div_u64_rem(uint64_t dividend, uint32_t divisor, uint32_t *remainder);
int64_t div_s64_rem(int64_t dividend, int32_t divisor, int32_t *remainder);
@@ -184,9 +189,8 @@ int32_t str_to_int32(const char *str);
/* Converts from string to uint32_t */
uint32_t srt_to_uint32(const char *str);
int crc16_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint16_t* crcVal);
bool crc16_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly);
int crc8_gen(uint8_t* buffPtr, uint32_t dataLen, uint16_t poly, uint8_t* crcVal);
bool crc8_check(uint8_t* buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly);
int crc16_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint16_t *crcVal);
bool crc16_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly);
int crc8_gen(uint8_t *buffPtr, uint32_t dataLen, uint16_t poly, uint8_t *crcVal);
bool crc8_check(uint8_t *buffPtr, uint32_t dataLen, uint32_t crcOffset, uint16_t poly);
#endif // __UTIL_H_
datalink814_ge/unit_test/tmc_board_unit_test.c
+38 -29
View File
@@ -7,30 +7,32 @@
#define HARDWARE_VERSION 0x00000001
#define MAX_OPT_DELAY 100 // 1us = 100*10ns
TIMECTRL_CONFIG tmcConfig = {
.baseAddr = 0/*TODO: fill withe real base address*/,
.reset = 10
};
.baseAddr = 0 /*TODO: fill withe real base address*/,
.reset = 10};
TIMECTRL_INST* tmcInstPtr = NULL;
TIMECTRL_INST *tmcInstPtr = NULL;
void tmc_setup(void){
void tmc_setup(void)
{
int32_t Status;
Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
ASSERT_TRUE(SUCCESS == Status);
}
void tmc_teardown(void){
void tmc_teardown(void)
{
TimeCtrl_Destroy(tmcInstPtr);
}
TEST(tmc_version) {
TEST(tmc_version)
{
ASSERT_EQUAL(HARDWARE_VERSION, TimeCtrl_GetVersion(tmcInstPtr));
return;
}
TEST(tmc_read_time) {
TEST(tmc_read_time)
{
TIME_WORD timeStamp;
TIME_INT timeH;
TIME_FRAC timeL;
@@ -39,17 +41,20 @@ TEST(tmc_read_time) {
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStamp);
timeH = TimeCtrl_GetIntCnt(tmcInstPtr);
timeL = TimeCtrl_GetFracCnt(tmcInstPtr);
if(timeL >= timeStamp.frac_part) {
if (timeL >= timeStamp.frac_part)
{
ASSERT_EQUAL(timeH, timeStamp.int_part);
ASSERT_TRUE(timeL - timeStamp.frac_part < MAX_OPT_DELAY);
}
else {
ASSERT_EQUAL(timeH, timeStamp.int_part+1);
else
{
ASSERT_EQUAL(timeH, timeStamp.int_part + 1);
ASSERT_TRUE(timeL + TIME_FRAC_MAX - timeStamp.frac_part < MAX_OPT_DELAY);
}
}
TEST(tmc_reset) {
TEST(tmc_reset)
{
TIME_WORD timeStamp;
TimeCtrl_Reset(tmcInstPtr);
@@ -58,7 +63,8 @@ TEST(tmc_reset) {
ASSERT_TRUE(timeStamp.frac_part < MAX_OPT_DELAY);
}
TEST(tmc_adjustment) {
TEST(tmc_adjustment)
{
TIME_WORD timeStampPre, timeStampPost, timeAdj;
// Adjustment -- set
@@ -73,19 +79,21 @@ TEST(tmc_adjustment) {
timeAdj.int_part = 0x00000203;
TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_SET);
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
ASSERT_EQUAL(timeAdj.int_part, timeStampPost.int_part+1);
ASSERT_EQUAL(timeAdj.int_part, timeStampPost.int_part + 1);
ASSERT_TRUE(timeStampPost.frac_part < MAX_OPT_DELAY);
timeAdj.frac_part = TIME_FRAC_MAX*2;
timeAdj.frac_part = TIME_FRAC_MAX * 2;
timeAdj.int_part = 0x00000203;
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_SET);
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
if(timeStampPost.frac_part >= timeStampPre.frac_part) {
if (timeStampPost.frac_part >= timeStampPre.frac_part)
{
ASSERT_EQUAL(timeStampPost.int_part, timeStampPre.int_part);
ASSERT_TRUE(timeStampPost.frac_part - timeStampPre.frac_part < MAX_OPT_DELAY);
}
else {
else
{
ASSERT_EQUAL(timeStampPost.int_part, timeStampPre.int_part + 1);
ASSERT_TRUE(timeStampPost.frac_part + TIME_FRAC_MAX - timeStampPre.frac_part < MAX_OPT_DELAY);
}
@@ -96,10 +104,10 @@ TEST(tmc_adjustment) {
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_ADD);
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
if(timeStampPre.frac_part + timeAdj.frac_part > TIME_FRAC_MAX) {
if (timeStampPre.frac_part + timeAdj.frac_part > TIME_FRAC_MAX)
{
ASSERT_TRUE(timeStampPost.int_part <= timeStampPre.int_part + timeAdj.int_part + 2);
ASSERT_TRUE(timeStampPost.frac_part + TIME_FRAC_MAX - timeStampPre.frac_part
< MAX_OPT_DELAY + timeAdj.frac_part);
ASSERT_TRUE(timeStampPost.frac_part + TIME_FRAC_MAX - timeStampPre.frac_part < MAX_OPT_DELAY + timeAdj.frac_part);
}
timeAdj.frac_part = -253;
@@ -107,19 +115,21 @@ TEST(tmc_adjustment) {
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPre);
TimeCtrl_TimeAdjust(tmcInstPtr, &timeAdj, TM_ADJ_ADD);
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStampPost);
if(timeStampPre.frac_part + timeAdj.frac_part > TIME_FRAC_MAX) {
if (timeStampPre.frac_part + timeAdj.frac_part > TIME_FRAC_MAX)
{
ASSERT_TRUE(timeStampPost.int_part <= timeStampPre.int_part + timeAdj.int_part + 2);
ASSERT_TRUE(timeStampPost.frac_part + TIME_FRAC_MAX - timeStampPre.frac_part
< MAX_OPT_DELAY + timeAdj.frac_part);
ASSERT_TRUE(timeStampPost.frac_part + TIME_FRAC_MAX - timeStampPre.frac_part < MAX_OPT_DELAY + timeAdj.frac_part);
}
}
TEST(tmc_setopt) {
TEST(tmc_setopt)
{
// TODO:
return;
}
TEST_SUITE(tmc_unit_test) {
TEST_SUITE(tmc_unit_test)
{
TEST_SUITE_CONFIGURE(tmc_setup, tmc_teardown);
RUN_TEST(tmc_version);
RUN_TEST(tmc_read_time);
@@ -128,10 +138,9 @@ TEST_SUITE(tmc_unit_test) {
RUN_TEST(tmc_setopt);
}
int main(void) {
int main(void)
{
RUN_TEST_SUITE(tmc_unit_test);
TEST_REPORT();
return TEST_EXIT_CODE;
}
datalink814_ge/unit_test/tmc_emulate_unit_test.c
+28 -23
View File
@@ -13,50 +13,55 @@ uint32_t tmcReg[32] = {0};
TIMECTRL_CONFIG tmcConfig = {
.baseAddr = 0,
.reset = 10
};
.reset = 10};
TIMECTRL_INST* tmcInstPtr = NULL;
TIMECTRL_INST *tmcInstPtr = NULL;
void tmc_setup(void){
void tmc_setup(void)
{
int32_t Status;
tmcReg[1] = HARDWARE_VERSION; // Version
tmcReg[2] = 253; // Int
tmcReg[3] = 253; // Frac
tmcReg[4] = 0; // Reset
tmcReg[5] = 0; // Preset H
tmcReg[6] = 0; // Preset L
tmcReg[7] = 0; // Adjustment H
tmcReg[8] = 0; // Adjustment L
tmcReg[9] = 0; // Adjustment Trigger
tmcReg[10] = 0; // step opt
tmcReg[2] = 253; // Int
tmcReg[3] = 253; // Frac
tmcReg[4] = 0; // Reset
tmcReg[5] = 0; // Preset H
tmcReg[6] = 0; // Preset L
tmcReg[7] = 0; // Adjustment H
tmcReg[8] = 0; // Adjustment L
tmcReg[9] = 0; // Adjustment Trigger
tmcReg[10] = 0; // step opt
tmcConfig.baseAddr = (uintptr_t)tmcReg;
Status = TimeCtrl_Init(&tmcInstPtr, &tmcConfig);
ASSERT_EQUAL(SUCCESS, Status);
}
void tmc_teardown(void){
void tmc_teardown(void)
{
TimeCtrl_Destroy(tmcInstPtr);
}
TEST(tmc_version) {
TEST(tmc_version)
{
ASSERT_EQUAL(HARDWARE_VERSION, TimeCtrl_GetVersion(tmcInstPtr));
return;
}
TEST(tmc_basic_operations) {
TEST(tmc_basic_operations)
{
TIME_WORD timeStamp;
TIME_INT timeH;
TIME_FRAC timeL;
TimeCtrl_GetCurrentTime(tmcInstPtr, &timeStamp);
timeH = TimeCtrl_GetIntCnt(tmcInstPtr);
timeL = TimeCtrl_GetFracCnt(tmcInstPtr);
if(timeL >= timeStamp.frac_part) {
if (timeL >= timeStamp.frac_part)
{
ASSERT_EQUAL(timeH, timeStamp.int_part);
ASSERT_TRUE(timeL - timeStamp.frac_part < MAX_OPT_DELAY);
}
else {
ASSERT_EQUAL(timeH, timeStamp.int_part+1);
else
{
ASSERT_EQUAL(timeH, timeStamp.int_part + 1);
ASSERT_TRUE(timeL + TIME_FRAC_MAX - timeStamp.frac_part < MAX_OPT_DELAY);
}
@@ -69,16 +74,16 @@ TEST(tmc_basic_operations) {
return;
}
TEST_SUITE(tmc_unit_test) {
TEST_SUITE(tmc_unit_test)
{
TEST_SUITE_CONFIGURE(tmc_setup, tmc_teardown);
RUN_TEST(tmc_version);
RUN_TEST(tmc_basic_operations);
}
int main(void) {
int main(void)
{
RUN_TEST_SUITE(tmc_unit_test);
TEST_REPORT();
return TEST_EXIT_CODE;
}