diff options
Diffstat (limited to 'Src/Network/IndustrialStack_LLD.cpp')
| -rw-r--r-- | Src/Network/IndustrialStack_LLD.cpp | 278 |
1 files changed, 278 insertions, 0 deletions
diff --git a/Src/Network/IndustrialStack_LLD.cpp b/Src/Network/IndustrialStack_LLD.cpp new file mode 100644 index 0000000..f025dec --- /dev/null +++ b/Src/Network/IndustrialStack_LLD.cpp @@ -0,0 +1,278 @@ +/* + * Video On Demand Samples + * + * Copyright (C) 2015 Microchip Technology Germany II GmbH & Co. KG + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + * + * You may also obtain this software under a propriety license from Microchip. + * Please contact Microchip for further information. + * + */ + +//#define LLD_TRACE + +/*----------------------------------------------------------*/ +/*! \file */ +/*! \brief Base Board initialisation */ +/*----------------------------------------------------------*/ + +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <unistd.h> +#include <time.h> +#include <pthread.h> +#include <assert.h> + +#include "Console.h" +#include "Board.h" +#include "DriverConfiguration.h" +#include "IndustrialStack_LLD.h" + +/*----------------------------------------------------------*/ +/* Static C helper functions */ +/*----------------------------------------------------------*/ + +static void Queue_Init( Queue_t *queue ) +{ + assert(NULL != queue); + queue->testPattern = QUEUE_TESTPATTERN; + queue->rxPos = 0; + queue->txPos = 0; + queue->pRx = queue->dataQueue; + queue->pTx = queue->dataQueue; +} + +static QueueEntry_t *Queue_GetRxPtr( Queue_t *queue ) +{ + assert(NULL != queue && QUEUE_TESTPATTERN == queue->testPattern); + QueueEntry_t *pEntry = NULL; + if( queue->txPos - queue->rxPos > 0 ) + { + pEntry = queue->pRx; + } + return pEntry; +} + +static void Queue_PopRx( Queue_t *queue ) +{ + assert(NULL != queue && QUEUE_TESTPATTERN == queue->testPattern); + if( ++queue->pRx >= queue->dataQueue + BOARD_PMS_RX_QUEUE ) + queue->pRx = queue->dataQueue; + ++queue->rxPos; +} + +static QueueEntry_t *Queue_GetTxPtr( Queue_t *queue ) +{ + assert(NULL != queue && QUEUE_TESTPATTERN == queue->testPattern); + QueueEntry_t *pEntry = NULL; + if( ( uint32_t )BOARD_PMS_RX_QUEUE + queue->rxPos - queue->txPos > 0 ) + { + pEntry = queue->pTx; + } + return pEntry; +} + +static void Queue_PopTx( Queue_t *queue ) +{ + assert(NULL != queue && QUEUE_TESTPATTERN == queue->testPattern); + if( ++queue->pTx >= queue->dataQueue + BOARD_PMS_RX_QUEUE ) + queue->pTx = queue->dataQueue; + ++queue->txPos; +} + +static void *ReceiveThread( void *tag ) +{ + assert(NULL != tag); + CIndustrialStackLld *var = ( CIndustrialStackLld * )tag; + while( var->allowThreadRun ) + { + if( -1 == var->hControlRx ) + { + ConsolePrintf( PRIO_ERROR, RED"File handle for Control-RX is invalid, stopping reader thread."RESETCOLOR"\n" ); + if (NULL != var->listener) + var->listener->OnReadThreadEnd(var); + pthread_exit( NULL ); + } + QueueEntry_t *pEntry = Queue_GetTxPtr(&var->rxQueue); + if( NULL != pEntry ) + { + int16_t payloadLen = 0; + payloadLen = read( var->hControlRx, pEntry->buffer, sizeof( pEntry->buffer ) ); + if( payloadLen < 0 ) + { + if( var->allowThreadRun ) + { + ConsolePrintf( PRIO_ERROR, RED"Stopping reader thread, because of error: '%s'"RESETCOLOR"\n", + GetErrnoString() ); + if (NULL != var->listener) + var->listener->OnReadThreadEnd(var); + } + pthread_exit( NULL ); + } + else if( payloadLen != 0 ) + { + pEntry->payloadLen = payloadLen; + Queue_PopTx(&var->rxQueue); +#ifdef LLD_TRACE + { + ConsolePrintfStart( PRIO_HIGH, BLUE"%04d: MSG_RX: ", GetTickCountWord()); + for ( int16_t i = 0; i < pEntry->payloadLen; i++ ) + { + ConsolePrintfContinue( "%02X ", pEntry->buffer[i] ); + } + ConsolePrintfExit(RESETCOLOR"\n"); + } +#endif + } + } + else + { + ConsolePrintf( PRIO_ERROR, RED"WARNING, RX QUEUE FULL !!\nPlease increase BOARD_PMS_RX_QUEUE value, or increase polling speed\n"RESETCOLOR"\n" ); + usleep( 10000 ); + } + } + ConsolePrintf( PRIO_LOW, "Control Receive Thread ends\n" ); + if( var->allowThreadRun && NULL != var->listener) + var->listener->OnReadThreadEnd(var); + pthread_exit( NULL ); +} + +static void *SendThread( void *tag ) +{ + assert(NULL != tag); + CIndustrialStackLld *var = ( CIndustrialStackLld * )tag; + while( var->allowThreadRun ) + { + sem_wait( &var->txSem ); + if (!var->allowThreadRun) + pthread_exit( NULL ); + QueueEntry_t *pEntry = Queue_GetRxPtr( &var->txQueue ); + assert(NULL != pEntry); + if( -1 == var->hControlTx ) + { + ConsolePrintf( PRIO_ERROR, RED"File handle for Control-TX is invalid, stopping send thread."RESETCOLOR"\n" ); + pthread_exit( NULL ); + } +#ifdef LLD_TRACE + { + uint32_t i; + ConsolePrintfStart( PRIO_MEDIUM, YELLOW"%04d: MSG_TX: ", GetTickCountWord()); + for ( i = 0; i < pEntry->payloadLen; i++ ) + { + ConsolePrintfContinue( "%02X ", pEntry->buffer[i] ); + } + ConsolePrintfExit(RESETCOLOR"\n"); + } +#endif + if( write( var->hControlTx, pEntry->buffer, pEntry->payloadLen ) != pEntry->payloadLen ) + { + ConsolePrintf( PRIO_ERROR, RED"Failed to send %d bytes to the MOST control channel"RESETCOLOR"\n", pEntry->payloadLen ); + usleep(1000); + } + else + { + Queue_PopRx(&var->txQueue); + } + + } + pthread_exit( NULL ); +} + +/*----------------------------------------------------------*/ +/* Public method implementations */ +/*----------------------------------------------------------*/ + +CIndustrialStackLld::CIndustrialStackLld( int controlRxHandle, int controlTxHandle ) + : hControlRx(controlRxHandle), hControlTx(controlTxHandle), allowThreadRun(true), listener(NULL) +{ + Queue_Init(&rxQueue); + Queue_Init(&txQueue); + + if (sem_init(&txSem, 0, 0) == -1) + { + ConsolePrintf(PRIO_ERROR, RED"CIndustrialStackLld constructor: sem_init failed"RESETCOLOR"\n"); + return; + } + + pthread_create( &rxThread, NULL, ReceiveThread, this ); + pthread_create( &txThread, NULL, SendThread, this ); +} + +CIndustrialStackLld::~CIndustrialStackLld() +{ + void *dummy; + allowThreadRun = false; + sem_post( &txSem ); + + pthread_join( txThread, &dummy ); + pthread_join( rxThread, &dummy ); + + sem_destroy( &txSem ); +} + +void CIndustrialStackLld::ClearQueues() +{ + ConsolePrintf( PRIO_LOW, "Clearing INIC queues\n" ); + QueueEntry_t *pEntry; + while( NULL != ( pEntry = Queue_GetRxPtr(&rxQueue) ) ) + Queue_PopRx(&rxQueue); + while( NULL != ( pEntry = Queue_GetRxPtr(&txQueue) ) ) + Queue_PopRx(&txQueue); +} + +uint16_t CIndustrialStackLld::DataAvailable() +{ + uint16_t readLen = 0; + QueueEntry_t *pEntry = Queue_GetRxPtr(&rxQueue); + if( NULL != pEntry ) + readLen = pEntry->payloadLen; + return readLen; +} + +uint16_t CIndustrialStackLld::Read( uint8_t *pData, uint32_t bufferLen ) +{ + uint16_t readLen = 0; + QueueEntry_t *pEntry = Queue_GetRxPtr(&rxQueue); + if( NULL != pData && NULL != pEntry ) + { + readLen = pEntry->payloadLen; + if (readLen > bufferLen) + { + ConsolePrintf(PRIO_ERROR, RED"CIndustrialStackLld::Read buffer"\ + " length is too small"RESETCOLOR"\n"); + readLen = bufferLen; + } + memcpy( pData, pEntry->buffer, readLen ); + Queue_PopRx(&rxQueue); + } + return readLen; +} + +bool CIndustrialStackLld::Write( uint16_t wLen, uint8_t *pData ) +{ + if( -1 == hControlTx ) + return false; + QueueEntry_t *pEntry = Queue_GetTxPtr( &txQueue ); + if( NULL != pData && NULL != pEntry ) + { + memcpy( pEntry->buffer, pData, wLen ); + pEntry->payloadLen = wLen; + Queue_PopTx( &txQueue ); + sem_post( &txSem ); + return true; + } + return false; +} |