forked from 0xha0/datalink_repo
328 lines
8.2 KiB
C
328 lines
8.2 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* Description: Functions for framing operations.
|
|
*
|
|
* Date: 2022-07-16 14:17
|
|
* LastEditors: Fan Hao
|
|
* LastEditTime: 2022-07-16 17:11
|
|
* Revisions: Rev x.x
|
|
* History:
|
|
* [YYYY-MM-DD]: Description of modification.
|
|
*
|
|
* Copyright(c) 2022 Beijing Institute of Technology.
|
|
* Lab of Communication and Networking
|
|
*
|
|
******************************************************************************/
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <stdint.h>
|
|
#include "linklayer814.h"
|
|
#include "msg_def.h"
|
|
#include "../error.h"
|
|
#include "../common.h"
|
|
#include "../bit_array/bit_array.h"
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
// Definition of each slot content
|
|
L2_MSG_TYPE intraBmFrameMsgs[1] = {
|
|
L2_MSG_BM
|
|
};
|
|
L2_MSG_TYPE intraW2AcqFrameMsgs[6] = {
|
|
L2_MSG_SCAN_INIT,
|
|
L2_MSG_SCAN_CONT_1,
|
|
L2_MSG_SCAN_CONT_2,
|
|
L2_MSG_SCAN_CONT_3,
|
|
L2_MSG_SCAN_CONT_4,
|
|
L2_MSG_SCAN_CONT_5
|
|
};
|
|
|
|
L2_MSG_TYPE intraMgmtFrameMsgs[6] = {
|
|
L2_MSG_TK_INIT,
|
|
L2_MSG_TK_EXTD,
|
|
L2_MSG_TK_CONT_1,
|
|
L2_MSG_TK_CONT_2,
|
|
L2_MSG_TK_CONT_3,
|
|
L2_MSG_NQT_INIT
|
|
};
|
|
|
|
L2_MSG_TYPE taFrameMsgs[4] = {
|
|
L2_MSG_TA_INIT,
|
|
L2_MSG_TA_EXTD,
|
|
L2_MSG_TA_CONT_1,
|
|
L2_MSG_TA_CONT_2
|
|
};
|
|
|
|
L2_MSG_TYPE interSvcFrameMsgs[2] = {
|
|
L2_MSG_LINK_INIT,
|
|
L2_MSG_LINK_CONT_1
|
|
};
|
|
|
|
L2_MSG_TYPE rttReqFrameMsgs[3] = {
|
|
L2_MSG_RTT_REQ_INIT,
|
|
L2_MSG_RTT_REQ_EXTD,
|
|
L2_MSG_RTT_REQ_CONT
|
|
};
|
|
|
|
L2_MSG_TYPE rttRspFrameMsgs[3] = {
|
|
L2_MSG_RTT_RSP_INIT,
|
|
L2_MSG_RTT_RSP_EXTD,
|
|
L2_MSG_RTT_RSP_CONT
|
|
};
|
|
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Insert a data chunk to a bit array.
|
|
*
|
|
* @param bitarr pointer to target bit array.
|
|
* offset offset of insert start position.
|
|
* numBit number of insert bits
|
|
* chunk 64 bit data chunk, contains the data should be stored
|
|
* in LSB style.
|
|
*
|
|
* @return FAILURE insert operation exceed bit array size.
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
void FrameInsertChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t chunk){
|
|
ASSERT(bitarr != NULL);
|
|
ASSERT(bitarr->num_of_bits >= offset+numBit && numBit <= 64);
|
|
#ifdef CHUNK_FILL_STYLE_XJY
|
|
int32_t bitRmn = numBit;
|
|
uint64_t dataWr = 0;
|
|
uint32_t bitOff = 0;
|
|
uint32_t byteOff = 0;
|
|
uint32_t bitWr = 0;
|
|
uint32_t bitWrCnt = 0;
|
|
while (bitRmn > 0){
|
|
bitOff = offset%8;
|
|
byteOff = offset/8;
|
|
bitWr = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
|
|
dataWr = chunk>>(numBit-(bitWr + bitWrCnt));
|
|
bit_array_set_wordn(bitarr, byteOff*8 + (8-bitOff-bitWr), dataWr, bitWr);
|
|
bitWrCnt += bitWr;
|
|
bitRmn -= bitWr;
|
|
offset += bitWr;
|
|
}
|
|
#else
|
|
bit_array_set_wordn(bitarr, offset, chunk, numBit);
|
|
#endif
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Get a data chunk to a bit array.
|
|
*
|
|
* @param bitarr pointer to target bit array.
|
|
* offset offset of data chunk start position.
|
|
* numBit number of data chunk
|
|
* chunk pointer to a 64 bit data chunk container, data will be stored
|
|
* in LSB style.
|
|
*
|
|
* @return FAILURE read operation exceed bit array size.
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
void FrameGetChunk(BIT_ARRAY* bitarr, uint32_t offset, uint32_t numBit, uint64_t* chunk){
|
|
ASSERT(bitarr != NULL);
|
|
ASSERT(bitarr->num_of_bits >= offset+numBit && numBit <= 64);
|
|
|
|
#ifdef CHUNK_FILL_STYLE_XJY
|
|
int32_t bitRmn = numBit;
|
|
uint64_t dataRd = 0;
|
|
uint32_t bitOff = offset%8;
|
|
uint32_t byteOff = offset/8;
|
|
uint32_t bitRd = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
|
|
uint32_t bitRdCnt = 0;
|
|
while (bitRmn > 0){
|
|
dataRd |= bit_array_get_wordn(bitarr, byteOff*8 + (8-bitOff-bitRd), bitRd);
|
|
bitRdCnt += bitRd;
|
|
bitRmn -= bitRd;
|
|
offset += bitRd;
|
|
bitOff = offset%8;
|
|
byteOff = offset/8;
|
|
bitRd = (bitRmn > (8-bitOff)) ? (8-bitOff) : bitRmn;
|
|
dataRd = dataRd<<bitRd;
|
|
}
|
|
*chunk = dataRd;
|
|
return SUCCESS;
|
|
#else
|
|
*chunk = bit_array_get_wordn(bitarr, offset, numBit);
|
|
#endif
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Generate crc of specified data.
|
|
*
|
|
* @param dataBuff pointer to data buffer.
|
|
* dataLen length of specified data.
|
|
* crcType type of crc
|
|
* crc a pointer to 32 bit crc container.
|
|
*
|
|
* @return FAILURE crc type not support.
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
static int32_t L2_CrcGen(uint8_t* dataBuff, uint32_t dataLen, L2_CRC_TYPE crcType, uint32_t* crc){
|
|
// ASSERT(dataBuff != NULL);
|
|
int32_t Status = SUCCESS;
|
|
uint32_t crcVal = 0;
|
|
switch (crcType){
|
|
case L2_CRC16_1021:
|
|
Status = crc16_gen(dataBuff, dataLen, 0x1021, (uint16_t*)&crcVal);
|
|
break;
|
|
case L2_CRC16_8005:
|
|
Status = crc16_gen(dataBuff, dataLen, 0x8005, (uint16_t*)&crcVal);
|
|
break;
|
|
case L2_CRC8_31:
|
|
Status = crc8_gen(dataBuff, dataLen, 0x31, (uint8_t*)&crcVal);
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
if(Status == SUCCESS){
|
|
*crc = crcVal;
|
|
}
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Append a crc of specified data, to a specified position.
|
|
*
|
|
* @param dataBuff pointer to data buffer.
|
|
* dataLen length of specified data.
|
|
* crcPos position of crc insert position
|
|
* crcType type of crc
|
|
*
|
|
* @return FAILURE crc type not support.
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
int32_t L2_CrcAppend(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){
|
|
// ASSERT(dataBuff != NULL);
|
|
int32_t Status = SUCCESS;
|
|
uint32_t crcVal = 0;
|
|
uint8_t* crcPtr = dataBuff+crcPos;
|
|
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
|
|
if(Status != SUCCESS){
|
|
return Status;
|
|
}
|
|
#ifdef CHUNK_FILL_STYLE_XJY
|
|
switch (crcType){
|
|
case L2_CRC16_1021:
|
|
*crcPtr = (uint8_t)(crcVal>>8);
|
|
*(crcPtr+1) = (uint8_t)(crcVal);
|
|
break;
|
|
case L2_CRC16_8005:
|
|
*crcPtr = (uint8_t)(crcVal>>8);
|
|
*(crcPtr+1) = (uint8_t)(crcVal);
|
|
break;
|
|
case L2_CRC8_31:
|
|
*crcPtr = (uint8_t)crcVal;
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
#else
|
|
switch (crcType){
|
|
case L2_CRC16_1021:
|
|
*(crcPtr+1) = (uint8_t)(crcVal>>8);
|
|
*crcPtr = (uint8_t)(crcVal);
|
|
break;
|
|
case L2_CRC16_8005:
|
|
*(crcPtr+1) = (uint8_t)(crcVal>>8);
|
|
*crcPtr = (uint8_t)(crcVal);
|
|
break;
|
|
case L2_CRC8_31:
|
|
*crcPtr = (uint8_t)crcVal;
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
#endif
|
|
return Status;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Check data validation by crc.
|
|
*
|
|
* @param dataBuff pointer to data buffer.
|
|
* dataLen length of specified data.
|
|
* crcPos position of crc insert position
|
|
* crcType type of crc
|
|
*
|
|
* @return FAILURE crc type not support.
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
bool L2_CrcCheck(uint8_t* dataBuff, uint32_t dataLen, uint32_t crcPos, L2_CRC_TYPE crcType){
|
|
ASSERT(dataBuff != NULL);
|
|
|
|
int32_t Status = SUCCESS;
|
|
uint32_t crcVal = 0;
|
|
uint32_t crcComp = 0;
|
|
uint8_t* crcPtr = dataBuff+crcPos;
|
|
Status = L2_CrcGen(dataBuff,dataLen, crcType, &crcVal);
|
|
if(Status != SUCCESS){
|
|
return false;
|
|
}
|
|
#ifdef CHUNK_FILL_STYLE_XJY
|
|
switch (crcType){
|
|
case L2_CRC16_1021:
|
|
crcComp = ((uint32_t)(*crcPtr)) << 8;
|
|
crcComp |= (uint32_t)(*(crcPtr+1));
|
|
break;
|
|
case L2_CRC16_8005:
|
|
crcComp = ((uint32_t)(*crcPtr)) << 8;
|
|
crcComp |= (uint32_t)(*(crcPtr+1));
|
|
break;
|
|
case L2_CRC8_31:
|
|
crcComp = (uint32_t)(*crcPtr);
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
#else
|
|
switch (crcType){
|
|
case L2_CRC16_1021:
|
|
crcComp = ((uint32_t)(*(crcPtr+1))) << 8;
|
|
crcComp |= (uint32_t)(*crcPtr);
|
|
break;
|
|
case L2_CRC16_8005:
|
|
crcComp = ((uint32_t)(*(crcPtr+1))) << 8;
|
|
crcComp |= (uint32_t)(*crcPtr);
|
|
break;
|
|
case L2_CRC8_31:
|
|
crcComp = (uint32_t)(*crcPtr);
|
|
break;
|
|
default:
|
|
Status = EPARAM;
|
|
break;
|
|
}
|
|
#endif
|
|
if(Status == SUCCESS){
|
|
// if ( crcVal == 0 )
|
|
// {
|
|
// return false ;
|
|
// }
|
|
// else
|
|
if(crcComp == crcVal){
|
|
return true;
|
|
}
|
|
else{
|
|
return false;
|
|
}
|
|
}
|
|
else{
|
|
return false;
|
|
}
|
|
|
|
}
|