diff --git a/fsw/public_inc/aos_sdlp.h b/fsw/public_inc/aos_sdlp.h new file mode 100644 index 0000000..95c2021 --- /dev/null +++ b/fsw/public_inc/aos_sdlp.h @@ -0,0 +1,434 @@ +/******************************************************************************/ +/** \file aos_sdlp.h +* +* Copyright 2017 United States Government as represented by the Administrator +* of the National Aeronautics and Space Administration. No copyright is +* claimed in the United States under Title 17, U.S. Code. +* All Other Rights Reserved. +* +* \author Alan A Asp, Guy de Carufel (Odyssey Space Research), NASA, JSC, ER6 +* +* \brief Header file for aos_SDLP Protocol +* +* \par Limitations, Assumptions, External Events, and Notes: +* - This library provides a service interface to the aosTF protocol. +* - This implementation is based on Chapter 4 - without SDLS option. +* - The VCA service is user defined +* - The maximum frame length is user defined, and is dependent on the +* channel coding startegy used. +* - User may use the IO_LIB_UTIL_GenPseudoRandomSeq to generate an idle +* data sequence. +* +* \par Modification History: +* - 2015-04-26 | Alan A. Asp | OSR | Code Started (originally in aostf.h) +* - 2015-10-22 | Guy de Carufel | OSR | Migrated from aostf.h. +* Major revision: structs, idle data, overflow, API. +*******************************************************************************/ + +#ifndef _AOS_SDLP_H_ +#define _AOS_SDLP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* +** Includes +*******************************************************************************/ +#include "io_lib.h" +#include "aostf.h" +#include "crypto.h" + + +/******************************************************************************* +** Macro Definitions +*******************************************************************************/ +#define AOS_SDLP_SUCCESS (0) +#define AOS_SDLP_ERROR (-1) +#define AOS_SDLP_INVALID_POINTER (-2) +#define AOS_SDLP_INVALID_LENGTH (-3) +#define AOS_SDLP_FRAME_NOT_INIT (-4) +#define AOS_SDLP_FRAME_NOT_READY (-5) +#define AOS_SDLP_OVERFLOW_FULL (-6) + +/******************************************************************************* +** Structure definitions +*******************************************************************************/ +/** Following Structure is the user defined managed / configuration parameters + * for all Transfer Frames over the physical channel. */ +typedef struct +{ + uint16 scId; /* Spacecraft ID (8 bits) */ + uint16 frameLength; /* The length of the frame */ + uint8 hasFecf; /* Has the Frame Error Control Field */ + uint8 hasFhec; /* Has the Frame Header Error Control */ + uint8 hasInsertZone; /* Has the Insert Zone */ + uint16 insertZoneLength; /* Length of Insert Zone if present */ +} AOS_SDLP_GlobalConfig_t; + + +/** Following Structure is the user defined managed / configuration parameters + for a a specific Virtual Channel */ +typedef struct +{ + uint8 vcId; /* Virtual channel ID (6 bits, 0-62) */ + uint8 dataType; /* Type-0: M_PDU(packet), Type-1: B_PDU, Type-2: VCA_SDU */ + uint8 ocfFlag; /* The value of the OCF flag (0/1) */ + uint8 replayFlag; /* Replay flag setting */ + uint8 vcFrameCountUsage; /* VC Frame Count Usage Flag */ + uint8 vcFrameCountCycle; /* VC Frame Count Cycle (4 bits) */ + uint16 overflowSize; /* Size of overflow buffer */ +} AOS_SDLP_ChannelConfig_t; + + +/** Working parameters for overflow buffer */ +typedef struct +{ + uint16 buffSize; /* Overflow buffer size */ + uint16 freeOctets; /* number of free octets remaining in buffer */ + uint16 partialOctets; /* Partial octets at the start of the + overflow queue */ + uint8 *dataStart; /* Pointer to start of data (queue start) */ + uint8 *dataEnd; /* Pointer to end of data (queue end) */ + uint8 *buffer; /* Pointer to overflow buffer */ +} AOS_SDLP_OverflowInfo_t; + + +/** Working paramters of frame */ +typedef struct +{ + uint16 dataFieldLength; /* Length of the data field */ + uint16 dataFieldOffset; /* offset in octets from the start of frame + to the data field */ + uint16 insertZoneOffset; /* Offset in octets from the start of frame + to the Insert Zone */ + uint16 ocfOffset; /* Offset in octets from the start of frame + to the OCF */ + uint16 fecfOffset; /* Offset in octets from the start of frame + to the Frame Error Control Field */ + uint16 freeOctets; /* Number of free octets remaining in the + data field */ + uint16 currentDataOffset; /* Offset to next free data field octet from + start of frame */ + uint32 mutexId; /* The mutex ID to protect the TF buffer + and overflow buffer */ + uint32 vcFrameCount; /* 24-bit VC Frame Counter */ + bool isReady; /* Indicates the TF is ready to add data */ + bool isInitialized; /* Indicates the TF is initialized */ + AOS_SDLP_OverflowInfo_t overflowInfo; /* Overflow Info Structure */ + AOS_SDLP_GlobalConfig_t *globConfig; /* Pointer to global config */ + AOS_SDLP_ChannelConfig_t *chnlConfig; /* Pointer to channel config */ + AOSTF_PriHdr_t *frame; /* Pointer to Transfer frame */ +} AOS_SDLP_FrameInfo_t; + + + +/******************************************************************************* +** Function Declarations +*******************************************************************************/ +/******************************************************************************/ +/** \brief Initialize the Idle Data +* +* \par Description/Algorithm +* Initializes an Idle Data Buffer with a repeating pattern sequence. +* +* \par Assumptions, External Events, and Notes: +* - The Idle Data is used for filling unused portions of AOS frames +* - User may use the IO_LIB_UTIL_GenPseudoRandomSeq function to generate +* idle data pattern. Pattern should be "sufficiently" random. +* - The Idle data buffer length must be at least as large as the +* frameLength. +* - The IdleData is used in both AddIdleData and SetIdleFrame +* +* \param[in,out] pIdleData Pointer to the Idle Data Buffer. +* \param[in] pIdlePattern A bit pattern to repeat in idle data +* \param[in] bufferLength Length of the Idle Buffer in bytes. +* \param[in] patternBitLength Length of the repeating pattern in bits. +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_INVALID_LENGTH If an input length is invalid +* +* \see +* #AOS_SDLP_AddData +* #AOS_SDLP_AddIdlePacket +* #AOS_SDLP_SetIdleFrame +* #IO_LIB_UTIL_GenPseudoRandomSeq +*******************************************************************************/ +int32 AOS_SDLP_InitIdlePacket(CFE_MSG_Message_t *pIdleData, uint8 *pIdlePattern, + uint16 bufferLength, uint32 patternBitLength); + +/******************************************************************************/ +/** \brief Initialize a specific Virtual Channel +* +* \par Description/Algorithm +* This function will populate the Channel and overflow info structures +* based on provided configuration data and buffer pointers. +* +* \par Assumptions, External Events, and Notes: +* - The idle data sequence must be >= frame data field length. +* - Insert Zone is optional and configured via global config +* +* \param[out] pFrameInfo Pointer to the Frame info/working struct. +* \param[out] pTfBuffer Pointer to the Transfer Frame buffer +* \param[out] pOfBuffer Pointer to the Overflow buffer +* \param[in] pGlobalConfig Pointer to the Global configuration struct. +* \param[in] pChannelConfig Pointer to the Channel configuration struct. +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_INVALID_LENGTH If frame length is too short +* +* \see +* #AOSTF_SetReplayFlag +* #AOSTF_SetVcFrameCountUsageFlag +* #AOSTF_SetVcFrameCountCycle +*******************************************************************************/ +int32 AOS_SDLP_InitChannel(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pTfBuffer, uint8 *pOfBuffer, + AOS_SDLP_GlobalConfig_t *pGlobalConfig, + AOS_SDLP_ChannelConfig_t *pChannelConfig); + + +/******************************************************************************/ +/** \brief Check if frame currently has data +* +* \par Description/Algorithm +* This function will return whether the frame has data or not +* +* \par Assumptions, External Events, and Notes: +* - Function does not check if frame has been initalized or started. +* +* \param[in] pFrameInfo Pointer to the Frame info/working struct. +* +* \return Frame has Data (0=no, 1=yes) +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* +* \see +* #AOS_SDLP_AddData +*******************************************************************************/ +int32 AOS_SDLP_FrameHasData(AOS_SDLP_FrameInfo_t *pFrameInfo); + + +/******************************************************************************/ +/** \brief Add a Packet to Transfer Frame (M_PDU) +* +* \par Description/Algorithm +* This function will add a CFE packet to a provided transfer frame as +* a Multiplexing Protocol Data Unit (M_PDU). +* +* \par Assumptions, External Events, and Notes: +* - A CFE packet becomes an M_PDU in AOS terminology +* - This function calls AOS_SDLP_AddData +* - Multiversion multiplexing is not-implemented +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pPacket Pointer to the CFE Packet (M_PDU) +* +* \return Frame FreeOctets +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_FRAME_NOT_INIT If frame has not been initialized +* \return AOS_SDLP_FRAME_NOT_READY If frame has not been started +* \return AOS_SDLP_OVERFLOW_FULL Data dropped. The overflow buffer is full +* +* \see +* #AOS_SDLP_AddData +*******************************************************************************/ +int32 AOS_SDLP_AddPacket(AOS_SDLP_FrameInfo_t *pFrameInfo, + CFE_MSG_Message_t *pPacket); + + +/******************************************************************************/ +/** \brief Add Bitstream Data to Transfer Frame (B_PDU) +* +* \par Description/Algorithm +* This function will add bitstream data to a provided transfer frame as +* a Bitstream Protocol Data Unit (B_PDU). +* +* \par Assumptions, External Events, and Notes: +* - B_PDU is used for bitstream data that doesn't follow packet structure +* - This function calls AOS_SDLP_AddData +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pData Pointer to bitstream data +* \param[in] dataLength Length of bitstream data +* +* \return Frame FreeOctets +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_FRAME_NOT_INIT If frame has not been initialized +* \return AOS_SDLP_FRAME_NOT_READY If frame has not been started +* \return AOS_SDLP_OVERFLOW_FULL Data dropped. The overflow buffer is full +* +* \see +* #AOS_SDLP_AddData +*******************************************************************************/ +int32 AOS_SDLP_AddBitstreamData(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pData, uint16 dataLength); + + +/******************************************************************************/ +/** \brief Add Idle Data to transfer frame +* +* \par Description/Algorithm +* Copies idle data to fill all free octets in TF data field. The data +* is based on the supplied idle pattern. +* +* \par Assumptions, External Events, and Notes: +* - The Idle data may be segmented if the TF data field does not have +* enough space left by saving the extra octets to the overflow buffer. +* - This function calls AOS_SDLP_AddData +* - User may use InitIdleData to initialize the Idle Data with a +* user specified repeating pattern. +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pIdleData Pointer to the idle data buffer +* \param[in] idleLength Length of available idle data +* +* \return AOS_SDLP_SUCCESS If successful (no free octets). +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_FRAME_NOT_INIT If frame has not been initialized +* \return AOS_SDLP_FRAME_NOT_READY If frame has not been started +* \return AOS_SDLP_OVERFLOW_FULL Data dropped. The overflow buffer is full +* +* \see +* #AOS_SDLP_AddData +* #AOS_SDLP_GenPseudoRandomSeq +*******************************************************************************/ +int32 AOS_SDLP_AddIdlePacket(AOS_SDLP_FrameInfo_t *pFrameInfo, + CFE_MSG_Message_t *pIdleData); + + +/******************************************************************************/ +/** \brief Add a Virtual Channel Access (VCA) PDU to the Transfer Frame +* +* \par Description/Algorithm +* Copies a VCA data buffer to the TF data field at the next free octet. +* +* \par Assumptions, External Events, and Notes: +* - VCA_SDU is used for virtual channel access service data +* - The dataLength should be the same for all PDUs added to a specific TF +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pData Pointer to data buffer +* \param[in] dataLength Length of data to copy +* +* \return Frame FreeOctets +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_FRAME_NOT_INIT If frame has not been initialized +* \return AOS_SDLP_FRAME_NOT_READY If frame has not been started +* \return AOS_SDLP_OVERFLOW_FULL Data dropped. The overflow buffer is full +* +* \see +* #AOS_SDLP_AddData +*******************************************************************************/ +int32 AOS_SDLP_AddVcaData(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pData, + uint16 dataLength); + + +/******************************************************************************/ +/** \brief Start a transfer frame +* +* \par Description/Algorithm +* Start a new transfer frame by copying any data from the overflow buffer +* into the empty transfer frame buffer. This readies the frame to accept +* new data. +* +* \par Assumptions, External Events, and Notes: +* - The transfer frame has no data in it's data field prior to call. +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_ERROR Data field is not empty +* +* \see +* #AOS_SDLP_AddIdlePacket +*******************************************************************************/ +int32 AOS_SDLP_StartFrame(AOS_SDLP_FrameInfo_t *pFrameInfo); + + +/******************************************************************************/ +/** \brief Set a Frame with Only Idle Data +* +* \par Description/Algorithm +* Copies the Idle Pattern into the frame data field for idle frames. +* +* \par Assumptions, External Events, and Notes: +* - The user is responsible for providing an idle buffer with +* sufficient randomness. +* - It is recommended that VCID 63 be used for idle frames. +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pIdleData Pointer to the idle data buffer. +* \param[in] idleLength Length of idle data available +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* +* \see +*******************************************************************************/ +int32 AOS_SDLP_SetIdleFrame(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pIdleData, uint16 idleLength); + + +/******************************************************************************/ +/** \brief Set Insert Zone Data +* +* \par Description/Algorithm +* Copies data to the Insert Zone field of the AOS frame if present. +* +* \par Assumptions, External Events, and Notes: +* - Insert Zone is optional in AOS frames +* - Insert Zone length is configured during initialization +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pInsertData Pointer to insert zone data +* \param[in] dataLength Length of insert zone data +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* \return AOS_SDLP_INVALID_LENGTH If data length exceeds insert zone size +* +* \see +*******************************************************************************/ +int32 AOS_SDLP_SetInsertZone(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pInsertData, uint16 dataLength); + + + +/******************************************************************************/ +/** \brief Complete a Frame to ready for transmission +* +* \par Description/Algorithm +* Fills out the final TF information including the frame counters, adds the +* OCF if available, updates the signaling field, and executes the frame +* error control field computation if included. +* +* \par Assumptions, External Events, and Notes: +* - This function represents the Virtual Channel Frame (VCF) Service +* - User is responsible for filling frame with idle data if it is +* incomplete prior to call. Call AddIdleData or SetIdleFrame. +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pOcf Pointer to Operational Control Field +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_POINTER If a input pointer is NULL +* +* \see +* #AOS_SDLP_AddIdlePacket +* #AOS_SDLP_SetIdleFrame +*******************************************************************************/ +int32 AOS_SDLP_CompleteFrame(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pOcf); + + +#ifdef __cplusplus +} +#endif + +#endif /* _AOS_SDLP_H_ */ + +/*============================================================================== +** End of file aos_sdlp.h +**============================================================================*/ \ No newline at end of file diff --git a/fsw/public_inc/aos_sync.h b/fsw/public_inc/aos_sync.h new file mode 100644 index 0000000..3bcc313 --- /dev/null +++ b/fsw/public_inc/aos_sync.h @@ -0,0 +1,138 @@ +/******************************************************************************/ +/** \file tm_sync.h +* +* Copyright 2017 United States Government as represented by the Administrator +* of the National Aeronautics and Space Administration. No copyright is +* claimed in the United States under Title 17, U.S. Code. +* All Other Rights Reserved. +* +* \author Guy de Carufel (Odyssey Space Research), NASA, JSC, ER6 +* +* \brief Provides the AOS Channel Synchronization service. +* +* \par References: +* - "AOS Synchronization and Channel Coding", CCSDS 131.0-B-2, +* Issue 2, August 2011 +* +* \par Limitations, Assumptions, External Events, and Notes: +* - Only sending end services are provided +* - This service provides the synchronization and pseudo-randomization +* services for frames, such as Transfer Frames or AOS TF. +* - The fixed synchronization header is the Attached Sync Marker (ASM) +* - Coding of the transfer frame (eg. Read-solomon coding) is not covered in +* this librarly. +* - Emplementing certain coding algorithms (eg. Turbo, LDPC) may require a +* licence +* - Define a code specific ASM header in the coding library. +* - the pseudo-randomization uses a Fibonacci LFSR with seed of 0xff and +* polynomial of x^8 + x^7 + x^5 + x^3 + 1. Period of 255 bits. +* +* \par Modification History: +* - 2015-10-29 | Guy de Carufel | OSR | Code Started +*******************************************************************************/ + +#ifndef _IO_AOS_SYNC_ +#define _IO_AOS_SYNC_ + +#ifdef __cplusplus +extern "C" { +#endif + +/******************************************************************************* +** Includes +*******************************************************************************/ +#include "io_lib.h" + + +/******************************************************************************* +** Macro Definitions +*******************************************************************************/ +/* The following AMS is used for uncoded data, convolutional, Reed-Solomon, + * concatenated and rate-7/8 LDPC coded data. */ +#define AOS_SYNC_ASM_STR "1ACFFC1D" +#define AOS_SYNC_ASM_SIZE 4 + +#define AOS_SYNC_SUCCESS 0 +#define AOS_SYNC_INVALID_POINTER -1 +#define AOS_SYNC_INVALID_ASM_SIZE -2 +#define AOS_SYNC_RANDOMIZE_ERROR -3 + + +/******************************************************************************* +** Function Declarations +*******************************************************************************/ +/******************************************************************************/ +/** \brief Initialize the static Pseudo-Random sequence +* +* \par Assumptions, External Events, and Notes: +* - Generates a pseudo-random sequence with poly:0xa9 and seed:0xff + +* \return AOS_SYNC_SUCCESS Always successful. +* +* \see +* #IO_LibInit +* #IO_LIB_UTIL_GenPseudoRandomSeq +*******************************************************************************/ +int32 AOS_SYNC_LibInit(void); + + +/******************************************************************************/ +/** \brief Perform AOS Synchronization +* +* \par Description/Algorithm +* Build the Channel Access Data UNIT (CADU) by appending the AMS to the +* frame. +* +* \par Assumptions, External Events, and Notes: +* - The frame being passed in may have already been coded (eg. Reed-solomon) +* Or the coding may be performed after sync. (eg. convolutional coding) +* - In the case of uncoded frame, pass the AOS_SYNC_ASM_UNCODED as the asmHdr +* and AOS_SYNC_ASM_UNCODED_SIZE as the asmSize parameters. +* - The frame must starts at pBuff + asmSize. +* - The size of the buffer must be asmSize + frameSize +* +* \param[out] pBuff Pointer to the buffer where CADU is stored. +* \param[in] asmStr The string representing the ASM to append +* \param[in] asmSize The size in octets of the final ASM (ASM Str size/2) +* \param[in] frameSize The size of the frame to synchronize +* \param[in] randomize Whether the frame should be pseudo-randomized +* +* \return Size of CADU Size of the CADU (asmSize + frameSize) +* \return AOS_SYNC_INVALID_ASM_LENGTH If the ASM length is not an even number +* or is < 4, as all ASM header are at least +* 4 bytes. +* \return AOS_SYNC_INVALID_POINTER If any input pointer is NULL +* +* \see +* #AOS_SYNC_PseudoRandomize +*******************************************************************************/ +int32 AOS_SYNC_Synchronize(uint8 *pBuff, char *asmStr, uint8 asmSize, + uint16 frameSize, bool randomize); + + + +/******************************************************************************/ +/** \brief Perform AOS Frame PseudoRandomization +* +* \par Description/Algorithm +* Perform the standard CCSDS AOS_Sync channel pseudo randomization. +* +* \par Assumptions, External Events, and Notes: +* - The presence or absence of pseudo-randomization is fixed for a +* physical channel and is a managed property, known by receiver. +* +* \param[in,out] pFrame The input frame to randomize +* \param[in] frameSize The size of the frame in bytes +* +* \return AOS_SYNC_SUCCESS Always successful. +* +* \see +* #AOS_SYNC_Synchronize +*******************************************************************************/ +int32 AOS_SYNC_PseudoRandomize(uint8 *pFrame, uint16 frameSize); + +#endif /* _AOS_SYNC_H_ */ + +/*============================================================================== +** End of file tm_sync.h +**============================================================================*/ diff --git a/fsw/public_inc/aostf.h b/fsw/public_inc/aostf.h new file mode 100644 index 0000000..d8aa39b --- /dev/null +++ b/fsw/public_inc/aostf.h @@ -0,0 +1,482 @@ +/******************************************************************************* + * File: AOStf.h + * + * Copyright 2017 United States Government as represented by the Administrator + * of the National Aeronautics and Space Administration. No copyright is + * claimed in the United States under Title 17, U.S. Code. + * All Other Rights Reserved. + * + * Purpose: + * Provide the Transfer Frame format API for the AOS_SDLP Service. + * + * Reference(s): + * - _AOS Space Data Link Protocol_, CCSDS 132.0-B-1_ (Issue 1, Sept. 2003) + * - _Space Packet Protocol_, CCSDS 133.0-B-1_ (Issue 1, Sept. 2003) + * - _A Painless Guide To CRC Error Detection Algorithm_ (Version 3, 1993), + * Ross N. Williams. http://www.ross.net/crc/download/crc_v3.txt + * + * Notes: + * -The AOS Transfer Frame is the protocol data unit (PDU) of the Telemetry + * Space Data Link Protocol (AOS-SDLP). + * + * History: + * 04/26/2015, A. Asp, Odyssey Space Research, LLC + * -Created + * 10/22/2015, G. de Carufel, Odyssey Space Research, LLC + * -Moved all services to AOS_sdlp.h + * + ******************************************************************************/ + +#ifndef _AOS_TRANSFER_FRAME_H_ +#define _AOS_TRANSFER_FRAME_H_ + +#include "common_types.h" + +/*------------------------------------------------------------------------------ + * Items below should not require user updates + */ + +/* Return codes */ +#define AOSTF_SUCCESS (0) +#define AOSTF_ERROR (-1) +#define AOSTF_INVALID_POINTER (-2) +#define AOSTF_INVALID_SECHDR (-3) +#define AOSTF_INVALID_LENGTH (-4) + +/* Fixed parameters to compute Frame Error Control Field (FECF) */ +#define AOSTF_FECF_INIT_REGISTRY 0xffffU +#define AOSTF_FECF_POLYNOMIAL 0x11021UL + +/* Max number of virtual channels */ +#define AOSTF_MAX_VC 63 + +/* Fixed values */ +#define AOSTF_VERSION 1 /* AOS Version 2 = binary '01' */ + +#define AOSTF_PRIHDR_LENGTH 6 /* 6 octets (without optional FHEC) */ +#define AOSTF_PRIHDR_LENGTH_WITH_FHEC 8 /* 8 octets (with optional FHEC) */ +#define AOSTF_OCF_LENGTH 4 /* Operational Control Field */ +#define AOSTF_FHEC_LENGTH 2 /* Frame Header Error Control */ +#define AOSTF_FECF_LENGTH 2 /* Frame Error Control Field */ +#define AOSTF_INSERT_ZONE_MAX_LENGTH 1019 /* Maximum Insert Zone length */ + +/* AOS-specific constants */ +#define AOSTF_IDLE_VCID 63 /* Reserved VCID for idle frames */ +#define AOSTF_REPLAY_FLAG_NO_REPLAY 0 /* No replay flag value */ +#define AOSTF_REPLAY_FLAG_REPLAY 1 /* Replay flag value */ + +typedef struct +{ + uint8 Id[2]; /* MCID (10 bits) + VCID (6 bits) */ + uint8 VcFrameCount[3]; /* Virtual Channel Frame Count (24 bits) */ + uint8 SignalingField; /* Replay Flag + VC Frame Count Usage Flag + Reserved + VC Frame Count Cycle */ + uint8 FrameHeaderErrorControl[2]; /* Optional Frame Header Error Control (16 bits) */ +} AOSTF_PriHdr_t; + +/* + * Function: AOSTF_LibInit + * + * Purpose: + * Initialize the static AOSTF CRC Table + * + * Arguments: + * + * Return: + * AOSTF_SUCCESS Always returns success. + * + * Notes: + * - Called by IO_LibInit() + */ +int32 AOSTF_LibInit(void); + + + +/* + * Function: AOSTF_SetVersion + * + * Purpose: + * Sets the version number for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the version number + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + * Notes: + * For CCSDS 132.0-B-1, Sept. 2003, the Transfer Frame Version Number + * shall be set to binary '00'. + * + */ +int32 AOSTF_SetVersion(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetScId + * + * Purpose: + * Set spacecraft ID for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the spacecraft ID + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetScId(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetVcId + * + * Purpose: + * Set virtual channel ID for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the virtual channel ID + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetVcId(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetOcfFlag + * + * Purpose: + * Set operational control field flag for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : bool value to set the flag + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetOcfFlag(AOSTF_PriHdr_t *tfPtr, bool val); + + +/* + * Function: AOSTF_GeAOScId + * + * Purpose: + * Get the Master Channel Id (Version Num + SCID) + * + * Arguments: + * tfPtr: pointer to the transfer frame + * + * Return: + * Master Channel ID (VersionNumber + SCID) + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_GetAOSMcId(AOSTF_PriHdr_t *tfPtr); + +/* + * Function: AOSTF_GetGlobalVcId + * + * Purpose: + * Get the global VC Id (MCID + VCID) (not including OCF) + * + * Arguments: + * tfPtr: pointer to the transfer frame + * + * Return: + * Global Virtual Channel ID (MCID + VCID) + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_GetGlobalVcId(AOSTF_PriHdr_t *tfPtr); + + + +/* + * Function: AOSTF_SeAOScFrameCount + * + * Purpose: + * Set master channel frame count for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the master channel frame count + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetAOSFrameCount(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetVcFrameCount + * + * Purpose: + * Set virtual channel frame count for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the virtual channel frame count + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetVcFrameCount(AOSTF_PriHdr_t *tfPtr, uint32 val); + + +/* + * Function: AOSTF_IncrVcFrameCount + * + * Purpose: + * Increment virtual channel frame count for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_IncrVcFrameCount(AOSTF_PriHdr_t *tfPtr); + + +/* + * Function: AOSTF_SetSecHdrFlag + * + * Purpose: + * Set flag indicating the presence or absence of the secondary header for + * the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : bool value to set the flag + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetSecHdrFlag(AOSTF_PriHdr_t *tfPtr, bool val); + + +/* + * Function: AOSTF_SetSyncFlag + * + * Purpose: + * Set the sync flag for the transfer frame + * false = octet-synchronized and forward ordered packets or Idle Data inserted + * true = VCA_SDU inserted + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : bool value to set the flag + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetSyncFlag(AOSTF_PriHdr_t *tfPtr, bool val); + + +/* + * Function: AOSTF_SetPacketOrderFlag + * + * Purpose: + * Set the packet order flag for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : bool value to set the flag + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetPacketOrderFlag(AOSTF_PriHdr_t *tfPtr, bool val); + + +/* + * Function: AOSTF_SetSegLengthId + * + * Purpose: + * Set the segment length ID for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the segment length ID + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + */ +int32 AOSTF_SetSegLengthId(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetFirstHdrPtr + * + * Purpose: + * Set the location of the first header pointer for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the first header pointer + * + * Return: + * AOSTF_SUCCESS if the value is set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * + * Notes: + * -Constants AOSTF_NO_FIRST_HDR_PTR and AOSTF_OID_FIRST_HDR_PTR defined to be used + * for 'val' in situations where there's no first header pointer in the current + * TF and there's only idle data in the packet, respectively. + * + */ +int32 AOSTF_SetFirstHdrPtr(AOSTF_PriHdr_t *tfPtr, uint16 val); + + +/* + * Function: AOSTF_SetSecHdrLength + * + * Purpose: + * Set the size of the secondary header for the transfer frame + * + * Arguments: + * tfPtr: pointer to the transfer frame + * val : value to set the length of the secondary header. This value is one + * octet less than the actual length of the secondary header. + * + * Return: + * AOSTF_SUCCESS if the value was set + * AOSTF_INVALID_POINTER if the input pointer is NULL + * AOSTF_INVALID_SECHDR if the secondary header flag is not set + * AOSTF_INVALID_LENGTH if the length is too short (0 octets) or too long (> 63 octets) + * + */ +int32 AOSTF_SetSecHdrLength(AOSTF_PriHdr_t *tfPtr, uint8 val); + + +/* + * Function: AOSTF_SetSecHdrData + * + * Purpose: + * Set the Data of the secondary header for the transfer frame + * + * Arguments: + * tfPtr : pointer to the transfer frame + * data : pointer to the data to be copied to the secondary header data field + * length: number of octets to copy + * + * Return: + * AOSTF_SUCCESS if the data is copied + * AOSTF_INVALID_POINTER if an input pointer is NULL + * AOSTF_INVALID_SECHDR if the secondary header flag is not set + * AOSTF_INVALID_LENGTH if the length of the data being copied is larger than + * the secondary header data field + * + * Notes: + * + */ +int32 AOSTF_SetSecHdrData(AOSTF_PriHdr_t *tfPtr, uint8 *data, uint8 length); + + +/* + * Function: AOSTF_SetOcf + * + * Purpose: + * Copies the input data to the Operational Control Field (OCF) of the TF + * + * Arguments: + * tfPtr : pointer to the transfer frame + * data : pointer to the data to be copied + * offset: number of octets from the start of the frame to the OCF + * + * Return: + * AOSTF_SUCCESS if the data is copied + * AOSTF_INVALID_POINTER if an input pointer is NULL + * AOSTF_ERROR if the OCF flag is not set for the frame + * + * Notes: + * + */ +int32 AOSTF_SetOcf(AOSTF_PriHdr_t *tfPtr, uint8 *data, uint16 offset); + + + +/* + * Function: AOSTF_UpdateErrCtrlField + * + * Purpose: + * Calculates the value of the error control field and copies it to the TF trailer + * + * Arguments: + * tfPtr : pointer to the transfer frame + * offset: number of octets from the start of the frame to the error control field + * + * Return: + * AOSTF_SUCCESS if the value is successfully calculated and copied + * AOSTF_INVALID_POINTER if an input pointer is NULL + * + * Notes: + * + */ +int32 AOSTF_UpdateErrCtrlField(AOSTF_PriHdr_t *tfPtr, uint16 offset); + +/* + * Function: AOSTF_SetReplayFlag + * + * Notes: + * + */ +int32 AOSTF_SetReplayFlag(AOSTF_PriHdr_t *tfPtr, bool val); + +/* + * Function: AOSTF_SetVcFrameCountUsageFlag + * + * Notes: + * + */ +int32 AOSTF_SetVcFrameCountUsageFlag(AOSTF_PriHdr_t *tfPtr, bool val); + +/* + * Function: AOSTF_SetVcFrameCountCycle + * + * Notes: + * + */ +int32 AOSTF_SetVcFrameCountCycle(AOSTF_PriHdr_t *tfPtr, uint16 val); + +/* + * Function: AOSTF_UpdateFrameHeaderErrorControl + * + * Notes: + * + */ +int32 AOSTF_UpdateFrameHeaderErrorControl(AOSTF_PriHdr_t *tfPtr); + +/* + * Function: AOSTF_UpdateFrameHeaderErrorControl + * + * Notes: + * + */ +int32 AOSTF_UpdateFrameErrorControlField(AOSTF_PriHdr_t *tfPtr, uint16 offset); + +#endif diff --git a/fsw/public_inc/io_lib_events.h b/fsw/public_inc/io_lib_events.h index 5e0f5e9..080c5ed 100644 --- a/fsw/public_inc/io_lib_events.h +++ b/fsw/public_inc/io_lib_events.h @@ -43,6 +43,7 @@ typedef enum IO_LIB_TRANS_SELECT_EID = 9, IO_LIB_COP1_EID = 10, IO_LIB_TM_SDLP_EID = 11, + IO_LIB_AOS_SDLP_EID = 12, IO_LIB_EVT_CNT } IO_LIB_Events_t; diff --git a/fsw/src/formats/aostf.c b/fsw/src/formats/aostf.c new file mode 100644 index 0000000..aa64695 --- /dev/null +++ b/fsw/src/formats/aostf.c @@ -0,0 +1,509 @@ +/******************************************************************************* +** File: AOStf.c +** +** Copyright 2017 United States Government as represented by the Administrator +** of the National Aeronautics and Space Administration. No copyright is +** claimed in the United States under Title 17, U.S. Code. +** All Other Rights Reserved. +** +** Purpose: +** Provides functionality to handling telemetry space data link protocol +** (AOS SDLP) transfer frames (TF). +** +** History: +** 04/26/15, A. Asp, Odyssey Space Research, LLC +** * Created + * 10/22/2015, G. de Carufel, Odyssey Space Research, LLC + * -AddIdlePacket and CRC computation. Revised overflow buffer as queue. +** +*******************************************************************************/ + +#include "cfe.h" + +#include "../../public_inc/aostf.h" + +static uint16 fecfTable[256]; /* CRC Table generated through GenFecfTable */ + + +/*------------------------------------------------------------------------------ + * + * Macros for reading and writing the fields in a Telemetry Space Data Link + * Protocol Transfer Frame. All of the macros are used in a similar way: + * + * AOSTF_RD_xxx(hdr) -- Read field xxx from TF header. + * AOSTF_WR_xxx(hdr,value) -- Write value to field xxx of TF header. + * + * Note that hdr is a reference to the actual TF structure, + * not to a pointer to the structure. If using a pointer, one must + * refer to the structure as *pointer. + * + * The AOSTF_WR macros may refer to the 'hdr' more than once; thus + * the expression for 'hdr' must NOT contain any side effects. + * + *----------------------------------------------------------------------------*/ +#define AOSTF_WR_TF_VERSION(hdr,val) ((hdr).Id[0] = ((hdr).Id[0] & 0x3F) | \ + ((val) << 6)) + +#define AOSTF_WR_SCID(hdr,val) ((hdr).Id[0] = (((hdr).Id[0] & 0xC0) | \ + (((val) >> 2) & 0x3F))), \ + ((hdr).Id[1] = (((hdr).Id[1] & 0x3F) | \ + (((val) & 0x03) << 6))) + +#define AOSTF_WR_VCID(hdr,val) ((hdr).Id[1] = (((hdr).Id[1] & 0xC0) | \ + ((val) & 0x3F))) + +// OCF Flag is not present in AOS frames - OCF is handled differently +// No direct equivalent macro needed + +// MCID (Master Channel Identifier) - 10 bits in AOS +#define AOSTF_RD_MCID(hdr) ((((hdr).Id[0] & 0x3F) << 4) | \ + (((hdr).Id[1] & 0xF0) >> 4)) + +// GVCID equivalent would be MCID + VCID +#define AOSTF_RD_GVCID(hdr) ((((hdr).Id[0] & 0x3F) << 10) | \ + (((hdr).Id[1] & 0xF0) << 2) | \ + ((hdr).Id[1] & 0x3F)) + +// MC Frame Count doesn't exist in AOS - no equivalent + +// VC Frame Count - 3 octets (24 bits) in AOS +#define AOSTF_WR_VCFRMCNT(hdr,val) ((hdr).VcFrameCount[0] = ((val) >> 16) & 0xFF), \ + ((hdr).VcFrameCount[1] = ((val) >> 8) & 0xFF), \ + ((hdr).VcFrameCount[2] = (val) & 0xFF) + +// Replay Flag - 1 bit in AOS Signaling Field +#define AOSTF_RD_REPLAYFLAG(hdr) (((hdr).SignalingField & 0x80) >> 7) +#define AOSTF_WR_REPLAYFLAG(hdr,val) ((hdr).SignalingField = (((val) << 7) | \ + ((hdr).SignalingField & 0x7F))) + +// VC Frame Count Usage Flag - 1 bit in AOS Signaling Field +#define AOSTF_RD_VCFCNTUSAGEFLAG(hdr) (((hdr).SignalingField & 0x40) >> 6) +#define AOSTF_WR_VCFCNTUSAGEFLAG(hdr,val) ((hdr).SignalingField = (((val) << 6) | \ + ((hdr).SignalingField & 0xBF))) + +// Reserved Spare bits - 2 bits in AOS Signaling Field +#define AOSTF_WR_RSVDSPARE(hdr,val) ((hdr).SignalingField = (((val) << 4) & 0x30) | \ + ((hdr).SignalingField & 0xCF)) + +// VC Frame Count Cycle - 4 bits in AOS Signaling Field +#define AOSTF_WR_VCFCNTCYCLE(hdr,val) ((hdr).SignalingField = ((val) & 0x0F) | \ + ((hdr).SignalingField & 0xF0)) + +// Frame Header Error Control - 2 octets (optional in AOS) +#define AOSTF_WR_FHEC(hdr,val) ((hdr).FrameHeaderErrorControl[0] = ((val) >> 8) & 0xFF), \ + ((hdr).FrameHeaderErrorControl[1] = (val) & 0xFF) + +// CRC equivalent - Frame Error Control Field (optional in AOS) +#define AOSTF_WR_FECF(ptr,val) (((ptr)[0] = (val >> 8) & 0xFF),\ + ((ptr)[1] = (val) & 0xFF)) + +/* Prototypes for internal functions */ +static void AOSTF_GenFecfTable(uint32 polynomial); + + +/* + * Function: AOSTF_LibInit + * + * Notes: + * -Called by IO_LibInit() + * + */ +int32 AOSTF_LibInit(void) +{ + AOSTF_GenFecfTable(AOSTF_FECF_POLYNOMIAL); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetVersion + * + * Notes: + * + */ +int32 AOSTF_SetVersion(AOSTF_PriHdr_t *tfPtr, uint16 val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + AOSTF_WR_TF_VERSION(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetScId + * + * Notes: + * + */ +int32 AOSTF_SetScId(AOSTF_PriHdr_t *tfPtr, uint16 val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + AOSTF_WR_SCID(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetVcId + * + * Notes: + * + */ +int32 AOSTF_SetVcId(AOSTF_PriHdr_t *tfPtr, uint16 val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + /* Validate VCID range (0-62, 63 reserved for idle) */ + if (val > 63) + { + return AOSTF_INVALID_LENGTH; + } + + AOSTF_WR_VCID(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetReplayFlag + * + * Notes: + * + */ +int32 AOSTF_SetReplayFlag(AOSTF_PriHdr_t *tfPtr, bool val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + AOSTF_WR_REPLAYFLAG(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetVcFrameCountUsageFlag + * + * Notes: + * + */ +int32 AOSTF_SetVcFrameCountUsageFlag(AOSTF_PriHdr_t *tfPtr, bool val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + AOSTF_WR_VCFCNTUSAGEFLAG(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetVcFrameCountCycle + * + * Notes: + * + */ +int32 AOSTF_SetVcFrameCountCycle(AOSTF_PriHdr_t *tfPtr, uint16 val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + /* Validate 4-bit value */ + if (val > 15) + { + return AOSTF_INVALID_LENGTH; + } + + AOSTF_WR_VCFCNTCYCLE(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_GetMcId + * + * Notes: + * + */ +int32 AOSTF_GetMcId(AOSTF_PriHdr_t *tfPtr) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + return AOSTF_RD_MCID(*tfPtr); +} + + +/* + * Function: AOSTF_GetGlobalVcId + * + * Notes: + * + */ +int32 AOSTF_GetGlobalVcId(AOSTF_PriHdr_t *tfPtr) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + return AOSTF_RD_GVCID(*tfPtr); +} + + +/* + * Function: AOSTF_SetVcFrameCount + * + * Notes: + * + */ +int32 AOSTF_SetVcFrameCount(AOSTF_PriHdr_t *tfPtr, uint32 val) +{ + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + /* Validate 24-bit value */ + if (val > 0xFFFFFF) + { + return AOSTF_INVALID_LENGTH; + } + + AOSTF_WR_VCFRMCNT(*tfPtr, val); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_IncrVcFrameCount + * + * Notes: + * + */ +int32 AOSTF_IncrVcFrameCount(AOSTF_PriHdr_t *tfPtr) +{ + uint32 currentCount = 0; + + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + /* Read current 24-bit counter value */ + currentCount = (tfPtr->VcFrameCount[0] << 16) | + (tfPtr->VcFrameCount[1] << 8) | + tfPtr->VcFrameCount[2]; + + /* Increment with 24-bit rollover */ + currentCount = (currentCount + 1) & 0xFFFFFF; + + /* Write back the incremented value */ + tfPtr->VcFrameCount[0] = (currentCount >> 16) & 0xFF; + tfPtr->VcFrameCount[1] = (currentCount >> 8) & 0xFF; + tfPtr->VcFrameCount[2] = currentCount & 0xFF; + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetInsertZone + * + * Notes: + * + */ +int32 AOSTF_SetInsertZone(AOSTF_PriHdr_t *tfPtr, uint8 *data, uint16 length) +{ + uint8 *insertZonePtr = NULL; + + if ((tfPtr == NULL) || (data == NULL)) + { + return AOSTF_INVALID_POINTER; + } + + if (length > AOSTF_INSERT_ZONE_MAX_LENGTH) + { + return AOSTF_INVALID_LENGTH; + } + + /* Insert zone follows the primary header */ + insertZonePtr = (uint8 *)(tfPtr) + AOSTF_PRIHDR_LENGTH; + + CFE_PSP_MemCpy(insertZonePtr, data, length); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_SetOcf + * + * Notes: + * + */ +int32 AOSTF_SetOcf(AOSTF_PriHdr_t *tfPtr, uint8 *data, uint16 offset) +{ + if ((tfPtr == NULL) || (data == NULL)) + { + return AOSTF_INVALID_POINTER; + } + + CFE_PSP_MemCpy((uint8 *)tfPtr + offset, data, AOSTF_OCF_LENGTH); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_UpdateFrameHeaderErrorControl + * + * Notes: + * + */ +int32 AOSTF_UpdateFrameHeaderErrorControl(AOSTF_PriHdr_t *tfPtr) +{ + uint16 reg = 0xffffU; + uint8 *octPtr = NULL; + uint8 byte; + uint32 len = 0; + + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + /* First clear the FHEC bytes */ + CFE_PSP_MemSet((void *)tfPtr->FrameHeaderErrorControl, 0x00, 2); + + octPtr = (uint8 *) tfPtr; + + /* Calculate FHEC over first 6 octets of header */ + len = 6; + while (len--) + { + byte = (reg >> 8) & 0xff; + reg = (reg << 8) | *octPtr; + reg ^= fecfTable[byte]; + octPtr++; + } + + AOSTF_WR_FHEC(*tfPtr, reg); + + return AOSTF_SUCCESS; +} + + +/* + * Function: AOSTF_UpdateFrameErrorControlField + * + */ +int32 AOSTF_UpdateFrameErrorControlField(AOSTF_PriHdr_t *tfPtr, uint16 offset) +{ + uint16 reg = 0xffffU; + uint8 *octPtr = NULL; + uint8 byte; + uint32 len = 0; + + if (tfPtr == NULL) + { + return AOSTF_INVALID_POINTER; + } + + if (offset < sizeof(AOSTF_PriHdr_t)) + { + return AOSTF_INVALID_LENGTH; + } + + /* First clear the FECF bytes */ + CFE_PSP_MemSet((void *)((uint8 *)tfPtr + offset), 0x00, 2); + + octPtr = (uint8 *) tfPtr; + + len = offset + 2; + while (len--) + { + byte = (reg >> 8) & 0xff; + reg = (reg << 8) | *octPtr; + reg ^= fecfTable[byte]; + octPtr++; + } + + AOSTF_WR_FECF((uint8 *)tfPtr + offset, reg); + + return AOSTF_SUCCESS; +} + + + +/* --------------------------- Helper Functions ----------------------------- */ + + +/* + * Function: AOSTF_GenFecfTable + * + * Purpose: + * Generate the FECF Table for FECF computation + + * Arguments: + * polynomial : The polynomial coefficients (eg. 0x1021: x^16 + x^12 + x^5 + 1) + * + * Note: + * - The highest order coefficient is not required in the polynomial. + * + */ +void AOSTF_GenFecfTable(uint32 polynomial) +{ + uint32 remainder = 0; + uint16 topbit = 1 << 15; + uint16 val = 0; + uint8 bit = 0; + + for (val = 0; val < 256; ++val) + { + /* The first remainder (16-bit) is the divident. */ + remainder = val << 8; + + /* Perform modulo-2 division, one bit at a time */ + for (bit = 0; bit < 8; ++bit) + { + /* If the remainder has topbit, divide by polynomial */ + if (remainder & topbit) + { + remainder = (uint16)(remainder << 1) ^ (uint16)(polynomial); + } + else + { + remainder = remainder << 1; + } + } + + fecfTable[val] = (uint16)remainder; + } +} \ No newline at end of file diff --git a/fsw/src/services/aos_sdlp.c b/fsw/src/services/aos_sdlp.c new file mode 100644 index 0000000..725cf1a --- /dev/null +++ b/fsw/src/services/aos_sdlp.c @@ -0,0 +1,1060 @@ +/******************************************************************************/ +/** \file AOS_sdlp.c +* +* Copyright 2017 United States Government as represented by the Administrator +* of the National Aeronautics and Space Administration. No copyright is +* claimed in the United States under Title 17, U.S. Code. +* All Other Rights Reserved. +* +* \brief Function Definitions for AOS_SDLP +* +* \par +* Provides Telemetry Space Data Link Protocol (AOS_SDLP) services +* +* \par Modification History: +* - 2015-04-26 | Alan A. Asp | OSR | Code Started (originally in AOStf.c) +* - 2015-10-22 | Guy de Carufel | OSR | Migrated from AOStf.c. +* Major revision: Comments, Structs, idle data, overflow, API. +*******************************************************************************/ + +#include "aos_sdlp.h" + +static int32 AOS_SDLP_AddData(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pData, + uint16 dataLength, bool isPacket); +static int32 AOS_SDLP_CopyToOverflow(AOS_SDLP_OverflowInfo_t *pOverflow, + uint8 *data, uint16 length, + bool isPartial); +static int32 AOS_SDLP_CopyFromOverflow(AOS_SDLP_FrameInfo_t *pFrameInfo); + + + +/*****************************************************************************/ +/** \brief AOS_SDLP_InitIdlePacket +******************************************************************************/ +int32 AOS_SDLP_InitIdlePacket(CFE_MSG_Message_t *pIdlePacket, uint8 *pIdlePattern, + uint16 bufferLength, uint32 patternBitLength) +{ + uint8 *pIdleData = NULL; + uint16 idleDataLength = 0; + uint16 byte = 0; + uint32 bit = 0; + uint16 bitOffset = 0; + uint16 byteIdx = 0; + uint16 patternLength = 0; + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pIdlePacket == NULL || pIdlePattern == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitIdlePacket Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + if (patternBitLength == 0 || bufferLength == 0) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitIdlePacket Error: " + "Input length is 0."); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + /* Idle packet, as specified in CCSDS 133.0-B-1 */ + CFE_MSG_Init(pIdlePacket, CFE_SB_ValueToMsgId(0x7ffU), bufferLength); + idleDataLength = CFE_SB_GetUserDataLength(pIdlePacket); + pIdleData = CFE_SB_GetUserData(pIdlePacket); + + patternLength = patternBitLength / 8; + if (patternBitLength % 8 != 0) + { + patternLength++; + } + + /* Build the idle data from a provided pattern */ + for (byte = 0; byte < idleDataLength; ++byte) + { + bit = (byte * 8) % patternBitLength; + byteIdx = bit / 8; + bitOffset = bit % 8; + + pIdleData[byte] = (pIdlePattern[byteIdx] << bitOffset) | + (pIdlePattern[(byteIdx + 1) % patternLength] >> + (8-bitOffset)); + } + +end_of_function: + return iStatus; +} + + +/*****************************************************************************/ +/** \brief AOS_SDLP_InitChannel +******************************************************************************/ +int32 AOS_SDLP_InitChannel(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pTfBuffer, uint8 *pOverflowBuffer, + AOS_SDLP_GlobalConfig_t *pGlobalConfig, + AOS_SDLP_ChannelConfig_t *pChannelConfig) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + int32 dataFieldLength; + uint16 dataFieldOffset; + uint8 sdlsSecurityHeaderLength = 0; + uint8 sdlsSecurityTrailerLength = 0; + char mutName[OS_MAX_API_NAME]; + SecurityAssociation_t* sa_ptr = NULL; + + if (pGlobalConfig == NULL || pChannelConfig == NULL || pFrameInfo == NULL || + pOverflowBuffer == NULL || pTfBuffer == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitChannel Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Validate VCID range (0-62, 63 reserved for idle) */ + if (pChannelConfig->vcId > 62) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitChannel Error: " + "Invalid VCID:%d (must be 0-62)", pChannelConfig->vcId); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + /* Validate VC Frame Count Cycle (4-bit value) */ + if (pChannelConfig->vcFrameCountCycle > 15) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitChannel Error: " + "Invalid VC Frame Count Cycle:%d (must be 0-15)", + pChannelConfig->vcFrameCountCycle); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + dataFieldLength = (int32) pGlobalConfig->frameLength; + dataFieldOffset = AOSTF_PRIHDR_LENGTH; + + /* Add Frame Header Error Control if present */ + if (pGlobalConfig->hasFhec == true) + { + dataFieldOffset += AOSTF_FHEC_LENGTH; + } + + /* Add Insert Zone if present */ + if (pGlobalConfig->hasInsertZone == true) + { + dataFieldOffset += pGlobalConfig->insertZoneLength; + } + + // Need SA information for security parameter lengths + // Query SA DB for active SA / SDLS parameters + if (sa_if == NULL) // This should not happen, but tested here for safety + { + printf(KRED "ERROR: SA DB Not initalized! -- CRYPTO_LIB_ERR_NO_INIT, Will Exit\n" RESET); + iStatus = CRYPTO_LIB_ERR_NO_INIT; + } + else + { + iStatus = sa_if->sa_get_operational_sa_from_gvcid(1, (uint16)pGlobalConfig->scId, (uint16)pChannelConfig->vcId, 0, &sa_ptr); + + if (iStatus != CRYPTO_LIB_SUCCESS) + { + printf(KRED "Error retrieving operational SA. Error code %d. scId = %d, vcId = %d \n" RESET, iStatus, pGlobalConfig->scId, pChannelConfig->vcId); + goto end_of_function; + } + } + + // IF using SDLS + // TODO Review this if_statement + if (1) + { + sdlsSecurityHeaderLength = Crypto_Get_Security_Header_Length(sa_ptr); + dataFieldOffset += sdlsSecurityHeaderLength; + } + + // Reduce available field length based on cumulative offset + dataFieldLength -= dataFieldOffset; + + // IF using SDLS + if (1) + { + sdlsSecurityTrailerLength = Crypto_Get_Security_Trailer_Length(sa_ptr); + dataFieldLength -= sdlsSecurityTrailerLength; + } + + if (pChannelConfig->ocfFlag == true) + { + dataFieldLength -= AOSTF_OCF_LENGTH; + } + + if (pGlobalConfig->hasFecf == true) + { + dataFieldLength -= AOSTF_FECF_LENGTH; + } + +#ifdef AOS_DEBUG + printf("AOS_SDLP Initializing channel:\n"); + printf("\t Primary header length: \t%d\n", AOSTF_PRIHDR_LENGTH); + printf("\t Frame Header Error Control: \t%d\n", pGlobalConfig->hasFhec ? AOSTF_FHEC_LENGTH : 0); + printf("\t Insert Zone length: \t%d\n", pGlobalConfig->hasInsertZone ? pGlobalConfig->insertZoneLength : 0); + printf("\t\t SPI Length: 2 bytes\n"); + printf("\t\t IV Length: %d bytes\n", sa_ptr->shivf_len); + printf("\t\t SNF Length Length: %d bytes\n", sa_ptr->shsnf_len); + printf("\t\t PLF Length: %d bytes\n", sa_ptr->shplf_len); + printf("\t Security header length: \t%d\n", sdlsSecurityHeaderLength); + printf("\t Data field offset: \t%d\n", dataFieldOffset); + printf("\t Data field length: \t%d\n", dataFieldLength); + printf("\t Security trailer length: \t%d\n", sdlsSecurityTrailerLength); + printf("\t OCF Length: \t%d\n", AOSTF_OCF_LENGTH); + printf("\t FECF length: \t%d\n", AOSTF_FECF_LENGTH); +#endif + + if (dataFieldLength < 0) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_InitChannel Error: " + "Invalid Length Configuration."); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + /* Update the Transfer Frame Info */ + pFrameInfo->dataFieldLength = (uint16) dataFieldLength; + pFrameInfo->dataFieldOffset = dataFieldOffset; + pFrameInfo->insertZoneOffset = AOSTF_PRIHDR_LENGTH + + (pGlobalConfig->hasFhec ? AOSTF_FHEC_LENGTH : 0); + pFrameInfo->ocfOffset = dataFieldOffset + (uint16) dataFieldLength; + pFrameInfo->freeOctets = pFrameInfo->dataFieldLength; + pFrameInfo->currentDataOffset = pFrameInfo->dataFieldOffset; + pFrameInfo->vcFrameCount = 0; + pFrameInfo->globConfig = pGlobalConfig; + pFrameInfo->chnlConfig = pChannelConfig; + pFrameInfo->frame = (AOSTF_PriHdr_t *) pTfBuffer; + pFrameInfo->isReady = false; + + if (pChannelConfig->ocfFlag == true) + { + pFrameInfo->fecfOffset = pFrameInfo->ocfOffset + AOSTF_OCF_LENGTH; + } + else + { + pFrameInfo->fecfOffset = pFrameInfo->ocfOffset; + } + + /* Set the overflowInfo */ + pFrameInfo->overflowInfo.buffSize = pChannelConfig->overflowSize; + pFrameInfo->overflowInfo.freeOctets = pChannelConfig->overflowSize; + pFrameInfo->overflowInfo.partialOctets = 0; + pFrameInfo->overflowInfo.dataStart = pOverflowBuffer; + pFrameInfo->overflowInfo.dataEnd = pOverflowBuffer; + pFrameInfo->overflowInfo.buffer = pOverflowBuffer; + + /* Initialize the TF buffer */ + CFE_PSP_MemSet((void *)pTfBuffer, 0, pGlobalConfig->frameLength); + AOSTF_SetVersion(pFrameInfo->frame, AOSTF_VERSION); + AOSTF_SetScId(pFrameInfo->frame, pGlobalConfig->scId); + AOSTF_SetVcId(pFrameInfo->frame, pChannelConfig->vcId); + + /* Set AOS Signaling Field components */ + AOSTF_SetReplayFlag(pFrameInfo->frame, pChannelConfig->replayFlag); + AOSTF_SetVcFrameCountUsageFlag(pFrameInfo->frame, pChannelConfig->vcFrameCountUsage); + AOSTF_SetVcFrameCountCycle(pFrameInfo->frame, pChannelConfig->vcFrameCountCycle); + + /* Initialize the Overflow buffer */ + CFE_PSP_MemSet((void *)pOverflowBuffer, 0, pChannelConfig->overflowSize); + + /* Create the Mutex */ + AOSTF_GetGlobalVcId(pFrameInfo->frame); + OS_MutSemCreate(&pFrameInfo->mutexId, mutName, 0); + + pFrameInfo->isInitialized = true; + +end_of_function: + return iStatus; +} + + +/*****************************************************************************/ +/** \brief AOS_SDLP_FrameHasData +******************************************************************************/ +int32 AOS_SDLP_FrameHasData(AOS_SDLP_FrameInfo_t *pFrameInfo) +{ + int32 hasData = 0; + + if (pFrameInfo == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_FrameHasData Error: " + "Input Pointer is Null."); + + hasData = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + +#ifdef AOS_DEBUG + printf("*** DATA LENGTH INFO!***\n"); + printf("*** Free Octets: %d\n", pFrameInfo->freeOctets); + printf("*** dataFieldLength: %d\n", pFrameInfo->dataFieldLength); +#endif + + if (pFrameInfo->freeOctets < pFrameInfo->dataFieldLength) + { + hasData = 1; + } + +end_of_function: + return hasData; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_AddPacket +*******************************************************************************/ +int32 AOS_SDLP_AddPacket(AOS_SDLP_FrameInfo_t *pFrameInfo, CFE_MSG_Message_t *pPacket) +{ + size_t length = 0; + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL || pPacket == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddPacket Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddPacket Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + CFE_MSG_GetSize(pPacket, &length); + + OS_MutSemTake(pFrameInfo->mutexId); + iStatus = AOS_SDLP_AddData(pFrameInfo, (uint8 *) pPacket, length, true); + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_AddIdlePacket +*******************************************************************************/ +int32 AOS_SDLP_AddIdlePacket(AOS_SDLP_FrameInfo_t *pFrameInfo, + CFE_MSG_Message_t *pIdlePacket) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + uint16 lengthToCopy = 0; + CFE_SB_MsgId_t MsgId = CFE_SB_INVALID_MSG_ID; + + if (pFrameInfo == NULL || pIdlePacket == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddIdlePacket Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddIdlePacket Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + + lengthToCopy = pFrameInfo->freeOctets; + + /* If no free octets, no idle data to add. Done. */ + if (lengthToCopy == 0) + { + OS_MutSemGive(pFrameInfo->mutexId); + iStatus = AOS_SDLP_SUCCESS; + goto end_of_function; + } + + /* Limit copy length to available idle data */ + if (lengthToCopy < 7) + { + lengthToCopy = 7; + } + + CFE_MSG_GetMsgId(pIdlePacket, &MsgId); + + /* The Message ID of the idle buffer should always be 0x7ff (Idle Packet). */ + if (CFE_SB_MsgIdToValue(MsgId) != 0x7ffU) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "TM_SDLP_AddIdlePacket Error: " + "The IdlePacket has MsgId other than 0x7ff."); + + OS_MutSemGive(pFrameInfo->mutexId); + iStatus = AOS_SDLP_ERROR; + goto end_of_function; + } + + /* Set the idlePacket length in header to lengthToCopy */ + CFE_MSG_SetSize(pIdlePacket, lengthToCopy); + + /* Add the idle data. May spill over to overflow buffer. */ + /* iStatus should return remaining free octets if successful. */ + iStatus = AOS_SDLP_AddData(pFrameInfo, (uint8 *) pIdlePacket, lengthToCopy, true); + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_AddBitstreamData +*******************************************************************************/ +int32 AOS_SDLP_AddBitstreamData(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pData, uint16 dataLength) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL || pData == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddBitstreamData Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddBitstreamData Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + iStatus = AOS_SDLP_AddData(pFrameInfo, pData, dataLength, false); + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_AddVcaData +*******************************************************************************/ +int32 AOS_SDLP_AddVcaData(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pData, + uint16 dataLength) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL || pData == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddVcaData Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Check if the frame is initialized */ + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_AddVcaData Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + iStatus = AOS_SDLP_AddData(pFrameInfo, pData, dataLength, false); + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_StartFrame +*******************************************************************************/ +int32 AOS_SDLP_StartFrame(AOS_SDLP_FrameInfo_t *pFrameInfo) +{ + uint16 lengthToCopy = 0; + uint16 lengthCopied = 0; + AOS_SDLP_OverflowInfo_t *pOverflow; + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_StartFrame Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Check if frame has been initialized */ + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_StartFrame Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + + /* If the frame is already started, issue a warning. */ + if (pFrameInfo->isReady == true) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_INFORMATION, + "AOS_SDLP_StartFrame: " + "The frame was already started. Will continue."); + } + + /* Set Frame as ready */ + pFrameInfo->isReady = true; + + pOverflow = &pFrameInfo->overflowInfo; + lengthToCopy = pOverflow->buffSize - pOverflow->freeOctets; + + if (lengthToCopy > 0) + { + /* Only copy as much as TF allows */ + if (lengthToCopy > pFrameInfo->dataFieldLength) + { + lengthToCopy = pFrameInfo->dataFieldLength; + } + + while (lengthToCopy > 0) + { + lengthCopied = AOS_SDLP_CopyFromOverflow(pFrameInfo); + lengthToCopy -= lengthCopied; + } + } + + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_SetIdleFrame +*******************************************************************************/ +int32 AOS_SDLP_SetIdleFrame(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pIdleData, uint16 idleLength) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL || pIdleData == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetIdleFrame Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Check if frame has been initialized */ + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetIdleFrame Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + + /* If the frame is not empty, This method should not be called. */ + if (pFrameInfo->freeOctets != pFrameInfo->dataFieldLength) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetIdleFrame Error: " + "The frame is not empty. VC ID:%u", + pFrameInfo->chnlConfig->vcId); + + OS_MutSemGive(pFrameInfo->mutexId); + iStatus = AOS_SDLP_ERROR; + goto end_of_function; + } + + /* Set VCID to 63 (idle frame VCID) if not already set */ + if (pFrameInfo->chnlConfig->vcId != AOSTF_IDLE_VCID) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_INFORMATION, + "AOS_SDLP_SetIdleFrame: " + "Setting VCID to 63 (idle frame)"); + AOSTF_SetVcId(pFrameInfo->frame, AOSTF_IDLE_VCID); + } + + /* Ensure we don't try to copy more data than frame can hold */ + uint16 copyLength = (idleLength < pFrameInfo->dataFieldLength) ? idleLength : pFrameInfo->dataFieldLength; + + /* Add idle data to fill the frame */ + iStatus = AOS_SDLP_AddData(pFrameInfo, pIdleData, copyLength, false); + + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_SetInsertZone +*******************************************************************************/ +int32 AOS_SDLP_SetInsertZone(AOS_SDLP_FrameInfo_t *pFrameInfo, + uint8 *pInsertData, uint16 dataLength) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL || pInsertData == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetInsertZone Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Check if frame has been initialized */ + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetInsertZone Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + /* Check if frame has Insert Zone */ + if (pFrameInfo->globConfig->hasInsertZone == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetInsertZone Error: " + "Frame does not have Insert Zone configured."); + + iStatus = AOS_SDLP_ERROR; + goto end_of_function; + } + + /* Check if data length exceeds Insert Zone size */ + if (dataLength > pFrameInfo->globConfig->insertZoneLength) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_SetInsertZone Error: " + "Data length %u exceeds Insert Zone length %u.", + dataLength, pFrameInfo->globConfig->insertZoneLength); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + + /* Copy data to the Insert Zone */ + CFE_PSP_MemCpy((void *)((char*)pFrameInfo->frame + pFrameInfo->insertZoneOffset), + pInsertData, dataLength); + + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SDLP_CompleteFrame +*******************************************************************************/ +int32 AOS_SDLP_CompleteFrame(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pOcf) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + + if (pFrameInfo == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_CompleteFrame Error: " + "Input Pointer is Null."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + /* Check if frame has been initialized */ + if (pFrameInfo->isInitialized == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_CompleteFrame Error: " + "The channel is not initialized."); + + iStatus = AOS_SDLP_FRAME_NOT_INIT; + goto end_of_function; + } + + if (pFrameInfo->chnlConfig->ocfFlag == true && pOcf == NULL) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP_CompleteFrame Error: " + "The Input OCF Pointer is NULL."); + + iStatus = AOS_SDLP_INVALID_POINTER; + goto end_of_function; + } + + OS_MutSemTake(pFrameInfo->mutexId); + + /* Set VC Frame Count (24-bits) */ + AOSTF_SetVcFrameCount(pFrameInfo->frame, pFrameInfo->vcFrameCount); + + /* Increment VC frame count for next frame */ + pFrameInfo->vcFrameCount = (pFrameInfo->vcFrameCount + 1) & 0xFFFFFF; + + /* Set AOS signaling field components */ + AOSTF_SetReplayFlag(pFrameInfo->frame, pFrameInfo->chnlConfig->replayFlag); + AOSTF_SetVcFrameCountUsageFlag(pFrameInfo->frame, pFrameInfo->chnlConfig->vcFrameCountUsage); + AOSTF_SetVcFrameCountCycle(pFrameInfo->frame, pFrameInfo->chnlConfig->vcFrameCountCycle); + + /* If an OCF Field is present, set it. */ + if (pFrameInfo->chnlConfig->ocfFlag) + { + AOSTF_SetOcf(pFrameInfo->frame, pOcf, pFrameInfo->ocfOffset); + } + + /* If Frame Header Error Control is present, update it */ + if (pFrameInfo->globConfig->hasFhec) + { + AOSTF_UpdateFrameHeaderErrorControl(pFrameInfo->frame); + } + + /* If Frame Error Control Field is present, set it. */ + if (pFrameInfo->globConfig->hasFecf) + { + AOSTF_UpdateFrameErrorControlField(pFrameInfo->frame, pFrameInfo->fecfOffset); + } + + /* Reset frame metadata */ + pFrameInfo->freeOctets = pFrameInfo->dataFieldLength; + pFrameInfo->currentDataOffset = pFrameInfo->dataFieldOffset; + pFrameInfo->isReady = false; + + OS_MutSemGive(pFrameInfo->mutexId); + +end_of_function: + return iStatus; +} + + +/******************************************************************************* +** Static Functions +*******************************************************************************/ + +/******************************************************************************/ +/** \brief Add Generic Data to the Transfer Frame +* +* \par Description/Algorithm +* Copies a data buffer to the TF data field at the next free octet. +* +* \par Assumptions, External Events, and Notes: +* - Lower level function called by AddPacket, AddIdleData, AddBitstreamData, AddVcaData +* - Data unit will be segmented into overflow buffer if frame is full. +* - Transfer frame will be populated with overflow data if frame is +* empty prior to adding data. +* - isPacket flag is used for M_PDU processing (multiplex protocol data units) +* +* \param[in,out] pFrameInfo Pointer to the Frame info/working struct. +* \param[in] pData Pointer to data buffer +* \param[in] dataLength Length of data to copy +* \param[in] isPacket Data is a packet (M_PDU or similar) +* +* \return Frame FreeOctets +* \return AOS_SDLP_FRAME_NOT_READY If frame has not been started +* \return AOS_SDLP_OVERFLOW_FULL Data dropped. The overflow buffer is full +* +* \see +* #AOS_SDLP_AddPacket +* #AOS_SDLP_AddIdlePacket +*******************************************************************************/ +static int32 AOS_SDLP_AddData(AOS_SDLP_FrameInfo_t *pFrameInfo, uint8 *pData, + uint16 dataLength, bool isPacket) +{ + uint16 lengthToCopy = dataLength; + int32 iStatus = AOS_SDLP_SUCCESS; + bool isPartial = false; + + /* Check if the frame is ready to add new data. */ + if (pFrameInfo->isReady == false) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP Error: " + "The Frame is not ready. Call StartFrame()."); + + iStatus = AOS_SDLP_FRAME_NOT_READY; + goto end_of_function; + } + + /* If data needs to be segmented, copy extra octets to overflow buffer */ + if (pFrameInfo->freeOctets < lengthToCopy) + { + lengthToCopy = pFrameInfo->freeOctets; + if (lengthToCopy > 0) + { + isPartial = true; + } + + iStatus = AOS_SDLP_CopyToOverflow(&pFrameInfo->overflowInfo, + pData + lengthToCopy, + dataLength - lengthToCopy, isPartial); + if (iStatus < 0) + { + goto end_of_function; + } + } + + CFE_PSP_MemCpy((void *) ((char*)pFrameInfo->frame + pFrameInfo->currentDataOffset), + pData, lengthToCopy); + pFrameInfo->freeOctets -= lengthToCopy; + pFrameInfo->currentDataOffset += lengthToCopy; + iStatus = (int32) pFrameInfo->freeOctets; + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief Copy data to the end of the overflow queue +* +* \par Description/Algorithm +* Copy data to overflow queue +* +* \par Assumptions, External Events, and Notes: + * - If overflow buffer is full, message is dropped in AddData + * - The overflow queue is implemented as a sliding window buffer. + * - Function called by AddData() + * - Input pointers are checked by calling function. +* +* \param[in,out] pOverflow Pointer to the Overflow info +* \param[in] data Pointer to the data to copy +* \param[in] length Length of the data to copy +* \param[in] isPartial Is data partial +* +* \return AOS_SDLP_SUCCESS If successful. +* \return AOS_SDLP_INVALID_LENGTH If an input length is invalid +* \return AOS_SDLP_OVERFLOW_FULL If overflow buffer is full +* +* \see +* #AOS_SDLP_AddData +*******************************************************************************/ +static int32 AOS_SDLP_CopyToOverflow(AOS_SDLP_OverflowInfo_t *pOverflow, uint8 *data, + uint16 length, bool isPartial) +{ + int32 iStatus = AOS_SDLP_SUCCESS; + uint16 lengthToEnd; + + if (length > pOverflow->buffSize) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_ERROR, + "AOS_SDLP ERROR: " + "Message Length too large for overflow Buffer."); + + iStatus = AOS_SDLP_INVALID_LENGTH; + goto end_of_function; + } + + /* If the buffer is full, drop message. */ + if (length > pOverflow->freeOctets) + { + CFE_EVS_SendEvent(IO_LIB_AOS_SDLP_EID, CFE_EVS_EventType_INFORMATION, + "AOS_SDLP Warning: " + "The Frame's OverflowBuffer is Full. Message Dropped."); + + iStatus = AOS_SDLP_OVERFLOW_FULL; + goto end_of_function; + } + + /* Get size left in buffer after dataEnd cursor */ + lengthToEnd = pOverflow->buffSize - (pOverflow->dataEnd - pOverflow->buffer); + + /* Copy the data at the dataEnd cursor */ + if (length < lengthToEnd) + { + CFE_PSP_MemCpy(pOverflow->dataEnd, data, length); + pOverflow->dataEnd += length; + } + /* Wrap arround data in overflow buffer */ + else + { + CFE_PSP_MemCpy(pOverflow->dataEnd, data, lengthToEnd); + CFE_PSP_MemCpy(pOverflow->buffer, data + lengthToEnd, + length - lengthToEnd); + pOverflow->dataEnd = pOverflow->buffer + (length - lengthToEnd); + } + + /* If we are passing only partial data, set the partialOctets */ + if (isPartial == true) + { + pOverflow->partialOctets = length; + } + + pOverflow->freeOctets -= length; + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief Copy one packet or partial octects from overflow queue. +* +* \par Description/Algorithm +* Copy one packet or partial octects from the start of the overflow queue +* to the transfer frame. +* +* \par Assumptions, External Events, and Notes: +* - The overflow queue is implemented as a sliding window buffer. +* - Function called by StartFrame() +* - Input pointers are checked by calling function. +* +* \param[out] pFrameInfo Pointer to the Frame info/working struct. +* +* \return lengthCopied Length of data copied +* +* \see +* #AOS_SDLP_StartFrame +*******************************************************************************/ +static int32 AOS_SDLP_CopyFromOverflow(AOS_SDLP_FrameInfo_t *pFrameInfo) +{ + uint8 msgHdr[6]; + CFE_MSG_Message_t* MsgPtr = (CFE_MSG_Message_t*) msgHdr; + size_t lengthToCopy; + size_t lengthToEnd; + bool isPacket; + size_t freeOctets; + AOS_SDLP_OverflowInfo_t *pOverflow = &pFrameInfo->overflowInfo; + + lengthToEnd = pOverflow->buffSize - + (pOverflow->dataStart - pOverflow->buffer); + + freeOctets = pFrameInfo->freeOctets; + + /* First Determine how many octets to copy from overflow buffer. */ + + /* If there are partial octets, copy those. */ + if (pFrameInfo->overflowInfo.partialOctets > 0) + { + lengthToCopy = pFrameInfo->overflowInfo.partialOctets; + isPacket = false; + } + /* Otherwise, copy the first packet */ + else + { + /* If the the lengthToEnd is shorter than the Packet Primary Header, + * Copy the header locally first. */ + if (lengthToEnd < 6) + { + CFE_PSP_MemCpy((void *)msgHdr, (void *) pOverflow->dataStart, + lengthToEnd); + CFE_PSP_MemCpy((void *)(msgHdr + lengthToEnd), + (void *) pOverflow->buffer, 6 - lengthToEnd); + CFE_MSG_GetSize(MsgPtr, &lengthToCopy); + } + else + { + CFE_MSG_GetSize((CFE_MSG_Message_t *) pOverflow->dataStart, &lengthToCopy); + } + + isPacket = true; + } + + /* Only copy as many octets as TF has free octets available */ + if (freeOctets < lengthToCopy) + { + /* If we are passing a full packet, set the new value of partialOctets */ + if (isPacket == true) + { + pFrameInfo->overflowInfo.partialOctets = lengthToCopy - freeOctets; + } + /* If we are passing partial octets, revise the partial octet value */ + else + { + pFrameInfo->overflowInfo.partialOctets -= freeOctets; + } + + /* The length to copy is freeOctets */ + lengthToCopy = freeOctets; + } + /* If we are copying partial octets and there is enough freeOctets for all + * partial octets, reset the partialOctets to 0. */ + else if (isPacket == false) + { + pFrameInfo->overflowInfo.partialOctets = 0; + } + + /* If the length to the end is greater than the length to copy, copy it. */ + if (lengthToEnd > lengthToCopy) + { + freeOctets = AOS_SDLP_AddData(pFrameInfo, pOverflow->dataStart, + lengthToCopy, isPacket); + pOverflow->dataStart += lengthToCopy; + } + /* Wrap arround overflow buffer if required */ + else + { + freeOctets = AOS_SDLP_AddData(pFrameInfo, pOverflow->dataStart, + lengthToEnd, isPacket); + freeOctets = AOS_SDLP_AddData(pFrameInfo, pOverflow->buffer, + lengthToCopy - lengthToEnd, false); + pOverflow->dataStart = pOverflow->buffer + + (lengthToCopy - lengthToEnd); + } + + /* Revise the number of free Octets in overflow buffer */ + pOverflow->freeOctets += lengthToCopy; + pFrameInfo->freeOctets = freeOctets; + + return lengthToCopy; +} \ No newline at end of file diff --git a/fsw/src/services/aos_sync.c b/fsw/src/services/aos_sync.c new file mode 100644 index 0000000..968ecbf --- /dev/null +++ b/fsw/src/services/aos_sync.c @@ -0,0 +1,81 @@ +/******************************************************************************/ +/** \file tm_sync.c +* +* Copyright 2017 United States Government as represented by the Administrator +* of the National Aeronautics and Space Administration. No copyright is +* claimed in the United States under Title 17, U.S. Code. +* All Other Rights Reserved. +* +* \brief Provides the AOS Channel Synchronization service. +* +* \par Modification History: +* - 2015-10-29 | Guy de Carufel | OSR | Code Started +*******************************************************************************/ +#include + +#include "aos_sync.h" +#include "io_lib_utils.h" + +static uint8 prSeq[32]; + +/*****************************************************************************/ +/** \brief AOS_SYNC_LibInit +******************************************************************************/ +int32 AOS_SYNC_LibInit(void) +{ + IO_LIB_UTIL_GenPseudoRandomSeq(&prSeq[0], 0xa9, 0xff); + + return AOS_SYNC_SUCCESS; +} + + +/*****************************************************************************/ +/** \brief AOS_SYNC_Synchronize +******************************************************************************/ +int32 AOS_SYNC_Synchronize(uint8 *pBuff, char *asmStr, uint8 asmSize, + uint16 frameSize, bool randomize) +{ + uint16 byte; + char *hexchar = asmStr; + int32 iStatus = AOS_SYNC_SUCCESS; + + if (pBuff == NULL || asmStr == NULL) + { + iStatus = AOS_SYNC_INVALID_POINTER; + goto end_of_function; + } + + if (asmSize % 2 != 0 || asmSize < 4) + { + iStatus = AOS_SYNC_INVALID_ASM_SIZE; + goto end_of_function; + } + + + /* Store the ASM into the buffer based on the fixed ASM String. */ + for (byte = 0; byte < asmSize; ++byte) + { + sscanf(hexchar, "%2hhx", (uint8 *) &pBuff[byte]); + hexchar += 2; + } + + if (randomize == true) + { + AOS_SYNC_PseudoRandomize(&pBuff[asmSize], frameSize); + } + + /* Return the full size of the CADU. */ + iStatus = asmSize + frameSize; + +end_of_function: + return iStatus; +} + + +/******************************************************************************/ +/** \brief AOS_SYNC_PseudoRandomize +*******************************************************************************/ +int32 AOS_SYNC_PseudoRandomize(uint8 *pFrame, uint16 frameSize) +{ + return IO_LIB_UTIL_PseudoRandomize(pFrame, frameSize, prSeq); +}