/*                                              -*- mode:C++; fill-column:100 -*-
  SFBFrame.cpp - struct and routines for FIFOing SFB Frames
  Copyright (C) 2008 The Regents of the University of New Mexico.  All rights reserved.

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.

  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.

  You should have received a copy of the GNU General Public License
  along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
  USA
*/

#include "SFBFrame.h"

SFBFrame::SFBFrame(SFBRxByteBuffer & r, SFBTxByteBuffer & t, PacketBuffer & p) 
  : rxBuf(r), txBuf(t), deframeBuffer(p)
{
  reset();
}

void SFBFrame::reset() {
  rxBuf.resetBG();
  txBuf.resetBG();
  txCount = 0;
  rxCount = -1;                 // Resetting a frame loses sync
  rxEscape = false;             // Not waiting on escape follow-up
}

bool SFBFrame::rxStoreRaw(u8 byte) {
  if (rxCount < MAX_PACKET_LENGTH) {      // Room to stash it?
    deframeBuffer.bbuf[rxCount++] = byte; // Yes
    return false;                         // Packet not ruined
  }
  rxSetupError(PK_BUFFER);      // Doh you blew your buffer
  return true;                  // And here's your ruined packet
}

void SFBFrame::rxSetupError(u8 flags) {
  API_ASSERT_NONZERO(flags);        // You can't get me to make a non-broken packet
  deframeBuffer.h.wh = 0;           // Nuke the packet header
  deframeBuffer.h.ph.flags = flags; // Stash the error codes
  deframeBuffer.h.ph.length = rxCount; // Save the length that we had before dying
  deframeBuffer.bbuf[rxCount] = 0; // Null terminate, pretty pointless but we promised
  rxCount = -1;               // Sadly lose sync for later
}

void SFBFrame::rxSetupPacket() {
  deframeBuffer.h.wh = 0;           // Nuke whole packet header (flags, cursor, zero == 0)
  deframeBuffer.h.ph.length = rxCount; // Save packet length
  deframeBuffer.bbuf[rxCount] = 0;     // Null terminate for the old folks
  rxCount = 0;                         // And we are ready to go again
}

bool SFBFrame::rxByte(int val) {
  if (rxCount < 0) {            // If we don't have sync, it's all meaningless..
    if (val==(int)PFSC_END) {   // ..until we see a packet end
      rxCount = 0;              // Ah good.  We're starting out fresh.  No data,
      rxEscape = false;         // and not halfway through an escape sequence
    }
    return false;               // But that didn't finish a packet
  }
  
  // Here rxCount >= 0

  if (val < 0) {                      // Hit an error condition?

    API_ASSERT_ZERO(val&SFBRxByteBuffer::BBFLAG_ZERO); // Ensure we've got valid condition flags
    API_ASSERT_NONZERO((val&~SFBRxByteBuffer::BBFLAG_ZERO)&0xff); // ..ditto..

    rxSetupError(val&0xff);     // Setup an error packet condition
    return true;                // But, we've got a broken, but dispatchable packet now
  }

  // Here val is a real byte.  Deframe it

  if (rxEscape) {               // Are we waiting to finish an escape code?

    rxEscape = false;           // Yes, but won't be after this

    if (val==PFSC_EEND)         // Got escaped end code
      return rxStoreRaw(PFSC_END); // Stash unescaped, okay unless overflow

    if (val==PFSC_EESC)         // Got escaped escape code
      return rxStoreRaw(PFSC_ESC); // Stash unescaped, okay unless overflow

    rxSetupError(PK_BAD_ESCAPE); // Invalid escape code.  Error.
    return true;                 // Go tell somebody about that.
  }

  // Here we are not partway through an escape sequence

  if (val==PFSC_ESC) {          // An escape sequence starting?
    rxEscape = true;            // Yes
    return false;               // A packet is not complete
  }

  if (val==PFSC_END) {          // Are we finishing a packet cleanly?
    rxSetupPacket();              // Yes!
    return true;
  }

  return rxStoreRaw((u8) val);  // Just normal data. Not complete unless overflow
}

u8 * SFBFrame::pollForRxPacket() {
  while (1) {
    int val = rxBuf.fetchByteBG();
    if (val == -1)              // No more data, done for now,
      return 0;                 // Tell caller nothing to do
    if (rxByte(val))            // Parse that input, see if dispatchable packet
      break;                    // Finished a packet!
  }
  return ((u8*) &deframeBuffer)+sizeof(PacketHeader); // There you go.  Valid till reset or you call me again
}

void SFBFrame::txEndPacket() {
  if (txCount < 0)              // If blown at frame level..
    txBuf.resetBG();            // ..forget any pending bytes
  else {
    txBuf.storeByteBG(PFSC_END); // else put on the terminator,
    txBuf.shipBytesBG();         // and ship if okay at byte level
  }
  txCount = 0;                  // Fresh start at frame level
}

bool SFBFrame::txCanAddByte() {
  if (txCount < 0 || txCount+1 < MAX_PACKET_LENGTH)
    return true;
  return txBuf.canStoreBytesBG(2); // We're definitely safe if we can
                                   // store two bytes, to allow for
                                   // escaping?
}

void SFBFrame::txDataByte(u8 data) {
  if (txCount < 0)              // Already blown
    return;

  if (++txCount > MAX_PACKET_LENGTH) { // If were at max length
    txCount = -1;                      // You're blown for packet length
    return;
  }

  bool isEnd = data == PFSC_END;   // Check for the two values we must
  if (isEnd || data == PFSC_ESC) { // escape, remembering which we saw

    txBuf.storeByteBG(PFSC_ESC); // Yes, send escape code plus appropriate
    txBuf.storeByteBG(isEnd?PFSC_EEND:PFSC_EESC); // follow-up code

  } else
    txBuf.storeByteBG(data);    // If not packet special, just ship it
}
