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