/*
* 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 2 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;
}