//  ----------------------------------------------------------------------------
//          ATMEL Microcontroller Software Support  -  ROUSSET  -
//  ----------------------------------------------------------------------------
//  DISCLAIMER:  THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR
//  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
//  MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
//  DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR ANY DIRECT, INDIRECT,
//  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
//  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
//  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
//  EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//  ----------------------------------------------------------------------------
// File Name           : po_kernel.c
// Object              : Encapsulation of specific kernel functions
// Creation            : JCB 15/apr/2005
// Modif               :
// ----------------------------------------------------------------------------
#include <stdlib.h>     // malloc etc...
#include <string.h>     // memcpy, memset, etc...

#include "trace.h"
#include "po_types.h"
#include "po_kernel.h"


ULONG CPT_LOCK = 0;

/******************************************************************
*
* SUB-ROUTINE  po_unlock
*
*------------------------------------------------------------------
*
* purpose : releases a spin lock and restores the original IRQL at
*      which the caller was running.
*
* input parameters :
*
* output parameters :
*
* global data :
*
******************************************************************/
#ifdef ADS_COMPIL
void po_unlock()
#else
__ramfunc void po_unlock()
#endif
{
  CPT_LOCK--;
  if( 0 == CPT_LOCK )
  {
    AT91F_enable_interrupt();
  }
}

/******************************************************************
*
* SUB-ROUTINE  po_lock
*
*------------------------------------------------------------------
*
* purpose : acquires a spin lock so the caller can synchronize access
*      to shared data in a multiprocessor-safe way by raising IRQL.
*
* input parameters :
*
* output parameters :
*
* global data :
*
******************************************************************/
#ifdef ADS_COMPIL
void po_lock()
#else
__ramfunc void po_lock()
#endif
{
  if( 0 == CPT_LOCK )
  {
    AT91F_disable_interrupt();
  }
  CPT_LOCK++;
}

/******************************************************************
*
* SUB-ROUTINE  po_malloc
*
*------------------------------------------------------------------
*
* purpose : kernel memory allocation in non paged poool
*
* input parameters : size to allocate
*
* output parameters : pointer on allocated memory zone.
*
* global data : no
*
******************************************************************/
void* po_malloc( ULONG size )
{
  TRACE_DEBUG_H( "malloc %d\n\r", (int)size );
  return malloc( (unsigned int)size );
}

/******************************************************************
*
* SUB-ROUTINE  po_free
*
*------------------------------------------------------------------
*
* purpose : Kernel memory free routine
*
* input parameters : buffer to free
*
* output parameters :
*
* global data :
*
******************************************************************/
void po_free( void* buffer )
{
  TRACE_DEBUG_H( "free buff\n\r" );
  free( buffer );
}

/******************************************************************
*
* SUB-ROUTINE  po_memcpy
*
*------------------------------------------------------------------
*
* purpose : memcpy
*
* input parameters : dest
*                    src
*                    len
*
* output parameters :
*
* global data :
*
******************************************************************/
#ifdef ADS_COMPIL
void po_memcpy( void *dest, void *src, unsigned int len )
#else
__ramfunc void po_memcpy( void *dest, void *src, unsigned int len )
#endif
{
  unsigned char *_dst = dest;
  const unsigned char *_src1 = src;

  // memcpy(dest,src,len);
  while( len-- > 0 )
  {
    *_dst++ = *_src1++;
  }
}

/******************************************************************
*
* SUB-ROUTINE  po_rngNBytes
*
*------------------------------------------------------------------
*
* purpose : return number of bytes in rounding buffer
*
* input parameters : rounding buffer
*
*
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_rngNBytes( RING_ID * pRingId )
{
  int _len;

  po_lock();
  _len = (pRingId->last - pRingId->first) - pRingId->NbOctetsLibres;
  po_unlock();
  return( _len );
}

/******************************************************************
*
* SUB-ROUTINE  po_rngFlush
*
*------------------------------------------------------------------
*
* purpose : flush rounding buffer
*
* input parameters : rounding buffer
*
*
*
* output parameters :
*
* global data :
*
******************************************************************/
#ifdef ADS_COMPIL
void po_rngFlush( RING_ID * pRingId )
#else
__ramfunc void po_rngFlush( RING_ID * pRingId )
#endif
{
  po_lock();
  pRingId->current = pRingId->first;
  pRingId->free    = pRingId->first;
  pRingId->NbOctetsLibres = pRingId->last - pRingId->first;
  po_unlock();
}

/******************************************************************
*
* SUB-ROUTINE  po_rngIsEmpty
*
*------------------------------------------------------------------
*
* purpose : return 0 if rounding buffer empty
*
* input parameters : rounding buffer
*
*
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_rngIsEmpty( RING_ID * pRingId )
{
  int _ret = 0;

  po_lock();
  if( pRingId->NbOctetsLibres == (pRingId->last - pRingId->first) )
  {
    _ret = 1;
  }
  po_unlock();

  return( _ret );
}

/******************************************************************
*
* SUB-ROUTINE  po_rngBufPut
*
*------------------------------------------------------------------
*
* purpose : write Bytes in FIFO
*
* input parameters : pRingId
*                    buffer
*                    len
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_rngBufPut( RING_ID * pRingId, char * buffer, int len )
{
  char *_free;
  char *_first;
  char *_last;
  int  _NbOctetsLibres ;
  int  _i = len;

  po_lock();

  _NbOctetsLibres =  pRingId->NbOctetsLibres;
  _free = pRingId->free;
  _first = pRingId->first;
  _last = pRingId->last;

  if( len > _NbOctetsLibres )
  {
    TRACE_ERROR( "PB BufPut:len=%d\n\r", len );
    po_unlock();
    return -1;
  }

  while( _i-- > 0 )
  {
    *_free++ = *buffer++;

    if( _free >= _last )
    {
      _free = _first;
    }
  }

  pRingId->NbOctetsLibres -= len;

  pRingId->free = _free;

  po_unlock();

  return( len );
}

/******************************************************************
*
* SUB-ROUTINE  po_rngBufGet
*
*------------------------------------------------------------------
*
* purpose : read Bytes from FIFO
*
* input parameters : pRingId
*                    buffer
*                    len
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_rngBufGet( RING_ID * pRingId, char * buffer, int len )
{
  char *_current;
  char *_first;
  char *_last;
  int _NbOctetsLibres;
  int _NbOctetsFifo;
  int _ret;

  po_lock();
  //TRACE_DEBUG_L("rngBufGet\n\r");

  _current = pRingId->current;
  _first   = pRingId->first;
  _last    = pRingId->last;
  _NbOctetsLibres =  pRingId->NbOctetsLibres;

  _NbOctetsFifo = (_last - _first) - _NbOctetsLibres ;
  if( 0 == _NbOctetsFifo )
  {
    TRACE_ERROR( "PB BufGet\n\r" );
    po_unlock();
    return 0;
  }

  if( len > _NbOctetsFifo )
  {
    len = _NbOctetsFifo;
  }
  _ret = len;

  while( len-- > 0)
  {
    *buffer++= *_current++;

    if(_current >= _last)
    {
      _current = _first;
    }
  }
  pRingId->current = _current;
  pRingId->NbOctetsLibres += _ret;
  po_unlock();

  return( _ret );
}

/******************************************************************
*
* SUB-ROUTINE  po_rngCreate
*
*------------------------------------------------------------------
*
* purpose : create FIFO
*
* input parameters : len FIFO size
*
* output parameters :
*
* global data :
*
******************************************************************/
RING_ID po_rngCreate( int len )
{
  RING_ID _ringId;
  char *  _buff;

  _buff = po_malloc(len);
  if( NULL == _buff )
  {
    TRACE_ERROR( "rng malloc PB\n\r" );
    _ringId.status = -1;
  }
  else
  {
    _ringId.free    = _buff;
    _ringId.first   = _buff;
    _ringId.current = _ringId.first;
    _ringId.last    = _ringId.first + len;
    _ringId.NbOctetsLibres = len;
    _ringId.status  = 1;
  }
  return( _ringId );
}

/******************************************************************
*
* SUB-ROUTINE  po_rngDelete
*
*------------------------------------------------------------------
*
* purpose : delete FIFO
*
* input parameters : ringId
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_rngDelete( RING_ID * ringId )
{
  int _ret = -1;

  if(ringId->status == 1)
  {
    po_free(ringId->first);
    ringId->first = ringId->last = ringId->current= NULL;
    ringId->NbOctetsLibres = 0;
    ringId->status = -1;
    _ret = 0;
  }
  return( _ret );
}

/******************************************************************
*
* SUB-ROUTINE po_strcmp
*
*------------------------------------------------------------------
*
* purpose : compare two string buffers, return 0 if equals
*
* input parameters : s1
*                    s2
*
* output parameters :
*
* global data :
*
******************************************************************/
int po_strcmp( const char* s1, const char* s2 )
{
  return( strcmp(s1,s2) );
}