roclib/src_sp605/Iterator.cxx (r4864/r3416)

Go to the documentation of this file.

#include "sp605/Iterator.h"

#include "sp605/Board.h"

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

sp605::Iterator::Iterator(int fmt) :
   fBoard(0),
   fFormat(fmt),
   fBuffer(0),
   fBufferLen(0),
   fBufferPos(0),
   fInterm(0),
   fIntermSize(0),
   fIntermFill(0),
   fSP605Id(0xffffffff),
   fKind(kindNone),
   fMsg(),
   fLastSenderId(0xffffffff),
   fRocMsg(),
   fLastRocEpoch(0)
{
   for (int n=0;n<MaxSpadicId;n++)
      for (int g=0;g<MaxGroupId;g++)
         fLastEpoch[n][g] = 0;
}

sp605::Iterator::~Iterator()
{
  if (fInterm) { free(fInterm); fInterm = 0; }
}

bool sp605::Iterator::assign(void* buf, unsigned sz)
{
   fBuffer = buf;
   fBufferLen = sz;
   fBufferPos = 0;
   fKind = kindNone;

   return fBufferLen >= 8;
}

bool sp605::Iterator::nextBuffer(sp605::Board* brd, double tmout)
{
   if (brd==0) return false;

   void *buf = 0;
   unsigned len = 0;

   if (!brd->getNextBuffer(buf, len, tmout)) return false;

   fFormat = brd->getMsgFormat();

   return assign(buf, len);
}


void sp605::Iterator::allocateInterm()
{
   if (fInterm) { free(fInterm); fInterm = 0; }

   fInterm = malloc(128);
   fIntermSize = 128;
   fIntermFill = 0;
}


bool sp605::Iterator::next(double tmout)
{
   //  if we had previous message set, clear data from intermediate buffer
   if (fKind != kindNone) fIntermFill = 0;

   fKind = kindNone;

   while (true) {

      if ((fBuffer==0) || (fBufferPos>=fBufferLen))
         if (!nextBuffer(fBoard, tmout)) return false;

      void* curr = (char*) fBuffer + fBufferPos;

      if (fFormat == formatSpadic10Direct) {
         // process direct format here

         unsigned rest_len = fBufferLen - fBufferPos;

         if (fIntermFill>0) {
            // this is case of cuted message, one need to extend buffer slowly by 2 bytes

            if ((rest_len<2) || (fIntermFill>=fIntermSize)) {
               printf("Internal error!!!\n");
               fBufferPos = fBufferLen;
               return false;
            }
            // append two bytes more
            memcpy((char*) fInterm + fIntermFill, curr, 2);
            fBufferPos+=2;
            fIntermFill+=2;

            unsigned msg_sz = fMsg.assign(fInterm, fIntermFill);

            if (msg_sz==0) return false;

            if (msg_sz>fIntermFill) continue;

         } else {

            unsigned msg_sz = fMsg.assign(curr, rest_len);
            if (msg_sz==0) return false;

            if (msg_sz > rest_len) {
               // keep part of message in intermediate buffer, continue reading
               if (fInterm==0) allocateInterm();

               if (fIntermSize < rest_len) {
                  printf("Internal buffer too short !!!!");
                  fBufferPos = fBufferLen;
                  return false;
               }

               memcpy(fInterm, curr, rest_len);

               fIntermFill = rest_len;

               continue; // try to get rest part of data
            }

            fBufferPos += msg_sz;
         }

         fKind = kindSpadic;

         // always use spadic 0 number
         fMsg.setSpadicNumber(0);

         // extract last spadic epoch
         if (fMsg.isEpoch()) fLastEpoch[fMsg.getSpadicNumber()][fMsg.getGroup()] = fMsg.getEpoch();

         return true;

         // end of direct format processing
      }

      if (fFormat != formatSpadic10Optic) {
         printf("Only spadic1.0 formats are supported");
         fBufferPos = fBufferLen;
         return false;
      }


      // if we are here, we want to get some etra data

      if (fBufferLen - fBufferPos < 8) {
         printf("Wrong %u bytes number in the buffer, cannot be interpreted as optic stream \n", fBufferLen);
         fBufferPos = fBufferLen;
         return false;
      }

      // we shit pointer immediately
      fBufferPos+=8;

      // in first two bytes sender id is
      unsigned sender_id = *((uint16_t*) curr);

      if (fLastSenderId != 0xffffffff)
         if (sender_id!=fLastSenderId) {
            printf("We expect sender id 0x%x got 0x%x\n", fLastSenderId, sender_id);
            fBufferPos = fBufferLen;
            return false;
         }

      // first 5 bits are used for spadic id / SYNC producer coding
      unsigned kindid = sender_id & 0x1f;

      // next 10 bits are used for sp605 id coding
      unsigned sp605id = (sender_id >> 5) & 0x3ff;

      // check that sp605id remained
      if (fSP605Id == 0xffffffff) fSP605Id = sp605id;
      if (fSP605Id != sp605id) {
         printf("Mismatch of SP605 id - current %u and previous %x\n", sp605id, fSP605Id);
         fSP605Id = sp605id;
      }

      // special case - messages from the SYNC/AUX receiver of sp605 board, use roc::Message here
      if (kindid == 0x1f) {
         fRocMsg.assign(curr, roc::formatOptic2);
         fRocMsg.setRocNumber(fSP605Id);
         fKind = kindRoc;
         if (fRocMsg.isEpochMsg()) fLastRocEpoch = fRocMsg.getEpochNumber();
         return true;
      }

      // copy next portion of data into the intermediate buffer
      if (fInterm==0) allocateInterm();

      if (fIntermFill > fIntermSize - 6) {
         printf("Not enough space in intermediate buffer to accumulate message\n");
         fBufferPos = fBufferLen;
         return false;
      }

      // we need to change bytes order here - it comes with big-endian !!!
      uint8_t* tgt = (uint8_t*) fInterm + fIntermFill;
      uint8_t* src = (uint8_t*) curr + 2;
      // memcpy((uint8_t*) fInterm + fIntermFill, (uint8_t*) curr + 2, 6);
      tgt[0] = src[5]; tgt[1] = src[4]; tgt[2] = src[3];
      tgt[3] = src[2]; tgt[4] = src[1]; tgt[5] = src[0];

      // uint16_t* p = (uint16_t*) fInterm;
      // printf("%4x %4x %4x\n", p[0], p[1], p[2]);

      fIntermFill += 6;

      unsigned msg_sz = fMsg.assign(fInterm, fIntermFill);

      // printf("Message size = %u\n", msg_sz);

      if (msg_sz==0) {
         fprintf(stderr, "Wrong spadic1.0 message format\n");
         fBufferPos = fBufferLen;
         return false;
      }

      if (msg_sz > fIntermFill) {
         // this is indication that message size is bigger than available in the intermediate buffer
         // try to get next portion, verify that sender id are the same
         fLastSenderId = sender_id;
      } else {
         fLastSenderId = 0xffffffff; // indicate that data in intermediate buffer is ready

         // SPDIC message complete, analyze it if necessary

         fKind = kindSpadic;

         // only 4 Spadics are supported
         setSpadicId((kindid >> 1) & 0x3);

         fMsg.setSpadicNumber(getSpadicId());

         if (fMsg.isEpoch()) fLastEpoch[fMsg.getSpadicNumber()][fMsg.getGroup()] = fMsg.getEpoch();

         if (msg_sz == fIntermFill) return true;

         if ((msg_sz == fIntermFill-2) && (((uint8_t*) fInterm) [fIntermFill-1] == 0xf5)) {
            // printf("NOP at the end\n");
            return true;
         }

         fprintf(stderr, "Rest of the data in intermediate buffer is not NOP!!!");

         return true;
      }

   }

   return false;
}

uint64_t sp605::Iterator::getMsgFullTime() const
{
  if (isRocMsg()) return fRocMsg.getMsgFullTime(fLastRocEpoch);
  if (isSpadicMsg()) return fMsg.getMsgFullTime(fLastEpoch[fMsg.getSpadicNumber()][fMsg.getGroup()]);

  return 0;
}

double sp605::Iterator::getMsgFullTimeD() const
{
   if (isRocMsg()) return fRocMsg.getMsgFullTimeD(fLastRocEpoch);
   if (isSpadicMsg()) return fMsg.getMsgFullTimeD(fLastEpoch[fMsg.getSpadicNumber()][fMsg.getGroup()]);
   return 0.;
}


void sp605::Iterator::printMessage()
{
   if (isRocMsg()) fRocMsg.printData();
   if (isSpadicMsg()) fMsg.printData();
}