296 lines
9.9 KiB
C
296 lines
9.9 KiB
C
/*****************************************************************************/
|
|
/**
|
|
* Description: Implementations for 814 datalink service net.
|
|
* Date: 2021-11-16 00:08
|
|
* LastEditors: Fan Hao
|
|
* LastEditTime: 2022-07-16 17:04
|
|
* Revisions: Rev 0.1
|
|
* History:
|
|
* [2022-07-16]: New branch for DiJian.
|
|
* [2021-11-16]: Initial version.
|
|
*
|
|
* Copyright(c) 2021 Beijing Institute of Technology.
|
|
* Lab of Communication and Networking
|
|
*
|
|
******************************************************************************/
|
|
|
|
/***************************** Include Files *********************************/
|
|
#include <string.h>
|
|
#include "linklayer814.h"
|
|
#include "linklayer814_types.h"
|
|
#include "msg_def.h"
|
|
#include "../error.h"
|
|
#include "../common.h"
|
|
|
|
|
|
#ifdef _MPC_CTRL
|
|
|
|
#endif
|
|
|
|
/************************** Constant Definitions *****************************/
|
|
#define SQUENCE_SVC_SLOTS 0
|
|
#define INER_SVC_SLOTS 2
|
|
#define INRA_SVC_SLOTS 1
|
|
#define NONE_SVC_SLOTS 0
|
|
#define TEST_UDU_PDLEN 16
|
|
#define TEST_UDU_LEN (UDUH_LEN/8 + TEST_UDU_PDLEN + PDU_CRC_LEN/8 )
|
|
/***************** Macros (Inline Functions) Definitions *********************/
|
|
|
|
/************************** Function Prototypes ******************************/
|
|
#ifdef _MPC_CTRL
|
|
#endif
|
|
|
|
/**************************** Type Definitions *******************************/
|
|
|
|
/************************** Variable Definitions *****************************/
|
|
#ifdef _MPC_CTRL
|
|
|
|
#endif
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing UDU Header to a bit array.
|
|
*
|
|
* @param bitarr pointer to bit array buffer.
|
|
* dst Destination node address.
|
|
* dataLen Length of UDU data.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
static int UduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, uint8_t dst, uint32_t dataLen){
|
|
ASSERT(bitarr != NULL);
|
|
if(bitarr->num_of_bits < UDUH_LEN){
|
|
return EPARAM;
|
|
}
|
|
uint64_t dataChunk = 0;
|
|
memset(bitarr->words, 0xF0, (size_t)(UDUH_LEN/8));
|
|
// priority
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, UDUH_PRI_OFFSET, UDUH_PRI_LEN, dataChunk);
|
|
// message type
|
|
dataChunk = 2;
|
|
FrameInsertChunk(bitarr, UDUH_MSG_TYPE_OFFSET, UDUH_MSG_TYPE_LEN, dataChunk);
|
|
// data length
|
|
dataChunk = dataLen;
|
|
FrameInsertChunk(bitarr, UDUH_LENGTH_OFFSET, UDUH_LENGTH_LEN, dataChunk);
|
|
// source node id
|
|
dataChunk = (instPtr->myInfo.nodeId & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, UDUH_SRC_ADDR_OFFSET, UDUH_SRC_ADDR_LEN, dataChunk);
|
|
// destination node id
|
|
dataChunk = (dst & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, UDUH_DST_ADDR_OFFSET, UDUH_DST_ADDR_LEN, dataChunk);
|
|
// UDU Sequence number
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, UDUH_UDU_SEQ_NUM_OFFSET, UDUH_UDU_SEQ_NUM_LEN, dataChunk);
|
|
// retransmission times
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, UDUH_RETRANS_NUM_OFFSET, UDUH_RETRANS_NUM_LEN, dataChunk);
|
|
// number of slice
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, UDUH_NUM_SLICE_OFFSET, UDUH_NUM_SLICE_LEN, dataChunk);
|
|
// slice id
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, UDUH_SLICE_ID_OFFSET, UDUH_SLICE_ID_LEN, dataChunk);
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Packing PDU Header to a bit array.
|
|
*
|
|
* @param bitarr pointer to bit array buffer.
|
|
* headerType PDU header type.
|
|
* dst Destination node address.
|
|
*
|
|
* @return EPARAM
|
|
* SUCCESS
|
|
*
|
|
******************************************************************************/
|
|
static int PduHeader_Packing(L2_INST* instPtr, BIT_ARRAY* bitarr, L2_PDU_HEADER_TYPE headerType, uint8_t dst){
|
|
ASSERT(bitarr != NULL);
|
|
if(headerType < L2_PDU_HEADER_0 || headerType > L2_PDU_HEADER_1){
|
|
return EPARAM;
|
|
}
|
|
uint64_t dataChunk = 0;
|
|
if(headerType == L2_PDU_HEADER_0){
|
|
if(bitarr->num_of_bits < PDUH0_LEN){
|
|
return EPARAM;
|
|
}
|
|
memset(bitarr->words, 0xF0, (size_t)(PDUH0_LEN/8));
|
|
// PDU type
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, PDUH0_TYPE_OFFSET, PDUH0_TYPE_LEN, dataChunk);
|
|
// frame length
|
|
dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
|
|
FrameInsertChunk(bitarr, PDUH0_LENGTH_OFFSET, PDUH0_LENGTH_LEN, dataChunk);
|
|
// source node id
|
|
dataChunk = (instPtr->myInfo.nodeId & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, PDUH0_SRC_ADDR_OFFSET, PDUH0_SRC_ADDR_LEN, dataChunk);
|
|
// destination node id
|
|
dataChunk = (dst & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, PDUH0_DST_ADDR_OFFSET, PDUH0_DST_ADDR_LEN, dataChunk);
|
|
// Sequence number
|
|
dataChunk = 0;
|
|
FrameInsertChunk(bitarr, PDUH0_SEQ_NUM_OFFSET, PDUH0_SEQ_NUM_LEN, dataChunk);
|
|
// Append CRC
|
|
L2_CrcAppend((uint8_t*)bitarr->words, PDUH0_HEADER_CRC_OFFSET/8, PDUH0_HEADER_CRC_OFFSET/8, L2_CRC8_31);
|
|
}
|
|
else if(headerType == L2_PDU_HEADER_1){
|
|
if(bitarr->num_of_bits < PDUH1_LEN){
|
|
return EPARAM;
|
|
}
|
|
memset(bitarr->words, 0xF0, (size_t)(PDUH1_LEN/8));
|
|
// PDU type
|
|
dataChunk = 1;
|
|
FrameInsertChunk(bitarr, PDUH1_TYPE_OFFSET, PDUH1_TYPE_LEN, dataChunk);
|
|
// frame length
|
|
dataChunk = INTRA_SVC_FRAME_LENGTH_MAX;
|
|
FrameInsertChunk(bitarr, PDUH1_LENGTH_OFFSET, PDUH1_LENGTH_LEN, dataChunk);
|
|
// source node id
|
|
dataChunk = (instPtr->myInfo.nodeId & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, PDUH1_SRC_ADDR_OFFSET, PDUH1_SRC_ADDR_LEN, dataChunk);
|
|
// destination node id
|
|
dataChunk = (dst & 0x1F)|((instPtr->myInfo.clustId & 0x1F) << 5);
|
|
FrameInsertChunk(bitarr, PDUH1_DST_ADDR_OFFSET, PDUH1_DST_ADDR_LEN, dataChunk);
|
|
// Slice tage...
|
|
// Append CRC
|
|
L2_CrcAppend((uint8_t*)bitarr->words, PDUH1_HEADER_CRC_OFFSET/8, PDUH1_HEADER_CRC_OFFSET/8, L2_CRC8_31);
|
|
}
|
|
else{
|
|
return FAILURE;
|
|
}
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Initialize service net FSM
|
|
*
|
|
* @param serviceFSM A pointer to service FSM instance.
|
|
*
|
|
* @return SUCCESS Initialize successfully.
|
|
* Other Initialize failed.
|
|
*
|
|
******************************************************************************/
|
|
int32_t ServiceNet_Init(L2_SVC_FSM** serviceFSM) {
|
|
(*serviceFSM) = (L2_SVC_FSM*) malloc(sizeof(L2_SVC_FSM));
|
|
if (*serviceFSM == NULL) {
|
|
return EALLOC;
|
|
}
|
|
ServiceNet_Reset(*serviceFSM);
|
|
return SUCCESS;
|
|
}
|
|
|
|
|
|
void ServiceNet_Reset(L2_SVC_FSM* serviceFSM) {
|
|
serviceFSM->isBisar = false;
|
|
serviceFSM->isBisarEven = false;
|
|
serviceFSM->bisarDst = NONE_ID;
|
|
}
|
|
|
|
void ServiceNet_Destroy(L2_SVC_FSM* serviceFSM){
|
|
free(serviceFSM);
|
|
}
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Handle service received packet. Send data up to MPC
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
*
|
|
* @return SUCCESS message has been successfully handled.
|
|
* other error occurred.
|
|
*
|
|
******************************************************************************/
|
|
int32_t ServiceNet_PktHandler(L2_INST* instPtr) {
|
|
// TODO:
|
|
return SUCCESS;
|
|
}
|
|
|
|
/*****************************************************************************/
|
|
/**
|
|
* @brief Service net FSM procedure.
|
|
*
|
|
* @param instPtr a pointer to linklayer layer instance
|
|
* @param rtneEntry
|
|
*
|
|
* @return Slot has been used.
|
|
*
|
|
******************************************************************************/
|
|
int32_t ServiceNet_Routine(L2_INST* instPtr, const L2_RTNE_TABLE_ENTRY* rtneEntry) {
|
|
ASSERT(instPtr != NULL);
|
|
int32_t Status;
|
|
L2_TOR_TYPE torState;
|
|
DBG_INFO("-------- ServiceNet Routine --------\n");
|
|
|
|
if(rtneEntry->slotSeq != 0){
|
|
return SQUENCE_SVC_SLOTS;
|
|
}
|
|
|
|
// Determine TOR State.
|
|
if(rtneEntry->src0 == instPtr->myInfo.nodeId){
|
|
torState = L2_TOR_TX;
|
|
}
|
|
else if(rtneEntry->dst0 == instPtr->myInfo.nodeId){
|
|
torState = L2_TOR_RX;
|
|
}
|
|
else{
|
|
torState = L2_TOR_SUSPEND;
|
|
}
|
|
|
|
BIT_ARRAY* svcMsg = bit_array_create(L2_MSG_MAX);
|
|
if(svcMsg == NULL){
|
|
DBG_ERROR("Failed to allocate memory for svcMsg.\n");
|
|
return NUM_SLOTS_NONE;
|
|
}
|
|
memset(instPtr->sendPktData, 0xCC, (size_t)(INTRA_SVC_FRAME_LENGTH_MAX));
|
|
switch(rtneEntry->slotType){
|
|
case SLOT_INTER_SVC:
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTER_SVC_FRAME);
|
|
break;
|
|
case SLOT_INTRA_SVC:
|
|
if(torState == L2_TOR_TX){
|
|
//TODO:
|
|
memset(instPtr->sendPktData, 0xAA, INTRA_SVC_FRAME_LENGTH_MAX);
|
|
// Pack PDU header
|
|
if(PduHeader_Packing(instPtr, svcMsg, L2_PDU_HEADER_0, rtneEntry->dst0) != SUCCESS){
|
|
DBG_ERROR("Failed to packing PduHeader.\n");
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
memcpy(instPtr->sendPktData, svcMsg->words, (size_t)(PDUH0_LEN/8));
|
|
// Pack UDU Header
|
|
if(UduHeader_Packing(instPtr, svcMsg, rtneEntry->dst0,
|
|
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8)
|
|
!= SUCCESS){
|
|
DBG_ERROR("Failed to packing UduHeader.\n");
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
memcpy(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8), svcMsg->words, (size_t)(UDUH_LEN/8));
|
|
// For ZhiQiang
|
|
// Payload Byte3, bit0~2: message class, bit3~7 message subclass
|
|
// FIXME: message class tag 0x01, only for test.
|
|
memset(instPtr->sendPktData+((PDUH0_LEN+MSG_KEY_LENGTH_LEN+UDUH_LEN)/8) + 3, 0x01, 1);
|
|
L2_CrcAppend(instPtr->sendPktData + ((PDUH0_LEN+MSG_KEY_LENGTH_LEN)/8),
|
|
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
|
|
INTRA_SVC_FRAME_LENGTH_MAX-(PDUH0_LEN+MSG_KEY_LENGTH_LEN+PDU_CRC_LEN)/8,
|
|
L2_CRC16_8005
|
|
);
|
|
}
|
|
L2_SetupNextSlot(instPtr, torState, L2_INTRA_SVC_FRAME);
|
|
break;
|
|
default:
|
|
DBG_ERROR("UNKNOWN slot type [%d].\n", rtneEntry->slotType);
|
|
DBG_ERROR("Failed to packing PduHeader.\n");
|
|
Status = FAILURE;
|
|
goto routine_err;
|
|
}
|
|
bit_array_free(svcMsg);// free memory
|
|
return SUCCESS;
|
|
routine_err:
|
|
bit_array_free(svcMsg);// free memory
|
|
return Status;
|
|
}
|