forked from 0xha0/datalink_repo
Initial version.
This commit is contained in:
Gatsby Van
@@ -0,0 +1,295 @@
|
||||
/*****************************************************************************/
|
||||
/**
|
||||
* 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;
|
||||
}
|
||||
Reference in New Issue
Block a user