Mailbox and Mail

#ifndef __MAILBOX_H__
#define __MAILBOX_H__

#include <stdint.h>
#include <stdlib.h>
#include <string.h>

typedef struct
{
  // uint32_t Capacity;
  uint8_t * Memory;
  uint32_t MailSize;
  uint32_t MailCount;
  uint32_t ReadIndex;
  uint32_t ReadCount;
} MAILBOX;

// Creates a new mailbox and Init it.
MAILBOX * MBX_Create( uint32_t MailSize, uint32_t MailCount );

// Deletes a specified mailbox.
void MBX_Delete( MAILBOX * Mailbox );

// Clears all messages in a specified mailbox.
void MBX_Clear( MAILBOX * Mailbox );

// Init a new mailbox.
void MBX_Init( MAILBOX * Mailbox, uint32_t MailSize, uint32_t MailCount,
  void * Memory );

// Stores a new message of a predefined size in a mailbox.
uint32_t MBX_Write( MAILBOX * Mailbox, void * Mail );

/*
 * Stores a new message of a predefined size into a mailbox in front of all
 * other messages, if the mailbox is able to accept one more message.
 * The new message will be retrieved first.
 */
uint32_t MBX_WriteFront( MAILBOX * Mailbox, void * Mail );

/* Retrieves a new message of a predefined size from a mailbox.
 * Mail : Pointer to the memory area that the message should be stored at.
 * Make sure that it points to a valid memory area and that there is sufficient
 * space for an entiremessage. The message size (in bytes) was defined
 * when the mailbox was created.
 */
uint32_t MBX_Read( MAILBOX * Mailbox, void * Mail );

// Returns the number of mail currently available in a specified mailbox.
uint32_t MBX_GetCount( MAILBOX * Mailbox );

#endif /* __MAILBOX_H__ */

#include "mailbox.h"
#include "macro_misc.h"
#include "cmsis_os.h"

// Creates a new mailbox and Init it.
MAILBOX * MBX_Create( uint32_t MailSize, uint32_t MailCount )
{
  uint32_t Size = ALIGN_UP( sizeof(MAILBOX), 4 ) + MailSize * MailCount;
  MAILBOX * Mailbox = (MAILBOX *) osMalloc( Size, osWaitForever );
  if ( Mailbox == 0 )
    return Mailbox;

  uint8_t * Memory = //
    (uint8_t *) ( ( (uint32_t) ( Mailbox ) ) + ALIGN_UP( sizeof(MAILBOX), 4 ) );

  MBX_Init( Mailbox, MailSize, MailCount, Memory );

  return Mailbox;
}

// Deletes a specified mailbox.
void MBX_Delete( MAILBOX * Mailbox )
{
  osFree( Mailbox );
}

// Clears all messages in a specified mailbox.
void MBX_Clear( MAILBOX * Mailbox )
{
  Mailbox->ReadCount = 0;
}

// Returns the number of mail currently available in a specified mailbox.
uint32_t MBX_GetCount( MAILBOX * Mailbox )
{
  return Mailbox->ReadCount;
}

// Init a new mailbox.
void MBX_Init( MAILBOX * Mailbox, uint32_t MailSize, uint32_t MailCount,
  void * Memory )
{
  // Mailbox->Capacity = MailCount * MailSize;
  Mailbox->MailSize = MailSize;
  Mailbox->MailCount = MailCount;
  Mailbox->Memory = Memory;
  Mailbox->ReadIndex = 0;
  Mailbox->ReadCount = 0;
}

/* Stores a new message of a predefined size in a mailbox.
 *
 * 0: Message could not be stored (mailbox is full).
 * 1: Success; message stored.
 */
uint32_t MBX_Write( MAILBOX * Mailbox, void * Mail )
{
  if ( Mailbox->ReadCount == Mailbox->MailCount )
    return 0;

  uint32_t Value = osDisableInterrupt( );

  uint32_t IndexToWrite = ( Mailbox->ReadIndex + Mailbox->ReadCount );
  if ( IndexToWrite == Mailbox->MailCount )
    IndexToWrite = 0;

  uint32_t MemoryIndexToWrite = IndexToWrite * Mailbox->MailSize;
  memcpy( &Mailbox->Memory[ MemoryIndexToWrite ], Mail, Mailbox->MailSize );
  Mailbox->ReadCount++;

  osRestoreInterrupt( Value );

  return 1;
}

/*
 * Stores a new message of a predefined size into a mailbox in front of all
 * other messages, if the mailbox is able to accept one more message.
 * The new message will be retrieved first.
 *
 * 0: Message could not be stored (mailbox is full).
 * 1: Success; message stored.
 */
uint32_t MBX_WriteFront( MAILBOX * Mailbox, void * Mail )
{
  if ( Mailbox->ReadCount == Mailbox->MailCount )
    return 0;

  if ( Mailbox->ReadCount == 0 )
    return MBX_Write( Mailbox, Mail );

  uint32_t Value = osDisableInterrupt( );

  uint32_t IndexToWrite;

  if ( Mailbox->ReadIndex )
    IndexToWrite = Mailbox->ReadIndex - 1;
  else
    IndexToWrite = Mailbox->MailCount - 1;

  uint32_t MemoryIndexToWrite = IndexToWrite * Mailbox->MailSize;
  memcpy( &Mailbox->Memory[ MemoryIndexToWrite ], Mail, Mailbox->MailSize );

  Mailbox->ReadIndex = IndexToWrite;
  Mailbox->ReadCount++;

  osRestoreInterrupt( Value );

  return 1;
}

/* Retrieves a new message of a predefined size from a mailbox.
 * Mail : Pointer to the memory area that the message should be stored at.
 * Make sure that it points to a valid memory area and that there is sufficient
 * space for an entiremessage. The message size (in bytes) was defined
 * when the mailbox was created.
 *
 * 0: Message could not be retrieved (mailbox is empty)
 * 1: Success; message retrieved.
 */
uint32_t MBX_Read( MAILBOX * Mailbox, void * Mail )
{
  if ( Mailbox->ReadCount == 0 )
    return 0;

  uint32_t Value = osDisableInterrupt( );

  uint32_t MemoryIndexToRead = Mailbox->ReadIndex * Mailbox->MailSize;
  memcpy( Mail, &Mailbox->Memory[ MemoryIndexToRead ], Mailbox->MailSize );

  Mailbox->ReadIndex++;
  if ( Mailbox->ReadIndex == Mailbox->MailCount )
    Mailbox->ReadIndex = 0;
  Mailbox->ReadCount--;

  osRestoreInterrupt( Value );

  return 1;
}