simulator/distri.cpp (35ed4cc) - libetherpix · BlinkenArea

distri.cpp

/*
 * EtherPix simulator
 *
 * Copyright 2017 Stefan Schuermans <stefan schuermans info>
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, version 3 of the License.
 *
 *
 * 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 more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 */

#include <arpa/inet.h>
#include <errno.h>
#include <gtkmm.h>
#include <map>
#include <sstream>
#include <stdexcept>
#include <stdint.h>
#include <string.h>
#include <vector>

#include "bbox.h"
#include "distri.h"
#include "mapping.h"
#include "pixel.h"
#include "transform.h"

/// constructor
Distri::Distri():
  m_distno(0),
  m_outputs(0),
  m_pixels(0),
  m_mappings(),
  m_outputMap(),
  m_sock(-1),
  m_packets(0)
{
  // initialize address to localhost and automatic port
  m_addr.sin_family = AF_INET;
  m_addr.sin_addr.s_addr = INADDR_LOOPBACK;
  m_addr.sin_port = htons(0);
}

/// destructor
Distri::~Distri()
{
  stop();
}

/**
 * @brief initialize
 * @param[in] distno distributor number
 * @param[in] outputs number of outputs
 * @param[in] pixels number of pixels per output
 */
void Distri::init(unsigned long distno, unsigned long outputs,
                  unsigned long pixels)
{
  m_distno = distno;
  m_outputs = outputs;
  m_pixels = pixels;
}

/**
 * @brief set network address
 * @param[in] addr IPv4/port network address (UDP)
 */
void Distri::setAddr(struct sockaddr_in const &addr)
{
  m_addr = addr;
}

/**
 * @brief get mapping
 * @param[in] chan color channel
 * @return mapping mapping object of color channel
 * @throws std::exception in case of error
 */
Mapping const& Distri::getMapping(Mapping::Channel chan) const
{
  if (chan >= Mapping::ChannelCount) {
    std::stringstream msg;
    msg << "invalid channel: " << chan;
    throw std::runtime_error(msg.str());
  }
  return m_mappings[chan];
}

/**
 * @brief set mapping
 * @param[in] chan color channel
 * @param[in] mapping new mapping object for color channel
 * @throws std::exception in case of error
 */
void Distri::setMapping(Mapping::Channel chan, Mapping const &mapping)
{
  if (chan >= Mapping::ChannelCount) {
    std::stringstream msg;
    msg << "invalid channel: " << chan;
    throw std::runtime_error(msg.str());
  }
  m_mappings[chan] = mapping;
}

/**
 * @brief add output to distributor
 * @param[in] outno number of output
 * @throws std::exception in case of error
 */
void Distri::addOutput(unsigned long outno)
{
  if (outno >= m_outputs) {
    std::stringstream msg;
    msg << "invalid output number " << outno << ": only " << m_outputs
        << " outputs supported";
    throw std::runtime_error(msg.str());
  }

if (m_outputMap.find(outno) != m_outputMap.end()) {
    std::stringstream msg;
    msg << "output " << outno << " already exists";
    throw std::runtime_error(msg.str());
  }

m_outputMap[outno];
}

/**
 * @brief add pixel to output of distributor
 * @param[in] outno number of output
 * @param[in] pixel pixel object to add
 * @throws std::exception in case of error
 */
void Distri::addPixel(unsigned long outno, Pixel const &pixel)
{
  if (m_outputMap.find(outno) == m_outputMap.end()) {
    std::stringstream msg;
    msg << "output " << outno << " does not exist";
    throw std::runtime_error(msg.str());
  }

Output &output = m_outputMap[outno];

if (output.m_pixels.size() >= m_pixels) {
    std::stringstream msg;
    msg << "cannot add pixel to output " << outno << ": already " << m_pixels
        << " pixels at this output (this is the maximum)";
    throw std::runtime_error(msg.str());
  }

output.m_pixels.push_back(pixel);
}

/**
 * @brief add all pixels of this distributor to the bounding box
 * @param[in,out] bb bounding box
 */
void Distri::updateBBox(BBox &bb) const
{
  OutputMap::const_iterator out;
  for (out = m_outputMap.begin(); out != m_outputMap.end(); ++out) {
    Output const &output = out->second;
    std::vector<Pixel>::const_iterator pix;
    for (pix = output.m_pixels.begin(); pix != output.m_pixels.end(); ++pix) {
      Pixel const &pixel = *pix;
      pixel.updateBBox(bb);
    }
  }
}

/**
 * @brief create socket and listen for incoming packets
 * @throws std::exception in case of error
 */
void Distri::start()
{
  // leave if  socket already exists
  if (m_sock != -1) {
    return;
  }

// create socket
  m_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (m_sock == -1) {
    std::stringstream msg;
    msg << "could not create IPv4/UDP socket: " << strerror(errno);
    throw::std::runtime_error(msg.str());
  }
  try {

// bind socket
    if (bind(m_sock, (struct sockaddr *)&m_addr, sizeof(m_addr)) != 0) {
      std::stringstream msg;
      msg << "could not bind IPv4/UDP socket to \""
          << inet_ntoa(m_addr.sin_addr) << ":" << ntohs(m_addr.sin_port)
          << "\": " << strerror(errno);
      throw::std::runtime_error(msg.str());
    }

// register callback on incomimg packets
    m_conn = Glib::signal_io().connect(sigc::mem_fun(*this, &Distri::on_packet),
                                       m_sock, Glib::IO_IN);

} catch (...) {
    close(m_sock);
    m_sock = -1;
    throw;
  }
}

/**
 * @brief stop listening for incoming packets and close socket
 */
void Distri::stop()
{
  // leave if no socket
  if (m_sock == -1) {
    return;
  }

// uninstall callback
  m_conn.disconnect();

// close socket
  close(m_sock);

// socket in no more there
  m_sock = -1;
}

/// get and reset packet conter
unsigned long Distri::getPacketCounterAndReset()
{
  unsigned long packets = m_packets;
  m_packets = 0;
  return packets;
}

/**
 * @brief draw pixels of this distributor
 * @param[in] cairo cairo context for drawing
 * @param[in] tf coordinate transformation
 */
void Distri::draw(Cairo::RefPtr<Cairo::Context> &cairo,
                  Transform const &tf) const
{
  OutputMap::const_iterator out;
  for (out = m_outputMap.begin(); out != m_outputMap.end(); ++out) {
    Output const &output = out->second;
    std::vector<Pixel>::const_iterator pix;
    for (pix = output.m_pixels.begin(); pix != output.m_pixels.end(); ++pix) {
      Pixel const &pixel = *pix;
      pixel.draw(cairo, tf);
    }
  }
}

/**
 * @brief callback on packet input on socket
 */
bool Distri::on_packet(Glib::IOCondition condition)
{
  // incoming packet
  if (condition & Glib::IO_IN) {

// read packet
    uint8_t buffer[65536];
    ssize_t sz = recv(m_sock, buffer, sizeof(buffer), 0);
    // success and long enough for header
    if (sz >= 12) {
      // check magic, 3 channels and maxval 0xFF
      if (buffer[ 0] == 0x23 && buffer[ 1] == 0x54 &&
          buffer[ 2] == 0x26 && buffer[ 3] == 0x66 &&
          buffer[ 8] == 0x00 && buffer[ 9] == 0x03 &&
          buffer[10] == 0x00 && buffer[11] == 0xFF) {
        // get outputs (height) and pixels (width)
        unsigned int outputs = (unsigned int)buffer[4] << 8 |
                               (unsigned int)buffer[5];
        unsigned int pixels = (unsigned int)buffer[6] << 8 |
                              (unsigned int)buffer[7];
        // check data length
        if (sz >= 12 + outputs * pixels * 3) {
          uint8_t const *data = buffer + 12;
          // process pixel data
          procPixelData(outputs, pixels, data);
        }
      }
    }

} // if IO_IN

// keep callback installed
  return true;
}

/**
 * @brief process pixel data
 * @param[in] outputs number of outputs in pixel data
 * @param[in] pixels number of pixels per output in pixel data
 * @param[in] data pixel data
 */
void Distri::procPixelData(unsigned long outputs, unsigned long pixels,
                           uint8_t const *data)
{
  // get mappings
  Mapping const & mapRed   = m_mappings[Mapping::Red  ];
  Mapping const & mapGreen = m_mappings[Mapping::Green];
  Mapping const & mapBlue  = m_mappings[Mapping::Blue ];

// traverse all outputs and pixels
  OutputMap::iterator out = m_outputMap.begin();
  OutputMap::iterator outend = m_outputMap.end();
  unsigned long o = 0;
  unsigned long oi = 0;
  for (; out != outend; ++out, ++o, oi += pixels * 3) {
    Output &output = out->second;
    std::vector<Pixel>::iterator pix = output.m_pixels.begin();
    std::vector<Pixel>::iterator pixend = output.m_pixels.end();
    unsigned long p = 0;
    unsigned long pi = oi;
    for (; pix != pixend; ++pix, ++p, pi += 3) {
      Pixel &pixel = *pix;

// default color: black (in case pixel is not in pixel data)
      uint8_t r = 0, g = 0, b = 0;
      // get color of pixel from data
      if (o < outputs && p < pixels) {
        r = mapRed  .display2video(data[pi + 0]);
        g = mapGreen.display2video(data[pi + 1]);
        b = mapBlue .display2video(data[pi + 2]);
      }
      // store color
      pixel.setColor(r, g, b);
    } // for pix, p
  } // for out, o

// count packet
  ++m_packets;
}

libetherpix

commen typo fix