faspplugin/src/Transport.cxx (r4864/r3700)

Go to the documentation of this file.

/********************************************************************
 * The Data Acquisition Backbone Core (DABC)
 ********************************************************************
 * Copyright (C) 2009-
 * GSI Helmholtzzentrum fuer Schwerionenforschung GmbH
 * Planckstr. 1
 * 64291 Darmstadt
 * Germany
 * Contact:  http://dabc.gsi.de
 ********************************************************************
 * This software can be used under the GPL license agreements as stated
 * in LICENSE.txt file which is part of the distribution.
 ********************************************************************/

#include "fasp/Transport.h"

#include "base/commons.h"

#include "dabc/timing.h"
#include "dabc/Port.h"
#include "dabc/version.h"
#include "dabc/Manager.h"
#include "mbs/MbsTypeDefs.h"
#include "mbs/Iterator.h"

// #include <iostream>
#include <errno.h>

#include <math.h>

fasp::Transport::Transport(dabc::Reference port, dabc::Command cmd) :
      dabc::SocketWorker(), dabc::Transport(port.Ref()),
      dabc::MemoryPoolRequester(),
      fQueueMutex(),
      fQueue(
      ((dabc::Port*) port())->InputQueueCapacity()),
      fTgtBuf()
{
   // we will react on all input packets
   fFlushTimeout = 0.3;
   fBufferSize = 32576;

   rcvCounterNew = 0;
   rcvCounterDubl = 0;

   fSockfd = -1;

   fIfName = "eth0";

   memset(fSourceMAC, 0, sizeof(fSourceMAC));
   memset(fDestMAC, 0, sizeof(fDestMAC));
   fSendDelay = 0.01;
   fPlainData = false;

   ConfigureFor((dabc::Port*) port(), cmd);
//   DOUT0(("Create %p instance of Transport", this));
}

fasp::Transport::~Transport()
{
//   DOUT0(("Delete %p instance of Transport", this));

   fRawSync = 0;
   fDataPtr.reset();
   fHdrPtr.reset();
   fTgtBuf.Release();
   fQueue.Cleanup();
   fPool.Release();
}

void fasp::Transport::ConfigureFor(dabc::Port* port, dabc::Command cmd)
{
   //  EV need to close socket after
   if ((fSockfd = socket(AF_PACKET, SOCK_RAW, htons(ETH_P_ALL))) == -1) {
      EOUT(("socket NOT created"));
      return;
   } else {
      DOUT0(("RAW socket created"));
   }

   fBufferSize = port->Cfg(dabc::xmlBufferSize, cmd).AsInt(fBufferSize);
   fFlushTimeout = port->Cfg(dabc::xmlFlushTimeout, cmd).AsDouble(fFlushTimeout);
   fIfName = port->Cfg("if", cmd).AsStdStr(fIfName);
   fSendDelay = port->Cfg("BurstDelay", cmd).AsDouble(fSendDelay);
   fPlainData = port->Cfg("PlainData", cmd).AsBool(fPlainData);

   std::string src_mac = port->Cfg("SourceMAC", cmd).AsStdStr();
   std::string dest_mac = port->Cfg("DestMAC", cmd).AsStdStr();
   int if_index = port->Cfg("if_index", cmd).AsInt(-1);

   if (dest_mac.length() == 2*6 + 5) {
      unsigned ddd[6];
      sscanf(dest_mac.c_str(),"%x:%x:%x:%x:%x:%x", ddd, ddd+1, ddd+2, ddd+3, ddd+4,ddd+5);
      for (int n=0;n<6;n++) fDestMAC[n] = ddd[n];
   } else {
      fDestMAC[0] = 0xB8;
      fDestMAC[1] = 0xAC;
      fDestMAC[2] = 0x6F;
      fDestMAC[3] = 0x92;
      fDestMAC[4] = 0x2B;
      fDestMAC[5] = 0x34;
   }


   DOUT0(("FASP transport buffer size %u flush timeout %3.2f", (unsigned) fBufferSize, fFlushTimeout));

   fPool = port->GetMemoryPool();

//   fSrcMAC = port->Cfg("SourceAddr").AsUInt(fSrcMAC);
//   fDestMAC = port->Cfg("DestAddr").AsUInt(fDestMAC);

   struct ifreq ifr;
   memset(&ifr, 0, sizeof(ifr));
   strcpy(ifr.ifr_name, fIfName.c_str());
   if (setsockopt(fSockfd, SOL_SOCKET, SO_BINDTODEVICE, (void *)&ifr, sizeof(ifr)) < 0) {
      EOUT(("FAIL to bind socket to interface %s", fIfName.c_str()));
   } else {
      DOUT0(("Bind socket to interface %s", fIfName.c_str()));
   }

   // Interface (eth0) preparing
   if (if_index < 0) {
      struct ifreq if_idx;
      // Get the index of the interface to send on
      memset (&if_idx, 0, sizeof(struct ifreq));
      strncpy (if_idx.ifr_name, fIfName.c_str(), sizeof(if_idx.ifr_name));
      if (ioctl(fSockfd, SIOCGIFINDEX, &if_idx) < 0) {
         EOUT(("Cannot get index for interface %s, use 2", fIfName.c_str()));
         if_index = 2;
      } else {
         if_index = if_idx.ifr_ifindex;
      }
   }

   if (src_mac.length() == 2*6 + 5) {
      unsigned ddd[6];
      sscanf(src_mac.c_str(),"%x:%x:%x:%x:%x:%x", ddd, ddd+1, ddd+2, ddd+3, ddd+4,ddd+5);
      for (int n=0;n<6;n++) fSourceMAC[n] = ddd[n];
   } else {
      // Get the MAC address of the interface to send on
      struct ifreq if_mac;
      memset (&if_mac, 0, sizeof(struct ifreq));
      strncpy (if_mac.ifr_name, fIfName.c_str(), sizeof(if_mac.ifr_name));
      if (ioctl(fSockfd, SIOCGIFHWADDR, &if_mac) < 0) {
         EOUT(("Cannot get MAC address for interface %s", fIfName.c_str()));
      } else {
         memcpy(fSourceMAC, &if_mac.ifr_hwaddr.sa_data, 6);
      }
   }


   // Construct the Ethernet header
   memset (fSendbuf, 0, BUF_SIZ);

   DOUT0(("Source MAC: %02X:%02X:%02X:%02X:%02X:%02X",
           fSourceMAC[0], fSourceMAC[1], fSourceMAC[2],
           fSourceMAC[3], fSourceMAC[4], fSourceMAC[5]));

   DOUT0(("Destination MAC: %02X:%02X:%02X:%02X:%02X:%02X",
         fDestMAC[0], fDestMAC[1], fDestMAC[2],
         fDestMAC[3], fDestMAC[4], fDestMAC[5]));

   memcpy(fSendbuf, fDestMAC, 6);
   memcpy(fSendbuf+6, fSourceMAC, 6);
   
   fSendbuf[12] = 0xff;
   fSendbuf[13] = 0xff;

   // Ethertype field
//   short int typ = htons(ETH_P_IP);
//   memcpy(fSendbuf+12, &typ, 2);
   // Packet data
   for (int jj = 14; jj<60; jj++) fSendbuf[jj] = 0x66;

//   for (int jj = 0; jj<60; jj++) printf("0x%02x\n", (unsigned) fSendbuf[jj]); exit(1);

   fSockAddr.sll_family = PF_PACKET;
   fSockAddr.sll_protocol = htons(ETH_P_IP);
   fSockAddr.sll_ifindex = if_index;
   // Address length
   fSockAddr.sll_hatype = ARPHRD_ETHER;
   fSockAddr.sll_pkttype = PACKET_OTHERHOST;
   fSockAddr.sll_halen = ETH_ALEN;
   // Destination MAC

   memcpy (fSockAddr.sll_addr, fDestMAC, 6);

  DOUT0(("Socket initialized. ifindex=%d", if_index));

   SetSocket(fSockfd);
}


bool fasp::Transport::ReadFromSocket()
{
   uint8_t recvbuf[BUF_SIZ];

   size_t res = recvfrom (fSockfd, recvbuf, BUF_SIZ, 0, 0, 0);

   struct ether_header * recvH = (struct ether_header*) recvbuf;

   if (res != 60) return false;

//   bool is_me(true);
//   for (int n=0;n<6;n++) 
//      if (recvbuf[n] != fSourceMAC[n]) is_me = false;
//   printf("%s packet of size %4d dest:%02X:%02X:%02X:%02X:%02X:%02X  src:%02X:%02X:%02X:%02X:%02X:%02X  \n", (is_me ? "My " : "   "),  res,
//   (unsigned) recvbuf[0], (unsigned) recvbuf[1], (unsigned) recvbuf[2], (unsigned) recvbuf[3], (unsigned) recvbuf[4], (unsigned) recvbuf[5],
//   (unsigned) recvbuf[6], (unsigned) recvbuf[7], (unsigned) recvbuf[8], (unsigned) recvbuf[9], (unsigned) recvbuf[10], (unsigned) recvbuf[11]);

/*
   printf ("header: ");
   for (int i=0; i<6; i++)
     printf ("%02x:%02x\t", recvH->ether_shost[i], (uint8_t)fDestMAC[i]);
   printf ("\n");
*/
   // check the adress of the package
   for (int i=0; i<6; i++)
      if (recvH->ether_shost[i] != (uint8_t) fDestMAC[i])
      //if ((uint8_t) fRecvbuf[i+6] != (uint8_t) fDestMAC[i])
        return false;

//   printf ("%02x.%02x.%02x\t", (uint8_t)fRecvbuf[48], (uint8_t)fRecvbuf[49], (uint8_t)fRecvbuf[50]);

   uint8_t* rawbuf = ((uint8_t*) recvbuf) + 14;

   // compare the last block with the saved one
   bool dummyBlock = (rawbuf[0] == 0x66) && (rawbuf[1] == 0x66) && (rawbuf[2] == 0x66);
   bool newBlockRecieved = false;
   if (!dummyBlock)
      for (int i=0; i<46; i++) {
         if ((unsigned) recvbuf[14+i] != (unsigned) fLastRecvedBlock[i])
            newBlockRecieved = true;
      }

   // Here you are if the block is the same as the previous
   if (dummyBlock || !newBlockRecieved) {
      rcvCounterDubl++;
      // DOUT0(("Getting same data"));
      return true;
   }

   static unsigned last_id = 0;

   unsigned id = rawbuf[FaspSyncPos] * 0x10000 + rawbuf[FaspSyncPos+1] * 0x100 + rawbuf[FaspSyncPos+2];
   
   if (id == 0x666666) {
      printf("find dummy id - ignore\n");
      return true;
   }

   bool strange = false;
   if (last_id!=0)
       if (id - last_id > 10000) strange = true;

//   bool is_me(true);
//   for (int n=0;n<6;n++) 
//      if (recvbuf[n] != fSourceMAC[n]) is_me = false;

if (strange) {
    printf("Ignore strange ID %u %06x  previous is %u %06x \n", id, id, last_id, last_id);

   printf("packet %6u of size %4d dest:%02X:%02X:%02X:%02X:%02X:%02X  src:%02X:%02X:%02X:%02X:%02X:%02X  %02X %02X %02X %02X %02X %02X \n", id, (int) res,
   (unsigned) recvbuf[0], (unsigned) recvbuf[1], (unsigned) recvbuf[2], (unsigned) recvbuf[3], (unsigned) recvbuf[4], (unsigned) recvbuf[5],
   (unsigned) recvbuf[6], (unsigned) recvbuf[7], (unsigned) recvbuf[8], (unsigned) recvbuf[9], (unsigned) recvbuf[10], (unsigned) recvbuf[11],
   
   (unsigned) recvbuf[48], (unsigned) recvbuf[49], (unsigned) recvbuf[50], (unsigned) recvbuf[51], (unsigned) recvbuf[52], (unsigned) recvbuf[53]
   );
   
   for (int n=0;n<44;n++)
      printf("%02x ", rawbuf[n]);
   printf("\n");
   
      return true;
  }
   last_id = id;

   // Save recieved block
   memcpy(fLastRecvedBlock, recvbuf + 14, 46);


//   for (int n=0; n<60; n++) printf("%02x ", (unsigned) recvbuf[n]);
//   printf("\n");

//   DOUT0(("Get packet for event %u  %x %x %x" , id, (unsigned) rawbuf[FaspSyncPos], (unsigned) rawbuf[FaspSyncPos+1] , (unsigned)rawbuf[FaspSyncPos+2]));

   // if new event has other event, close event
   if ((fRawSync != DummySync) && (fRawSync != id))
      CloseCurrentEvent();

   if (fDataPtr.null() || (fDataPtr.fullsize() < FaspBlockSize)) {
      EOUT(("No buffer when data is received"));
      return true;
   }
   // when new event comes, set place for events header
   if (fRawSync == DummySync) {
      fRawSync = id;
   }

   if (fRawSync != id) {
      EOUT(("Something went wrong"));
      return true;
   }

   fDataPtr.copyfrom(fLastRecvedBlock, FaspBlockSize);

   fDataPtr.shift(FaspBlockSize);

   if (fPlainData) CloseCurrentEvent();

   if (fDataPtr.fullsize() < 2* FaspBlockSize)
      FlushBuffer();

   return true;
}

void fasp::Transport::WriteToSocket()
{
  // Send packet
  if (sendto(fSockfd, fSendbuf, 60, 0, (struct sockaddr*)&fSockAddr, sizeof(struct sockaddr_ll)) < 0) {
//  if (send(fSockfd, fSendbuf, 60, 0) < 0)
    EOUT(("Package send failed"));
  } else {
//    DOUT0(("Package sent"));
     fLastSendTime.GetNow();
  }
}

void fasp::Transport::ProcessEvent(const dabc::EventId& evnt)
{
   switch (evnt.GetCode()) {
      case evntSocketRead: {
         if (ReadFromSocket() || (fLastSendTime.SpentTillNow() > fSendDelay))
             WriteToSocket();
         SetDoingInput(true);
         break;
      }

      default:
         dabc::SocketWorker::ProcessEvent(evnt);
         break;
   }
}

double  fasp::Transport::ProcessTimeout(double lastdiff)
{
   if (fFlushTimeout > 0) {
      double spent = fLastFlushTime.SpentTillNow();
      if (spent > fFlushTimeout)
         FlushBuffer(spent > fFlushTimeout*10);
   }
   
   if (fLastSendTime.SpentTillNow() > fSendDelay)
      WriteToSocket();

   double min = 0.1;
   if ((fSendDelay>0) && (fSendDelay < min)) min = fSendDelay;
   return min;
}

void fasp::Transport::CloseCurrentEvent()
{
   unsigned fullsize = fHdrPtr.distance_to(fDataPtr);
   mbs::EventHeader ev;
   ev.Init(fRawSync);
   ev.SetFullSize(fullsize);

//   DOUT0(("Building event %u of size %u === %u", fRawSync, fullsize, ev.FullSize()));

   mbs::SubeventHeader sub;
   sub.Init(0, roc::proc_FASP, 0);
   sub.SetFullSize(fullsize - sizeof(ev));

   if (fHdrPtr.fullsize() < 30) {
      EOUT(("Something went wrong   evid = %u hdrptr size = %u", fRawSync, fHdrPtr.fullsize()));
   }

   fHdrPtr.copyfrom(&ev, sizeof(ev));
   fHdrPtr.shift(sizeof(ev));
   fHdrPtr.copyfrom(&sub, sizeof(sub));
   fHdrPtr.shift(fullsize - sizeof(ev));
   fRawSync = DummySync;



   fDataPtr = fHdrPtr;
   fDataPtr.shift(sizeof(mbs::EventHeader) + sizeof(mbs::SubeventHeader));
//   DOUT0(("Starting dummy event with size %u", fHdrPtr.distance_to(fDataPtr)));

   if (fDataPtr.fullsize() < FaspBlockSize) {
      EOUT(("No data for new block"));
   }

}


void fasp::Transport::FlushBuffer(bool force)
{
    dabc::Buffer newbuf;
    unsigned newbufused(0);
    unsigned newsyncid(DummySync);

    if (fRawSync != DummySync) {
       // we need to copy part of event in the new buffer
       if (fHdrPtr.fullsize() == fTgtBuf.GetTotalSize()) {
          // this is a case when event start from buffer begin, therefore copy of event data will not help
          if (!force) return;
          DOUT0(("Very special case - closing event when events starts from buffer begin and must be flushed"));
          CloseCurrentEvent();
       } else {
          // we copy partial data  to new buffer

//          DOUT0(("Coping of partial data into new buffer"));

          newbuf = fPool.TakeBufferReq(this, fBufferSize);
          dabc::Pointer new_ptr = newbuf.GetPointer();
          newbufused = fHdrPtr.distance_to(fDataPtr);
          new_ptr.copyfrom(fHdrPtr, newbufused);

          newsyncid = fRawSync;
          // mark as we do not fill data in the fDataPtr
          fRawSync = DummySync;
       }
    }

    if (fRawSync != DummySync) {
       EOUT(("Something went wrong"));
       return;
    }

    if (!fTgtBuf.null() && (fHdrPtr.fullsize() < fTgtBuf.GetTotalSize())) {
        fTgtBuf.SetTotalSize(fTgtBuf.GetTotalSize() - fHdrPtr.fullsize());
        fTgtBuf.SetTypeId(mbs::mbt_MbsEvents);

//        DOUT0(("Closing buffer of size %u", fTgtBuf.GetTotalSize()));
//        mbs::ReadIterator iter(fTgtBuf);
//        while (iter.NextEvent())
//           DOUT0(("   Event %u  size %u max %u", iter.evnt()->EventNumber(), iter.evnt()->FullSize(), iter.GetEventSize()));

        bool dropbuffer = false;
        
        dabc::Buffer temp = fTgtBuf.HandOver();
        {
           dabc::LockGuard lock(fQueueMutex);
           if (fQueue.Full()) {
              DOUT0(("Output queue is full - drop data"));
              dropbuffer = true;
           } else {
              fQueue.Push(temp);
           }
        }
        
        if (dropbuffer) temp.Release();
                   else FirePortInput();
    }

    fLastFlushTime.GetNow();

    if (newbuf.null())
       fTgtBuf = fPool.TakeBufferReq(this, fBufferSize);
    else
       fTgtBuf = newbuf.HandOver();

    fHdrPtr = fTgtBuf.GetPointer();
    fDataPtr = fHdrPtr;

    if ((newbufused>0) && (newsyncid!=DummySync)) {
       fDataPtr.shift(newbufused);
       fRawSync = newsyncid;
    } else {
       fDataPtr.shift(sizeof(mbs::EventHeader) + sizeof(mbs::SubeventHeader));
    }
}

bool fasp::Transport::Recv(dabc::Buffer& buf)
{
   {
      dabc::LockGuard lock(fQueueMutex);
      if (fQueue.Size() <= 0) return false;

      fQueue.PopBuffer(buf);
   }
   return !buf.null();
}

unsigned fasp::Transport::RecvQueueSize() const
{
   dabc::LockGuard guard(fQueueMutex);
   DOUT5(("%s Transport::RecvQueueSize()=%d",GetName(),fQueue.Size()));
   return fQueue.Size();
}

dabc::Buffer& fasp::Transport::RecvBuffer(unsigned indx) const
{
   dabc::LockGuard lock(fQueueMutex);
   DOUT5(("%s Transport::RecvBuffer %d ",GetName(),indx));
   return fQueue.ItemRef(indx);
}

bool fasp::Transport::ProcessPoolRequest()
{
   return true;
}

void fasp::Transport::StartTransport()
{
   DOUT3(("Starting FASP transport"));

   if (!IsRawSocket()) return;

   SetDoingInput(true);
   
   fLastFlushTime.GetNow();
   
   fTgtBuf = fPool.TakeBufferReq(this, fBufferSize);
   fHdrPtr = fTgtBuf.GetPointer();
   fDataPtr = fHdrPtr; fDataPtr.shift(sizeof(mbs::EventHeader) + sizeof(mbs::SubeventHeader));

   fRawSync = DummySync;

   DOUT0(("Starting transport - we have hdrptr size = %u", fHdrPtr.fullsize()));


   if(!ActivateTimeout(fFlushTimeout > 0. ? fFlushTimeout : 3.))
      EOUT(("could not activate timeout in FASP transport"));
}

void fasp::Transport::StopTransport()
{
   DOUT0(("Stopping FASP transport. New packages: %d, Duplicating: %d", rcvCounterNew, rcvCounterDubl));
}


//ssize_t fasp::Transport::DoUdpRecvFrom(void* buf, size_t len, int flags)
//{
//   ssize_t res = 0;
//   size_t socklen = (size_t) sizeof(fSockAddr);
//   sockaddr* saptr = (sockaddr*) &fSockAddr; // trick for compiler
//   //std::cout <<"recvfrom writes to "<<(int) buf <<", len="<<len<< std::endl;
//   res = recvfrom(fSocket, buf, len, flags, saptr, (socklen_t *) &socklen);
//   //std::cout <<"recvfrom returns"<<res << std::endl;
//   if (res == 0)
//      OnConnectionClosed();
//   else if (res < 0) {
//      if (errno != EAGAIN)
//         OnSocketError(errno, "When recvfrom()");
//   }
//   return res;
//}

extern "C" void RunFaspTest()
{
   dabc::mgr.CreateMemoryPool("Pool", 50000, 50);

   dabc::mgr.CreateModule("mbs::TransmitterModule", "FaspTest", "ModuleThrd");

   dabc::mgr.CreateTransport("FaspTest/Input", "fasp::Transport", "TransportThrd");

   // dabc::mgr.CreateTransport("Repeater/Input", mbs::typeClientTransport, "MbsTransport");

   dabc::mgr.CreateTransport("FaspTest/Output", mbs::typeServerTransport, "MbsTransport");


}