ts-qt/components/ffmsep/tactile/tacdec.cc

#include "tacdec.hh"
#include "components/ffmsep/cpdecoder.hh"
#include <algorithm>
#include <array>
#include <cstddef>
#include <cstdint>
#include <new>
#include <optional>
#include <vector>

namespace ffmsep::tactile {
namespace {

constexpr std::size_t kHeaderSize = 4U; // start bytes + length field
constexpr std::size_t kFixedSectionSize = 1U + 1U + 1U + 4U + 2U + 1U; // address..status
constexpr std::size_t kMinimumFrameSize = kHeaderSize + kFixedSectionSize + 1U; // + CRC byte
constexpr std::uint8_t kCrcPolynomial = 0x07U;
constexpr std::uint8_t kCrcInitial = 0x00U;
constexpr std::uint8_t kCrcXorOut = 0xA9U;
constexpr std::array<std::uint8_t, 2> kStartSequence{
    kStartByteFirst,
    kStartByteSecond
};

struct TactileDecoderContext {
    std::vector<std::uint8_t> fifo;
    bool end_of_stream = false;
    std::int64_t next_pts = 0;
};

const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
    return buf.empty() ? nullptr : buf.data();
}

std::uint8_t crc8_with_xorout(const std::uint8_t* data, std::size_t length) {
    std::uint8_t reg = kCrcInitial;
    for (std::size_t i = 0; i < length; ++i) {
        reg ^= data[i];
        for (int bit = 0; bit < 8; ++bit) {
            if ((reg & 0x80U) != 0U) {
                reg = static_cast<std::uint8_t>((reg << 1U) ^ kCrcPolynomial);
            } else {
                reg = static_cast<std::uint8_t>(reg << 1U);
            }
        }
    }
    return static_cast<std::uint8_t>(reg ^ kCrcXorOut);
}

TactileDecoderContext* get_priv(CPCodecContext* ctx) {
    return ctx ? ctx->priv_as<TactileDecoderContext>() : nullptr;
}

int tactile_init(CPCodecContext* ctx) {
    if (!ctx) {
        return CP_ERROR_INVALID_ARGUMENT;
    }
    if (!ctx->priv_data) {
        ctx->ensure_priv_storage(sizeof(TactileDecoderContext));
    }
    auto* storage = static_cast<TactileDecoderContext*>(ctx->priv_data);
    new (storage) TactileDecoderContext();
    return CP_SUCCESS;
}

void tactile_close(CPCodecContext* ctx) {
    if (!ctx || !ctx->priv_data) {
        return;
    }
    if (auto* priv = get_priv(ctx); priv != nullptr) {
        priv->~TactileDecoderContext();
    }
}

int tactile_send_packet(CPCodecContext* ctx, const CPPacket& packet) {
    auto priv = get_priv(ctx);
    if (!priv) {
        return CP_ERROR_INVALID_STATE;
    }
    if (packet.flush) {
        priv->fifo.clear();
        priv->end_of_stream = false;
        priv->next_pts = 0;
    }

    if (!packet.payload.empty()) {
        priv->fifo.insert(priv->fifo.end(), packet.payload.begin(), packet.payload.end());
    }

    if (packet.end_of_stream) {
        priv->end_of_stream = true;
    }

    return CP_SUCCESS;
}

int tactile_receive_frame(CPCodecContext* ctx, CPFrame& frame) {
    auto* priv = get_priv(ctx);
    if (!priv) {
        return CP_ERROR_INVALID_STATE;
    }

    auto& buf = priv->fifo;

    while (true) {
        if (buf.empty()) {
            if (priv->end_of_stream) {
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }

        const auto start_it = std::search(buf.begin(), buf.end(),
            kStartSequence.begin(), kStartSequence.end());
        if (start_it == buf.end()) {
            buf.clear();
            if (priv->end_of_stream) {
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }

        if (start_it != buf.begin()) {
            buf.erase(buf.begin(), start_it);
        }

        if (buf.size() < kHeaderSize) {
            if (priv->end_of_stream) {
                buf.clear();
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }

        const std::uint8_t* data = buffer_data(buf);
        if (!data) {
            buf.clear();
            continue;
        }

        const std::uint16_t data_length =
            static_cast<std::uint16_t>(data[2]) |
            static_cast<std::uint16_t>(static_cast<std::uint16_t>(data[3]) << 8U);

        if (data_length < kFixedSectionSize) {
            buf.erase(buf.begin());
            continue;
        }

        const std::size_t total_frame_length = kHeaderSize + static_cast<std::size_t>(data_length) + 1U;
        if (buf.size() < total_frame_length) {
            if (priv->end_of_stream) {
                buf.clear();
                priv->end_of_stream = false;
                return CP_ERROR_EOF;
            }
            return CP_ERROR_EAGAIN;
        }

        const std::uint8_t computed_crc = crc8_with_xorout(data + kHeaderSize, data_length);
        const std::uint8_t frame_crc = data[kHeaderSize + static_cast<std::size_t>(data_length)];
        if (computed_crc != frame_crc) {
            buf.erase(buf.begin());
            continue;
        }

        frame.data.assign(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
        frame.pts = priv->next_pts++;
        frame.key_frame = true;
        frame.valid = true;

        buf.erase(buf.begin(), buf.begin() + static_cast<std::ptrdiff_t>(total_frame_length));
        return CP_SUCCESS;
    }
}

const CPCodec kTactileCodec {
    .name = "tactile_serial",
    .long_name = "Framed tactile sensor serial protocol decoder",
    .type = CPMediaType::Data,
    .id = CPCodecID::Tactile,
    .priv_data_size = sizeof(TactileDecoderContext),
    .init = &tactile_init,
    .close = &tactile_close,
    .send_packet = &tactile_send_packet,
    .receive_frame = &tactile_receive_frame
};
}

std::optional<TactileFrame> parse_frame(const CPFrame& frame) {
    if (!frame.valid || frame.data.size() < kMinimumFrameSize) {
        return std::nullopt;
    }
    const auto* bytes = frame.data.data();
    const std::size_t size = frame.data.size();

    if (bytes[0] != kStartByteFirst || bytes[1] != kStartByteSecond) {
        return std::nullopt;
    }

    const std::uint16_t data_length =
        static_cast<std::uint16_t>(bytes[2]) |
        static_cast<std::uint16_t>(static_cast<std::uint16_t>(bytes[3]) << 8U);
    if (data_length < kFixedSectionSize) {
        return std::nullopt;
    }

    const std::size_t expected_size = kHeaderSize + static_cast<std::size_t>(data_length) + 1U;
    if (size != expected_size) {
        return std::nullopt;
    }

    const std::uint8_t crc_byte = bytes[size - 1U];
    const std::uint8_t computed_crc = crc8_with_xorout(bytes + kHeaderSize, data_length);
    if (computed_crc != crc_byte) {
        return std::nullopt;
    }

    const std::uint8_t device_address = bytes[4];
    const std::uint8_t reserved = bytes[5];
    const std::uint8_t response_function = bytes[6];
    const std::uint32_t start_address =
        static_cast<std::uint32_t>(bytes[7]) |
        (static_cast<std::uint32_t>(bytes[8]) << 8U) |
        (static_cast<std::uint32_t>(bytes[9]) << 16U) |
        (static_cast<std::uint32_t>(bytes[10]) << 24U);
    const std::uint16_t return_byte_count =
        static_cast<std::uint16_t>(bytes[11]) |
        (static_cast<std::uint16_t>(bytes[12]) << 8U);
    const std::uint8_t status = bytes[13];

    const std::size_t payload_offset = kHeaderSize + kFixedSectionSize;
    const std::size_t payload_length = static_cast<std::size_t>(data_length) - kFixedSectionSize;
    if (payload_length != return_byte_count) {
        return std::nullopt;
    }

    TactileFrame parsed{};
    parsed.device_address = device_address;
    parsed.reserved = reserved;
    parsed.response_function = response_function;
    parsed.function = static_cast<FunctionCode>(response_function & 0x7FU);
    parsed.start_address = start_address;
    parsed.return_byte_count = return_byte_count;
    parsed.status = status;
    parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + payload_length);
    return parsed;
}

std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
    if (frame.payload.size() != frame.return_byte_count) {
        return {};
    }
    if (frame.payload.empty() || (frame.payload.size() % 2U != 0U)) {
        return {};
    }
    std::vector<std::uint16_t> values;
    values.reserve(frame.payload.size() / 2U);
    for (std::size_t idx = 0; idx + 1U < frame.payload.size(); idx += 2U) {
        const std::uint16_t value = static_cast<std::uint16_t>(
            static_cast<std::uint16_t>(frame.payload[idx]) |
            static_cast<std::uint16_t>(frame.payload[idx + 1U] << 8U));
        values.push_back(value);
    }
    return values;
}

std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame) {
    if (frame.payload.size() != 2U) {
        return std::nullopt;
    }
    MatrixSize size{};
    size.long_edge = frame.payload[0];
    size.short_edge = frame.payload[1];
    return size;
}

const CPCodec* tactile_codec() {
    return &kTactileCodec;
}

void register_tactile_codec() {
    cpcodec_register(&kTactileCodec);
}
}