Lemz_SwCtrlStmProj/App/usbtmc/queue.c

#include "queue.h"
#include <string.h> // memcpy
#include "system.h"
#include <stddef.h> // ptrdiff_t

    #pragma message("Cycled queue v1.7 / 29.08.18")  // refactored 29.08.18.

    //#define __queue_isempty( pQueue )                ((((QUEUE*)pQueue)->size==0)||(((QUEUE*)pQueue)->pQueueHead==0)||((((QUEUE*)pQueue)->pReadPointer)==(((QUEUE*)pQueue)->pWritePointer)))
    //#define __queue_isfull( pQueue )                 ((((QUEUE*)pQueue)->size>0)&&(((QUEUE*)pQueue)->pQueueHead!=0)  /*&&((((QUEUE*)pQueue)->pReadPointer)!=(((QUEUE*)pQueue)->pWritePointer))*/  )
    
    //#define __queue_size( pQueue )                   (((QUEUE*)pQueue)->size)
    //#define __queue_count( pQueue )                  (((QUEUE*)pQueue)->count)
    //#define __queue_bytesavailable(pQueue)           ((__queue_size(pQueue)>0)?(((QUEUE*)pQueue)->bytes_available):0)
    
    //#define __queue_head( pQueue )                   (((QUEUE*)pQueue)->pQueueHead)
    //#define __queue_write( pQueue )                  (((QUEUE*)pQueue)->pWritePointer)
    //#define __queue_read( pQueue )                   (((QUEUE*)pQueue)->pReadPointer)
    
    //#define __w__queue_end_remaining( pQueue )       ((ptrdiff_t)__w__queue_end( pQueue ) - (ptrdiff_t)((QUEUE*)pQueue)->pWritePointer)
    //#define __r__queue_end_remaining( pQueue )       ((ptrdiff_t)__w__queue_end( pQueue ) - (ptrdiff_t)((QUEUE*)pQueue)->pReadPointer)
    //#define __w__queue_end( pQueue )                 (  (QUEUE_ENTRY*)(__queue_size(pQueue)+  ((BYTE*)__queue_head(pQueue)))  )
    //#define __r__queue_end( pQueue )                 (  (QUEUE_ENTRY*)(__queue_size(pQueue)+  ((BYTE*)__queue_head(pQueue)))  )
    
    //#define __queue_itemsize( pQueue )               ( (size_t) ((QUEUE*)pQueue)->pReadPointer->cbytes )
    //#define __queue_itemdata( pQueue )               ( ((BYTE*)((QUEUE*)pQueue)->pReadPointer)+sizeof(QUEUE_ENTRY) )
    //#define __w__queue_itemdata( pQueue )            ( ((BYTE*)((QUEUE*)pQueue)->pWritePointer)+sizeof(QUEUE_ENTRY) )
    //#define __w__queue_itemsize( pQueue )            ( ((QUEUE*)pQueue)->pWritePointer->cbytes )
    
    //#define PTR(value)                               ((ptrdiff_t)(value))


// CHECK_ALIGN_FOR_TYPE()
// Checks if @nSize is aligned to the TYPE's size edje
#define CHECK_ALIGN_FOR_TYPE( nSize, TYPE )  (0 == sizeAlign( nSize, sizeof(TYPE) ))

// sizeAlign()
// calculates amount of padding bytes align @size bytes by the @align edje:
// e.g. for size=5 and align=8: 3 extra padding bytes required
// e.g. for size=3 and align=7: 4 extra padding bytes required
// e.g. for size=17 and align=2: 1 extra padding byte required
// e.g. for size=16 and align=2 no extra padding bytes required
// e.g. for size=12 and align=6 no extra padding bytes required
static inline size_t sizeAlign( size_t size, size_t align )
{
    if( (size % align) )
    {
        return (align - (size % align));
    }
    
    return 0;  // no aligment bytes needed
}

// queue_shift_fwd_wptr: shift the W-ptr forward with rollovering
static inline void queue_shift_fwd_wptr( QUEUE * pQueue, size_t shift )
{
    if( 0 == shift )
      return;
    
    if( (ptrdiff_t)pQueue->pWritePointer + shift <
          (ptrdiff_t)pQueue->pQueueHead + pQueue->size )
    {
        // just shift the pointer, no rollover will performed
        pQueue->pWritePointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pWritePointer + shift );
    }
    else
    {
        // the pointer will rollover
      
        // calculate the distance since the W-ptr til the end of buffer
        // and substract this value from the desired @shift
        shift -= ((ptrdiff_t)pQueue->pQueueHead + pQueue->size - (ptrdiff_t)pQueue->pWritePointer);
          
        // the rest amount of @shift points is the distance from the beginning of the buffer
        pQueue->pWritePointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pQueueHead + shift );  
    }
}
        
// queue_shift_fwd_rptr: shift the R-ptr forward with rollovering
static inline void queue_shift_fwd_rptr( QUEUE * pQueue, size_t shift )
{
    if( 0 == shift )
      return;
    
    if( (ptrdiff_t)pQueue->pReadPointer + shift <
          (ptrdiff_t)pQueue->pQueueHead + pQueue->size )
    {
        // just shift the pointer, no rollover will performed
        pQueue->pReadPointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pReadPointer + shift );
    }
    else
    {
        // the pointer will rollover
      
        // calculate the distance since the R-ptr til the end of buffer
        // and substract this value from the desired @shift
        shift -= ((ptrdiff_t)pQueue->pQueueHead + pQueue->size - (ptrdiff_t)pQueue->pReadPointer);
          
        // the rest amount of @shift points is the distance from the beginning of the buffer
        pQueue->pReadPointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pQueueHead + shift );  
    }
}


// queue_shift_bkw_wptr: shift the W-ptr backward with rollovering
static inline void queue_shift_bkw_wptr( QUEUE * pQueue, size_t shift )
{
    if( 0 == shift )
      return;
    
    // check the limits
    if( (ptrdiff_t)pQueue->pWritePointer - shift >=
          (ptrdiff_t)pQueue->pQueueHead )
    {
        // just shift the pointer, no rollover will performed
        pQueue->pWritePointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pWritePointer - shift );
    }
    else
    {
        // the pointer will rollover
      
        // calculate the distance since the W-ptr til the begining of buffer
        // and substract this value from the desired @shift
        shift -= ((ptrdiff_t)pQueue->pWritePointer - (ptrdiff_t)pQueue->pQueueHead);
          
        // the rest amount of @shift points is the distance from the beginning of the buffer
        pQueue->pWritePointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pQueueHead +
                                                  pQueue->size - shift );
    }
}
/*
// queue_shift_bkw_wptr: shift the R-ptr backward with rollovering
static inline void queue_shift_bkw_rptr( QUEUE * pQueue, size_t shift )
{
    if( 0 == shift )
      return;
    
    // check the limits
    if( (ptrdiff_t)pQueue->pReadPointer - shift >=
          (ptrdiff_t)pQueue->pQueueHead )
    {
        // just shift the pointer, no rollover will performed
        pQueue->pReadPointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pReadPointer - shift );
    }
    else
    {
        // the pointer will rollover
      
        // calculate the distance since the R-ptr til the begining of buffer
        // and substract this value from the desired @shift
        shift -= ((ptrdiff_t)pQueue->pReadPointer - (ptrdiff_t)pQueue->pQueueHead);
          
        // the rest amount of @shift points is the distance from the beginning of the buffer
        pQueue->pReadPointer = (QUEUE_ENTRY*)( (ptrdiff_t)pQueue->pQueueHead +
                                                  pQueue->size - shift );
    }
}
*/
// queue_pushbytes:
// Copyies the data into the queue with checking available size
// In case the TRUE is returned, the function modifies the W-ptr and available bytes
// In case the FALSE is returned, no changes peformed in the queue
static BOOL queue_pushbytes( QUEUE * pQueue, const void * pBytes, size_t nSize )
{
   // validate pointers  
   if( pQueue->pWritePointer == pQueue->pReadPointer )
   {
       // check for FULL condition
       if( 0 == pQueue->bytes_available || 0 != pQueue->count )
       {
           return false;
       }
   }
   
   if(  (ptrdiff_t)pQueue->pReadPointer <= (ptrdiff_t)pQueue->pWritePointer ) 
   {
     // . - free
     // # - used
     // R - read ptr
     // W - write ptr     
     // Buffer: [.....R######W.....]
     // Bank:    -1st-        -2nd-
     // bytes free since the W-ptr til the end of buffer
     size_t nBytesFreeTilEnd =   ((ptrdiff_t)pQueue->pQueueHead)
                               + (size_t)pQueue->size
                               - ((ptrdiff_t)pQueue->pWritePointer);
     
     // bytes free since the begining of the buffer til the R-ptr
     size_t nBytesFreeSinceStart = (size_t)(
                                    (ptrdiff_t)pQueue->pReadPointer 
                                  - (ptrdiff_t)pQueue->pQueueHead );
     
     // Check free space
     if( nSize > nBytesFreeTilEnd + nBytesFreeSinceStart )
     {
         return false;
     }
     
     
     // copy the data into the queue
     
     // the data will be copied in two portions:
     // - since W-ptr til the end (1st bank)
     // - since the head of buffer til the R-ptr (2nd bank)
     
     // try to copy @nCpy of data into 2nd bank:        
     size_t nCpy = nSize;
     
     if( nCpy > nBytesFreeTilEnd )
     {
         // limit the amount of bytes by size of 2nd bank: @nBytesFreeTilEnd
         nCpy = nBytesFreeTilEnd;
     }
     
     // copy data into 2nd bank (1st portion of data)
     memcpy( (void*)pQueue->pWritePointer,
             (void*)(pBytes),
             nCpy );
     
     // decrease amount of bytes that needed to be copied
     nSize -= nCpy;
            
     // copy the rest part (2nd portion of data) into 1st bank
     memcpy( (void*)pQueue->pQueueHead,
             (void*)((ptrdiff_t)pBytes + nCpy),
             nSize );
     
     // shift the W-ptr
     // pQueue->pWritePointer = (QUEUE_ENTRY*)( ((ptrdiff_t)pQueue->pQueueHead) + nSize );
     queue_shift_fwd_wptr( pQueue, (nSize + nCpy) );
     
     // calculate the bytes availabe
     pQueue->bytes_available -= (nSize + nCpy);
     
     // do not change the count of elements (do it in the caller)
     (void)pQueue->count;
   }
   else
   {
     // . - free
     // # - used
     // R - read ptr
     // W - write ptr     
     // Buffer: [#####W.....R######]
     // Bank:          -1st-
     // bytes free since the W-ptr til the R-ptr
     size_t nBytesTilRead  =   ((ptrdiff_t)pQueue->pReadPointer)
                             - ((ptrdiff_t)pQueue->pWritePointer);
     
     // Check free space
     if( nSize > nBytesTilRead )
     {
         return false;
     }
     
     // copy the data into the queue:
     // for this case: just copy at W-ptr 
     memcpy( (void*)pQueue->pWritePointer,
             (void*)pBytes,
             nSize );
          
     // shift the W-ptr
     // pQueue->pWritePointer = (QUEUE_ENTRY*)( ((ptrdiff_t)pQueue->pWritePointer) + nSize );
     queue_shift_fwd_wptr( pQueue, (nSize) );
     
     // calculate the bytes availabe
     pQueue->bytes_available -= nSize;
     
     // do not change the count of elements (do it in the caller)
     (void)pQueue->count;
   }
   
   return true;
}


static BOOL queue_cat_ex( QUEUE * pQueue, const uint8_t * pData, size_t bytes, bool bAddAsNew )
{
    BOOL rc = FALSE;
    //--------------------------------------
    // Check input arguments
    if(     NULL == pQueue
        ||  NULL == pData
        ||  0    == bytes )
    return false;
    
    CRITICAL_SECTION_ENTER();
    //--------------------------------------      
    if( bAddAsNew )
    {
        // add as new element
        size_t alignBytes = sizeAlign( bytes, sizeof(QUEUE_ENTRY) );
        
        // check free space
        if( pQueue->bytes_available < bytes + alignBytes )
        {
            // no free space
            goto L_queue_cat_ex_EXIT;
        }
        
        QUEUE_ENTRY qEntry = { .cbytes = bytes };
        
        // remember the beginning of the last element writtend
        pQueue->LastWrite = pQueue->pWritePointer;
        size_t bytesAvailableBackup = pQueue->bytes_available;
        
        // push the element header
        if( !queue_pushbytes( pQueue, &qEntry, sizeof(qEntry) ) )
        {
            // error
          
            // roll back the changes: just for order
            pQueue->pWritePointer = pQueue->LastWrite;
            pQueue->bytes_available = bytesAvailableBackup;
            
            goto L_queue_cat_ex_EXIT;
        }
        
        // push the data
        if( !queue_pushbytes( pQueue, pData, bytes ) )
        {
            // roll back the changes done at first call of @queue_pushbytes(..&qEntry..)
            pQueue->pWritePointer = pQueue->LastWrite;
            pQueue->bytes_available = bytesAvailableBackup;
            
            // error
            goto L_queue_cat_ex_EXIT;
        }
        
        // push the aligning bytes: just modify the W-ptr and @bytes_available
        queue_shift_fwd_wptr( pQueue, alignBytes );
        
        // calculate the bytes availabe
        pQueue->bytes_available -= alignBytes;

        // increment amount of elements
        pQueue->count++;
        
        rc = true;
    }
    else
    {
        // concatinate the data to the existing element

        // To concatinate at least one element is required
        if( (0 == pQueue->count) )
        goto L_queue_cat_ex_EXIT;
        
        // get the last written element size
        size_t topElementSize = pQueue->LastWrite->cbytes;
        
        // store the backup info
        QUEUE_ENTRY * pWritePointerBackup = pQueue->pWritePointer;
        size_t bytesAvailableBackup = pQueue->bytes_available;
        
        // check it's aligment
        size_t alignBytes = sizeAlign( topElementSize, sizeof(QUEUE_ENTRY) );        
        
        if( 0 < alignBytes )
        {
            // the element has aligment bytes  
            
            // check free space
            if( pQueue->bytes_available + alignBytes < bytes )
            {
                // no free space
                goto L_queue_cat_ex_EXIT;
            }
          
            // roll the W-ptr back by aligning bytes
            queue_shift_bkw_wptr( pQueue, alignBytes );
            
            // calculate the bytes availabe
            pQueue->bytes_available += alignBytes;
            
            // now @alignBytes is aligning bytes of @pData[0..bytes)
            alignBytes = sizeAlign( (bytes - alignBytes), sizeof(QUEUE_ENTRY) );
            
            // concatinate the new data to the existing element
            if( !queue_pushbytes( pQueue, pData, bytes ) )
            {
                // error
              
                // roll back the changes: just for order              
                pQueue->pWritePointer = pWritePointerBackup;
                pQueue->bytes_available = bytesAvailableBackup;
                
                goto L_queue_cat_ex_EXIT;
            }
            
            // push the aligning bytes: just modify the W-ptr and @bytes_available            
            queue_shift_fwd_wptr( pQueue, alignBytes );
            
            // calculate the bytes availabe
            pQueue->bytes_available -= alignBytes;
            
            // modify the element header
            pQueue->LastWrite->cbytes += bytes;
            
            rc = true;
        }
        else
        {
            // the element does not have any alignment bytes.
             
            // now @alignBytes is aligning bytes of @pData[0..bytes)
            size_t alignBytes = sizeAlign( bytes, sizeof(QUEUE_ENTRY) );
            
            // check free space
            if( pQueue->bytes_available < bytes + alignBytes )
            {
                // no free space
                goto L_queue_cat_ex_EXIT;
            }
            
            // push the data:
            if( !queue_pushbytes( pQueue, pData, bytes ) )
            {
                // error
              
                // roll back the changes: just for order
                pQueue->pWritePointer = pWritePointerBackup;
                pQueue->bytes_available = bytesAvailableBackup;
                
                goto L_queue_cat_ex_EXIT;
            }
            
            // push the aligning bytes:  just modify the W-ptr and @bytes_available            
            queue_shift_fwd_wptr( pQueue, alignBytes );
            
            // calculate the bytes availabe
            pQueue->bytes_available -= alignBytes;
            
            // modify the element header
            pQueue->LastWrite->cbytes += bytes;
            
            rc = true;
        }
   }
    
L_queue_cat_ex_EXIT: 
  
   CRITICAL_SECTION_LEAVE();
   
   // if( !rc ) asm("bkpt #0");
   
   return rc;
}

/*
static BOOL queue_cat_ex( QUEUE * pQueue, const BYTE * pData, size_t bytes, int AddAsNew)
{
    BOOL rc = FALSE;
    int rem;
    char * wr;
    int newbytes;
    int allbytes;
    unsigned int wr_aligment = 0;
    //-------------------------
    BOOL bAligment = FALSE;
    int nAligment = 0;
    int nAligmentSize = sizeof(QUEUE_ENTRY);

    // îïðåäåëÿåì âûðàâíèâàíèå
    if(bytes%nAligmentSize)
        { bAligment=TRUE; nAligment = bytes%nAligmentSize; }
    //-------------------------

    allbytes = bytes + nAligment + ((AddAsNew)?sizeof(QUEUE_ENTRY):0); // fixed 28/08/18
    // allbytes = bytes + nAligment + sizeof(QUEUE_ENTRY); 
    CRITICAL_SECTION_ENTER();

    if( pQueue != 0 )
        if( pData != 0 )
            if( bytes != 0 && __queue_bytesavailable(pQueue) >= allbytes )
                if(AddAsNew==1 || (AddAsNew==0 && __queue_count(pQueue) >0 && (!(pQueue->pWritePointer == pQueue->LastWrite || pQueue->LastWrite==NULL))))
                {
                    // åñëè äîáàâëÿåì êàê íîâûé ýëåìåíò, ÷èñëî ýëåìåíòîâ ðàñòåò
                    if(AddAsNew==1)
                        pQueue->count++;

                    // âû÷èñëÿåì îñòâøååñÿ ìåñòî
                    //pQueue->bytes_available -= (AddAsNew==1)?allbytes:allbytes-sizeof(QUEUE_ENTRY);
                    pQueue->bytes_available -= allbytes; // fixed 28/08/18

                    // åñëè êîíêàòèíèðóåì òî âîññòàíàâëèâàåì óêàçàòåëü çàïèñè íà ïðåäûäóùèé ýëåìåíò
                    if(AddAsNew==0)
                    {   // ïîëó÷àåì óêàçàòåëü äëÿ çàïèñè äàííûõ ïðîïóñêàÿ çàãîëîâîê
                        wr = (char*)pQueue->pWritePointer;
                        pQueue->pWritePointer = pQueue->LastWrite;
                        if( pQueue->pWritePointer->cbytes % nAligmentSize ) 
                          {
                            wr_aligment = pQueue->pWritePointer->cbytes % nAligmentSize;

                            if( bAligment==TRUE )  nAligment-=wr_aligment;
                            else                   { bAligment = TRUE; nAligment =wr_aligment;} // íîâûõ òî áàéòîâ ÷åòíîå êîëè÷åñòâî, à âîò ñòàðûõ òî íå÷åòíîå
                          } 


                    } else { 
                        wr_aligment = 0;
                        // ïîëó÷àåì óêàçàòåëü äëÿ çàïèñè äàííûõ ïðîïóñêàÿ çàãîëîâîê
                        wr = (char*)__w__queue_itemdata(pQueue); // âûðîâíåí íà 2 áàéòà
                    } 
                   

                    //  åñëè êîíêàòèíèðóåì òî ïåðåäâèãàåì óêàçàòåëü çàïèñè äàííûõ íà êîíåö äàííûõ ýòîãî ýëåìåíòà
                    //if(AddAsNew==0) // !!! wr = wr + (*)
                    //   if(PTR(wr+=__w__queue_itemsize(pQueue)) >= PTR(__w__queue_end(pQueue)))
                    //       wr = ((PTR(wr) - PTR(__w__queue_end(pQueue)))) + (char*)pQueue->pQueueHead - 2?;


                    // åñëè äîáàâëÿåì íîâûé ýëåìåíò
                    if(AddAsNew==1)
                    {   // òî ïðîâåðÿåì âîéäåò ëè çàãîëîâîê (óêàçàòåëü çàïèñè âûðîâíåí íà 2 áàéòà (ðàçìåð çàãîëîâêà)
                        if( PTR(__queue_write(pQueue))  >= PTR(__w__queue_end(pQueue)) )
                        {
                            __queue_write(pQueue) = __queue_head(pQueue);

                            wr = (char*)__w__queue_itemdata(pQueue);
                        }
                        newbytes = bytes; // newbytes ñêîëüêî êîïèðîâàòü - êîïèðîâàòü âñ¸
                    }
                    else
                    {
                        newbytes = bytes; // newbytes ñêîëüêî êîïèðîâàòü - êîïèðîâàòü íîâûå áàéòû
                        bytes    = newbytes + __w__queue_itemsize(pQueue);// åñëè êîíêàòèíèðóåì, ÷èñëî áàéòîâ óâåëè÷èòñÿ. íàäî ïðî÷èòàòü ïðîøëîå êîëè÷åñòâî è ïðèáàâèòü Bytes
                    }

                    // óñòàíàâëèâàåì ðàçìåð ýëåìåíòà
                    __w__queue_itemsize(pQueue) = bytes;

                    wr-=wr_aligment;  // ïåðåïèøåì âûðâàíèâàþùèå áàéòû

                    // rem - áàéòîâ äî êîíöà áëîêà ïàìÿòè
                    if(AddAsNew==0) rem = ((int)__w__queue_end( pQueue ) - (int)(wr)); //-sizeof(QUEUE_ENTRY)?? ;
                    else            rem = __w__queue_end_remaining(pQueue) - sizeof(QUEUE_ENTRY);

                    // ìåòêà ïîñëåäíåé çàïèñè
                    pQueue->LastWrite = __queue_write(pQueue);

                    if( rem >= newbytes + nAligment ) // newbytes ñêîëüêî êîïèðîâàòü
                    {
                        // newbytes ñêîëüêî êîïèðîâàòü
                        s_memcpy( wr, pData, newbytes );

                        // ê êîíöó äàííûõ ê íà÷àëó ñëåäóþùåãî QUEUE_ENTRY
                        __queue_write(pQueue) = (QUEUE_ENTRY*)( wr + newbytes + nAligment);

                    }
                    else
                    {
                        s_memcpy( wr, pData, rem );

                        newbytes-=rem; // newbytes ñêîëüêî êîïèðîâàòü

                        wr = (char*)pQueue->pQueueHead;

                        pData+=rem;

                       // newbytes ñêîëüêî êîïèðîâàòü
                        s_memcpy( wr, pData, newbytes );  

                        __queue_write(pQueue) = (QUEUE_ENTRY*)(wr + newbytes + nAligment);
                    }

                    // ÷èñòèì ìóñîð , âåäü ìû äîáàâèëè áîëüøå ÷åì íàäî (bAligment=1)
                    if(bAligment) wr[newbytes] = 0x00;

                    rc = TRUE;
                }
    CRITICAL_SECTION_LEAVE();
    return rc;
}

*/

BOOL queue_add( QUEUE * pQueue, const BYTE * pData, size_t bytes)
{
    return queue_cat_ex( pQueue, (const uint8_t*)pData, bytes, true );
}

BOOL queue_cat( QUEUE * pQueue, const BYTE * pData, size_t bytes)
{
    return queue_cat_ex( pQueue, (const uint8_t*)pData, bytes, false );
}
// --------------------------------------------------------

static BOOL queue_popbytes( QUEUE * pQueue, void * pBytes, size_t nSize )
{
   // validate pointers  
   if( pQueue->pWritePointer == pQueue->pReadPointer )
   {
       // check for EMPTY condition
       if( pQueue->size == pQueue->bytes_available || 0 == pQueue->count )
       {
           return false;
       }
   }
   
   if(  (ptrdiff_t)pQueue->pReadPointer < (ptrdiff_t)pQueue->pWritePointer ) 
   {
     // . - free
     // # - used
     // R - read ptr
     // W - write ptr     
     // Buffer: [.....R######W.....]
     // Bank:          -1st--
     // bytes used since the R-ptr til the W-ptr
     size_t nBytesUsedTilWrite = (size_t)(
                                    (ptrdiff_t)pQueue->pWritePointer
                                  - (ptrdiff_t)pQueue->pReadPointer );
     
     // Check the limits
     if( nSize > nBytesUsedTilWrite )
     {
         return false;
     }
     
     // move the data from the queue    
     memcpy( (void*)(pBytes),
             (void*)pQueue->pReadPointer,
             nSize );
          
     // shift the R-ptr     
     queue_shift_fwd_rptr( pQueue, (nSize) );
     
     // calculate the bytes availabe
     pQueue->bytes_available += (nSize);
     
     // do not change the count of elements (do it in the caller)
     (void)pQueue->count;
   }
   else
   {
     // . - free
     // # - used
     // R - read ptr
     // W - write ptr     
     // Buffer: [#####W.....R######]
     // Bank:    -2nd-        -1st- 
     
     // bytes used since the R-ptr til the end of buffer (1st bank)
     size_t nBytesTilEnd  = ((ptrdiff_t)pQueue->pQueueHead)
                              + pQueue->size
                               - ((ptrdiff_t)pQueue->pReadPointer);
     
     // bytes used since the beginning of the buffer til the W-ptr (2st bank)
     size_t nBytesSinceStart = ((ptrdiff_t)pQueue->pWritePointer)
                               - ((ptrdiff_t)pQueue->pQueueHead);
     
     // Check the limits
     if( nSize > nBytesTilEnd + nBytesSinceStart )
     {
         return false;
     }
     
     size_t nCpy = nSize;
     
     // Check the limits
     if( nCpy > nBytesTilEnd )
     {
         nCpy = nBytesTilEnd;
     }
     
     // move the data from the queue (1st bank)     
     memcpy( (void*)pBytes,
             (void*)pQueue->pReadPointer,
             nCpy );
        
     // shift the R-ptr     
     queue_shift_fwd_rptr( pQueue, (nCpy) );
     
     // calculate the bytes availabe
     pQueue->bytes_available += nCpy;
     
     // decrease the size to copy
     nSize -= nCpy;
     
     // move the data from the queue (2st bank)     
     memcpy( (void*)((ptrdiff_t)pBytes + nCpy),
             (void*)pQueue->pQueueHead,
             nSize );
     
     // shift the R-ptr     
     queue_shift_fwd_rptr( pQueue, (nSize) );
     
     // calculate the bytes availabe
     pQueue->bytes_available += nSize;
     
     // do not change the count of elements (do it in the caller)
     (void)pQueue->count;
   }
   
   return true;
}

BOOL queue_get( QUEUE * pQueue, BYTE * pBuffer, size_t cbMaxSize, size_t * pLength )
{
   BOOL rc = false;

   // check the input arguments
   if(   NULL == pQueue
      || NULL == pBuffer
    /*|| 0    == cbMaxSize */  // allow zero buffer to retireve the top element size
      || NULL == pLength )
   return false;
   
   CRITICAL_SECTION_ENTER();
   
   // check the limits
   if(   0 == pQueue->count
      || pQueue->size == pQueue->bytes_available )
   goto L_queue_get_EXIT;
   
   {
       if( (ptrdiff_t)pQueue->pReadPointer ==
           (ptrdiff_t)pQueue->pQueueHead + pQueue->size )
       {
           pQueue->pReadPointer = pQueue->pQueueHead;       
       }
       
       // retrieve the top element size
       size_t nElementSize = pQueue->pReadPointer->cbytes;
       
       // return the element size
       *(pLength) = nElementSize;
       
       // check the input buffer size
       if( nElementSize > cbMaxSize )
       goto L_queue_get_EXIT;
       
       {
           // calculate amount of aligning bytes in the element
           size_t alignBytes = sizeAlign( nElementSize, sizeof(QUEUE_ENTRY) );   
              
           if( nElementSize + alignBytes + pQueue->bytes_available >
                pQueue->size )
           {
               #warning debug
               //asm("bkpt #0");
               goto L_queue_get_EXIT;
           }
           
           {
               // store the backup info
               QUEUE_ENTRY * pReadPointerBackup = pQueue->pReadPointer;
               size_t bytesAvailableBackup = pQueue->bytes_available;   
               
               // shift the R-ptr: skip the element header (QUEUE_ENTRY)
               queue_shift_fwd_rptr( pQueue, sizeof(QUEUE_ENTRY) );
               
               // calculate the available bytes
               pQueue->bytes_available += sizeof(QUEUE_ENTRY);
               
               // retrieve the data from the queue
               if( !queue_popbytes( pQueue, pBuffer, nElementSize ) )
               {
                   // error
                 
                   // roll back the changes
                   pQueue->bytes_available = bytesAvailableBackup;
                   pQueue->pReadPointer = pReadPointerBackup;
                   
                   goto L_queue_get_EXIT;
               }
               
               {
                   // pop the aligning bytes
                   queue_shift_fwd_rptr( pQueue, alignBytes );
                   
                   // and recalculate the bytes available
                   pQueue->bytes_available += alignBytes;
                   
                   // decrease amount of elements
                   pQueue->count--;
                   
                   // just for order
                   if( 0 == pQueue->count )
                   {
                       pQueue->LastWrite = NULL;
                   }
                   
                   rc = true;
               }
           }
       }
   }
     
L_queue_get_EXIT:
  
   CRITICAL_SECTION_LEAVE();
  
   return rc;
}

////// pReturnData -óêàçàòåëü íà ïðèåìíûé áóôåð
////// cbMaxSize - ðàçìåð ïðèåìíîãî áóôåðà
////// pLength - óêàçàòåëü íà ïåðåìåííóþ êóäà çàïèøåòñÿ ðåàëüíàÿ äëèííà
////BOOL queue_get_foo( QUEUE * pQueue, BYTE * pReturnData, size_t cbMaxSize, size_t * pLength )
////{
////    BOOL rc = FALSE;
////    int rem, bytes;
////    int nAligmentSize = sizeof(QUEUE_ENTRY);
////    CRITICAL_SECTION_ENTER();
//////printf("pQueue=%x\npReturnData=%x\npLength=%d\nqueue_count=%d\nqueue_isfull=%d\n");
////    if( pQueue != 0 )
////        if( pReturnData != 0 )
////            if( pLength != 0 )
////                if( __queue_count(pQueue) > 0 )
////                    if(  __queue_isfull(pQueue) )
////                    {
////                        if(PTR(__r__queue_end(pQueue))  <= (PTR(__queue_read(pQueue)) + sizeof(QUEUE_ENTRY)))
////                        {
////                            __queue_read(pQueue) = __queue_head(pQueue);
////
////
////                        }
////
////                        bytes = __queue_itemsize(pQueue);
////
////                        if( bytes <= cbMaxSize )
////                        {
////                            *pLength = bytes;
////                            pQueue->count--;
////                            pQueue->bytes_available += (__queue_itemsize(pQueue) + sizeof(QUEUE_ENTRY));
////
////                            rem = __r__queue_end_remaining(pQueue)-sizeof(QUEUE_ENTRY);
////
////                            if(bytes/*+sizeof(QUEUE_ENTRY)*/ <= rem )
////                            {
////                                s_memcpy(pReturnData,  __queue_itemdata(pQueue), __queue_itemsize(pQueue) );
////
////                                __queue_read(pQueue) = (QUEUE_ENTRY*)(__queue_itemdata(pQueue) + __queue_itemsize(pQueue)) ;
////
////                                // ------------------------- ALIGMENT --------------------------------------------
////                                if( PTR(pQueue->pReadPointer) % nAligmentSize )
////                                    __queue_read(pQueue) = (QUEUE_ENTRY*)(PTR(pQueue->pReadPointer) + (PTR(pQueue->pReadPointer)%nAligmentSize));
////
////                                if( (int)pQueue->pReadPointer >= (int)__r__queue_end(pQueue))
////                                    __queue_read(pQueue) = (QUEUE_ENTRY*)((BYTE*)(__queue_head(pQueue)));
////                                //- ------------------------------------------------------------------------------
////
////                            }
////                            else
////                            {
////
////                                // rem ÍÅ âêëþ÷àåò ðàçìåð çàãîëîâêà
////                                s_memcpy(pReturnData,  __queue_itemdata(pQueue), rem/*-sizeof(QUEUE_ENTRY)*/);
////                                
////                                *pLength = bytes;
//////#error 4 ðàçà îòïðàâèòü "PATH:STAT? A,B"  è ÷èòàòü ñðàçó. íà ïÿòûé ÷òåíèå ïîïàäåò ñþäà. ñì - äâà áàéüà âûëàçÿò çà ãðàíèöó. êîñÿê â add?
////                                bytes-=(rem/*-sizeof(QUEUE_ENTRY)*/);
////
////                                s_memcpy(pReturnData+rem/*-sizeof(QUEUE_ENTRY)*/,  __queue_head(pQueue), bytes);
////
////                                __queue_read(pQueue) = (QUEUE_ENTRY*)((BYTE*)(__queue_head(pQueue)) + bytes );
////    
////                                // ------------------------- ALIGMENT --------------------------------------------
////                                if( PTR(pQueue->pReadPointer) % nAligmentSize )
////                                    __queue_read(pQueue) = (QUEUE_ENTRY*)(PTR(pQueue->pReadPointer) + (PTR(pQueue->pReadPointer)%nAligmentSize));
////
////                                if( (int)pQueue->pReadPointer >= (int)__r__queue_end(pQueue))
////                                    __queue_read(pQueue) = (QUEUE_ENTRY*)((BYTE*)(__queue_head(pQueue)));
////                                //- ------------------------------------------------------------------------------
////                            }
////
////
////                            rc = TRUE;
////                        } else if(pLength != NULL) *pLength = bytes;
////                    }
////    CRITICAL_SECTION_LEAVE();
////    return rc;
////}
/*
BOOL queue_peeklastbyte( QUEUE * pQueue, BYTE * pReturnByte )
{
    BOOL rc = FALSE;
    unsigned int dwLength;
    if( TRUE==queue_get_topitemsize( pQueue, &dwLength ))
        if( queue_peekbyte(pQueue,pReturnByte,dwLength-1))
            return TRUE;
}

BOOL queue_peekbyte( QUEUE * pQueue, BYTE * pReturnByte, int Number )
{
    BOOL rc = FALSE;
    int rem, bytes;

    CRITICAL_SECTION_ENTER();
    if( pQueue != 0 )
        if( pReturnByte != 0 )
            if( __queue_count(pQueue) > 0 )
                if( __queue_isfull(pQueue) )
                {
                    if(__r__queue_end(pQueue) - __queue_read(pQueue) < 2)
                    {
                        __queue_read(pQueue) = __queue_head(pQueue);

                    }

                    bytes = __queue_itemsize(pQueue);

                    if( Number < bytes )
                    {
                        rem = __r__queue_end_remaining(pQueue);

                        if(Number+1 <= rem )
                        {
                            *pReturnByte = *(__queue_itemdata(pQueue)+Number);

                        }
                        else
                        {
                            Number-=rem;
                            *pReturnByte = *( (BYTE*)__queue_head(pQueue) +Number);
                        }
                        rc = TRUE;
                    }
                }
    CRITICAL_SECTION_LEAVE();
    return rc;
}
*/
// --------------------------------------------------------
/*
BOOL queue_getsize( QUEUE * pQueue, size_t * pSize )
{
    BOOL rc = FALSE;
    CRITICAL_SECTION_ENTER();
    if( pQueue != 0 )
        if( pSize != 0 )
        {
            *pSize = __queue_size(pQueue);
            rc = TRUE;
        }
    CRITICAL_SECTION_LEAVE();
    return rc;
}
*/
// --------------------------------------------------------
BOOL queue_getcount( QUEUE * pQueue, size_t * pCount )
{
    BOOL rc = FALSE;
    CRITICAL_SECTION_ENTER();
    if( pQueue != 0 )
        if( pCount != 0 )
        {
            *pCount = pQueue->count;
            rc = TRUE;
        }
    CRITICAL_SECTION_LEAVE();
    return rc;
}
// --------------------------------------------------------
/*
BOOL queue_bytes_available( QUEUE * pQueue, size_t * pBytes )
{
    BOOL rc = FALSE;
    CRITICAL_SECTION_ENTER();
    if( pQueue != 0)
        if( pBytes != 0 )
        {
            *pBytes = __queue_bytesavailable(pQueue);
            rc = TRUE;
        }
    CRITICAL_SECTION_LEAVE();
    return rc;
}
*/
// --------------------------------------------------------
// queue_clear()
// clears the queue: erases all the data from the queue
BOOL queue_clear( QUEUE * pQueue )
{
    BOOL rc = FALSE;
    
    if( NULL != pQueue )
    {
        CRITICAL_SECTION_ENTER();
        
        pQueue->pWritePointer = pQueue->pQueueHead;
        pQueue->pReadPointer = pQueue->pQueueHead;
        pQueue->bytes_available = pQueue->size;
        pQueue->count = 0;
        pQueue->LastWrite = NULL;
        
        CRITICAL_SECTION_LEAVE();
        
        rc = true;
    }
    
    return rc;
}
// --------------------------------------------------------
// queue_create()
// creates the queue on the memory block
BOOL queue_create( BYTE * memoryblock, size_t size, QUEUE * pQueue )
{
    BOOL rc = FALSE;
    
    // check the input agruments
    if(    NULL == memoryblock
        || 0 == size 
        || NULL == pQueue )
    return false;
    
    if(   size > 0 
       && size <= MAX_QUEUE_SIZE 
       && CHECK_ALIGN_FOR_TYPE(size, uint32_t) )
    {  
       CRITICAL_SECTION_ENTER();
       pQueue->size = size;
       pQueue->pQueueHead = (QUEUE_ENTRY*)memoryblock;
       pQueue->pWritePointer = (QUEUE_ENTRY*)memoryblock;
       pQueue->pReadPointer = (QUEUE_ENTRY*)memoryblock;
       pQueue->bytes_available = size;
       pQueue->count = 0;
       pQueue->LastWrite = NULL;
       CRITICAL_SECTION_LEAVE();
       
       rc = true;   
    }    

    return rc;
}
// ------------------------------------------------------------------
// queue_get_topitemsize()
// retieves the size of the top element in the queue
BOOL queue_get_topitemsize( QUEUE * pQueue, size_t * pSize )
{
    BOOL rc = FALSE;
    CRITICAL_SECTION_ENTER();
    if( NULL != pQueue )
    if( NULL != pSize )
    if( 0    != pQueue->count )
    {
       *pSize = ( (size_t) ((QUEUE*)pQueue)->pReadPointer->cbytes );
       rc = TRUE;
    }
    CRITICAL_SECTION_LEAVE();
    return rc;
}
// --------------------------------------------------------
/*
uint8_t queue_test_buffer[ 276 ];
uint8_t queue_test_buffer_rx[ 32 ];

QUEUE testQueue;

void queue_test1();
void queue_test2();
void queue_test3();

void queue_test()
{
   queue_create( queue_test_buffer, sizeof(queue_test_buffer), & testQueue );
   
   
   do
   {
   queue_test1();
   queue_test2();
   queue_test1();
   queue_test3();
   queue_test1();
   queue_test2();
   queue_test2();
   queue_test3();
   queue_test2();
   queue_test3();
   queue_test1();
   }
   while(1);
   
   asm("bkpt #0");
}

void queue_test1()
{
   // queue_create( queue_test_buffer, sizeof(queue_test_buffer), & testQueue );
   
   // while( true )
   {
       queue_add( & testQueue, "[Element #1]", 12 );
       queue_add( & testQueue, "[Element_ #2]", 13 );
       queue_add( & testQueue, "[Element___ #3]", 15 );
     
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 12 ) asm("bkpt #1");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 13 ) asm("bkpt #2");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 15 ) asm("bkpt #3");
       }
       
       queue_add( & testQueue, "[Element #4]", 12 );
       queue_add( & testQueue, "[Element_ #5]", 13 );
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 12 ) asm("bkpt #4");
       }
       
       queue_add( & testQueue, "[Element___ #6]", 15 );       
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 13 ) asm("bkpt #5");
       }
       
       queue_add( & testQueue, "[Element_____ #7]", 17 );
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 15 ) asm("bkpt #6");
       }
       
       queue_add( & testQueue, "[Element_____ #8]", 17 );
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 17 ) asm("bkpt #7");
       }
       
       queue_add( & testQueue, "[Element_ #9]", 13 );
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 17 ) asm("bkpt #8");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );
       if( rx != 13 ) asm("bkpt #9");
       }
   }
}

void queue_test2()
{
   // queue_create( queue_test_buffer, sizeof(queue_test_buffer), & testQueue );
   
   // while( true )
   {
       {
       queue_add( & testQueue, "[Element #1", 11 );
       queue_cat( & testQueue, "]", 1 );
       }
       
       {
       queue_add( & testQueue, "[Element_ #2]", 13 );
       }
       
       {
       queue_add( & testQueue, "[Element___ #", 13 );
       queue_cat( & testQueue, "3]", 2 );
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 12 ) asm("bkpt #1");
       }
       
       queue_add( & testQueue, "[Element_ #4]", 13 );
       
       queue_add( & testQueue, "[Element_ ", 10 );
       queue_cat( & testQueue, "#5]", 3 );
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 13 ) asm("bkpt #2");
       }
       
       {
       queue_add( & testQueue, "[Element", 8 );
       queue_cat( & testQueue, " #6]", 4 );
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 15 ) asm("bkpt #3");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 13 ) asm("bkpt #4");
       }
       
       {
       queue_add( & testQueue, "[Element #", 10 );       
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 13 ) asm("bkpt #5");
       }
       
       {
       queue_cat( & testQueue, "7]", 2 );       
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 12 ) asm("bkpt #6");
       }
              
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 12 ) asm("bkpt #7");
       }
   }
}

void queue_test3()
{
   // queue_create( queue_test_buffer, sizeof(queue_test_buffer), & testQueue );
   
   // while( true )
   {
       {
       queue_add( & testQueue, "[Element           #1", 21 );
       queue_cat( & testQueue, "]", 1 );
       }
       
       {
       queue_add( & testQueue, "[Element_                 #2]", 29 );
       }
       
       {
       queue_add( & testQueue, "[Element___           #", 23 );
       queue_cat( & testQueue, "3]", 2 );
       }

       {
       queue_add( & testQueue, "[Element_           #4]", 23 );
       }
       
       {
       queue_add( & testQueue, "[Element_               ", 24 );
       queue_cat( & testQueue, "#5", 2 );
       queue_cat( & testQueue, "]", 1 );
       }
       
       {
       queue_add( & testQueue, "[Element          ", 18 );
       queue_cat( & testQueue, " #6]", 4 );
       }
       
       
       {
       queue_add( & testQueue, "[Element            #", 21 ); 
       queue_cat( & testQueue, "7]", 2 );             
       }
       
       {
       queue_add( & testQueue, "[Element      ", 14 );
       queue_cat( & testQueue, " #", 2 );
       queue_cat( & testQueue, "8]", 2 );
       }
       
       {
       queue_add( & testQueue, "[Element            ", 20 );
       queue_cat( & testQueue, " #9]", 4 );
       }
       
       {
       queue_add( & testQueue, "[Element    ", 12 );       
       queue_cat( & testQueue, "  ", 2 );
       queue_cat( & testQueue, "#10]", 4 );
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 22 ) asm("bkpt #1");
       }
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 29 ) asm("bkpt #2");
       }
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 25 ) asm("bkpt #3");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 23 ) asm("bkpt #4");
       }
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 27 ) asm("bkpt #5");
       }
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 22 ) asm("bkpt #6");
       }
              
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 23 ) asm("bkpt #7");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 18 ) asm("bkpt #8");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 24 ) asm("bkpt #9");
       }
       
       {
       size_t rx;
       queue_get( & testQueue, queue_test_buffer_rx, sizeof(queue_test_buffer_rx), &rx );       
       if( rx != 18 ) asm("bkpt #10");
       }
   }
}
*/