parser.cpp

#include "parser.h"

#include <arpa/inet.h>
#include <cstddef>
#include <string.h>
#include <stdexcept>
#include <sys/types.h>

ssize_t parseCANFrame(canfd_frame* frame, const uint8_t* rawData, const uint8_t* rawDataEnd) {
  using namespace cannelloni;
  const uint8_t* rawDataOrig = rawData;
  canid_t tmp;
  memcpy(&tmp, rawData, sizeof (canid_t));
  frame->can_id = ntohl(tmp);
  /* += 4 */
  rawData += sizeof (canid_t);
  frame->len = *rawData;
  /* += 1 */
  rawData += sizeof (frame->len);
  /* If this is a CAN FD frame, also retrieve the flags */
  if (frame->len & CANFD_FRAME)
  {
      frame->flags = *rawData;
      /* += 1 */
      rawData += sizeof (frame->flags);
  }
  /* RTR Frames have no data section although they have a dlc */
  if ((frame->can_id & CAN_RTR_FLAG) == 0)
  {
      /* Check again now that we know the dlc */
      if (rawData + canfd_len(frame) > rawDataEnd)
      {
          frame->len = 0;
          return -1;
      }

      memcpy(frame->data, rawData, canfd_len(frame));
      rawData += canfd_len(frame);
  }
  return rawData-rawDataOrig;
}

void parseFrames(uint16_t len, const uint8_t* buffer, std::function<canfd_frame*()> frameAllocator,
        std::function<void(canfd_frame*, bool)> frameReceiver)
{
    using namespace cannelloni;

    const struct CannelloniDataPacket* data;
    /* Check for OP Code */
    data = reinterpret_cast<const struct CannelloniDataPacket*> (buffer);
    if (data->version != CANNELLONI_FRAME_VERSION)
        throw std::runtime_error("Received wrong version");

    if (data->op_code != DATA)
        throw std::runtime_error("Received wrong OP code");

    if (ntohs(data->count) == 0)
        return; // Empty packets silently ignored

    const uint8_t* rawData = buffer + CANNELLONI_DATA_PACKET_BASE_SIZE;
    const uint8_t* bufferEnd = buffer + len;

    for (uint16_t i = 0; i < ntohs(data->count); i++)
    {
        if (rawData - buffer + CANNELLONI_FRAME_BASE_SIZE > len)
            throw std::runtime_error("Received incomplete packet");

        /* We got at least a complete canfd_frame header */
        canfd_frame* frame = frameAllocator();
        if (!frame)
            throw std::runtime_error("Allocation error.");

        ssize_t bytesParsed = parseCANFrame(frame, rawData, bufferEnd);
        rawData+=bytesParsed;
        if (bytesParsed > 0) {
            frameReceiver(frame, true);
        } else {
            frameReceiver(frame, false);
            throw std::runtime_error("Received incomplete packet / can header corrupt!");
        }
    }
}

size_t encodeFrame(uint8_t *data, canfd_frame *frame) {
    using namespace cannelloni;
    uint8_t *dataOrig = data;
    canid_t tmp = htonl(frame->can_id);
    memcpy(data, &tmp, sizeof(canid_t));
    /* += 4 */
    data += sizeof(canid_t);
    *data = frame->len;
    /* += 1 */
    data += sizeof(frame->len);
    /* If this is a CAN FD frame, also send the flags */
    if (frame->len & CANFD_FRAME) {
        *data = frame->flags;
        /* += 1 */
        data += sizeof(frame->flags);
    }
    if ((frame->can_id & CAN_RTR_FLAG) == 0) {
        memcpy(data, frame->data, canfd_len(frame));
        data += canfd_len(frame);
    }
    return data-dataOrig;
}

uint8_t* buildPacket(uint16_t len, uint8_t* packetBuffer,
        std::list<canfd_frame*>& frames, uint8_t seqNo,
        std::function<void(std::list<canfd_frame*>&, std::list<canfd_frame*>::iterator)> handleOverflow)
{
    using namespace cannelloni;

    uint16_t frameCount = 0;
    uint8_t* data = packetBuffer + CANNELLONI_DATA_PACKET_BASE_SIZE;
    for (auto it = frames.begin(); it != frames.end(); it++)
    {
        canfd_frame* frame = *it;
        /* Check for packet overflow */
        if ((data - packetBuffer + CANNELLONI_FRAME_BASE_SIZE + canfd_len(frame)
                + ((frame->len & CANFD_FRAME) ? sizeof(frame->flags) : 0))
                > len)
        {
            handleOverflow(frames, it);
            break;
        }
        data += encodeFrame(data, frame);
        frameCount++;
    }
    struct CannelloniDataPacket* dataPacket;
    dataPacket = (struct CannelloniDataPacket*) (packetBuffer);
    dataPacket->version = CANNELLONI_FRAME_VERSION;
    dataPacket->op_code = DATA;
    dataPacket->seq_no = seqNo;
    dataPacket->count = htons(frameCount);

    return data;
}