canpie_fifo.c

canpie 一个can bus的协议栈 - CAN interface for embedded control - CAN interface for PC (without local

/*****************************************************************************\
*  CANpie                                                                     *
*                                                                             *
*  File        : cpfifo.c                                                     *
*  Description :                                                              *
*  Author      : Uwe Koppe                                                    *
*  e-mail      : [e-mail]                                       *
*                                                                             *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
*                                                                             *
*   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.                                       *
*                                                                             *
* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *
*                                                                             *
*  History                                                                    *
*  Vers.    Date        Comment                                         Aut.  *
*  -----    ----------  ---------------------------------------------   ----  *
*  0.1      04.09.1999  Initial version                                 UK    *
*  0.2      18.01.2000  Fixed bug in pointer calculation                UK    *
*  0.3                  - no changes -                                        *
*  0.4                  - no changes -                                        *
*  0.5                  - no changes -                                        *
*  0.6      15.06.2000  added fixed FIFO size support                   UK    *
*                       set to CANpie release 0.6                             *
*  0.7      30.11.2000  Bugfix for static FIFO (overflow)               UK    *
*  0.8      14.02.2001  complete rewrite, check FIFO status             UK    *
*                                                                             *
\*****************************************************************************/

#include <linux/can/cpfifo.h>

#include <linux/string.h>     // for memcpy()
#include <linux/vmalloc.h>		// for kmalloc() and kfree()


/*----------------------------------------------------------------------------*\
** Functions                                                                  **
**                                                                            **
\*----------------------------------------------------------------------------*/



//----------------------------------------------------------------------------//
// CpFifoInit()                                                               //
// initialize the FIFO structure                                              //
//----------------------------------------------------------------------------//
_U08 CpFifoInit(_TsCpFifo * ptsFifoV, _U16 uwSizeV)
{
   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return(CpErr_FIFO_PARM);

   if(uwSizeV == 0)   return(CpErr_FIFO_PARM);
   
   //-----------------------------------------------------------------
   // allocate memory for FIFO 
   //
   ptsFifoV->ptsMsgList = vmalloc(sizeof(_TsCpCanMsg) * uwSizeV);
   if(ptsFifoV->ptsMsgList == 0L)
   {
      return(CpErr_FIFO_SIZE);      
   }

   //-----------------------------------------------------------------
   // store FIFO size and make it empty (head = tail = 0)
   //
   ptsFifoV->uwFifoSize = uwSizeV;
   ptsFifoV->uwHeadPos  = 0;
   ptsFifoV->uwTailPos  = 0;

   return (CpErr_OK);
}


//----------------------------------------------------------------------------//
// CpFifoRelease()                                                            //
// release the FIFO structure                                                 //
//----------------------------------------------------------------------------//
_U08 CpFifoRelease(_TsCpFifo * ptsFifoV)
{
   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return(CpErr_FIFO_PARM);

   vfree(ptsFifoV->ptsMsgList);
   ptsFifoV->uwFifoSize = 0;
   ptsFifoV->uwHeadPos  = 0;
   ptsFifoV->uwTailPos  = 0;

   return (CpErr_OK);
}


//----------------------------------------------------------------------------//
// CpFifoClear()                                                              //
// clear FIFO                                                                 //
//----------------------------------------------------------------------------//
void CpFifoClear(_TsCpFifo * ptsFifoV )
{
   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return;
   
   ptsFifoV->uwHeadPos  = 0;
   ptsFifoV->uwTailPos  = 0;
}


//----------------------------------------------------------------------------//
// CpFifoMsgCount()                                                           //
// return number of messages inside the given FIFO                            //
//----------------------------------------------------------------------------//
_U16 CpFifoMsgCount(_TsCpFifo * ptsFifoV)
{
   _U16  uwMsgCountT = 0;
   
   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return(0);


   if( (ptsFifoV->uwHeadPos) >= (ptsFifoV->uwTailPos) )
   {
      uwMsgCountT = (ptsFifoV->uwHeadPos) - (ptsFifoV->uwTailPos);
   }
   else
   {
      uwMsgCountT = (ptsFifoV->uwFifoSize) - (ptsFifoV->uwTailPos);
      uwMsgCountT = uwMsgCountT + (ptsFifoV->uwHeadPos);
   }
   return(uwMsgCountT);
}


//----------------------------------------------------------------------------//
// CpFifoPush()                                                               //
// push CAN message into FIFO                                                 //
//----------------------------------------------------------------------------//
_U08 CpFifoPush(_TsCpFifo * ptsFifoV, _TsCpCanMsg * ptsCanMsgV)
{
   register _TsCpCanMsg *  ptsMsgSlotT;


   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return(CpErr_FIFO_PARM);

   //----------------------------------------------------------------
   // test if FIFO is full:
   //
   if( (ptsFifoV->uwHeadPos == 0) && 
       (ptsFifoV->uwTailPos == ptsFifoV->uwFifoSize - 1) )
   {

      return (CpErr_FIFO_FULL);
   }
   
   if(ptsFifoV->uwHeadPos == ptsFifoV->uwTailPos + 1)
   {
      return (CpErr_FIFO_FULL);
   }


   
   //----------------------------------------------------------------
   // FIFO is not full, insert the message
   //
   ptsMsgSlotT = (ptsFifoV->ptsMsgList);
   if(ptsMsgSlotT == 0L)
   {
      return(CpErr_FIFO_SIZE);
   }    
   ptsMsgSlotT = ptsMsgSlotT + (ptsFifoV->uwTailPos);

   memcpy(ptsMsgSlotT, ptsCanMsgV, sizeof(_TsCpCanMsg));

   //-----------------------------------------------------------------
   // increment tail position
   //
   ptsFifoV->uwTailPos += 1;
   if(ptsFifoV->uwTailPos == ptsFifoV->uwFifoSize) 
   {
      ptsFifoV->uwTailPos = 0;
   }
      
   return (CpErr_OK);
}


//----------------------------------------------------------------------------//
// CpFifoPop()                                                                //
// push CAN message into FIFO                                                 //
//----------------------------------------------------------------------------//
_U08 CpFifoPop(_TsCpFifo * ptsFifoV, _TsCpCanMsg * ptsCanMsgV)
{
   register _TsCpCanMsg *  ptsMsgSlotT;


   //----------------------------------------------------------------
   // test FIFO pointer
   //
   if(ptsFifoV == 0L) return(CpErr_FIFO_PARM);


   //-----------------------------------------------------------------
   // test if FIFO is empty
   //
   if(ptsFifoV->uwHeadPos == ptsFifoV->uwTailPos)
   {
      return (CpErr_FIFO_EMPTY);
   }

   //----------------------------------------------------------------- 
   // get message from FIFO
   //
   ptsMsgSlotT = (ptsFifoV->ptsMsgList) + (ptsFifoV->uwHeadPos);

   memcpy(ptsCanMsgV, ptsMsgSlotT, sizeof(_TsCpCanMsg));


   //----------------------------------------------------------------
   // increment head position
   //
   ptsFifoV->uwHeadPos += 1;
   if(ptsFifoV->uwHeadPos == ptsFifoV->uwFifoSize) 
   {
      ptsFifoV->uwHeadPos = 0;
   }

   return (CpErr_OK);
}