/*********************************  fifos.h  *********************************
 * $Source: /home/cvs/ESP/gen2/software/msp430/include/fifos.h,v $
 *  Copyright (C) 2003 MBARI
 *
 *  MBARI Proprietary Information. All rights reserved.
 * $Id: fifos.h,v 1.29 2005/12/12 09:22:54 brent Exp $
 *
 * Efficient Byte oriented FIFO primitives
 *
 * read* and write* functions update the head and tail offests after
 * the buffer is read or written respectively.  This allows FIFOs with only one
 * reader process and one writer process to operate _without_ 
 * any additional semaphores to serialize access.
 * Multiple readers or writers are still required to serialize access 
 * to these FIFOs via semaphore(s).
 *
 *****************************************************************************/

#ifndef FIFOS_H
#define FIFOS_H

// INCLUDES

#include "types.h"   //standard types

// TYPEDEFS

typedef struct {   //ring buffer (dynamic part) of a fifo
  byte *head, *tail;  //head and tail pointers
  byte   buffer[1];   //placeholder for actual fifo ring buffer
} fifoBody;

typedef const struct {   //FIFO descriptor  (the unchanging part)
  fifoBody *body;
  byte *end;           //points just past the end of body->buffer
  unsigned limit;      //buffer offset of FIFOend
} fifo;

//declare a fifo of specified size with the descriptor in ROM
#define FIFO(name,size) \
  struct {byte *head, *tail; byte buffer[1+(size)];} name##_body; \
  fifo name = {(fifoBody *) &name##_body, \
              name##_body.buffer+1+(size), 1+(size)}
  

// MACROS and INLINE functions

#define FIFObase(r) ((r)->body->buffer) // pointer to the base of the buffer
#define FIFOend(r)  ((r)->end)          // pointer just past  end of the buffer
#define FIFOsize(r) ((r)->limit - 1)    //# of bytes in fifo when full
#define FIFOtail(r) ((r)->body->tail)   // pointer to the next byte to write.
#define FIFOhead(r) ((r)->body->head)   // pointer to the next byte to read.


static inline 
void FIFOclear (fifo *r)
{
  fifoBody *body = r->body;
  body->head = body->tail = body->buffer;
}


//return tail - head
static inline 
unsigned FIFOdiff (fifo *r)
{
  fifoBody *body = r->body;
  return body->tail - body->head;
}

//return true iff FIFO is empty
#define FIFOempty(r) (!FIFOdiff(r))

// return the # of bytes currently in the fifo
static inline 
unsigned FIFOlen (fifo *r) {
  unsigned len = FIFOdiff(r);
  if (len >= r->limit) len += r->limit;
  return len;
}

// return the # of bytes available in the fifo
static inline 
unsigned FIFOfree (fifo *r) {
  fifoBody *body = r->body;
  unsigned avail = body->head - body->tail - 1;
  if (avail >= r->limit) avail += r->limit; 
  return avail;
}
  
#define FIFOfull(r)  (!FIFOfree(r))


// return the # of bytes available in the fifo
static inline 
unsigned FIFOfreeFrom (fifo *r, byte *tail) {
  unsigned avail = FIFOhead(r) - tail - 1;
  if (avail >= r->limit) avail += r->limit; 
  return avail;
}
  
#define FIFOfullFrom(r, tail)  (!FIFOfreeFrom(r, tail))


// use with FIFOhead or FIFOtail to manipulate data in the fifo without copying
//  Advance head or tail pointer by stride bytes
//  stride must be <= FIFOsize
static inline
byte *FIFOadvance (fifo *r, byte *headOrTail, unsigned stride)
{
  headOrTail += stride;
  if (headOrTail >= FIFOend(r)) headOrTail -= r->limit;
  return headOrTail;
}


//inverse of FIFOadvance
static inline
byte *FIFOretreat (fifo *r, byte *headOrTail, unsigned stride)
{
  headOrTail -= stride;
  if (headOrTail < FIFObase(r)) headOrTail += r->limit;
  return headOrTail;
}


// return the # of bytes cursor has been advanced from base
static inline 
unsigned FIFOdelta (fifo *r, byte *cursor, byte *base) {
  unsigned len = cursor - base;
  if (len >= r->limit) len += r->limit;
  return len;
}


// use with FIFOhead or FIFOtail to manipulate data in the fifo without copying
//  Increment head or tail pointer by 1 byte
static inline
byte *FIFOinc (fifo *r, byte *headOrTail)
{
  if (++headOrTail >= FIFOend(r)) headOrTail -= r->limit;
  return headOrTail;
}

//inverse of FIFOinc
static inline
byte *FIFOdec (fifo *r, byte *headOrTail)
{
  if (--headOrTail < FIFObase(r)) headOrTail += r->limit;
  return headOrTail;
}


// return the maximum # of contiguous bytes that may be added to the fifo
// returns zero if and only if the fifo is full
static inline
unsigned FIFOpoke (fifo *r)
{
  fifoBody *body = r->body;
  byte *tail = body->tail;
  byte *head = body->head;
  byte *bound;
  if (tail < head)
    bound = head-1;
  else {  //must leave empty slot at fifo's end if head is at its base
    bound = FIFOend(r);
    if (head == body->buffer) --bound;
  }
  return bound - tail;
}


// add len bytes of src data to specified end of fifo r
// without verifying that there is room.  See FIFOpoke above.
// assumes len is > 0
// returns new tail
static inline
byte *FIFOaddChunk (fifo *r, byte *src, byte *dst, unsigned len)
{
  do *dst = *src++, dst++; while (--len);
  if (dst >= FIFOend(r)) dst=r->body->buffer;
  return dst;
}


// add len bytes of src data to the end of fifo r
// without verifying that there is room.  See FIFOpoke above.
// assumes len is > 0
static inline
void FIFOwriteChunk (fifo *r, byte *src, unsigned len)
{
  FIFOtail(r) = FIFOaddChunk (r, src, FIFOtail(r), len);
}


// return the max # of contiguous bytes that may be removed from head of fifo
// returns zero if and only if the fifo is empty
static inline
unsigned FIFOpeek (fifo *r)
{
  fifoBody *body = r->body;
  byte *tail = body->tail;
  byte *head = body->head;
  byte *bound = tail;
  if (tail < head) bound = FIFOend(r);
  return bound - head;
}


// move len bytes of src, the specified head of fifo r, to dst
// without verifying that there really is that much in the fifo. See FIFOpeek.
// assumes len is > 0
// returns new head
static inline
byte *FIFOreadChunkAhead (fifo *r, byte *src, byte *dst, unsigned len)
{
  do *dst = *src++, dst++; while (--len);
  if (src >= FIFOend(r)) src=r->body->buffer;
  return src;
}


// move len bytes of src data from the beginning of fifo r to dst
// without verifying that there really is that much in the fifo. See FIFOpeek.
// assumes len is > 0
static inline
void FIFOreadChunk (fifo *r, byte *dst, unsigned len)
{
  FIFOhead(r) = FIFOreadChunkAhead (r, FIFOhead(r), dst, len);
}


// remove a byte from a specified head of the fifo (assumes FIFO is not empty!)
//  (this allows read ahead and rescan to be implemented)
static inline 
unsigned FIFOreadByteAhead(fifo *r, byte **headPtr)
{
  fifoBody *body = r->body;
  byte *head = *headPtr;
  unsigned result = *head++;
  if (head >= FIFOend(r)) head = body->buffer;
  *headPtr = head;
  return result;
}

// remove a byte from the head of the fifo (assumes FIFO is not empty!)
static inline 
unsigned FIFOreadByte(fifo *r)
{
// equivalent to:  FIFOreadByteAhead (r, &(r->body.head))
  fifoBody *body = r->body;
  byte *head = body->head;
  unsigned result = *head++;
  if (head >= FIFOend(r)) head = body->buffer;
  body->head = head;
  return result;
}


// apppend a byte to the tail of the fifo (assumes FIFO is not full!)
static inline
void FIFOwriteByte(fifo *r, byte octet)
{
  fifoBody *body = r->body;
  byte *tail = body->tail;
  *tail=octet;
  if (++tail >= FIFOend(r)) tail = body->buffer;
  body->tail = tail;
}
static inline
void FIFOwriteByteW(fifo *r, unsigned octet)
{
  fifoBody *body = r->body;
  byte *tail = body->tail;
  *tail=octet;
  if (++tail >= FIFOend(r)) tail = body->buffer;
  body->tail = tail;
}


// apppend a byte to the tail of the fifo (fails if FIFO already full)
// returns zero if operation failed.
static inline
unsigned FIFOwriteByteIfNotFull(fifo *r, unsigned octet)
{
  fifoBody *body = r->body;
  byte *tail = body->tail;
  byte *newTail = tail;
  unsigned diff;
  if (++newTail >= FIFOend(r)) newTail = body->buffer;
  if (diff = (newTail - body->head)) {
    *tail = octet;
    body->tail=newTail;
  }
  return diff;
}


// append a 16-bit word to the tail of the fifo (assumes FIFOfree>=2)
static inline void FIFOwrite16 (fifo *r, uint16 word) {
  FIFOwriteByteW(r, word>>8); FIFOwriteByteW(r, word);
}

// append a 24-bit word to the tail of the fifo (assumes FIFOfree>=3)
static inline void FIFOwrite24 (fifo *r, uint32 word) {
  FIFOwrite16(r, word>>8); FIFOwriteByteW(r, word);
}

// append a 24-bit signed word to the tail of the fifo (assumes FIFOfree>=3)
#define FIFOwrite24signed(fifo,word) FIFOwrite24(fifo,word)

// append a 32-bit word to the tail of the fifo (assumes FIFOfree>=4)
static inline void FIFOwrite32 (fifo *r, uint32 word) {
  FIFOwrite16(r, word>>16); FIFOwrite16(r, word);
}


//******************
//  Functions below are especially relevant to parsing and composing messages
//Note that the bufAdd* varients are for linear buffers

//like FIFOwriteByte, but operates on a distinct, cached tail pointer
//thus, analogous to FIFOreadByteAhead
static inline 
byte *FIFOaddByte (fifo *r, byte *tail, byte octet)
{
  *tail = octet;
  return FIFOinc(r, tail);
}
static inline 
byte *FIFOaddByteW (fifo *r, byte *tail, unsigned octet)
{
  *tail = octet;
  return FIFOinc(r, tail);
}

// bufAdd* just appends the specified data type into a linear byte buffer
static inline
byte *bufAddByte (byte *cursor, byte octet)
{
  *cursor = octet;
  return ++cursor;
}
static inline
byte *bufAddByteW (byte *cursor, unsigned octet)
{
  *cursor = octet;
  return ++cursor;
}


// append a 16-bit word to specified tail of the fifo (assumes FIFOfree>=2)
static inline byte *FIFOadd16 (fifo *r, byte *tail, uint16 word) {
  return FIFOaddByte(r, FIFOaddByte(r, tail, word>>8), word);
}

static inline
byte *bufAdd16 (byte *cursor, unsigned word)
{
  return bufAddByteW (bufAddByteW (cursor, word>>8), word);
}


// append a 24-bit word to specified tail of the fifo (assumes FIFOfree>=3)
static inline byte *FIFOadd24 (fifo *r, byte *tail, uint32 word) {
  return FIFOaddByteW(r, FIFOadd16(r, tail, word>>8), word);
}

static inline
byte *bufAdd24 (byte *cursor, uint32 word)
{
  return bufAddByteW (bufAdd16 (cursor, word>>8), word);
}

// append 24-bit signed word to specified tail of the fifo (assumes FIFOfree>=3)
#define FIFOadd24signed(fifo,tail,word) FIFOadd24(fifo,tail,word)
#define bufAdd24signed(cursor,lword) bufAdd24(cursor,lword)

// append a 32-bit word to specified tail of the fifo (assumes FIFOfree>=4)
static inline byte *FIFOadd32 (fifo *r, byte *tail, uint32 word) {
  return FIFOadd16(r, FIFOadd16(r, tail, word>>16), word);
}

static inline
byte *bufAdd32 (byte *cursor, uint32 word)
{
  return bufAdd16 (bufAdd16 (cursor, word>>16), word);
}


byte *FIFOaddBlock (fifo *r, byte *src, byte *dst, unsigned len);
/*
  append len byte block to FIFO at specified tail dst
  returns new tail
  Assumes that len > 0 and that the block will fit in the fifo
*/
  

// remove a 16-bit word from the head of the fifo (assumes FIFOlen>=2)
static inline uint16 FIFOread16 (fifo *r) {
  uint16 high = FIFOreadByte(r)<<8;
  return high | FIFOreadByte(r);
}

// remove a 16-bit word from specified head of the fifo (assumes FIFOlen>=2)
static inline uint16 FIFOread16Ahead (fifo *r, byte **headPtr) {
  uint16 high = FIFOreadByteAhead(r, headPtr)<<8;
  return high | FIFOreadByteAhead(r, headPtr);
}

// remove a 24-bit word from the head of the fifo (assumes FIFOlen>=3)
static inline uint32 FIFOread24 (fifo *r) {
  uint16 high = FIFOreadByte(r);
  return (uint32)high<<16 | FIFOread16(r);
}

// remove a 24-bit word from specified head of the fifo (assumes FIFOlen>=3)
static inline uint32 FIFOread24Ahead (fifo *r, byte **headPtr) {
  uint16 high = FIFOreadByteAhead(r, headPtr);
  return (uint32)high<<16 | FIFOread16Ahead(r, headPtr);
}

// remove a 24-bit signed word from the head of the fifo (assumes FIFOlen>=3)
static inline uint32 FIFOread24signed (fifo *r) {
  uint16 high = FIFOreadByte(r);
  return (int32)(((uint32)high<<16 | FIFOread16(r)) <<8) >> 8;
}

// remove 24-bit signed word from specified head of the fifo (assumes FIFOlen>=3)
static inline uint32 FIFOread24signedAhead (fifo *r, byte **headPtr) {
  uint16 high = FIFOread16Ahead(r, headPtr);
  return (int32)(((uint32)high<<16 | FIFOread16Ahead(r, headPtr)) <<8) >> 8;
}

// remove a 32-bit word from the head of the fifo (assumes FIFOlen>=4)
static inline uint32 FIFOread32 (fifo *r) {
  uint32 high = FIFOread16(r)<<16;
  return high | FIFOread16(r);
}

// remove a 32-bit word from specified head of the fifo (assumes FIFOlen>=4)
static inline uint32 FIFOread32Ahead (fifo *r, byte **headPtr) {
  uint32 high = FIFOread16Ahead(r, headPtr)<<16;
  return high | FIFOread16Ahead(r, headPtr);
}

byte *FIFOreadBlockAhead (fifo *r, byte *src, byte *dst, unsigned len);
/*
   move len bytes of src, the specified head of fifo r, to dst
   without verifying that there really is that much in the fifo. See FIFOpeek.
   assumes len is > 0
   returns new head
*/


#endif