Subversion Repositories w11

// $Id: RlinkPacketBuf.cpp 509 2013-04-21 20:46:20Z mueller $
//
// Copyright 2011-2013 by Walter F.J. Mueller <W.F.J.Mueller@gsi.de>
//
// This program is free software; you may redistribute and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation, either version 2, or at your option any later version.
//
// This program 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 General Public License
// for complete details.
// 
// Revision History: 
// Date         Rev Version  Comment
// 2013-04-21   509   1.0.4  add SndAttn() method
// 2013-02-03   481   1.0.3  use Rexception
// 2013-01-13   474   1.0.2  add PollAttn() method
// 2013-01-04   469   1.0.1  SndOob(): Add filler 0 to ensure escape state
// 2011-04-02   375   1.0    Initial version
// 2011-03-05   366   0.1    First draft
// ---------------------------------------------------------------------------
 
/*!
  \file
  \version $Id: RlinkPacketBuf.cpp 509 2013-04-21 20:46:20Z mueller $
  \brief   Implemenation of class RlinkPacketBuf.
 */
 
#include <sys/time.h>
 
// debug
#include <iostream>
 
#include "RlinkPacketBuf.hpp"
 
#include "librtools/RosFill.hpp"
#include "librtools/RosPrintf.hpp"
#include "librtools/RosPrintBvi.hpp"
#include "librtools/Rexception.hpp"
 
using namespace std;
 
/*!
  \class Retro::RlinkPacketBuf
  \brief FIXME_docs
*/
 
// all method definitions in namespace Retro
namespace Retro {
 
//------------------------------------------+-----------------------------------
// constants definitions
 
const uint32_t RlinkPacketBuf::kFlagSopSeen;
const uint32_t RlinkPacketBuf::kFlagEopSeen;
const uint32_t RlinkPacketBuf::kFlagNakSeen;
const uint32_t RlinkPacketBuf::kFlagAttnSeen;
const uint32_t RlinkPacketBuf::kFlagTout;
const uint32_t RlinkPacketBuf::kFlagDatDrop;
const uint32_t RlinkPacketBuf::kFlagDatMiss;
 
const uint8_t RlinkPacketBuf::kCPREF;
const uint8_t RlinkPacketBuf::kNCOMM;
const uint8_t RlinkPacketBuf::kCommaIdle;
const uint8_t RlinkPacketBuf::kCommaSop;
const uint8_t RlinkPacketBuf::kCommaEop;
const uint8_t RlinkPacketBuf::kCommaNak;
const uint8_t RlinkPacketBuf::kCommaAttn;
const uint8_t RlinkPacketBuf::kSymEsc;
 
//------------------------------------------+-----------------------------------
//! Default constructor
 
RlinkPacketBuf::RlinkPacketBuf()
  : fPktBuf(),
    fRawBuf(),
    fRawBufSize(0),
    fCrc(),
    fFlags(0),
    fNdone(0),
    fNesc(0),
    fNattn(0),
    fNidle(0),
    fNdrop(0)
{}
 
//------------------------------------------+-----------------------------------
//! Destructor
 
RlinkPacketBuf::~RlinkPacketBuf()
{}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
void RlinkPacketBuf::Init()
{
  fPktBuf.clear();
  fRawBufSize = 0;
  fCrc.Clear();
  fFlags = 0;
  fNdone = 0;
  fNesc  = 0;
  fNattn = 0;
  fNidle = 0;
  fNdrop = 0;
 
  return;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::SndPacket(RlinkPort* port, RerrMsg& emsg)
{
  fRawBuf.reserve(2*fPktBuf.size()+2);        // max. size of raw data
  fRawBuf.clear();
 
  fRawBuf.push_back(kCommaSop);
 
  size_t   ni = fPktBuf.size();
  uint8_t* pi = fPktBuf.data();
  for (size_t i=0; i<ni; i++) {
    uint8_t c = *pi++;
    if (c == kSymEsc || (c >= kCPREF && c <= kCPREF+kNCOMM)) {
      fRawBuf.push_back(kSymEsc);
      fRawBuf.push_back(((~kCPREF) & 0xf0) | (c & 0x0f));
      fNesc += 1;
    } else {
      fRawBuf.push_back(c);
    }
  }
 
  fRawBuf.push_back(kCommaEop);
 
  return SndRaw(port, emsg);
 
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::RcvPacket(RlinkPort* port, size_t nrcv, float timeout, 
                               RerrMsg& emsg)
{
  fPktBuf.clear();
 
  bool   escseen = false;                   // in esc
  bool   sopseen = false;                   // sop seen
  bool   eopseen = false;                   // eop seen
  bool   nakseen = false;                   // nak seen
 
  while (!(eopseen|nakseen)) {              // try till eop or nak received
    size_t   nread = nrcv - fPktBuf.size();
    // FIXME_code: if the 'enough data' handling below correct ?
    if (nread < 0)  return true;
 
    if (!sopseen) nread += 1;
    if (!eopseen) nread += 1;
 
    size_t sizeold = fRawBufSize;
    int irc = RcvRaw(port, nread, timeout, emsg);
 
    if (irc <= 0) {
      if (irc == RlinkPort::kTout) {
        SetFlagBit(kFlagTout);
        return true;
      } else {
        return false;
      }
    }
 
    uint8_t* pi = fRawBuf.data()+sizeold;
    for (int i=0; i<irc; i++) {
      uint8_t c = *pi++;
      if (escseen) {
        escseen = false;
        if (sopseen && !(nakseen || eopseen)) {
          fPktBuf.push_back((kCPREF & 0xf0) | (c & 0x0f));
        }
      } else if (c == kCommaSop) {
        if (!eopseen) {
          SetFlagBit(kFlagSopSeen);
          sopseen  = true;
        } else {
          // FIXME_code: handle multiple sop
        }
      } else if (c == kCommaEop) {
        SetFlagBit(kFlagEopSeen);
        eopseen  = true;
      } else if (c == kCommaNak) {
        SetFlagBit(kFlagNakSeen);
        nakseen  = true;
      } else if (c == kCommaAttn) {
        SetFlagBit(kFlagAttnSeen);
        fNattn += 1;
      } else if (c == kCommaIdle) {
        fNidle += 1;
      } else if (c == kSymEsc) {
        fNesc += 1;
        escseen = true;
      } else {
        if (sopseen && !(nakseen || eopseen)) {
          fPktBuf.push_back(c);
        } else {
          fNdrop += 1;
        }
      }
    } // for (int i=0; i<irc; i++)
 
  } // while (!(eopseen|nakseen))
 
  return true;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
double RlinkPacketBuf::WaitAttn(RlinkPort* port, double timeout, RerrMsg& emsg)
{
  if (timeout <= 0.)
    throw Rexception("RlinkPacketBuf::WaitAttn()", "Bad args: timeout <= 0.");
 
  struct timeval tval;
  gettimeofday(&tval, 0);
  double tbeg  = double(tval.tv_sec) + 1.e-6*double(tval.tv_usec);
  double trest = timeout;
 
  Init();
 
  while (trest > 0.) {
    size_t sizeold = fRawBufSize;
    int irc = RcvRaw(port, 1, trest, emsg);
 
    if (irc <= 0) {
      if (irc == RlinkPort::kTout) {
        SetFlagBit(kFlagTout);
        return -1.;
      } else {
        return -2.;
      }
    }
 
    gettimeofday(&tval, 0);
    double tend  = double(tval.tv_sec) + 1.e-6*double(tval.tv_usec);
    trest -= (tend-tbeg);
 
    uint8_t c = fRawBuf[sizeold];
 
    if (c == kCommaAttn) {
      fNattn += 1;
      SetFlagBit(kFlagAttnSeen);
      break;
    } else if (c == kCommaIdle) {
      fNidle += 1;
    } else {
      fNdrop += 1;
    }
 
    tbeg   = tend;
  }
 
  return timeout - trest;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
int RlinkPacketBuf::PollAttn(RlinkPort* port, RerrMsg& emsg)
{
  Init();
 
  int irc = RcvRaw(port, 128, 0., emsg);
 
  if (irc <= 0) {
    if (irc == RlinkPort::kTout) {
      SetFlagBit(kFlagTout);
      return 0;
    } else {
      return -2;
    }
  }
 
  for (size_t i=0; i<(size_t)irc; i++) {
    uint8_t c = fRawBuf[i];
    if (c == kCommaAttn) {
      fNattn += 1;
      SetFlagBit(kFlagAttnSeen);
      break;
    } else if (c == kCommaIdle) {
      fNidle += 1;
    } else {
      fNdrop += 1;
    }
  }
 
  return fNattn;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::SndOob(RlinkPort* port, uint16_t addr, uint16_t data, 
                            RerrMsg& emsg)
{
  Init();
 
  fRawBuf.clear();
  fRawBuf.push_back(kSymEsc);                       // ESC
  fRawBuf.push_back(kSymEsc);                       // ESC
  fRawBuf.push_back((uint8_t)addr);                 // ADDR 
  fRawBuf.push_back((uint8_t)(data & 0x00ff));      // DL
  fRawBuf.push_back((uint8_t)((data>>8) & 0x00ff)); // DH
  // write a filler char (just 0) to ensure that the 8b->9b stage in the
  // receiver (byte2cdata) is always out if the escape state...
  fRawBuf.push_back(0);                             // filler
 
  return SndRaw(port, emsg);
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::SndKeep(RlinkPort* port, RerrMsg& emsg)
{
  Init();
 
  fRawBuf.clear();
  fRawBuf.push_back(kSymEsc);               // ESC
  fRawBuf.push_back(kSymEsc);               // ESC
 
  return SndRaw(port, emsg);
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::SndAttn(RlinkPort* port, RerrMsg& emsg)
{
  Init();
 
  fRawBuf.clear();
  fRawBuf.push_back(kCommaAttn);            // Attn
 
  return SndRaw(port, emsg);
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
void RlinkPacketBuf::Dump(std::ostream& os, int ind, const char* text) const
{
  RosFill bl(ind);
  os << bl << (text?text:"--") << "RlinkPacketBuf @ " << this << endl;
  os << bl << "  fCrc:          " << RosPrintBvi(fCrc.Crc(), 0) << endl;
  os << bl << "  fFlags:        " << RosPrintBvi(fFlags, 0) << endl;
  os << bl << "  fNdone:        " << RosPrintf(fNdone,"d",4) << endl;
  os << bl << "  fNesc:         " << RosPrintf(fNesc,"d",4) << endl;
  os << bl << "  fNattn:        " << RosPrintf(fNattn,"d",4) << endl;
  os << bl << "  fNidle:        " << RosPrintf(fNidle,"d",4) << endl;
  os << bl << "  fNdrop:        " << RosPrintf(fNdrop,"d",4) << endl;
 
  os << bl << "  fPktBuf(size): " << RosPrintf(fPktBuf.size(),"d",4);
  size_t ncol  = max(1, (80-ind-4-6)/(2+1));
  for (size_t i=0; i< fPktBuf.size(); i++) {
    if (i%ncol == 0) os << "\n" << bl << "    " << RosPrintf(i,"d",4) << ": ";
    os << RosPrintBvi(fPktBuf[i],16) << " ";
  }
  os << endl;
 
  os << bl << "  fRawBuf(size): " << RosPrintf(fRawBufSize,"d",4);
  for (size_t i=0; i< fRawBufSize; i++) {
    if (i%ncol == 0) os << "\n" << bl << "    " << RosPrintf(i,"d",4) << ": ";
    os << RosPrintBvi(fRawBuf[i],16) << " ";
  }
  os << endl;
 
  return;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
bool RlinkPacketBuf::SndRaw(RlinkPort* port, RerrMsg& emsg)
{
  if (port==0 || !port->IsOpen())
    throw Rexception("RlinkPacketBuf::SndRaw()", "Bad state: port not open");
 
  fRawBufSize = fRawBuf.size();
  int irc = port->Write(fRawBuf.data(), fRawBuf.size(), emsg);
  if (irc < 0) return false;
  if ((size_t)irc != fRawBuf.size()) {
    emsg.Init("RlinkPacketBuf::SndRaw()", "failed to write all data");
    return false;
  }
 
  return true;
}
 
//------------------------------------------+-----------------------------------
//! FIXME_docs
 
int RlinkPacketBuf::RcvRaw(RlinkPort* port, size_t size, float timeout, 
                           RerrMsg& emsg)
{
  if (port==0 || !port->IsOpen())
    throw Rexception("RlinkPacketBuf::RcvRaw()", "Bad state: port not open");
 
  if (fRawBuf.size() < fRawBufSize+size) fRawBuf.resize(fRawBufSize+size);
  int irc = port->Read(fRawBuf.data()+fRawBufSize, size, timeout, emsg);
  if (irc == RlinkPort::kEof) {
    emsg.Init("RlinkPacketBuf::RcvRaw()", "eof on read");
  }
 
  if (irc > 0) {
    fRawBufSize += irc;
  }
 
  return irc;
}
 
} // end namespace Retro