Lemz_SwCtrlStmProj/App/scpi/scpi_errq.c

#include <string.h>
#include "app/scpi/scpi_errq.h"
#include "app/queue/queue.h"
#include "app/queue/queue_ex.h"

typedef struct
{
   queue_record_t header;      // queue record header
   int16_t errorCode;          // SCPI error code   
}
sSCPIErrorHeader_t;

void errq_clear( errq_t * self )
{
    while( !errq_isempty(self) )
          errq_removetop(self);
}

bool errq_removetop( errq_t * self )
{
    return (0 < queue_removefront( self ));
}

bool errq_isempty( errq_t * self )
{
    return (queue_empty( self ));
}

bool errq_peek( errq_t * self, int16_t * perrorCode, char * message, size_t * msgBufSize )
{
   sSCPIErrorHeader_t record;

   assert( self );
   
   if( sizeof(record.header) == queue_peek_payload( self, &record.header, sizeof(record.header), NULL, 0 ) )
   {
       assert( record.header.size >= sizeof(record) );

       int16_t msgBuf = 0;
       if( NULL != message && NULL != msgBufSize && 0 != *msgBufSize )
       {
           if( record.header.payload > *msgBufSize )
           {
               return false;
           }

           msgBuf = *msgBufSize;
       }
       
       if( record.header.size == queue_peek_payload( self, &record.header, sizeof(record), message, msgBuf ) )
       {
           if( NULL != perrorCode )
           {
               *perrorCode = record.errorCode;
           }

           if( NULL != message && 0 != msgBuf && NULL != msgBufSize ) 
           {
               *msgBufSize = record.header.payload;
           }

           return true;
       }
   }

   return false;
}

bool errq_pop( errq_t * self, int16_t * perrorCode, char * message, size_t * msgBufSize )
{
   bool rc = false;

   rc = errq_peek( self, perrorCode, message, msgBufSize );

   if( rc ) queue_removefront( self );

   return rc;
}

bool errq_push( errq_t * self, int16_t errorCode, const char * message )
{
   bool rc = false;
   sSCPIErrorHeader_t record = {
      .header = {
         .size = sizeof( sSCPIErrorHeader_t ),
         .payload = 0
      },
      .errorCode = errorCode 
   };

   if( NULL != message )
   {
       size_t len = strlen( message );

       record.header.size += (len+1);
       record.header.payload = (len+1);

       rc = (QUEUE_RC_NOERROR == queue_pushback_payload( self, &record.header, message ));
   }
   else
   {
       rc = (QUEUE_RC_NOERROR == queue_pushback( self, &record.header ));
   }

   return rc;
}