diff options
Diffstat (limited to 'lcmodule/source/cnh1605205_ldr_network_layer/source')
7 files changed, 2839 insertions, 0 deletions
diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_header.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_header.c new file mode 100644 index 0000000..7b7631b --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_header.c @@ -0,0 +1,201 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup a2_family + * @{ + * @addtogroup ldr_header + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include "c_system.h" +#include <string.h> +#include "t_basicdefinitions.h" +#include "r_a2_header.h" +#include "r_serialization.h" + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static uint8 A2_FindHeaderPattern(const uint8 *const HeaderData_p, uint32 *StartInBuffer_p); + + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + +/* + * Deserialize the header stored in network format to a structure. + * + * @param [out] Header_p Pointer to the header structure where the + * header data should be placed. + * @param [in] Data_p Pointer to the buffer with received header. + * + * @return none. + */ +void A2_DeserializeHeader(A2_Header_t *Header_p, void *Data_p) +{ + void *Iter_p = Data_p; + + Header_p->HeaderPattern = get_uint8(&Iter_p); + Header_p->Protocol = get_uint8(&Iter_p); + + if (PROTO_A2_ACK == Header_p->Protocol) { + Header_p->SourceAddress = get_uint8(&Iter_p); + Header_p->DestinationAddress = get_uint8(&Iter_p); + Header_p->SequenceNumber = get_uint8(&Iter_p); + Header_p->Reserved[0] = get_uint8(&Iter_p); + Header_p->Reserved[1] = get_uint8(&Iter_p); + Header_p->Reserved[2] = get_uint8(&Iter_p); + Header_p->DataLength = 0; + } else { + Header_p->SourceAddress = get_uint8(&Iter_p); + Header_p->DestinationAddress = get_uint8(&Iter_p); + Header_p->SequenceNumber = get_uint8(&Iter_p); + Header_p->Reserved[0] = get_uint8(&Iter_p); + Header_p->Reserved[1] = get_uint8(&Iter_p); + Header_p->Reserved[2] = get_uint8(&Iter_p); + Header_p->DataLength = get_uint32_le(&Iter_p); + } +} + +/* + * Serialize the header structure to network format. + * + * The data buffer must be of at least size HEADER_LENGTH. + * + * @param [out] Data_p Pointer to the buffer where the + * serialized header should be placed. + * @param [in] Header_p Pointer to the source header structure. + * + * @return none. + */ +void A2_SerializeHeader(void *Data_p, const A2_Header_t *Header_p) +{ + void *Iter_p = Data_p; + + if (PROTO_A2_ACK == Header_p->Protocol) { + put_uint8(&Iter_p, A2_HEADER_PATTERN); + put_uint8(&Iter_p, PROTO_A2_ACK); + put_uint8(&Iter_p, Header_p->SourceAddress); + put_uint8(&Iter_p, Header_p->DestinationAddress); + put_uint8(&Iter_p, Header_p->SequenceNumber); + put_uint8(&Iter_p, Header_p->Reserved[0]); + put_uint8(&Iter_p, Header_p->Reserved[1]); + put_uint8(&Iter_p, Header_p->Reserved[2]); + put_uint16_le(&Iter_p, 0); + } else { + put_uint8(&Iter_p, A2_HEADER_PATTERN); + put_uint8(&Iter_p, PROTO_A2); + put_uint8(&Iter_p, Header_p->SourceAddress); + put_uint8(&Iter_p, Header_p->DestinationAddress); + put_uint8(&Iter_p, Header_p->SequenceNumber); + put_uint8(&Iter_p, Header_p->Reserved[0]); + put_uint8(&Iter_p, Header_p->Reserved[1]); + put_uint8(&Iter_p, Header_p->Reserved[2]); + put_uint32_le(&Iter_p, Header_p->DataLength); + } +} + + +/* + * Get packet length in bytes given the information in Header_p. + * + * @param [in] Header_p Pointer to the header structure. + * + * @return The length of the packet in bytes. + */ +uint32 A2_GetPacketLength(const A2_Header_t *Header_p) +{ + return A2_HEADER_LENGTH + Header_p->DataLength; +} + + +/* + * Determines whether the first HEADER_LENGTH bytes of Data_p contains a + * valid header. + * + * @param [in] Data_p Pointer to the header candidate. + * + * @retval TRUE If header is valid. + * @retval FALSE If header is not valid. + */ +boolean A2_IsValidHeader(const void *Data_p) +{ + uint8 *Temp_p = (uint8 *)Data_p; + + if (A2_HEADER_PATTERN != *Temp_p) { + return FALSE; + } + + if ((PROTO_A2 != *(Temp_p + 1)) && (PROTO_A2_ACK != *(Temp_p + 1)) && (PROTO_CTRL_MSG != *(Temp_p + 1))) { + return FALSE; + } + + return TRUE; +} + +/* + * Checks if new header is received. + * + * @param [in] In_p Pointer to the structure with receiver information. + * + * @retval TRUE If header is received. + * @retval FALSE If header is not received. + */ +boolean A2_IsReceivedHeader(A2_Inbound_t *In_p) +{ + uint32 StartHeaderInBuffer = 0; + uint8 *TmpPointer_p = NULL; + uint8 Res = 0; + + Res = A2_FindHeaderPattern(In_p->Scratch, &StartHeaderInBuffer); + + if (A2_HEADER_PATTERN_MATCH == Res) { + /* Check start point of header in received data */ + if (StartHeaderInBuffer == 0) { + return TRUE; + } else { + In_p->ReqData = StartHeaderInBuffer; + TmpPointer_p = (uint8 *)((uint32)In_p->Target_p + StartHeaderInBuffer); + memcpy(In_p->Target_p, (uint8 *)TmpPointer_p, A2_HEADER_LENGTH - StartHeaderInBuffer); + In_p->ReqBuffOffset = A2_HEADER_LENGTH - StartHeaderInBuffer; + } + } else { + /* setup for receiving new packet */ + In_p->State = A2_RECEIVE_ERROR; + } + + return FALSE; +} + +static uint8 A2_FindHeaderPattern(const uint8 *const HeaderData_p, uint32 *StartInBuffer_p) +{ + uint8 Res = NO_A2_HEADER_PATTERN; + uint32 Offset = 0; + + do { + Res = NO_A2_HEADER_PATTERN; + + if ((*((uint8 *)HeaderData_p + Offset) == A2_HEADER_PATTERN)) { + Res = A2_HEADER_PATTERN_MATCH; + break; + } + + Offset++; + } while (Offset < (A2_HEADER_LENGTH - 1)); + + *StartInBuffer_p = Offset; + + return Res; +} + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_network.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_network.c new file mode 100644 index 0000000..19d7800 --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/a2_network.c @@ -0,0 +1,599 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup a2_family + * @{ + * @addtogroup ldr_network_layer + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include "c_system.h" +#include "r_basicdefinitions.h" +#include "r_a2_family.h" +#include "r_a2_transport.h" +#include "r_a2_network.h" +#include "r_a2_protocol.h" +#include "r_a2_header.h" +#include "r_communication_service.h" +#include "r_debug.h" +#include "r_debug_macro.h" + +#ifdef WIN32 +#include <windows.h> +#endif + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static ErrorCode_e A2_Network_ReceiveHeader(const Communication_t *const Communication_p); +static ErrorCode_e A2_Network_ReceivePayload(Communication_t *Communication_p); +static ErrorCode_e A2_Network_RegisterRetransmission(Communication_t *Communication_p, A2_PacketMeta_t *Packet_p); +static void A2_Network_RetransmissionCallback(Communication_t *Communication_p, const void *const Timer_p, void *Data_p); +static void A2_InHashCallback(const void *const Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p); +#ifdef CFG_ENABLE_LOADER_TYPE +static void A2_QueueCallback(const void *const Queue_p, void *Param_p); +#endif //CFG_ENABLE_LOADER_TYPE + +/******************************************************************************* + * File scope types, constants and variables + ******************************************************************************/ +#define A2_RESET_INBOUND(c, s) do { (c)->ReqData = 0; (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->Target_p = NULL; (c)->State = (s); } while(0); +#define A2_SYNC_HEADER(c, d, t) do { (c)->ReqData = d; (c)->Target_p = t; } while(0); +#define A2_SET_INBOUND(c, s, d) do { (c)->ReqData = d; (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->State = (s); } while(0); + +/******************************************************************************* + * Definition of external functions + ******************************************************************************/ + +/* + * Initializes the A2 network layer. + * + * @param [in,out] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + * @retval E_FAILED_TO_INIT_COM_DEVICE Fail to initialize the communication + * device. + */ +ErrorCode_e A2_Network_Initialize(Communication_t *Communication_p) +{ + memset(A2_NETWORK(Communication_p), 0, sizeof(A2_NetworkContext_t)); + + /* Simulate a finished read to get the inbound state-machine going. */ + A2_Network_ReadCallback(NULL, 0, Communication_p); + A2_NETWORK(Communication_p)->Outbound.InLoad = FALSE; + +#ifdef CFG_ENABLE_LOADER_TYPE + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, A2_QueueCallback, Communication_p); +#endif + A2_NETWORK(Communication_p)->Inbound.LCM_Error = E_SUCCESS; + A2_NETWORK(Communication_p)->Outbound.LCM_Error = E_SUCCESS; + + return E_SUCCESS; +} + +/* + * Shut down the A2 network layer. + * + * @param [in,out] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + */ +ErrorCode_e A2_Network_Shutdown(const Communication_t *const Communication_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_EMPTY, NULL, NULL); + + + /* Wait until the all packets in the queue has released. */ + while (!QUEUE(Communication_p, Fifo_IsEmpty_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p)) { + A2_PacketMeta_t *Packet_p = (A2_PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p); + + if (Packet_p->Header.Protocol != A2_PROTOCOL) { + ReturnValue = A2_Network_PacketRelease(Communication_p, Packet_p); + VERIFY(E_SUCCESS == ReturnValue, ReturnValue); + } + } + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_EMPTY, NULL, NULL); + + /* Wait until the all packets in the queue has released. */ + while (!QUEUE(Communication_p, Fifo_IsEmpty_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p)) { + ReturnValue = A2_Network_PacketRelease(Communication_p, (A2_PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p)); + } + +ErrorExit: + return ReturnValue; +} + + +/* + * Handler for received packets in A2 protocl family. + * + * This callback function handles the received packets. + * + * @param [in] Data_p Pointer to the received data. + * @param [in] Length Length of the received data. + * @param [in] Param_p Parameters; + * + * @return none. + */ +void A2_Network_ReadCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + IDENTIFIER_NOT_USED(Data_p); + Communication_t *Communication_p = (Communication_t *)Param_p; + + C_(printf("a2_network.c (%d) RecLength(%d) RecBackupData (%d)\n", __LINE__, Length, A2_NETWORK(Communication_p)->Inbound.RecBackupData);) + A2_NETWORK(Communication_p)->Inbound.RecData += Length + A2_NETWORK(Communication_p)->Inbound.RecBackupData; + A2_NETWORK(Communication_p)->Inbound.RecBackupData = 0; + + if (A2_NETWORK(Communication_p)->Inbound.ReqData == 0) { + ErrorCode_e ReturnValue = E_GENERAL_COMMUNICATION_ERROR; + + switch (A2_NETWORK(Communication_p)->Inbound.State) { + case A2_RECEIVE_HEADER: + ReturnValue = A2_Network_ReceiveHeader(Communication_p); + break; + case A2_RECEIVE_PAYLOAD: + ReturnValue = A2_Network_ReceivePayload(Communication_p); + break; + default: + A2_NETWORK(Communication_p)->Inbound.State = A2_RECEIVE_HEADER; + A2_NETWORK(Communication_p)->Inbound.RecData = 0; + A2_NETWORK(Communication_p)->Inbound.ReqData = A2_HEADER_LENGTH; + A2_NETWORK(Communication_p)->Inbound.Target_p = A2_NETWORK(Communication_p)->Inbound.Scratch; + A2_NETWORK(Communication_p)->Inbound.LCM_Error = E_SUCCESS; + ReturnValue = E_SUCCESS; + break; + } + +#ifdef CFG_ENABLE_LOADER_TYPE + ReturnValue = A2_Network_ReceiverHandler(Communication_p); +#endif + + if (E_SUCCESS != ReturnValue) { + A2_NETWORK(Communication_p)->Inbound.State = A2_RECEIVE_ERROR; + } + + A2_NETWORK(Communication_p)->Inbound.LCM_Error = ReturnValue; + } +} + +/* + * Handler for receiving new data. + * + * This function checks if new data has been received. + * + * @param [in,out] Communication_p Communication module context. + * + * @return none. + */ +ErrorCode_e A2_Network_ReceiverHandler(Communication_t *Communication_p) +{ + uint32 ReqData; + A2_Inbound_t *In_p = &(A2_NETWORK(Communication_p)->Inbound); + + if (In_p->ReqData > 0) { + if (Communication_p->BackupCommBufferSize != 0) { + if (Communication_p->BackupCommBufferSize < In_p->ReqData) { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, Communication_p->BackupCommBufferSize); + In_p->RecBackupData = Communication_p->BackupCommBufferSize; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + In_p->ReqBuffOffset + Communication_p->BackupCommBufferSize, + In_p->ReqData - Communication_p->BackupCommBufferSize, + A2_Network_ReadCallback, Communication_p); + C_(printf("a2_network.c (%d) ReqData(%d) RecData(%d)\n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("a2_network.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + Communication_p->BackupCommBufferSize = 0; + In_p->ReqData = 0; + } else { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, In_p->ReqData); + Communication_p->BackupCommBufferSize = Communication_p->BackupCommBufferSize - In_p->ReqData; + ReqData = In_p->ReqData; + In_p->ReqData = 0; + A2_Network_ReadCallback(In_p->Target_p + In_p->ReqBuffOffset, ReqData, Communication_p); + In_p->RecData = 0; + } + } else { + ReqData = In_p->ReqData; + In_p->ReqData = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + In_p->ReqBuffOffset, ReqData, A2_Network_ReadCallback, Communication_p); + C_(printf("a2_network.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("a2_network.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + } + } + + /* check for receiver sinhronization */ + if (In_p->State == A2_RECEIVE_ERROR) { + A2_RESET_INBOUND(In_p, A2_RECEIVE_HEADER); + A2_SYNC_HEADER(In_p, A2_HEADER_LENGTH, In_p->Scratch); + //(void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p, A2_HEADER_LENGTH, A2_Network_ReadCallback, Communication_p); + } + + return A2_NETWORK(Communication_p)->Inbound.LCM_Error; +} + +/* + * Cancel retransmission of packets. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] UniqueKey Unique key used for identification of packet + * for retransmission. + * + * @return none. + */ +ErrorCode_e A2_Network_CancelRetransmission(const Communication_t *const Communication_p, uint32 UniqueKey) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + uint32 SequenceNumber = A2_NETWORK(Communication_p)->Outbound.Packet_p->Header.SequenceNumber; + + if (SequenceNumber != UniqueKey) { + A_(printf("a2_network.c(%d): Cancel packet retransmission error: SequenceNumber = %u, UniqueKey = %u\n", __LINE__, SequenceNumber, UniqueKey);) + return E_GENERAL_FATAL_ERROR; + } + + (void)TIMER(Communication_p, TimerRelease_Fn)(OBJECT_TIMER(Communication_p), A2_NETWORK(Communication_p)->RetransmissionContext.TimerKey); + free(A2_NETWORK(Communication_p)->Outbound.Packet_p->Timer_p); + ReturnValue = A2_Network_PacketRelease(Communication_p, A2_NETWORK(Communication_p)->Outbound.Packet_p); + + if (E_SUCCESS != ReturnValue) { + A_(printf("a2_network.c(%d): Packet release failed\n", __LINE__);) + return ReturnValue; + } + + A2_NETWORK(Communication_p)->Outbound.State = A2_SEND_IDLE; + A2_NETWORK(Communication_p)->Outbound.Packet_p = NULL; + + return ReturnValue; +} + +/* + * Network packet allocation in A2 protocol family. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] BufferSize Size of buffer used for network packet. + * + * @retval Pointer Pointer to allocated packet meta info. + * @retval NULL if allocation fail. + */ +A2_PacketMeta_t *A2_Network_PacketAllocate(const Communication_t *const Communication_p, int BufferSize) +{ + A2_PacketMeta_t *Meta_p = NULL; + void *Buffer_p = NULL; + int BuffersNr = 0; + + /* Find the first unallocated buffers. */ + Buffer_p = BUFFER(Communication_p, BufferAllocate_Fn)(OBJECT_BUFFER(Communication_p), BufferSize); + + if (NULL == Buffer_p) { + A_(printf("a2_network.c (%d): ** Buffer allocation fail! **\n", __LINE__);) + goto ErrorExit; + } + + B_(printf("a2_network.c (%d): Buffer allocate (0x%x)! **\n", __LINE__, (uint32)Buffer_p);) + + /* packet meta info allocate */ + Meta_p = (A2_PacketMeta_t *)malloc(sizeof(A2_PacketMeta_t)); + + if (NULL == Meta_p) { + A2_NETWORK(Communication_p)->Inbound.LCM_Error = E_ALLOCATE_FAILED; + goto ErrorExit; + } + + /* packet meta info setup */ + Meta_p->Buffer_p = (uint8 *)Buffer_p; + Meta_p->BufferSize = BufferSize; + Meta_p->Flags = A2_BUF_ALLOCATED; + + do { + if (NULL == A2_NETWORK(Communication_p)->MetaInfoList[BuffersNr]) { + A2_NETWORK(Communication_p)->MetaInfoList[BuffersNr] = Meta_p; + break; + } + + BuffersNr++; + } while (BuffersNr < (A2_COMMAND_BUFFER_COUNT)); + +ErrorExit: + return Meta_p; +} + +/* + * Network packet release in A2 protocol family. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] Meta_p Meta info for used network packet. + * + * @retval E_SUCCESS Successful network packet release. + * @retval E_INVALID_INPUT_PARAMETERS Invalid input parameter. + */ +ErrorCode_e A2_Network_PacketRelease(const Communication_t *const Communication_p, A2_PacketMeta_t *Meta_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + int BuffersNr = 0; + + VERIFY(NULL != Meta_p, E_INVALID_INPUT_PARAMETERS); + + /* remove the meta info data from list */ + do { + if (Meta_p == A2_NETWORK(Communication_p)->MetaInfoList[BuffersNr]) { + /* release buffer */ + B_(printf("a2_network.c (%d): Buffer release (0x%x)! **\n", __LINE__, (uint32)(Meta_p->Buffer_p));) + ReturnValue = BUFFER(Communication_p, BufferRelease_Fn)(OBJECT_BUFFER(Communication_p), Meta_p->Buffer_p, Meta_p->BufferSize); + VERIFY(E_SUCCESS == ReturnValue, ReturnValue); + B_(printf("a2_network.c (%d): Buffer released! **\n", __LINE__);) + + A2_NETWORK(Communication_p)->MetaInfoList[BuffersNr] = NULL; + memset(Meta_p, 0, sizeof(A2_PacketMeta_t)); + break; + } + + BuffersNr++; + } while (BuffersNr < (A2_COMMAND_BUFFER_COUNT)); + +ErrorExit: + return ReturnValue; +} + +/* + * Handler function that is called after successful transmission of a packet. + * + * If new packet is ready for transmitting it starts + * the transmission of the packet. + * + * @param [in] Data_p Pointer to the data for transmitting. + * @param [in] Length Length of the received data. + * @param [in] Param_p Parameters. + * + * @return none. + */ +void A2_Network_WriteCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + IDENTIFIER_NOT_USED(Data_p); + IDENTIFIER_NOT_USED(Length); + Communication_t *Communication_p = (Communication_t *)Param_p; + A2_Outbound_t *Out_p = &(A2_NETWORK(Communication_p)->Outbound); + + if (A2_SENDING_PAYLOAD == Out_p->State) { + if (NULL != Out_p->Packet_p->Timer_p) { + (void)A2_Network_RegisterRetransmission(Communication_p, Out_p->Packet_p); + } else { + (void)A2_Network_PacketRelease(Communication_p, Out_p->Packet_p); + Out_p->State = A2_SEND_IDLE; + Out_p->Packet_p = NULL; + } + } else if (A2_SENDING_HEADER == Out_p->State) { + Out_p->State = A2_SEND_PAYLOAD; + } else { + /* should never happen + TODO: Error handling */ + } + +#ifdef CFG_ENABLE_LOADER_TYPE + + if (E_SUCCESS != A2_Network_TransmiterHandler(Communication_p)) { + A2_NETWORK(Communication_p)->Outbound.LCM_Error = E_GENERAL_COMMUNICATION_ERROR; + } + +#endif +} + + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ +static ErrorCode_e A2_Network_ReceiveHeader(const Communication_t *const Communication_p) +{ + A2_Inbound_t *In_p = &(A2_NETWORK(Communication_p)->Inbound); + ErrorCode_e ReturnValue = E_SUCCESS; + + if (In_p->RecData == 0) { + In_p->ReqData = A2_HEADER_LENGTH; + In_p->Target_p = In_p->Scratch; + In_p->ReqBuffOffset = 0; + } else { + if (A2_IsReceivedHeader(In_p)) { + if (A2_IsValidHeader(In_p->Scratch)) { + A2_DeserializeHeader(&In_p->Header, In_p->Scratch); + In_p->Packet_p = A2_Network_PacketAllocate(Communication_p, A2_COMMAND_BUFFER_SIZE); + VERIFY(NULL != In_p->Packet_p, E_FAILED_TO_FIND_COMM_BUFFER); + A2_DeserializeHeader(&In_p->Packet_p->Header, In_p->Scratch); + In_p->Packet_p->Flags = In_p->Packet_p->Flags | A2_BUF_HDR_CRC_OK; + In_p->Target_p = In_p->Packet_p->Buffer_p; + memcpy(In_p->Target_p, In_p->Scratch, A2_HEADER_LENGTH); + In_p->Target_p += A2_HEADER_LENGTH; + In_p->Packet_p->Payload_p = In_p->Packet_p->Buffer_p + A2_HEADER_LENGTH; + + /* check for expected payload */ + if (In_p->Header.DataLength != 0) { + A2_SET_INBOUND(In_p, A2_RECEIVE_PAYLOAD, In_p->Header.DataLength + A2_CRC_LENGTH); + } else { + (void)QUEUE(Communication_p, FifoEnqueue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, In_p->Packet_p); + + /* Sync the header. */ + In_p->Packet_p = NULL; + A2_RESET_INBOUND(In_p, A2_RECEIVE_HEADER); + + if (A2_SPEEDFLASH_START != A2_SPEEDFLASH(Communication_p)->State) { + A2_SYNC_HEADER(In_p, A2_HEADER_LENGTH, In_p->Scratch); + } + + C_(printf("a2_network.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, In_p->ReqData, In_p->RecData);) + } + } else { + /* Sync the header. */ + A2_RESET_INBOUND(In_p, A2_RECEIVE_HEADER); + A2_SYNC_HEADER(In_p, A2_HEADER_LENGTH, In_p->Scratch); + } + } + } + +ErrorExit: + return ReturnValue; +} + +static ErrorCode_e A2_Network_ReceivePayload(Communication_t *Communication_p) +{ + A2_Inbound_t *In_p = &(A2_NETWORK(Communication_p)->Inbound); + A2_PacketMeta_t *Packet_p = In_p->Packet_p; + + Packet_p->Communication_p = Communication_p; + + if (HASH_NONE != Communication_p->CurrentFamilyHash) { + Communication_p->HashDevice_p->Calculate(OBJECT_HASH(Communication_p), + Communication_p->CurrentFamilyHash, + Packet_p->Buffer_p, Packet_p->Header.DataLength + A2_HEADER_LENGTH, + Packet_p->CRC, (HashCallback_t)A2_InHashCallback, + (void *)Packet_p); + } else { + Packet_p->Flags = Packet_p->Flags | A2_BUF_PAYLOAD_CRC_OK | A2_BUF_RX_READY; + (void)QUEUE((Packet_p->Communication_p), FifoEnqueue_Fn)(OBJECT_QUEUE(Packet_p->Communication_p), Packet_p->Communication_p->Inbound_p, Packet_p); + } + + In_p->Packet_p = NULL; + A2_RESET_INBOUND(In_p, A2_RECEIVE_HEADER); + A2_SYNC_HEADER(In_p, A2_HEADER_LENGTH, In_p->Scratch); + return E_SUCCESS; +} + +ErrorCode_e A2_Network_TransmiterHandler(Communication_t *Communication_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + A2_Outbound_t *Out_p = &(A2_NETWORK(Communication_p)->Outbound); + + if (Out_p->InLoad) { + return E_SUCCESS; + } + + Out_p->InLoad = TRUE; + + switch (Out_p->State) { + case A2_SEND_IDLE: + /* check retransmission count before send */ + Out_p->Packet_p = (A2_PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p); + + if (NULL != Out_p->Packet_p) { + if (Out_p->Packet_p->Resend < MAX_RESENDS) { + Out_p->Packet_p->Resend++; + /* get next packet for transmitting */ + Out_p->State = A2_SEND_HEADER; + } else { + return E_RETRANSMITION_FAILED; + } + } else { + break; + } + + /* FALLTHROUGH */ + case A2_SEND_HEADER: + + /* set next state before calling the communication device, to avoid race condition + where write callback is called before new state is set */ + if (Out_p->Packet_p->Header.Protocol == PROTO_A2_ACK) { + Out_p->State = A2_SENDING_PAYLOAD; + } else { + Out_p->State = A2_SENDING_HEADER; + } + + if (E_SUCCESS == Communication_p->CommunicationDevice_p->Write((Out_p->Packet_p->Buffer_p), + A2_HEADER_LENGTH, A2_Network_WriteCallback, Communication_p)) { + C_(printf("a2_network.c (%d) Header Sent to comm device! \n", __LINE__);) + } else { + Out_p->State = A2_SEND_HEADER; + C_(printf("a2_network.c (%d) Error sending header to comm device! \n", __LINE__);) + } + + break; + case A2_SENDING_HEADER: + /* nothing to do, wait until sending is finished and state changed in write callback */ + break; + case A2_SEND_PAYLOAD: + Out_p->State = A2_SENDING_PAYLOAD; + + if (E_SUCCESS == Communication_p->CommunicationDevice_p->Write( + (Out_p->Packet_p->Buffer_p + A2_HEADER_LENGTH), + Out_p->Packet_p->Header.DataLength + A2_CRC_LENGTH, + A2_Network_WriteCallback, Communication_p)) { + C_(printf("a2_network.c (%d) Payload Sent to comm device! \n", __LINE__);) + } else { + Out_p->State = A2_SEND_PAYLOAD; + C_(printf("a2_network.c (%d) Error sending payload to comm device! \n", __LINE__);) + } + + break; + case A2_SENDING_PAYLOAD: + /* nothing to do, wait until sending is finished and state changed when packet ACK is received */ + break; + } + + Out_p->InLoad = FALSE; + + return ReturnValue; +} + +static ErrorCode_e A2_Network_RegisterRetransmission(Communication_t *Communication_p, A2_PacketMeta_t *Packet_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + + if (NULL == Packet_p->Timer_p->HandleFunction_p) { + Packet_p->Timer_p->HandleFunction_p = (HandleFunction_t)A2_Network_RetransmissionCallback; + Packet_p->Timer_p->Param_p = Communication_p; + } + + A2_NETWORK(Communication_p)->RetransmissionContext.TimerKey = TIMER(Communication_p, TimerGet_Fn)(OBJECT_TIMER(Communication_p), Packet_p->Timer_p); + A2_NETWORK(Communication_p)->RetransmissionContext.Timeout = Packet_p->Timer_p->Time; + + return ReturnValue; +} + +static void A2_Network_RetransmissionCallback(Communication_t *Communication_p, const void *const Timer_p, void *Data_p) +{ + (void)QUEUE(Communication_p, FifoEnqueue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, Data_p); +} + +static void A2_InHashCallback(const void *const Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p) +{ + IDENTIFIER_NOT_USED(Data_p); + IDENTIFIER_NOT_USED(Length); + A2_PacketMeta_t *Packet_p = (A2_PacketMeta_t *)Param_p; + + if (memcmp(Hash_p, Packet_p->Buffer_p + A2_HEADER_LENGTH + Packet_p->Header.DataLength, sizeof(uint16)) == 0) { + Packet_p->Flags = Packet_p->Flags | A2_BUF_PAYLOAD_CRC_OK | A2_BUF_RX_READY; + (void)QUEUE((Packet_p->Communication_p), FifoEnqueue_Fn)(OBJECT_QUEUE(Packet_p->Communication_p), Packet_p->Communication_p->Inbound_p, Packet_p); + } else { + /* Invalid packet */ + ErrorCode_e ReturnValue = E_GENERAL_FATAL_ERROR; + + if (E_SUCCESS != (ReturnValue = A2_Network_PacketRelease((Communication_t *)Packet_p->Communication_p, Packet_p))) { + A2_NETWORK((Communication_t *)((A2_PacketMeta_t *)Param_p)->Communication_p)->Inbound.LCM_Error = ReturnValue; + } + } +} + +#ifdef CFG_ENABLE_LOADER_TYPE +static void A2_QueueCallback(const void *const Queue_p, void *Param_p) +{ + if (E_SUCCESS != A2_Network_TransmiterHandler((Communication_t *)Param_p)) { + A2_NETWORK((Communication_t *)Param_p)->Outbound.LCM_Error = E_GENERAL_COMMUNICATION_ERROR; + } +} +#endif //CFG_ENABLE_LOADER_TYPE + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_header.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_header.c new file mode 100644 index 0000000..eab4a6a --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_header.c @@ -0,0 +1,237 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup protrom_family + * @{ + * @addtogroup ldr_header + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include "c_system.h" +#include <string.h> +#include "t_basicdefinitions.h" +#include "r_protrom_header.h" +#include "r_serialization.h" + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static uint8 Protrom_FindHeaderPattern(const uint8 *const HeaderData_p, uint32 *StartInBuffer_p); + + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + +/* + * Converts the header stored in network format to a structure. + * + * @param [out] Header_p Pointer to the header structure where the + * header data should be placed. + * @param [in] Data_p Pointer to the buffer with received header. + * + * @return none. + */ +void Protrom_DeserializeHeader(Protrom_Header_t *Header_p, void *Data_p) +{ + void *Iter_p = Data_p; + + Header_p->HeaderPattern = get_uint8(&Iter_p); + Header_p->Protocol = get_uint8(&Iter_p); + Header_p->SourceAddress = get_uint8(&Iter_p); + Header_p->DestinationAddress = get_uint8(&Iter_p); + Header_p->ReservedField = get_uint8(&Iter_p); + Header_p->PayloadLength = get_uint16(&Iter_p); +} + + +/* + * Converts the header structure to network format. + * + * The data buffer must be of at least size PROTROM_HEADER_LENGTH. + * + * @param [out] Data_p Pointer to the buffer where the + * serialized header should be placed. + * @param [in] Header_p Pointer to the source header structure. + * + * @return none. + */ +void Protrom_SerializeHeader(void *Data_p, const Protrom_Header_t *Header_p) +{ + void *Iter_p = Data_p; + + put_uint8(&Iter_p, PROTROM_HEADER_PATTERN); + put_uint8(&Iter_p, PROTO_PROTROM); + put_uint8(&Iter_p, PROTROM_SOURCE_ADDRESS); + put_uint8(&Iter_p, PROTROM_DESTINATION_ADDRESS); + put_uint8(&Iter_p, PROTROM_RESERVED_FIELD); + put_uint16(&Iter_p, Header_p->PayloadLength); +} + +/* + * Get packet length in bytes given the information in Header_p. + * + * @param [in] Header_p Pointer to the header structure. + * + * @return The length of the packet in bytes. + */ +uint32 Protrom_GetPacketLength(const Protrom_Header_t *Header_p) +{ + return PROTROM_HEADER_LENGTH + Header_p->PayloadLength; +} + + +/* + * Determines whether the first PROTROM_HEADER_LENGTH bytes of Data_p contains a + * valid header. + * + * @param [in] Data_p Pointer to the header candidate. + * + * @retval TRUE If header is valid. + * @retval FALSE If header is not valid. + */ +boolean Protrom_IsValidHeader(const void *Data_p) +{ + uint8 *Temp_p = (uint8 *)Data_p; + + if (PROTROM_HEADER_PATTERN != *Temp_p) { + return FALSE; + } + + if (PROTO_PROTROM != *(Temp_p + 1)) { + return FALSE; + } + + if (PROTROM_SOURCE_ADDRESS != *(Temp_p + 2)) { + return FALSE; + } + + if (PROTROM_DESTINATION_ADDRESS != *(Temp_p + 3)) { + return FALSE; + } + + if (PROTROM_RESERVED_FIELD != *(Temp_p + 4)) { + return FALSE; + } + + if (*(uint16 *)(Temp_p + 5) < 1) { + return FALSE; + } + + return TRUE; +} + + +/* + * Checks if new header is received. + * + * @param [in] In_p Pointer to the structure with receiver information. + * + * @retval TRUE If header is received. + * @retval FALSE If header is not received. + */ +boolean Protrom_IsReceivedHeader(Protrom_Inbound_t *In_p) +{ + uint32 StartHeaderInBuffer = 0; + uint8 *TmpPointer_p = NULL; + uint8 Res = 0; + + Res = Protrom_FindHeaderPattern(In_p->Scratch, &StartHeaderInBuffer); + + if (PROTROM_HEADER_PATTERN_CANDIDATE == Res) { + /* call for receiving the rest bytes in header */ + In_p->ReqData = StartHeaderInBuffer + PROTROM_HEADER_LENGTH - In_p->RecData; + TmpPointer_p = (uint8 *)((uint32)In_p->Target_p + StartHeaderInBuffer); + memcpy(In_p->Target_p, (uint8 *)TmpPointer_p, In_p->RecData - StartHeaderInBuffer); + In_p->ReqBuffOffset = In_p->RecData - StartHeaderInBuffer; + } else { + if (PROTROM_HEADER_PATTERN_MATCH == Res) { + /* Check start point of header in received data */ + if (StartHeaderInBuffer == 0) { + return TRUE; + } else { + In_p->ReqData = StartHeaderInBuffer + PROTROM_HEADER_LENGTH - In_p->RecData; + TmpPointer_p = (uint8 *)((uint32)In_p->Target_p + StartHeaderInBuffer); + memcpy(In_p->Target_p, (uint8 *)TmpPointer_p, In_p->RecData - StartHeaderInBuffer); + In_p->ReqBuffOffset = In_p->RecData - StartHeaderInBuffer; + } + } else { + In_p->ReqData = PROTROM_HEADER_LENGTH; + In_p->ReqBuffOffset = 0; + } + } + + return FALSE; +} + +/* + * Find header pattern in the received data. + * + * @param [in] HeaderData_p Pointer to the structure with receiver data. + * @param [out] StartInBuffer_p Offset in the buffer where is find header. + * + * @retval TRUE If header is received. + * @retval FALSE If header is not received. + */ +static uint8 Protrom_FindHeaderPattern(const uint8 *const HeaderData_p, uint32 *StartInBuffer_p) +{ + uint8 Res = NO_PROTROM_HEADER_PATTERN; + uint32 Offset = 0; + + do { + Res = NO_PROTROM_HEADER_PATTERN; + + if (*((uint8 *)HeaderData_p + Offset) == PROTROM_HEADER_PATTERN) { + Res = PROTROM_HEADER_PATTERN_CANDIDATE; + + if (Offset < 6) { + if (*((uint8 *)HeaderData_p + Offset + 1) == PROTO_PROTROM) { + if (Offset < 5) { + if (*((uint8 *)HeaderData_p + Offset + 2) == PROTROM_SOURCE_ADDRESS) { + if (Offset < 4) { + if (*((uint8 *)HeaderData_p + Offset + 3) == PROTROM_DESTINATION_ADDRESS) { + Res = PROTROM_HEADER_PATTERN_MATCH; + break; + } else { + Offset = Offset + 2; + Res = NO_PROTROM_HEADER_PATTERN; + } + } else { + Res = PROTROM_HEADER_PATTERN_CANDIDATE; + break; + } + } else { + Offset ++; + Res = NO_PROTROM_HEADER_PATTERN; + } + } else { + Res = PROTROM_HEADER_PATTERN_CANDIDATE; + break; + } + } else { + Res = NO_PROTROM_HEADER_PATTERN; + } + } else { + Res = PROTROM_HEADER_PATTERN_CANDIDATE; + break; + } + } + + Offset++; + } while (Offset < PROTROM_HEADER_LENGTH); + + *StartInBuffer_p = Offset; + + return Res; +} + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_network.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_network.c new file mode 100644 index 0000000..cce7a65 --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/protrom_network.c @@ -0,0 +1,380 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup protrom_family + * @{ + * @addtogroup ldr_network_layer + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include <stdlib.h> +#include <string.h> +#include "c_system.h" +#include "t_basicdefinitions.h" +#include "r_protrom_family.h" +#include "r_protrom_transport.h" +#include "r_protrom_network.h" +#include "r_protrom_protocol.h" +#include "r_protrom_header.h" +#include "r_communication_service.h" +#include "r_debug.h" +#include "r_debug_macro.h" + +#ifdef WIN32 +#include <windows.h> +#endif + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static ErrorCode_e Protrom_Network_ReceiveHeader(Communication_t *Communication_p); +static ErrorCode_e Protrom_Network_ReceivePayload(Communication_t *Communication_p); +static ErrorCode_e Protrom_Network_TransmiterHandler(Communication_t *Communication_p); +static void Protrom_InHashCallback(void *Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p); +static void Protrom_QueueCallback(const void *const Queue_p, void *Param_p); + +/******************************************************************************* + * File scope types, constants and variables + ******************************************************************************/ +#define PROTROM_RESET_INBOUND(c, s) do { (c)->ReqData = 0; (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->Target_p = NULL; (c)->State = (s); } while(0); +#define PROTROM_SYNC_HEADER(c, d, t) do { (c)->ReqData = (d); (c)->Target_p = (t); } while(0); +#define PROTROM_SET_INBOUND(c, s, d) do { (c)->ReqData = (d); (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->State = (s); } while(0); + +/******************************************************************************* + * Definition of external functions + ******************************************************************************/ + +/* + * Initializes the PROTOROM network layer. + * + * @param [in] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + * @retval E_FAILED_TO_INIT_COM_DEVICE Failed to initialize the communication + * device. + */ +ErrorCode_e Protrom_Network_Initialize(Communication_t *Communication_p) +{ + memset(PROTROM_NETWORK(Communication_p), 0, sizeof(Protrom_NetworkContext_t)); + + /* Simulate a finished read to get the inbound state-machine going. */ + Protrom_Network_ReadCallback(NULL, 0, Communication_p); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, Protrom_QueueCallback, Communication_p); + + return E_SUCCESS; +} + +/* + * Shutdown the PROTROM network layer. + * + * @param [in] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + */ +ErrorCode_e Protrom_Network_Shutdown(const Communication_t *const Communication_p) +{ + Protrom_Inbound_t *In_p = &(PROTROM_NETWORK(Communication_p)->Inbound); + + if (NULL != In_p->Packet_p) { + if (NULL != In_p->Packet_p->Buffer_p) { + free(In_p->Packet_p->Buffer_p); + } + + free(In_p->Packet_p); + In_p->Packet_p = NULL; + } + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_EMPTY, NULL, NULL); + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_EMPTY, NULL, NULL); + + return E_SUCCESS; +} + +/* + * This callback function handles the received packets. + * + * @param [in] Data_p Pointer to the received data. + * @param [in] Length Length of the received data. + * @param [in] Param_p Extra parameteres. + * + * @return none. + */ +void Protrom_Network_ReadCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + Communication_t *Communication_p = (Communication_t *)Param_p; + + A_(printf("protrom_family.c (%d) RecLength(%d) RecBackupData (%d)\n", __LINE__, Length, PROTROM_NETWORK(Communication_p)->Inbound.RecBackupData);) + PROTROM_NETWORK(Communication_p)->Inbound.RecData = Length + PROTROM_NETWORK(Communication_p)->Inbound.RecBackupData; + PROTROM_NETWORK(Communication_p)->Inbound.RecBackupData = 0; + + if (PROTROM_NETWORK(Communication_p)->Inbound.ReqData == 0) { + ErrorCode_e ReturnValue = E_GENERAL_COMMUNICATION_ERROR; + + if (PROTROM_NETWORK(Communication_p)->Inbound.StopTransfer) { + if (PROTROM_NETWORK(Communication_p)->Inbound.PacketsBeforeReceiverStop){ + PROTROM_NETWORK(Communication_p)->Inbound.PacketsBeforeReceiverStop--; + } + else { + PROTROM_NETWORK(Communication_p)->Inbound.State = PROTROM_RECEIVE_IDLE; + } + } + + switch (PROTROM_NETWORK(Communication_p)->Inbound.State) { + + case PROTROM_RECEIVE_IDLE: + ReturnValue = E_SUCCESS; + break; + + case PROTROM_RECEIVE_HEADER: + ReturnValue = Protrom_Network_ReceiveHeader(Communication_p); + break; + + case PROTROM_RECEIVE_PAYLOAD: + ReturnValue = Protrom_Network_ReceivePayload(Communication_p); + break; + + default: + PROTROM_NETWORK(Communication_p)->Inbound.State = PROTROM_RECEIVE_HEADER; + PROTROM_NETWORK(Communication_p)->Inbound.RecData = 0; + PROTROM_NETWORK(Communication_p)->Inbound.ReqData = PROTROM_HEADER_LENGTH; + PROTROM_NETWORK(Communication_p)->Inbound.Target_p = PROTROM_NETWORK(Communication_p)->Inbound.Scratch; + return; + } + + if (E_SUCCESS != ReturnValue) { + PROTROM_NETWORK(Communication_p)->Inbound.State = PROTROM_RECEIVE_ERROR; + return; + } + } +} + +/* + * This function checks if new data has been received. + * + * @param [in] Communication_p Communication module context. + * + * @return none. + */ +void Protrom_Network_ReceiverHandler(Communication_t *Communication_p) +{ + uint32 ReqData; + Protrom_Inbound_t *In_p = &(PROTROM_NETWORK(Communication_p)->Inbound); + + /* new data for receiving ? */ + if (In_p->ReqData > 0) { + if (Communication_p->BackupCommBufferSize != 0) { + if (Communication_p->BackupCommBufferSize < In_p->ReqData) { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, Communication_p->BackupCommBufferSize); + In_p->RecBackupData = Communication_p->BackupCommBufferSize; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + In_p->ReqBuffOffset + Communication_p->BackupCommBufferSize, + In_p->ReqData - Communication_p->BackupCommBufferSize, + Protrom_Network_ReadCallback, Communication_p); + C_(printf("protrom_network.c (%d) ReqData(%d) RecData(%d)\n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("protrom_network.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + Communication_p->BackupCommBufferSize = 0; + In_p->ReqData = 0; + } else { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, In_p->ReqData); + Communication_p->BackupCommBufferSize = Communication_p->BackupCommBufferSize - In_p->ReqData; + ReqData = In_p->ReqData; + In_p->ReqData = 0; + Protrom_Network_ReadCallback(In_p->Target_p + In_p->ReqBuffOffset, ReqData, Communication_p); + In_p->RecData = 0; + } + } else { + ReqData = In_p->ReqData; + In_p->ReqData = 0; + In_p->RecData = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + In_p->ReqBuffOffset, ReqData, Protrom_Network_ReadCallback, Communication_p); + C_(printf("protrom_network.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("protrom_network.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->ReqBuffOffset = 0; + } + } + + /* check for receiver sinhronization */ + if (In_p->State == PROTROM_RECEIVE_ERROR) { + In_p->ReqData = 0; + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p, PROTROM_HEADER_LENGTH, Protrom_Network_ReadCallback, Communication_p); + In_p->State = PROTROM_RECEIVE_HEADER; + } +} + +/* + * Handler function that is called after successful transmission + * of a packet. + * + * If new packet is ready for transmitting it starts + * the transmission of the packet. + * + * @param [in] Data_p Pointer to the data for transmitting. + * @param [in] Length Length of the received data. + * @param [in] Param_p Extra parameters. + * + * @return none. + */ +void Protrom_Network_WriteCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + Communication_t *Communication_p = (Communication_t *)Param_p; + Protrom_Outbound_t *Out_p = &(PROTROM_NETWORK(Communication_p)->Outbound); + + if (NULL != Out_p->Packet_p) { + if (NULL != Out_p->Packet_p->Buffer_p) { + free(Out_p->Packet_p->Buffer_p); + } + + free(Out_p->Packet_p); + Out_p->Packet_p = NULL; + } + + Out_p->State = PROTROM_SEND_IDLE; + + /* check for more stuff to send. */ + (void)Protrom_Network_TransmiterHandler(Communication_p); +} + + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + +static ErrorCode_e Protrom_Network_ReceiveHeader(Communication_t *Communication_p) +{ + Protrom_Inbound_t *In_p = &(PROTROM_NETWORK(Communication_p)->Inbound); + + if (In_p->RecData == 0) { + In_p->ReqData = PROTROM_HEADER_LENGTH; + In_p->Target_p = In_p->Scratch; + In_p->ReqBuffOffset = 0; + } else { + if (Protrom_IsReceivedHeader(In_p)) { + if (Protrom_IsValidHeader(In_p->Scratch)) { + In_p->Packet_p = (Protrom_Packet_t *)malloc(sizeof(Protrom_Packet_t)); + + if (NULL == In_p->Packet_p) { + return E_ALLOCATE_FAILED; + } + + In_p->Packet_p->Communication_p = Communication_p; + Protrom_DeserializeHeader(&In_p->Packet_p->Header, In_p->Scratch); + In_p->Packet_p->Buffer_p = (uint8 *)malloc(In_p->Packet_p->Header.PayloadLength + PROTROM_HEADER_LENGTH + PROTROM_CRC_LENGTH); + + if (NULL == In_p->Packet_p->Buffer_p) { + return E_ALLOCATE_FAILED; + } + + In_p->Target_p = In_p->Packet_p->Buffer_p; + memcpy(In_p->Target_p, In_p->Scratch, PROTROM_HEADER_LENGTH); + In_p->Target_p += PROTROM_HEADER_LENGTH; + PROTROM_SET_INBOUND(In_p, PROTROM_RECEIVE_PAYLOAD, In_p->Packet_p->Header.PayloadLength + PROTROM_CRC_LENGTH); + } else { + /* Sync the header. */ + PROTROM_RESET_INBOUND(In_p, PROTROM_RECEIVE_HEADER); + PROTROM_SYNC_HEADER(In_p, PROTROM_HEADER_LENGTH, In_p->Scratch); + } + } + } + + return E_SUCCESS; +} + + +static ErrorCode_e Protrom_Network_ReceivePayload(Communication_t *Communication_p) +{ + Protrom_Inbound_t *In_p = &(PROTROM_NETWORK(Communication_p)->Inbound); + Protrom_Packet_t *Packet_p = In_p->Packet_p; + + Packet_p->Communication_p = Communication_p; + Communication_p->HashDevice_p->Calculate(OBJECT_HASH(Communication_p), + Communication_p->CurrentFamilyHash, Packet_p->Buffer_p, + Packet_p->Header.PayloadLength + PROTROM_HEADER_LENGTH, + (uint8 *)&Packet_p->CRC, (HashCallback_t)Protrom_InHashCallback, + (void *)Packet_p); + + In_p->Packet_p = NULL; + PROTROM_RESET_INBOUND(In_p, PROTROM_RECEIVE_HEADER); + PROTROM_SYNC_HEADER(In_p, PROTROM_HEADER_LENGTH, In_p->Scratch); + return E_SUCCESS; +} + + +static ErrorCode_e Protrom_Network_TransmiterHandler(Communication_t *Communication_p) +{ + Protrom_Outbound_t *Out_p = &(PROTROM_NETWORK(Communication_p)->Outbound); + + if (Out_p->InLoad) { + return E_SUCCESS; + } + + Out_p->InLoad = TRUE; + + switch (Out_p->State) { + case PROTROM_SEND_IDLE: + Out_p->Packet_p = (Protrom_Packet_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p); + + if (NULL != Out_p->Packet_p) { + /* get next packet for transmiting */ + Out_p->State = PROTROM_SEND_PACKET; + } else { + break; + } + + /* FALLTHROUGH */ + + case PROTROM_SEND_PACKET: + Out_p->State = PROTROM_SENDING_PACKET; + + if (E_SUCCESS != Communication_p->CommunicationDevice_p->Write(Out_p->Packet_p->Buffer_p, + PROTROM_HEADER_LENGTH + Out_p->Packet_p->Header.PayloadLength + PROTROM_CRC_LENGTH, + Protrom_Network_WriteCallback, Communication_p)) { + Out_p->State = PROTROM_SEND_PACKET; + break; + } + + /* FALLTHROUGH */ + + case PROTROM_SENDING_PACKET: + break; + + } + + Out_p->InLoad = FALSE; + + return E_SUCCESS; +} + +static void Protrom_QueueCallback(const void *const Queue_p, void *Param_p) +{ + (void)Protrom_Network_TransmiterHandler((Communication_t *)Param_p); +} + +static void Protrom_InHashCallback(void *Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p) +{ + Protrom_Packet_t *Packet_p = (Protrom_Packet_t *)Param_p; + + if (memcmp(Hash_p, Packet_p->Buffer_p + PROTROM_HEADER_LENGTH + Packet_p->Header.PayloadLength, sizeof(uint16)) == 0) { + (void)QUEUE(Packet_p->Communication_p, FifoEnqueue_Fn)(OBJECT_QUEUE(Packet_p->Communication_p), Packet_p->Communication_p->Inbound_p, Packet_p); + } else { + /* Invalid packet */ + free(Packet_p->Buffer_p); + free(Packet_p); + } +} + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_header.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_header.c new file mode 100644 index 0000000..8058506 --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_header.c @@ -0,0 +1,325 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup r15_family + * @{ + * @addtogroup ldr_header + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include "c_system.h" +#include <string.h> +#include "t_basicdefinitions.h" +#include "r_r15_header.h" +#include "r_serialization.h" +#include "t_bulk_protocol.h" +#include "t_r15_network_layer.h" + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static uint8 memxor(const uint8 *Buf_p, uint32 Length); +static uint8 R15_FindHeaderPattern(R15_Inbound_t *In_p, uint32 *StartInBuffer_p); + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + +/* + * Deserialize the header stored in network format to a structure. + * + * @param [out] Header_p Pointer to the header structure where the + * header data should be placed. + * @param [in] Data_p Pointer to the buffer with received header. + * + * @return none. + */ +void R15_DeserializeHeader(R15_Header_t *Header_p, void *Data_p) +{ + void *Iter_p = Data_p; + + Header_p->HeaderPattern = get_uint8(&Iter_p); + Header_p->Protocol = get_uint8(&Iter_p); + Header_p->HeaderPatternExtension = get_uint16(&Iter_p); + Header_p->Flags = get_uint8(&Iter_p); + Header_p->ExtendedHeaderLength = get_uint8(&Iter_p); + Header_p->ExtendedHeaderChecksum = get_uint8(&Iter_p); + Header_p->PayloadLength = get_uint32_le(&Iter_p); + Header_p->PayloadChecksum = get_uint32_le(&Iter_p); + Header_p->HeaderChecksum = get_uint8(&Iter_p); +} + +/** + * Deserialize the extended header stored in network format to a structure. + * + * @param [out] Header_p Pointer to the extended header structure where the + * header data should be placed. + * @param [in] Data_p Pointer to the buffer with received header. + * + * @return none. + */ +void R15_DeserializeBulkExtendedHeader(BulkExtendedHeader_t *ExtendedHeader_p, void *Data_p) +{ + void *Iter_p = Data_p; + + ExtendedHeader_p->Session = get_uint16_le(&Iter_p); + ExtendedHeader_p->TypeFlags = get_uint8(&Iter_p); + ExtendedHeader_p->AcksChunk = get_uint8(&Iter_p); + ExtendedHeader_p->ChunkSize = get_uint32_le(&Iter_p); + ExtendedHeader_p->Offset = get_uint64_le(&Iter_p); + ExtendedHeader_p->Length = get_uint32_le(&Iter_p); +} + +/* + * Serialize the header structure to network format. + * + * The data buffer must be of at least size HEADER_LENGTH. + * + * @param [out] Data_p Pointer to the buffer where the + * serialized header should be placed. + * @param [in] Header_p Pointer to the source header structure. + * + * @return none. + */ +void R15_SerializeHeader(void *Data_p, const R15_Header_t *header_p) +{ + void *Iter_p = Data_p; + + put_uint8(&Iter_p, HEADER_PATTERN); + put_uint8(&Iter_p, header_p->Protocol); + put_uint8(&Iter_p, HEADER_PATTERN_EXTENSION); + put_uint8(&Iter_p, HEADER_PATTERN_EXTENSION); + put_uint8(&Iter_p, header_p->Flags); + + put_uint8(&Iter_p, header_p->ExtendedHeaderLength); + put_uint8(&Iter_p, header_p->ExtendedHeaderChecksum); + + put_uint32_le(&Iter_p, header_p->PayloadLength); + put_uint32_le(&Iter_p, header_p->PayloadChecksum); + + put_uint8(&Iter_p, memxor((uint8 *)Data_p, HEADER_LENGTH - 1)); +} + + +/* + * Converts the extended header structure to network format. + * + * @param [out] Data_p Pointer to the buffer where the + * serialized header should be placed. + * @param [in] ProtocolType The type of the protocol used for + * communication. + * @param [in] ExtHeader_p Pointer to the source header structure. + * @param [out] ExtHdrChecksum_p Calculated checksum for extended header data. + * + * @return none. + */ +void R15_SerializeExtendedHeader(void *Data_p, uint8 ProtocolType, const void *ExtHeader_p, uint8 *ExtHdrChecksum_p) +{ + void *Iter_p = Data_p; + + if (COMMAND_PROTOCOL == ProtocolType) { + put_uint16_le(&Iter_p, ((CommandExtendedHeader_t *)ExtHeader_p)->SessionState); + put_uint8(&Iter_p, ((CommandExtendedHeader_t *)ExtHeader_p)->Command); + put_uint8(&Iter_p, ((CommandExtendedHeader_t *)ExtHeader_p)->CommandGroup); + *ExtHdrChecksum_p = memxor((uint8 *)ExtHeader_p, COMMAND_EXTENDED_HEADER_LENGTH); + } else { + put_uint16_le(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->Session); + put_uint8(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->TypeFlags); + put_uint8(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->AcksChunk); + put_uint32_le(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->ChunkSize); + put_uint64_le(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->Offset); + put_uint32_le(&Iter_p, ((BulkExtendedHeader_t *)ExtHeader_p)->Length); + *ExtHdrChecksum_p = memxor((uint8 *)ExtHeader_p, BULK_EXTENDED_HEADER_LENGTH); + } +} + +/** + * Deserialize the command extended header stored in network format to a structure. + * + * @param [out] CommandExtendedHeader_p Pointer to the command extended header structure where the + * header data should be placed. + * @param [in] Data_p Pointer to the buffer with received header. + * + * @return none. + */ +void R15_DeserializeCommandExtendedHeader(CommandExtendedHeader_t *CommandExtendedHeader_p, void *Data_p) +{ + void *Iter_p = Data_p; + + CommandExtendedHeader_p->SessionState = get_uint16_le(&Iter_p); + CommandExtendedHeader_p->Command = get_uint8(&Iter_p); + CommandExtendedHeader_p->CommandGroup = get_uint8(&Iter_p); +} + +/* + * Get packet length in bytes given the information in Header_p. + * + * @param [in] Header_p Pointer to the header structure. + * + * @return The length of the packet in bytes. + */ +uint32 R15_GetPacketLength(const R15_Header_t *Header_p) +{ + return HEADER_LENGTH + Header_p->ExtendedHeaderLength + + Header_p->PayloadLength; +} + +/* + * Determines whether the first HEADER_LENGTH bytes of Data_p contains a + * valid header. + * + * @param [in] Data_p Pointer to the header candidate. + * + * @retval TRUE If header is valid. + * @retval FALSE If header is not valid. + */ +boolean R15_IsValidHeader(const void *Data_p) +{ + if (memxor((uint8 *)Data_p, HEADER_LENGTH) == 0) { + /** @todo It's possible to check other things here */ + return TRUE; + } + + return FALSE; +} + +/* + * Checks if new header is received. + * + * @param [in] In_p Pointer to the structure with receiver information. + * + * @retval TRUE If header is received. + * @retval FALSE If header is not received. + */ +boolean R15_IsReceivedHeader(R15_Inbound_t *In_p) +{ + uint32 StartHeaderInBuffer = 0; + uint8 *TmpPointer_p = NULL; + uint8 Res = 0; + + Res = R15_FindHeaderPattern(In_p, &StartHeaderInBuffer); + + if (HEADER_PATTERN_CANDIDATE == Res) { + /* call for receiving the rest bytes in header */ + In_p->ReqData = StartHeaderInBuffer + ALIGNED_HEADER_LENGTH - In_p->RecData; + TmpPointer_p = (uint8 *)((uint32)In_p->Target_p + StartHeaderInBuffer); + memcpy(In_p->Target_p, (uint8 *)TmpPointer_p, In_p->RecData - StartHeaderInBuffer); + In_p->ReqBuffOffset = In_p->RecData - StartHeaderInBuffer; + } else { + if (HEADER_PATTERN_MATCH == Res) { + /* Check start point of header in received data */ + if (StartHeaderInBuffer == 0) { + return TRUE; + } else { + In_p->ReqData = StartHeaderInBuffer + ALIGNED_HEADER_LENGTH - In_p->RecData; + TmpPointer_p = (uint8 *)((uint32)In_p->Target_p + StartHeaderInBuffer); + memcpy(In_p->Target_p, (uint8 *)TmpPointer_p, In_p->RecData - StartHeaderInBuffer); + In_p->ReqBuffOffset = In_p->RecData - StartHeaderInBuffer; + } + } else { + In_p->ReqData = ALIGNED_HEADER_LENGTH; + In_p->ReqBuffOffset = 0; + } + } + + return FALSE; +} + + +/* + * Verify received extended header. + * + * @param [in] Data_p Pointer to the buffer. + * @param [in] Length Length of the buffer. + * @param [in] Checksum Checksum of the buffer. + * + * @retval TRUE If is extended header verified. + * @retval FALSE If extended header is not verified. + */ +boolean R15_IsValidExtendedHeader(const void *Data_p, const uint8 Length, const uint8 Checksum) +{ + if (memxor((uint8 *)Data_p, Length) == Checksum) { + return TRUE; + } + + return FALSE; +} + + +static uint8 R15_FindHeaderPattern(R15_Inbound_t *In_p, uint32 *StartInBuffer_p) +{ + uint8 Res = NO_HEADER_PATTERN; + uint32 Offset = 0; + uint8 *HeaderData_p = In_p->Scratch; + + do { + Res = NO_HEADER_PATTERN; + + if (*((uint8 *)HeaderData_p + Offset) == HEADER_PATTERN) { + Res = HEADER_PATTERN_CANDIDATE; + + if (Offset < In_p->RecData - 1) { + if ((*((uint8 *)HeaderData_p + Offset + 1) == PROTO_COMMAND) || (*((uint8 *)HeaderData_p + Offset + 1) == PROTO_BULK)) { + if (Offset < In_p->RecData - 2) { + if (*((uint8 *)HeaderData_p + Offset + 2) == HEADER_PATTERN_EXTENSION) { + if (Offset < In_p->RecData - 3) { + if (*((uint8 *)HeaderData_p + Offset + 3) == HEADER_PATTERN_EXTENSION) { + Res = HEADER_PATTERN_MATCH; + break; + } else { + Offset = Offset + 2; + Res = NO_HEADER_PATTERN; + } + } else { + Res = HEADER_PATTERN_CANDIDATE; + break; + } + } else { + Offset ++; + Res = NO_HEADER_PATTERN; + } + } else { + Res = HEADER_PATTERN_CANDIDATE; + break; + } + } else { + Res = NO_HEADER_PATTERN; + } + } else { + Res = HEADER_PATTERN_CANDIDATE; + break; + } + } + + Offset++; + } while (Offset < In_p->RecData); + + *StartInBuffer_p = Offset; + + return Res; +} + + +static uint8 memxor(const uint8 *Buf_p, uint32 Length) +{ + uint8 Value = 0; + const uint8 *Stop_p = Buf_p + Length; + + /** @todo Optimize for 32 bit access? */ + do { + Value ^= *Buf_p++; + } while (Buf_p < Stop_p); + + return Value; +} + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_network_layer.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_network_layer.c new file mode 100644 index 0000000..3b3a2fc --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/r15_network_layer.c @@ -0,0 +1,931 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup r15_family + * @{ + * @addtogroup ldr_network_layer + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include "r_r15_network_layer.h" +#include "r_basicdefinitions.h" +#include <stdlib.h> +#include "r_debug.h" +#include "r_debug_macro.h" +#include <string.h> +#include "c_system.h" +#include "r_r15_transport_layer.h" +#include "r_r15_family.h" +#include "r_communication_service.h" +#include "r_r15_header.h" +#include "r_bulk_protocol.h" +#include "r_command_protocol.h" + +#ifdef CFG_ENABLE_MEASUREMENT_TOOL +#include "r_measurement_tool.h" +#include "r_time_utilities.h" +#endif + +#define FREE_TRANSMITER 0 +#define BUSY_TRANSMITER 1 + +#ifdef CFG_ENABLE_MEASUREMENT_TOOL +extern Measurement_t *Measurement_p; +#endif + +static PacketMeta_t PacketMetaInfo[COMMAND_BUFFER_COUNT + BULK_BUFFER_COUNT] = {0}; /* Packet Meta Info vector*/ + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ +static ErrorCode_e R15_Network_ReceiveHeader(const Communication_t *const Communication_p); +static ErrorCode_e R15_Network_ReceiveExtendedHeader(Communication_t *Communication_p); +static ErrorCode_e R15_Network_ReceivePayload(Communication_t *Communication_p); +static ErrorCode_e R15_Network_RegisterRetransmission(Communication_t *Communication_p, PacketMeta_t *Packet_p); +static void R15_Network_RetransmissionCallback(Communication_t *Communication_p, const void *const Timer_p, void *Data_p); +static void R15_InHashCallback(const void *const Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p); +#ifdef CFG_ENABLE_LOADER_TYPE +static void R15_QueueOutCallback(const void *const Queue_p, void *Param_p); +static void R15_QueueInCallback(const void *const Queue_p, void *Param_p); +#endif +static PacketMeta_t *R15_Network_GetAvailableMetaPacket(void); + + +/******************************************************************************* + * File scope types, constants and variables + ******************************************************************************/ + +#define RESET_INBOUND(c, s) do { (c)->ReqData = 0; (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->Target_p = NULL; (c)->State = (s); } while(0); +#define SYNC_HEADER(c, d, t) do { (c)->ReqData = d; (c)->Target_p = t; } while(0); +#define SET_INBOUND(c, s, d) do { (c)->ReqData = d; (c)->RecData = 0; (c)->ReqBuffOffset = 0; (c)->State = (s); } while(0); + +/******************************************************************************* + * Definition of external functions + ******************************************************************************/ + +/* + * Initializes the r15 network layer. + * + * @param [in,out] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + * @retval E_FAILED_TO_INIT_COM_DEVICE Fail to initialize the communication + * device. + */ +ErrorCode_e R15_Network_Initialize(Communication_t *Communication_p) +{ + memset(R15_NETWORK(Communication_p), 0, sizeof(R15_NetworkContext_t)); + + /* Simulate a finished read to get the inbound state-machine going. */ + R15_Network_ReadCallback(NULL, 0, Communication_p); + R15_NETWORK(Communication_p)->Outbound.InLoad = FALSE; +#ifdef CFG_ENABLE_LOADER_TYPE + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, R15_QueueOutCallback, Communication_p); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_NONEMPTY, R15_QueueInCallback, Communication_p); +#endif + R15_NETWORK(Communication_p)->Inbound.LCM_Error = E_SUCCESS; + R15_NETWORK(Communication_p)->Outbound.LCM_Error = E_SUCCESS; + + return E_SUCCESS; +} + +/* + * Shut down the r15 network layer. + * + * @param [in,out] Communication_p Communication module context. + * + * @retval E_SUCCESS After successful execution. + */ +ErrorCode_e R15_Network_Shutdown(const Communication_t *const Communication_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, QUEUE_EMPTY, NULL, NULL); + + /* Wait until the all packets in the queue has released. */ + while (!QUEUE(Communication_p, Fifo_IsEmpty_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p)) { + PacketMeta_t *Packet_p = (PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p); + BulkExtendedHeader_t ExtendedHeader = {0}; + + R15_DeserializeBulkExtendedHeader(&ExtendedHeader, Packet_p->ExtendedHeader_p); + + if (Packet_p->Header.Protocol != BULK_PROTOCOL || (ExtendedHeader.TypeFlags & MASK_BULK_COMMAND_SELECT) != CMD_BULK_DATA) { + ReturnValue = R15_Network_PacketRelease(Communication_p, Packet_p); + VERIFY(E_SUCCESS == ReturnValue, ReturnValue); + } + } + + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_NONEMPTY, NULL, NULL); + (void)QUEUE(Communication_p, Fifo_SetCallback_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, QUEUE_EMPTY, NULL, NULL); + + /* Wait until the all packets in the queue has released. */ + while (!QUEUE(Communication_p, Fifo_IsEmpty_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p)) { + ReturnValue = R15_Network_PacketRelease(Communication_p, (PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p)); + } + +ErrorExit: + return ReturnValue; +} + + +/* + * Handler for received packets in R15 protocol family. + * + * This callback function handles the received packets. + * + * @param [in] Data_p Pointer to the received data. + * @param [in] Length Length of the received data. + * @param [in] Param_p Parameters; + * + * @return none. + */ +void R15_Network_ReadCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + Communication_t *Communication_p = (Communication_t *)Param_p; + + C_(printf("r15_network_layer.c (%d) RecLength(%d) RecBackupData (%d)\n", __LINE__, Length, R15_NETWORK(Communication_p)->Inbound.RecBackupData);) + R15_NETWORK(Communication_p)->Inbound.RecData = Length + R15_NETWORK(Communication_p)->Inbound.RecBackupData; + R15_NETWORK(Communication_p)->Inbound.RecBackupData = 0; + + if (R15_NETWORK(Communication_p)->Inbound.ReqData == 0) { + ErrorCode_e ReturnValue = E_GENERAL_COMMUNICATION_ERROR; + + switch (R15_NETWORK(Communication_p)->Inbound.State) { + case RECEIVE_HEADER: + ReturnValue = R15_Network_ReceiveHeader(Communication_p); + break; + + case RECEIVE_EXTENDED_HEADER: + ReturnValue = R15_Network_ReceiveExtendedHeader(Communication_p); + break; + + case RECEIVE_PAYLOAD: + ReturnValue = R15_Network_ReceivePayload(Communication_p); + break; + + default: + R15_NETWORK(Communication_p)->Inbound.State = RECEIVE_HEADER; + R15_NETWORK(Communication_p)->Inbound.RecData = 0; + R15_NETWORK(Communication_p)->Inbound.ReqData = ALIGNED_HEADER_LENGTH; + R15_NETWORK(Communication_p)->Inbound.Target_p = R15_NETWORK(Communication_p)->Inbound.Scratch; + R15_NETWORK(Communication_p)->Inbound.LCM_Error = E_SUCCESS; + ReturnValue = E_SUCCESS; + break; + } + +#ifdef CFG_ENABLE_LOADER_TYPE + ReturnValue = R15_Network_ReceiverHandler(Communication_p); +#endif + + if (E_SUCCESS != ReturnValue) { + R15_NETWORK(Communication_p)->Inbound.State = RECEIVE_ERROR; + } + + R15_NETWORK(Communication_p)->Inbound.LCM_Error = ReturnValue; + } +} + +/* + * Handler for receiving new data. + * + * This function checks if new data has been received. + * + * @param [in,out] Communication_p Communication module context. + * + * @return none. + */ +ErrorCode_e R15_Network_ReceiverHandler(Communication_t *Communication_p) +{ + uint32 ReqData; + uint32 ReqBuffOffset = 0; + R15_Inbound_t *In_p = &(R15_NETWORK(Communication_p)->Inbound); + + /* new data for receiving ? */ + if (In_p->ReqData > 0) { + if (Communication_p->BackupCommBufferSize != 0) { + if (Communication_p->BackupCommBufferSize < In_p->ReqData) { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, Communication_p->BackupCommBufferSize); + In_p->RecBackupData = Communication_p->BackupCommBufferSize; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + In_p->ReqBuffOffset + Communication_p->BackupCommBufferSize, + In_p->ReqData - Communication_p->BackupCommBufferSize, + R15_Network_ReadCallback, Communication_p); + C_(printf("r15_network_layer.c (%d) ReqData(%d) RecData(%d)\n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("r15_network_layer.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + Communication_p->BackupCommBufferSize = 0; + In_p->ReqData = 0; + } else { + memcpy(In_p->Target_p + In_p->ReqBuffOffset, Communication_p->BackupCommBuffer_p, In_p->ReqData); + Communication_p->BackupCommBufferSize = Communication_p->BackupCommBufferSize - In_p->ReqData; + ReqData = In_p->ReqData; + In_p->ReqData = 0; + R15_Network_ReadCallback(In_p->Target_p + In_p->ReqBuffOffset, ReqData, Communication_p); + In_p->RecData = 0; + } + } else { + ReqData = In_p->ReqData; + ReqBuffOffset = (uint32)(In_p->ReqBuffOffset); + In_p->ReqBuffOffset = 0; + In_p->ReqData = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p + ReqBuffOffset, ReqData, R15_Network_ReadCallback, Communication_p); + C_(printf("r15_network_layer.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, In_p->ReqData, In_p->RecData);) + C_(printf("r15_network_layer.c (%d) Communication_p->BackupCommBufferSize(%d) RecBackupData (%d)\n", __LINE__, Communication_p->BackupCommBufferSize, In_p->RecBackupData);) + In_p->RecData = 0; + } + } + + /* check for receiver sinhronization */ + if (In_p->State == RECEIVE_ERROR) { + In_p->ReqData = 0; + In_p->RecData = 0; + In_p->ReqBuffOffset = 0; + In_p->State = RECEIVE_HEADER; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p, ALIGNED_HEADER_LENGTH, R15_Network_ReadCallback, Communication_p); + } + + return R15_NETWORK(Communication_p)->Inbound.LCM_Error; +} + +ErrorCode_e R15_Network_TransmiterHandler(Communication_t *Communication_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + R15_Outbound_t *Out_p = &(R15_NETWORK(Communication_p)->Outbound); + uint8 *HeaderStartInBuffer_p = NULL; + boolean IsBufferContinuous = FALSE; + boolean RegisterRetransmission = FALSE; + uint32 ContinuousBufferLength = 0; + uint32 ExtHdrLen = 0; + uint32 Aligned_Length = 0; + + if (Out_p->InLoad) { + return E_SUCCESS; + } + + Out_p->InLoad = TRUE; + + switch (Out_p->State) { + case SEND_IDLE: + /* check retransmission count before send */ + Out_p->Packet_p = (PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p); + + if (NULL != Out_p->Packet_p) { + if (Out_p->Packet_p->Resend < MAX_RESENDS) { + Out_p->Packet_p->Resend++; + /* get next packet for transmitting */ + Out_p->State = SEND_HEADER; + } else { + //Do_CommunicationInternalErrorHandler(E_RETRANSMITION_FAILED); + return E_RETRANSMITION_FAILED; + } + } else { + break; + } + + /* FALLTHROUGH */ + case SEND_HEADER: + HeaderStartInBuffer_p = Out_p->Packet_p->Buffer_p + HEADER_OFFSET_IN_BUFFER; + + if (Out_p->Packet_p->Header.ExtendedHeaderLength == COMMAND_EXTENDED_HEADER_LENGTH) { + ExtHdrLen = ALIGNED_COMMAND_EXTENDED_HEADER_LENGTH; + } else { + ExtHdrLen = ALIGNED_BULK_EXTENDED_HEADER_LENGTH; + } + + if ((HeaderStartInBuffer_p + ALIGNED_HEADER_LENGTH + ExtHdrLen) == Out_p->Packet_p->Payload_p) { + /* end of the header and beginning of the payload are same */ + IsBufferContinuous = TRUE; + } + + /* set next state before calling the communication device, to avoid race condition + where write callback is called before new state is set */ + if (IsBufferContinuous || Out_p->Packet_p->Header.PayloadLength == 0) { + /* we have a packet without payload or header and payload are contained in one + continuous buffer so it can be sent with one write request */ + ContinuousBufferLength = ALIGNED_HEADER_LENGTH + ExtHdrLen + Out_p->Packet_p->Header.PayloadLength; + ContinuousBufferLength = (ContinuousBufferLength + (ALIGN_SIZE - 1)) & (~(ALIGN_SIZE - 1)); + + Out_p->State = SENDING_PAYLOAD; + RegisterRetransmission = TRUE; + } else { + ContinuousBufferLength = ALIGNED_HEADER_LENGTH + ExtHdrLen; + Out_p->State = SENDING_HEADER; + } + + if (E_SUCCESS == Communication_p->CommunicationDevice_p->Write((Out_p->Packet_p->Buffer_p + HEADER_OFFSET_IN_BUFFER), + ContinuousBufferLength, + R15_Network_WriteCallback, Communication_p)) { + C_(printf("r15_network_layer.c (%d) Header Sent to comm device! \n", __LINE__);) + } else { + Out_p->State = SEND_HEADER; + RegisterRetransmission = FALSE; + C_(printf("r15_network_layer.c (%d) Error sending header to comm device! \n", __LINE__);) + } + + break; + case SENDING_HEADER: + /* nothing to do, wait until sending is finished and state changed by write callback */ + break; + case SEND_PAYLOAD: + Out_p->State = SENDING_PAYLOAD; + + Aligned_Length = (Out_p->Packet_p->Header.PayloadLength + (ALIGN_SIZE - 1)) & (~(ALIGN_SIZE - 1)); + + if (E_SUCCESS == Communication_p->CommunicationDevice_p->Write(Out_p->Packet_p->Payload_p, + Aligned_Length, + R15_Network_WriteCallback, Communication_p)) { + RegisterRetransmission = TRUE; + C_(printf("r15_network_layer.c (%d) Payload Sent to comm device! \n", __LINE__);) + } else { + Out_p->State = SEND_PAYLOAD; + C_(printf("r15_network_layer.c (%d) Error sending payload to comm device! \n", __LINE__);) + } + + break; + case SENDING_PAYLOAD: + /* nothing to do, wait until sending is finished and state changed by write callback */ + break; + } + + if (RegisterRetransmission) { + if (NULL != Out_p->Packet_p->Timer_p) { + C_(printf("r15_network_layer.c (%d) Register retransmission\n", __LINE__);) + (void)R15_Network_RegisterRetransmission(Communication_p, Out_p->Packet_p); + } + } + + Out_p->InLoad = FALSE; + + return ReturnValue; +} + +/* + * Cancel retransmission of packets. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] UniqueKey Unique key used for identification of packet + * for retransmission. + * + * @return none. + */ +ErrorCode_e R15_Network_CancelRetransmission(const Communication_t *const Communication_p, uint32 UniqueKey) +{ + ErrorCode_e ReturnValue = E_NOT_FOUND_ELEMENT_IN_RETRANSMISSION_LIST; + uint32 Index = 0; + + do { + if ((R15_NETWORK(Communication_p)->RetransmissionList[Index] != NULL) && (R15_NETWORK(Communication_p)->RetransmissionList[Index]->Key == UniqueKey)) { + (void)TIMER(Communication_p, TimerRelease_Fn)(OBJECT_TIMER(Communication_p), R15_NETWORK(Communication_p)->RetransmissionList[Index]->TimerKey); + + free(R15_NETWORK(Communication_p)->RetransmissionList[Index]->Packet_p->Timer_p); + ReturnValue = R15_Network_PacketRelease(Communication_p, R15_NETWORK(Communication_p)->RetransmissionList[Index]->Packet_p); + + if (E_SUCCESS != ReturnValue) { + A_(printf("r15_network_layer.c(%d): Packet release failed!\n", __LINE__);) + return ReturnValue; + } + + free(R15_NETWORK(Communication_p)->RetransmissionList[Index]); + R15_NETWORK(Communication_p)->RetransmissionList[Index] = NULL; + R15_NETWORK(Communication_p)->RetransmissionListCount--; + + for (; Index < R15_NETWORK(Communication_p)->RetransmissionListCount; Index++) { + R15_NETWORK(Communication_p)->RetransmissionList[Index] = R15_NETWORK(Communication_p)->RetransmissionList[Index+1]; + } + + R15_NETWORK(Communication_p)->RetransmissionList[Index] = NULL; + ReturnValue = E_SUCCESS; + break; + } + + Index++; + } while (Index < R15_NETWORK(Communication_p)->RetransmissionListCount); + + return ReturnValue; +} + +/* + * Create unique key. + * + * This function combine the session number and protocol type in one + * unique key for command packet. For the bulk packet the unique key is the + * combination of protocol type, session number and command. + * + * @param [in] Packet_p Pointer to the packet. + * @param [in] ExternalKey External key for marking the packet for retransmision. + * + * @return Unique key. + */ +uint32 R15_Network_CreateUniqueKey(const PacketMeta_t *const Packet_p, const uint8 ExternalKey) +{ + uint32 Key = Packet_p->Header.Protocol; + + if (Key == PROTO_COMMAND) { + return((Key << 16) | *(Packet_p->ExtendedHeader_p) & MASK_CLR_STATE); + } else { + return ((Key << 16) | ((*(Packet_p->ExtendedHeader_p) & 0x00FF) << 8) | ExternalKey); + } +} + + +/* + * Network packet allocation in R15 protocol family. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] BufferSize Size of buffer used for network packet. + * + * @retval Pointer Pointer to allocated packet meta info. + * @retval NULL if allocation fail. + */ +PacketMeta_t *R15_Network_PacketAllocate(const Communication_t *const Communication_p, int BufferSize) +{ + PacketMeta_t *Meta_p = NULL; + void *Buffer_p = NULL; + int BuffersNr = 0; + + /* Find the first unallocated buffers. */ + Buffer_p = BUFFER(Communication_p, BufferAllocate_Fn)(OBJECT_BUFFER(Communication_p), BufferSize); + + if (NULL == Buffer_p) { + A_(printf("r15_network_layer.c (%d): ** Buffer allocation fail! **\n", __LINE__);) + goto ErrorExit; + } + + B_(printf("r15_network_layer.c (%d): Buffer allocate (0x%x)! **\n", __LINE__, (uint32)Buffer_p);) + + /* packet meta info allocate, get free packet meta structure */ + + Meta_p = R15_Network_GetAvailableMetaPacket(); + + if (NULL == Meta_p) { + R15_NETWORK(Communication_p)->Inbound.LCM_Error = E_ALLOCATE_FAILED; + goto ErrorExit; + } + + /* packet meta info setup */ + C_( + + if (BULK_BUFFER_SIZE > BufferSize) + printf("CmdBuffGet:%x\n", Buffer_p); + else { + printf("ChunkBuffGet:%x\n", Buffer_p); + }) + + Meta_p->Buffer_p = (uint8 *)Buffer_p; + Meta_p->BufferSize = BufferSize; + SET_PACKET_FLAGS(Meta_p, PACKET_ALLOCATION_STATE_MASK, BUF_ALLOCATED); + + do { + if (NULL == R15_NETWORK(Communication_p)->MetaInfoList[BuffersNr]) { + R15_NETWORK(Communication_p)->MetaInfoList[BuffersNr] = Meta_p; + break; + } + + BuffersNr ++; + } while (BuffersNr < (COMMAND_BUFFER_COUNT + BULK_BUFFER_COUNT)); + +ErrorExit: + return Meta_p; +} + + +/* + * Network packet release in R15 protocol family. + * + * @param [in,out] Communication_p Communication module context. + * @param [in] Meta_p Meta info for used network packet. + * + * @retval E_SUCCESS Successful network packet release. + * @retval E_INVALID_INPUT_PARAMETERS Invalid input parameter. + */ +ErrorCode_e R15_Network_PacketRelease(const Communication_t *const Communication_p, PacketMeta_t *Meta_p) +{ + ErrorCode_e ReturnValue = E_SUCCESS; + int BuffersNr = 0; + + VERIFY(NULL != Meta_p, E_INVALID_INPUT_PARAMETERS); + + /* remove the meta info data from list */ + do { + if (Meta_p == R15_NETWORK(Communication_p)->MetaInfoList[BuffersNr]) { + R15_NETWORK(Communication_p)->MetaInfoList[BuffersNr] = NULL; + /* release buffer */ + B_(printf("r15_network_layer.c (%d): Buffer release (0x%x)! **\n", __LINE__, (uint32)(Meta_p->Buffer_p));) +#ifndef CFG_ENABLE_LOADER_TYPE + ReturnValue = BUFFER(Communication_p, BufferRelease_Fn)(OBJECT_BUFFER(Communication_p), Meta_p->Buffer_p, Meta_p->BufferSize); + VERIFY(E_SUCCESS == ReturnValue, ReturnValue); + B_(printf("r15_network_layer.c (%d): Buffer released! **\n", __LINE__);) +#else + C_( + + if (BULK_BUFFER_SIZE > Meta_p->BufferSize) + printf("CmdBuffRel:%x\n", Meta_p->Buffer_p); + else { + printf("ChunkBuffRel:%x\n", Meta_p->Buffer_p); + }) + ReturnValue = BUFFER(Communication_p, BufferRelease_Fn)(OBJECT_BUFFER(Communication_p), Meta_p->Buffer_p, Meta_p->BufferSize); + VERIFY(E_SUCCESS == ReturnValue, ReturnValue); + B_(printf("r15_network_layer.c (%d): Buffer released! **\n", __LINE__);) +#endif + + memset(Meta_p, 0, sizeof(PacketMeta_t)); + break; + } + + BuffersNr ++; + } while (BuffersNr < (COMMAND_BUFFER_COUNT + BULK_BUFFER_COUNT)); + +ErrorExit: + return ReturnValue; +} + +/* + * Handler function that is called after successful transmission of a packet. + * + * If new packet is ready for transmitting it starts + * the transmission of the packet. + * + * @param [in] Data_p Pointer to the data for transmitting. + * @param [in] Length Length of the received data. + * @param [in] Param_p Parameters. + * + * @return none. + */ +void R15_Network_WriteCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + Communication_t *Communication_p = (Communication_t *)Param_p; + R15_Outbound_t *Out_p = &(R15_NETWORK(Communication_p)->Outbound); + BulkExtendedHeader_t ExtendedHeader = {0}; + B_(printf("r15_network_layer.c (%d): Device write finished!! \n", __LINE__);) + + if (SENDING_HEADER == Out_p->State) { + Out_p->State = SEND_PAYLOAD; + } else if (SENDING_PAYLOAD == Out_p->State) { + if (NULL == Out_p->Packet_p->Timer_p) { + if (Out_p->Packet_p->Header.Protocol == BULK_PROTOCOL) { + R15_DeserializeBulkExtendedHeader(&ExtendedHeader, Out_p->Packet_p->ExtendedHeader_p); + } + + if (!((Out_p->Packet_p->Header.Protocol == BULK_PROTOCOL) && ((ExtendedHeader.TypeFlags & MASK_BULK_COMMAND_SELECT) == CMD_BULK_DATA))) { + (void)R15_Network_PacketRelease(Communication_p, Out_p->Packet_p); + } + } + + Out_p->State = SEND_IDLE; + SET_PACKET_FLAGS(Out_p->Packet_p, PACKET_TX_STATE_MASK, BUF_TX_SENT); + } + +#ifdef CFG_ENABLE_LOADER_TYPE + + if (E_SUCCESS != R15_Network_TransmiterHandler(Communication_p)) { + R15_NETWORK(Communication_p)->Outbound.LCM_Error = E_GENERAL_COMMUNICATION_ERROR; + } + +#endif +} + + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + +static ErrorCode_e R15_Network_ReceiveHeader(const Communication_t *const Communication_p) +{ + R15_Inbound_t *In_p = &(R15_NETWORK(Communication_p)->Inbound); + + if (In_p->RecData == 0) { + In_p->ReqData = ALIGNED_HEADER_LENGTH; + In_p->Target_p = In_p->Scratch; + } else { + if (R15_IsReceivedHeader(In_p)) { + if (R15_IsValidHeader(In_p->Scratch)) { + R15_DeserializeHeader(&In_p->Header, In_p->Scratch); + + In_p->Target_p += ALIGNED_HEADER_LENGTH; + if (In_p->Header.ExtendedHeaderLength == COMMAND_EXTENDED_HEADER_LENGTH) { + SET_INBOUND(In_p, RECEIVE_EXTENDED_HEADER, ALIGNED_COMMAND_EXTENDED_HEADER_LENGTH); + } else { + SET_INBOUND(In_p, RECEIVE_EXTENDED_HEADER, ALIGNED_BULK_EXTENDED_HEADER_LENGTH); + } + } else { + A_(printf("Not valid header!\n");) + /* Sync the header. */ + RESET_INBOUND(In_p, RECEIVE_HEADER); + SYNC_HEADER(In_p, ALIGNED_HEADER_LENGTH, In_p->Scratch); + } + } + else + { + uint32 Counter = 0; + + A_(printf("Invalid header! ");) + for (Counter = 0; Counter < 16; Counter++) + { + A_(printf(" %02X", In_p->Scratch[Counter]);) + } + A_(printf("\n\n");) + } + } + + return E_SUCCESS; +} + +static ErrorCode_e R15_Network_ReceiveExtendedHeader(Communication_t *Communication_p) +{ + R15_Inbound_t *In_p = &(R15_NETWORK(Communication_p)->Inbound); + ErrorCode_e ReturnValue = E_SUCCESS; + R15_Header_t R15Header = {0}; + BulkExtendedHeader_t BulkExtendedHeader = {0}; + boolean IsValidHeader = FALSE; + + VERIFY(NULL != In_p, E_GENERAL_FATAL_ERROR); + + R15_DeserializeHeader(&R15Header, In_p->Scratch); + R15_DeserializeBulkExtendedHeader(&BulkExtendedHeader, In_p->Target_p); + + IsValidHeader = R15_IsValidExtendedHeader(In_p->Target_p, + In_p->Header.ExtendedHeaderLength, + In_p->Header.ExtendedHeaderChecksum); + + if (IsValidHeader) { + if (R15Header.Protocol == BULK_PROTOCOL && + (BulkExtendedHeader.TypeFlags & MASK_BULK_COMMAND_SELECT) == CMD_BULK_DATA) { + In_p->Packet_p = R15_Network_PacketAllocate(Communication_p, BULK_BUFFER_SIZE); + } else { + In_p->Packet_p = R15_Network_PacketAllocate(Communication_p, COMMAND_BUFFER_SIZE); + } + + VERIFY(NULL != In_p->Packet_p, E_FAILED_TO_FIND_COMM_BUFFER); + + In_p->Packet_p->Header = R15Header; + In_p->Packet_p->ExtendedHeader_p = In_p->Packet_p->Buffer_p + HEADER_OFFSET_IN_BUFFER + ALIGNED_HEADER_LENGTH; + SET_PACKET_FLAGS(In_p->Packet_p, PACKET_RX_STATE_MASK, BUF_HDR_CRC_OK); + if (In_p->Packet_p->Header.ExtendedHeaderLength == COMMAND_EXTENDED_HEADER_LENGTH) { + In_p->Packet_p->Payload_p = In_p->Packet_p->ExtendedHeader_p + ALIGNED_COMMAND_EXTENDED_HEADER_LENGTH; + } else { + In_p->Packet_p->Payload_p = In_p->Packet_p->ExtendedHeader_p + ALIGNED_BULK_EXTENDED_HEADER_LENGTH; + } + memcpy(In_p->Packet_p->Buffer_p + HEADER_OFFSET_IN_BUFFER, &In_p->Packet_p->Header, HEADER_LENGTH); + memcpy(In_p->Packet_p->ExtendedHeader_p, In_p->Target_p, In_p->Header.ExtendedHeaderLength); + + In_p->Target_p = In_p->Packet_p->Payload_p; + + /* check for expected payload */ + if (In_p->Packet_p->Header.PayloadLength != 0) { + uint32 Aligned_Size = 0; + Aligned_Size = (In_p->Packet_p->Header.PayloadLength + (ALIGN_SIZE - 1)) & (~(ALIGN_SIZE - 1)); + + SET_INBOUND(In_p, RECEIVE_PAYLOAD, Aligned_Size); + } else { +#ifdef DISABLE_SECURITY + SET_PACKET_FLAGS(In_p->Packet_p, PACKET_RX_STATE_MASK, BUF_RX_READY); + (void)QUEUE(Communication_p, FifoEnqueue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p, In_p->Packet_p); +#else + PacketMeta_t *Packet_p = In_p->Packet_p; + Packet_p->Communication_p = Communication_p; + SET_PACKET_FLAGS(In_p->Packet_p, PACKET_RX_STATE_MASK, BUF_RX_READY); + // Copy the original packet checksum to avoid hash mismatch + memcpy(Packet_p->Hash, &Packet_p->Header.PayloadChecksum, 4); + + Communication_p->HashDevice_p->Calculate(OBJECT_HASH(Communication_p), + HASH_NONE, + Packet_p->Payload_p, Packet_p->Header.PayloadLength, + Packet_p->Hash, (HashCallback_t)R15_InHashCallback, + (void *)Packet_p); +#endif + + In_p->Packet_p = NULL; + RESET_INBOUND(In_p, RECEIVE_HEADER); + SYNC_HEADER(In_p, ALIGNED_HEADER_LENGTH, In_p->Scratch); + C_(printf("r15_network_layer.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, In_p->ReqData, In_p->RecData);) + } + } else { + RESET_INBOUND(In_p, RECEIVE_HEADER); + SYNC_HEADER(In_p, ALIGNED_HEADER_LENGTH, In_p->Scratch); + } + +ErrorExit: + return ReturnValue; +} + +static ErrorCode_e R15_Network_ReceivePayload(Communication_t *Communication_p) +{ + R15_Inbound_t *In_p = &(R15_NETWORK(Communication_p)->Inbound); + PacketMeta_t *Packet_p = In_p->Packet_p; + + Packet_p->Communication_p = Communication_p; + SET_PACKET_FLAGS(In_p->Packet_p, PACKET_RX_STATE_MASK, BUF_RX_READY); + +#ifndef CFG_ENABLE_LOADER_TYPE + Communication_p->CurrentFamilyHash = (HashType_e)Packet_p->Header.Flags; +#endif + + if (HASH_NONE != Packet_p->Header.Flags) { +#ifdef CFG_ENABLE_MEASUREMENT_TOOL + if(In_p->Packet_p->Header.Protocol == BULK_PROTOCOL){ + BulkExtendedHeader_t ExtendedHeader = {0}; + R15_DeserializeBulkExtendedHeader(&ExtendedHeader, Packet_p->ExtendedHeader_p); + + if ((ExtendedHeader.TypeFlags & MASK_BULK_COMMAND_SELECT) == CMD_BULK_DATA){ + (void)MP(Measurement_p, ExtendedHeader.AcksChunk, START_HASHINGCHUNK_TIME); + } + } +#endif + Communication_p->HashDevice_p->Calculate(OBJECT_HASH(Communication_p), + Communication_p->CurrentFamilyHash, + Packet_p->Payload_p, Packet_p->Header.PayloadLength, + Packet_p->Hash, (HashCallback_t)R15_InHashCallback, + (void *)Packet_p); + } else { + SET_PACKET_FLAGS(Packet_p, PACKET_RX_STATE_MASK, BUF_PAYLOAD_CRC_OK); + (void)QUEUE((Packet_p->Communication_p), FifoEnqueue_Fn)(OBJECT_QUEUE(Packet_p->Communication_p), Packet_p->Communication_p->Inbound_p, Packet_p); + } + + In_p->Packet_p = NULL; + RESET_INBOUND(In_p, RECEIVE_HEADER); + SYNC_HEADER(In_p, ALIGNED_HEADER_LENGTH, In_p->Scratch); + return E_SUCCESS; +} + +static ErrorCode_e R15_Network_RegisterRetransmission(Communication_t *Communication_p, PacketMeta_t *Packet_p) +{ + int i; + int Index; + ErrorCode_e ReturnValue = E_SUCCESS; + static RetransmissionContext_t *R_Ctx_p = NULL; + + if (R15_NETWORK(Communication_p)->RetransmissionListCount < MAX_SIZE_RETRANSMISSION_LIST) { + if (NULL == Packet_p->Timer_p->HandleFunction_p) { + Packet_p->Timer_p->HandleFunction_p = (HandleFunction_t)R15_Network_RetransmissionCallback; + Packet_p->Timer_p->Param_p = Communication_p; + } + + R_Ctx_p = (RetransmissionContext_t *)malloc(sizeof(RetransmissionContext_t)); + + if (NULL == R_Ctx_p) { + return E_ALLOCATE_FAILED; + } + + R_Ctx_p->TimerKey = TIMER(Communication_p, TimerGet_Fn)(OBJECT_TIMER(Communication_p), Packet_p->Timer_p); + R_Ctx_p->Timeout = Packet_p->Timer_p->Time; + R_Ctx_p->Packet_p = Packet_p; + R_Ctx_p->Key = R15_Network_CreateUniqueKey(Packet_p, (uint8)(*(R_Ctx_p->Packet_p->ExtendedHeader_p + sizeof(uint16)))); + + Index = 0; + C_(printf("r15_network_layer.c (%d) Key(%d) TKey(%d) \n", __LINE__, R_Ctx_p->Key, R_Ctx_p->TimerKey);) + + do { + if (NULL != R15_NETWORK(Communication_p)->RetransmissionList[Index]) { + if (R_Ctx_p->Timeout < TIMER(Communication_p, ReadTime_Fn)(OBJECT_TIMER(Communication_p), R15_NETWORK(Communication_p)->RetransmissionList[Index]->TimerKey)) { + i = R15_NETWORK(Communication_p)->RetransmissionListCount; + + for (; Index < i; i--) { + R15_NETWORK(Communication_p)->RetransmissionList[i] = R15_NETWORK(Communication_p)->RetransmissionList[i-1]; + } + + R15_NETWORK(Communication_p)->RetransmissionList[Index] = R_Ctx_p; + R_Ctx_p = NULL; + R15_NETWORK(Communication_p)->RetransmissionListCount++; + break; + } + + Index++; + } else { + R15_NETWORK(Communication_p)->RetransmissionList[Index] = R_Ctx_p; + R_Ctx_p = NULL; + R15_NETWORK(Communication_p)->RetransmissionListCount++; + break; + } + } while (Index < MAX_SIZE_RETRANSMISSION_LIST); + } else { + A_(printf("r15_network_layer.c (%d) ** Err: Retransmision List is full! ** \n", __LINE__);) + } + + return ReturnValue; +} + + +static void R15_Network_RetransmissionCallback(Communication_t *Communication_p, const void *const Timer_p, void *Packet_p) +{ + uint32 Index = 0; + + /* get first in list of packets for retransmission and remove */ + if (NULL != R15_NETWORK(Communication_p)->RetransmissionList[Index]) { + free(R15_NETWORK(Communication_p)->RetransmissionList[Index]); + R15_NETWORK(Communication_p)->RetransmissionList[Index] = NULL; + } + + if (R15_NETWORK(Communication_p)->RetransmissionListCount > 0) { + C_(printf("r15_network_layer.c (%d) RetransmissionListCount(%d) \n", __LINE__, R15_NETWORK(Communication_p)->RetransmissionListCount);) + R15_NETWORK(Communication_p)->RetransmissionListCount--; + + for (; Index < R15_NETWORK(Communication_p)->RetransmissionListCount; Index++) { + R15_NETWORK(Communication_p)->RetransmissionList[Index] = R15_NETWORK(Communication_p)->RetransmissionList[Index+1]; + } + + R15_NETWORK(Communication_p)->RetransmissionList[Index] = NULL; + + /* enqueue the packet for retransmission */ + (void)QUEUE(Communication_p, FifoEnqueue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Outbound_p, Packet_p); + } +} + + +static void R15_InHashCallback(const void *const Data_p, uint32 Length, const uint8 *const Hash_p, void *Param_p) +{ + PacketMeta_t *Packet_p = (PacketMeta_t *)Param_p; + +#ifdef CFG_ENABLE_MEASUREMENT_TOOL + if(Packet_p->Header.Protocol == BULK_PROTOCOL){ + BulkExtendedHeader_t ExtendedHeader = {0}; + R15_DeserializeBulkExtendedHeader(&ExtendedHeader, Packet_p->ExtendedHeader_p); + + if ((ExtendedHeader.TypeFlags & MASK_BULK_COMMAND_SELECT) == CMD_BULK_DATA){ + (void)MP(Measurement_p, ExtendedHeader.AcksChunk, END_HASHINGCHUNK_TIME); + } + } +#endif + + if (memcmp(Hash_p, &Packet_p->Header.PayloadChecksum, 4) == 0) { + SET_PACKET_FLAGS(Packet_p, PACKET_RX_STATE_MASK, BUF_PAYLOAD_CRC_OK); + (void)QUEUE((Packet_p->Communication_p), FifoEnqueue_Fn)(OBJECT_QUEUE(Packet_p->Communication_p), Packet_p->Communication_p->Inbound_p, Packet_p); + } else { + /* Invalid packet */ + ErrorCode_e ReturnValue = E_GENERAL_FATAL_ERROR; + + if (E_SUCCESS != (ReturnValue = R15_Network_PacketRelease((Communication_t *)Packet_p->Communication_p, Packet_p))) { + R15_NETWORK((Communication_t *)Packet_p->Communication_p)->Inbound.LCM_Error = ReturnValue; + } + } +} + +#ifdef CFG_ENABLE_LOADER_TYPE +static void R15_QueueOutCallback(const void *const Queue_p, void *Param_p) +{ + if (E_SUCCESS != R15_Network_TransmiterHandler((Communication_t *)Param_p)) { + R15_NETWORK((Communication_t *)Param_p)->Outbound.LCM_Error = E_GENERAL_COMMUNICATION_ERROR; + } +} + +static void R15_QueueInCallback(const void *const Queue_p, void *Param_p) +{ + ErrorCode_e ReturnValue = E_GENERAL_FATAL_ERROR; + Communication_t *Communication_p = (Communication_t *)Param_p; + PacketMeta_t *Packet_p = NULL; + + while ((Packet_p = (PacketMeta_t *)QUEUE(Communication_p, FifoDequeue_Fn)(OBJECT_QUEUE(Communication_p), Communication_p->Inbound_p)) != NULL) { + switch (Packet_p->Header.Protocol) { + case PROTO_BULK: + ReturnValue = R15_Bulk_Process(Communication_p, Packet_p); + + if (E_SUCCESS != ReturnValue) { + R15_NETWORK(Communication_p)->Inbound.LCM_Error = ReturnValue; + } + + break; + + case PROTO_COMMAND: + ReturnValue = R15_Command_Process(Communication_p, Packet_p); + + if (E_SUCCESS != ReturnValue) { + R15_NETWORK(Communication_p)->Inbound.LCM_Error = ReturnValue; + } + + break; + + default: + R15_NETWORK(Communication_p)->Inbound.LCM_Error = E_SUCCESS; + break; + } + } +} +#endif + +static PacketMeta_t *R15_Network_GetAvailableMetaPacket(void) +{ + uint8 i = 0; + PacketMeta_t *PacketMeta_p = NULL; + + do { + if (NULL == PacketMetaInfo[i].Buffer_p) { + PacketMeta_p = &PacketMetaInfo[i]; + break; + } + + i++; + } while (i < COMMAND_BUFFER_COUNT + BULK_BUFFER_COUNT); + + return PacketMeta_p; +} + +/** @} */ +/** @} */ +/** @} */ diff --git a/lcmodule/source/cnh1605205_ldr_network_layer/source/z_network.c b/lcmodule/source/cnh1605205_ldr_network_layer/source/z_network.c new file mode 100644 index 0000000..54abf46 --- /dev/null +++ b/lcmodule/source/cnh1605205_ldr_network_layer/source/z_network.c @@ -0,0 +1,166 @@ +/******************************************************************************* + * Copyright (C) ST-Ericsson SA 2011 + * License terms: 3-clause BSD license + ******************************************************************************/ +/** + * @addtogroup ldr_communication_serv + * @{ + * @addtogroup z_family + * @{ + * @addtogroup ldr_network_layer + * @{ + */ + +/******************************************************************************* + * Includes + ******************************************************************************/ +#include <stdlib.h> +#include <string.h> +#include "c_system.h" +#include "t_basicdefinitions.h" +#include "r_z_family.h" +#include "r_z_transport.h" +#include "r_z_network.h" +#include "r_z_protocol.h" +#include "r_debug.h" +//#include "r_debug_macro.h" +#include "r_communication_service.h" +#include "t_z_protocol.h" + +#ifdef WIN32 +#include <windows.h> +#endif + +/******************************************************************************* + * Declaration of file local functions + ******************************************************************************/ + +/******************************************************************************* + * File scope types, constants and variables + ******************************************************************************/ +#define Z_RESET_INBOUND(c, s) do { (c)->ReqData = 0; (c)->RecData = 0; (c)->Target_p = NULL; (c)->State = (s); } while(0); +#define Z_SYNC_HEADER(c, d, t) do { (c)->ReqData = d; (c)->Target_p = t; } while(0); +#define Z_SET_INBOUND(c, s, d) do { (c)->ReqData = d; (c)->RecData = 0; (c)->State = (s); } while(0); + +/******************************************************************************* + * Definition of external functions + ******************************************************************************/ + +ErrorCode_e Z_Network_Initialize(Communication_t *Communication_p) +{ + if (NULL == Communication_p) { + return E_INVALID_INPUT_PARAMETERS; + } + + memset(Z_NETWORK(Communication_p), 0, sizeof(Z_NetworkContext_t)); + + /* Simulate a finished read to get the inbound state-machine going. */ + Z_Network_ReadCallback(NULL, 0, Communication_p); + + return E_SUCCESS; +} + +ErrorCode_e Z_Network_Shutdown(const Communication_t *const Communication_p) +{ + + return E_SUCCESS; +} + +void Z_Network_ReadCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ + Communication_t *Communication_p = (Communication_t *)Param_p; + CommandData_t CmdData; + + Z_NETWORK(Communication_p)->Inbound.RecData += Length; + + if (Z_NETWORK(Communication_p)->Inbound.ReqData == 0) { + Z_Inbound_t *In_p = &(Z_NETWORK(Communication_p)->Inbound); + + if (In_p->RecData == 0) { + In_p->ReqData = Z_HEADER_LENGTH; + In_p->Target_p = In_p->Scratch; + } else { + CmdData.Payload.Data_p = In_p->Scratch; + (void)Communication_p->Do_CEH_Call_Fn(Communication_p->Object_p, &CmdData); + Z_RESET_INBOUND(In_p, Z_RECEIVE_HEADER); + Z_SYNC_HEADER(In_p, Z_HEADER_LENGTH, In_p->Scratch); + } + } +} + +void Z_Network_ReceiverHandler(Communication_t *Communication_p) +{ + uint32 ReqData; + Z_Inbound_t *In_p = &(Z_NETWORK(Communication_p)->Inbound); + + /* new data for receiving ? */ + if (In_p->ReqData > 0) { + ReqData = In_p->ReqData; + In_p->ReqData = 0; + In_p->RecData = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p, ReqData, Z_Network_ReadCallback, Communication_p); + C_(printf("z_network.c (%d) ReqData(%d) RecData(%d) \n", __LINE__, ReqData, In_p->RecData);) + } + + /* check for receiver sinhronization */ + if (In_p->State == Z_RECEIVE_ERROR) { + In_p->ReqData = 0; + In_p->RecData = 0; + (void)Communication_p->CommunicationDevice_p->Read(In_p->Target_p, Z_HEADER_LENGTH, Z_Network_ReadCallback, Communication_p); + } +} + + +void Z_Network_WriteCallback(const void *Data_p, const uint32 Length, void *Param_p) +{ +} + + +ErrorCode_e Z_Network_TransmiterHandler(Communication_t *Communication_p, Z_SendingContent_t *SendingContent_p) +{ + uint8 *Data_p = SendingContent_p->Data_p; + uint8 Size = SendingContent_p->Size; + Z_Outbound_t *Out_p = &(Z_NETWORK(Communication_p)->Outbound); + + if (Out_p->InLoad) { + return E_SUCCESS; + } + + Out_p->InLoad = TRUE; + + switch (Out_p->State) { + case Z_SEND_IDLE: + /* get next packet for transmiting */ + Out_p->State = Z_SEND_PACKET; + + /* FALLTHROUGH */ + case Z_SEND_PACKET: + + if (E_SUCCESS == Communication_p->CommunicationDevice_p->Write(Data_p, + Size, + Z_Network_WriteCallback, Communication_p)) { + Out_p->State = Z_SENDING_PACKET; + } else { + /* error state ?*/ + } + + /* FALLTHROUGH */ + case Z_SENDING_PACKET: + Out_p->State = Z_SEND_IDLE; + break; + + } + + Out_p->InLoad = FALSE; + + return E_SUCCESS; +} + +/******************************************************************************* + * Definition of internal functions + ******************************************************************************/ + + +/** @} */ +/** @} */ +/** @} */ |