qt5-legatus-legacy/Lts/Proxies/GuezNasat/tcp_socket_handler.cpp

/**
  * Websocket interface for building control unit
  * $Id$
  * $HeadURL$
  * $Author$
  * Created by Christian Ehrlicher, SLSE, 2017-01-30
  */

#include "tcp_socket_handler.h"
#include "guez_nasat_telegrams.h"

namespace GuezNasat
{
static constexpr unsigned char s_startMark = 0x02;
static constexpr unsigned char s_endMark = 0x03;
static inline QByteArray escapeData(const QByteArray &bufferData)
{
  QByteArray ret;
  ret.reserve(bufferData.size() * 6 / 5);
  for (const unsigned char c : bufferData)
  {
    if (c >= 0x20 && c < 0x7f)
      ret += c;
    else
      ret += "\\x" + QByteArray::number(static_cast<unsigned char>(c), 16);
  }
  return ret;
}

TcpSocketHandler::TcpSocketHandler(QObject *parent)
  : AbstractSocketHandler("TcpSocketHandler", parent)
  , m_tcpSocketServer(new QTcpServer(this))
{
  TRACE_LOG(m_logger) << LOGFUNCTION << " START";
  TRACE_LOG(m_logger) << LOGFUNCTION << " END";
}

bool TcpSocketHandler::init(quint16 listenPort)
{
  if (!m_tcpSocketServer->listen(QHostAddress::Any, listenPort))
  {
    WARNING_LOG(m_logger) << "Can not listen on port " << listenPort << " - aborting";
    return false;
  }
  connect(m_tcpSocketServer, &QTcpServer::newConnection,
          this, &TcpSocketHandler::onNewConnection);

  return true;
}

void TcpSocketHandler::onNewConnection()
{
  QTcpSocket *ts = m_tcpSocketServer->nextPendingConnection();
  if (!ts)
    return;
  DEBUG_LOG(m_logger) << "New TCP socket connection from " << ts->peerAddress().toString()
                      << ":" << ts->peerPort() << ", ptr: " << ts;

  connect(ts, &QTcpSocket::readyRead,
          this, &TcpSocketHandler::onReadyRead);
  connect(ts, &QTcpSocket::disconnected,
          this, &TcpSocketHandler::onTcpSocketDisconnected);

  m_tcpSockets.insert(ts, QByteArray());
}

bool TcpSocketHandler::sendTelegramInternal(const QByteArray &tele)
{
  if (tele.size() > std::numeric_limits<unsigned short>::max())
    return false;
  const unsigned short len = tele.size();
  // add framing
  QByteArray data(len + 4, Qt::Uninitialized);  // s_startMark, len, s_endMark
  data[0] = s_startMark;
  // big endian
  data[1] = (len & 0x00FFU);
  data[2] = (len & 0xFF00U) >> 16;
  char *dst = data.data() + 3;
  memcpy(dst, tele.data(), tele.size());
  data[data.size() - 1] = s_endMark;

  bool bOk = false;
  DEBUG_LOG(m_logger) << "Sending: " << escapeData(data);
  for (auto it = m_tcpSockets.cbegin(); it != m_tcpSockets.cend(); ++it)
  {
    QTcpSocket *ws = it.key();
    if (ws->write(data) <= 0)
      bOk = true;
  }
  return bOk;
}

void TcpSocketHandler::onReadyRead()
{
  QTcpSocket *s = qobject_cast<QTcpSocket *>(sender());
  if (!s)
  {
    Q_ASSERT(false);
    return;
  }
  auto &tcpBuffer = m_tcpSockets[s];
  const QByteArray bufferData = tcpBuffer + s->readAll();
  TRACE_LOG(m_logger) << "TcpSocketHandler::onReadyRead bufferData='" << escapeData(bufferData) << "'";
  // returns the unparsed part of the buffer
  tcpBuffer = parseBuffer(bufferData);
}

QByteArray TcpSocketHandler::parseBuffer(const QByteArray &buffer)
{
  int iEndPos = 0;
  int iCurReadPos = 0;
  while (findNextSyncStartByte(buffer, iCurReadPos))
  {
    const int iStartPos = iCurReadPos;
    if (!findNextSyncEndByte(buffer, iCurReadPos))
      break;
    iEndPos = iCurReadPos;
    // after s_startMark there are two bytes with len, we currently don't care about them
    textMessageReceived(buffer.mid(iStartPos + 3, iEndPos - iStartPos - 3));
  }
  return buffer.mid(iEndPos);
}

bool TcpSocketHandler::findNextSyncStartByte(const QByteArray &buffer, int &iCurReadPos)
{
  // Suchen nach s_startMark
  while (iCurReadPos < buffer.size())
  {
    const unsigned char data = buffer.at(iCurReadPos);
    if (data == s_startMark)
      return true;
    ++iCurReadPos;
  }
  return false;
}

bool TcpSocketHandler::findNextSyncEndByte(const QByteArray &buffer, int &iCurReadPos)
{
  // Suchen nach s_endMark
  while (iCurReadPos < buffer.size())
  {
    const unsigned char data = buffer.at(iCurReadPos);
    if (data == s_endMark)
      return true;
    ++iCurReadPos;
  }
  return false;
}

void TcpSocketHandler::onTcpSocketDisconnected()
{
  QTcpSocket *ws = qobject_cast<QTcpSocket *>(sender());
  if (!ws)
    return;
  DEBUG_LOG(m_logger) << "TCP socket connection closed, ptr: " << ws;
  m_tcpSockets.remove(ws);
  ws->deleteLater();
}

} // namespace GuezNasat