feat: add tactile codec

components/ffmsep/cpdecoder.cc

@@ -0,0 +1,215 @@
+//
+// Core FFmpeg-style codec registry and decoding helpers.
+//
+
+#include "cpdecoder.hh"
+
+#include <algorithm>
+#include <mutex>
+
+namespace ffmsep {
+
+namespace {
+
+std::vector<const CPCodec*>& codec_registry() {
+    static std::vector<const CPCodec*> registry;
+    return registry;
+}
+
+std::mutex& registry_mutex() {
+    static std::mutex m;
+    return m;
+}
+
+void attach_codec(CPCodecContext* ctx, const CPCodec* codec) {
+    if (!ctx) {
+        return;
+    }
+
+    ctx->codec = codec;
+    if (!codec) {
+        ctx->codec_type = CPMediaType::Unknown;
+        ctx->priv_data = nullptr;
+        ctx->release_priv_storage();
+        return;
+    }
+
+    ctx->codec_type = codec->type;
+    ctx->priv_data = ctx->ensure_priv_storage(codec->priv_data_size);
+}
+
+bool codec_name_equals(const CPCodec* codec, std::string_view name) {
+    if (!codec || !codec->name) {
+        return false;
+    }
+    return std::string_view(codec->name) == name;
+}
+
+} // namespace
+
+void* CPCodecContext::ensure_priv_storage(std::size_t size) {
+    if (size == 0U) {
+        priv_storage.clear();
+        priv_data = nullptr;
+        return nullptr;
+    }
+    if (priv_storage.size() != size) {
+        priv_storage.assign(size, static_cast<std::uint8_t>(0));
+    }
+    priv_data = priv_storage.data();
+    return priv_data;
+}
+
+void CPCodecContext::release_priv_storage() noexcept {
+    priv_storage.clear();
+    priv_data = nullptr;
+}
+
+void cpcodec_register(const CPCodec* codec) {
+    if (!codec || !codec->name) {
+        return;
+    }
+
+    std::lock_guard<std::mutex> lock(registry_mutex());
+    auto& reg = codec_registry();
+    auto already = std::find(reg.begin(), reg.end(), codec);
+    if (already != reg.end()) {
+        return;
+    }
+
+    auto same_id = std::find_if(reg.begin(), reg.end(), [codec](const CPCodec* entry) {
+        return entry && codec && entry->id == codec->id && codec->id != CPCodecID::Unknown;
+    });
+    if (same_id != reg.end()) {
+        *same_id = codec;
+        return;
+    }
+
+    reg.push_back(codec);
+}
+
+void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs) {
+    for (const CPCodec* codec : codecs) {
+        cpcodec_register(codec);
+    }
+}
+
+const CPCodec* cpcodec_find_decoder(CPCodecID id) {
+    std::lock_guard<std::mutex> lock(registry_mutex());
+    const auto& reg = codec_registry();
+    auto it = std::find_if(reg.begin(), reg.end(), [id](const CPCodec* codec) {
+        return codec && codec->id == id;
+    });
+    return it == reg.end() ? nullptr : *it;
+}
+
+const CPCodec* cpcodec_find_decoder_by_name(std::string_view name) {
+    std::lock_guard<std::mutex> lock(registry_mutex());
+    const auto& reg = codec_registry();
+    auto it = std::find_if(reg.begin(), reg.end(), [name](const CPCodec* codec) {
+        return codec_name_equals(codec, name);
+    });
+    return it == reg.end() ? nullptr : *it;
+}
+
+std::vector<const CPCodec*> cpcodec_list_codecs() {
+    std::lock_guard<std::mutex> lock(registry_mutex());
+    return codec_registry();
+}
+
+CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec) {
+    auto* ctx = new CPCodecContext();
+    if (codec) {
+        attach_codec(ctx, codec);
+    }
+    return ctx;
+}
+
+int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec) {
+    if (!ctx) {
+        return CP_ERROR_INVALID_ARGUMENT;
+    }
+
+    if (ctx->is_open) {
+        return CP_ERROR_INVALID_STATE;
+    }
+
+    if (codec) {
+        attach_codec(ctx, codec);
+    }
+
+    if (!ctx->codec) {
+        return CP_ERROR_INVALID_ARGUMENT;
+    }
+
+    ctx->is_open = true;
+    if (ctx->codec->init) {
+        int rc = ctx->codec->init(ctx);
+        if (rc < 0) {
+            ctx->is_open = false;
+            if (ctx->codec->close) {
+                ctx->codec->close(ctx);
+            }
+            return rc;
+        }
+    }
+    return CP_SUCCESS;
+}
+
+int cpcodec_close(CPCodecContext* ctx) {
+    if (!ctx) {
+        return CP_ERROR_INVALID_ARGUMENT;
+    }
+
+    if (!ctx->is_open) {
+        return CP_SUCCESS;
+    }
+
+    if (ctx->codec && ctx->codec->close) {
+        ctx->codec->close(ctx);
+    }
+
+    ctx->is_open = false;
+    ctx->release_priv_storage();
+    ctx->codec_type = CPMediaType::Unknown;
+    ctx->codec = nullptr;
+    ctx->priv_data = nullptr;
+    return CP_SUCCESS;
+}
+
+void cpcodec_free_context(CPCodecContext** ctx) {
+    if (!ctx || !*ctx) {
+        return;
+    }
+    cpcodec_close(*ctx);
+    delete *ctx;
+    *ctx = nullptr;
+}
+
+int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet) {
+    if (!ctx || !packet) {
+        return CP_ERROR_INVALID_ARGUMENT;
+    }
+    if (!ctx->is_open || !ctx->codec) {
+        return CP_ERROR_NOT_OPEN;
+    }
+    if (!ctx->codec->send_packet) {
+        return CP_ERROR_INVALID_STATE;
+    }
+    return ctx->codec->send_packet(ctx, *packet);
+}
+
+int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame) {
+    if (!ctx || !frame) {
+        return CP_ERROR_INVALID_ARGUMENT;
+    }
+    if (!ctx->is_open || !ctx->codec) {
+        return CP_ERROR_NOT_OPEN;
+    }
+    if (!ctx->codec->receive_frame) {
+        return CP_ERROR_INVALID_STATE;
+    }
+    return ctx->codec->receive_frame(ctx, *frame);
+}
+
+} // namespace ffmsep

components/ffmsep/cpdecoder.hh

@@ -0,0 +1,133 @@
+//
+// Simple FFmpeg-inspired serial decoding toolkit.
+//
+
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+#include <mutex>
+#include <optional>
+#include <string>
+#include <string_view>
+#include <vector>
+#include <initializer_list>
+
+namespace ffmsep {
+
+// Error codes loosely mirroring FFmpeg semantics.
+inline constexpr int CP_SUCCESS = 0;
+inline constexpr int CP_ERROR_EOF = -1;
+inline constexpr int CP_ERROR_EAGAIN = -2;
+inline constexpr int CP_ERROR_NOT_OPEN = -3;
+inline constexpr int CP_ERROR_INVALID_STATE = -4;
+inline constexpr int CP_ERROR_INVALID_ARGUMENT = -5;
+
+enum class CPMediaType : std::uint8_t {
+    Unknown = 0,
+    Data,
+    Audio,
+    Video
+};
+
+enum class CPCodecID : std::uint32_t {
+    Unknown = 0,
+    Tactile = 0x54514354u // 'T','Q','C','T' marker for tactile quick codec.
+};
+
+struct CPPacket {
+    std::vector<std::uint8_t> payload;
+    std::int64_t pts = 0;
+    std::int64_t dts = 0;
+    bool end_of_stream = false;
+    bool flush = false;
+
+    CPPacket() = default;
+    CPPacket(std::vector<std::uint8_t> data, std::int64_t pts_value = 0, std::int64_t dts_value = 0) noexcept
+        : payload(std::move(data)), pts(pts_value), dts(dts_value) {}
+
+    [[nodiscard]] bool empty() const noexcept { return payload.empty(); }
+};
+
+struct CPFrame {
+    std::vector<std::uint8_t> data;
+    std::int64_t pts = 0;
+    bool key_frame = false;
+    bool valid = false;
+
+    void reset() noexcept {
+        data.clear();
+        pts = 0;
+        key_frame = false;
+        valid = false;
+    }
+};
+
+struct CPCodecContext;
+
+struct CPCodec {
+    using InitFn = int (*)(CPCodecContext*);
+    using CloseFn = void (*)(CPCodecContext*);
+    using SendPacketFn = int (*)(CPCodecContext*, const CPPacket&);
+    using ReceiveFrameFn = int (*)(CPCodecContext*, CPFrame&);
+
+    const char* name = nullptr;
+    const char* long_name = nullptr;
+    CPMediaType type = CPMediaType::Unknown;
+    CPCodecID id = CPCodecID::Unknown;
+    std::size_t priv_data_size = 0;
+    InitFn init = nullptr;
+    CloseFn close = nullptr;
+    SendPacketFn send_packet = nullptr;
+    ReceiveFrameFn receive_frame = nullptr;
+};
+
+struct CPCodecContext {
+    const CPCodec* codec = nullptr;
+    void* priv_data = nullptr;
+    CPMediaType codec_type = CPMediaType::Unknown;
+    bool is_open = false;
+
+    void clear() noexcept {
+        codec = nullptr;
+        priv_data = nullptr;
+        codec_type = CPMediaType::Unknown;
+        is_open = false;
+        priv_storage.clear();
+    }
+
+    void* ensure_priv_storage(std::size_t size);
+    void release_priv_storage() noexcept;
+
+    template <typename T>
+    [[nodiscard]] T* priv_as() noexcept {
+        return static_cast<T*>(priv_data);
+    }
+
+    template <typename T>
+    [[nodiscard]] const T* priv_as() const noexcept {
+        return static_cast<const T*>(priv_data);
+    }
+
+private:
+    std::vector<std::uint8_t> priv_storage;
+
+    friend CPCodecContext* cpcodec_alloc_context3(const CPCodec*);
+    friend int cpcodec_open2(CPCodecContext*, const CPCodec*);
+    friend int cpcodec_close(CPCodecContext*);
+};
+
+void cpcodec_register(const CPCodec* codec);
+void cpcodec_register_many(std::initializer_list<const CPCodec*> codecs);
+const CPCodec* cpcodec_find_decoder(CPCodecID id);
+const CPCodec* cpcodec_find_decoder_by_name(std::string_view name);
+std::vector<const CPCodec*> cpcodec_list_codecs();
+
+CPCodecContext* cpcodec_alloc_context3(const CPCodec* codec);
+int cpcodec_open2(CPCodecContext* ctx, const CPCodec* codec = nullptr);
+int cpcodec_close(CPCodecContext* ctx);
+void cpcodec_free_context(CPCodecContext** ctx);
+int cpcodec_send_packet(CPCodecContext* ctx, const CPPacket* packet);
+int cpcodec_receive_frame(CPCodecContext* ctx, CPFrame* frame);
+
+} // namespace ffmsep

components/ffmsep/tactile/tacdec.h

@@ -0,0 +1,49 @@
+//
+// High level helpers for the tactile sensor binary protocol.
+//
+
+#pragma once
+
+#include "../cpdecoder.hh"
+
+#include <cstdint>
+#include <optional>
+#include <vector>
+
+namespace ffmsep::tactile {
+
+inline constexpr std::uint8_t kStartByte = 0x3A;
+inline constexpr std::uint8_t kEndByteFirst = 0x0D;
+inline constexpr std::uint8_t kEndByteSecond = 0x0A;
+
+enum class FunctionCode : std::uint8_t {
+    Unknown = 0x00,
+    ReadMatrix = 0x01,
+    ReadSingle = 0x02,
+    ReadTemperature = 0x03,
+    SetDeviceId = 0x51,
+    SetMatrixSize = 0x52,
+    CalibrationMode = 0x53
+};
+
+struct MatrixSize {
+    std::uint8_t long_edge = 0;
+    std::uint8_t short_edge = 0;
+};
+
+struct TactileFrame {
+    std::uint8_t device_address = 0;
+    FunctionCode function = FunctionCode::Unknown;
+    std::uint8_t data_length = 0;
+    std::vector<std::uint8_t> payload;
+};
+
+std::optional<TactileFrame> parse_frame(const CPFrame& frame);
+std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame);
+std::optional<MatrixSize> parse_matrix_size_payload(const TactileFrame& frame);
+std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame);
+
+const CPCodec* tactile_codec();
+void register_tactile_codec();
+
+} // namespace ffmsep::tactile

components/ffmsep/tactile/tecdec.cc

@@ -0,0 +1,286 @@
+//
+// Decoder for the tactile sensor framed binary protocol.
+//
+
+#include "tacdec.h"
+
+#include <algorithm>
+#include <cstddef>
+#include <cstdint>
+#include <new>
+
+namespace ffmsep::tactile {
+
+namespace {
+
+constexpr std::size_t kMinimumFrameSize = 1   // start
+    + 1                                       // address
+    + 1                                       // function
+    + 1                                       // length
+    + 0                                       // payload
+    + 2                                       // CRC
+    + 2;                                      // end markers
+
+constexpr std::uint16_t kCrcInitial = 0xFFFF;
+constexpr std::uint16_t kCrcPolynomial = 0xA001; // CRC-16/MODBUS (LSB first)
+
+struct TactileDecoderContext {
+    std::vector<std::uint8_t> fifo;
+    bool end_of_stream = false;
+    std::int64_t next_pts = 0;
+};
+
+std::uint16_t crc16_modbus(const std::uint8_t* data, std::size_t length) {
+    std::uint16_t crc = kCrcInitial;
+    for (std::size_t i = 0; i < length; ++i) {
+        crc ^= static_cast<std::uint16_t>(data[i]);
+        for (int bit = 0; bit < 8; ++bit) {
+            if ((crc & 0x0001U) != 0U) {
+                crc = static_cast<std::uint16_t>((crc >> 1U) ^ kCrcPolynomial);
+            } else {
+                crc = static_cast<std::uint16_t>(crc >> 1U);
+            }
+        }
+    }
+    return crc;
+}
+
+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;
+}
+
+std::size_t frame_length_from_payload(std::uint8_t payload_length) {
+    return 1U + 1U + 1U + 1U + payload_length + 2U + 2U;
+}
+
+const std::uint8_t* buffer_data(const std::vector<std::uint8_t>& buf) {
+    return buf.empty() ? nullptr : buf.data();
+}
+
+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;
+        }
+
+        // Discard bytes until start byte is found.
+        auto start_it = std::find(buf.begin(), buf.end(), kStartByte);
+        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() < kMinimumFrameSize) {
+            if (priv->end_of_stream) {
+                // Incomplete frame at end of stream: drop it and report EOF.
+                buf.clear();
+                priv->end_of_stream = false;
+                return CP_ERROR_EOF;
+            }
+            return CP_ERROR_EAGAIN;
+        }
+
+        const std::uint8_t* data = buffer_data(buf);
+        const std::uint8_t address = data[1];
+        const std::uint8_t function = data[2];
+        const std::uint8_t payload_length = data[3];
+
+        const std::size_t total_frame_length = frame_length_from_payload(payload_length);
+        if (buf.size() < total_frame_length) {
+            if (priv->end_of_stream) {
+                // Not enough data before stream end: treat as EOF and drop buffer.
+                buf.clear();
+                priv->end_of_stream = false;
+                return CP_ERROR_EOF;
+            }
+            return CP_ERROR_EAGAIN;
+        }
+
+        const std::size_t payload_offset = 4U;
+        const std::size_t crc_offset = payload_offset + payload_length;
+        const std::size_t end_offset = crc_offset + 2U;
+
+        const std::uint8_t crc_lo = data[crc_offset];
+        const std::uint8_t crc_hi = data[crc_offset + 1U];
+        const std::uint16_t crc_value = static_cast<std::uint16_t>(crc_lo) |
+            static_cast<std::uint16_t>(crc_hi << 8U);
+
+        const std::uint8_t end_first = data[end_offset];
+        const std::uint8_t end_second = data[end_offset + 1U];
+
+        if (end_first != kEndByteFirst || end_second != kEndByteSecond) {
+            // Invalid end marker, drop start byte and retry.
+            buf.erase(buf.begin());
+            continue;
+        }
+
+        const std::size_t crc_region_length = 3U + payload_length; // address + function + length + payload
+        const std::uint16_t computed_crc = crc16_modbus(data + 1U, crc_region_length);
+        if (computed_crc != crc_value) {
+            buf.erase(buf.begin());
+            continue;
+        }
+
+        (void)address;
+        (void)function;
+
+        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 {
+    "tactile_serial",
+    "Framed tactile sensor serial protocol decoder",
+    CPMediaType::Data,
+    CPCodecID::Tactile,
+    sizeof(TactileDecoderContext),
+    &tactile_init,
+    &tactile_close,
+    &tactile_send_packet,
+    &tactile_receive_frame
+};
+
+} // namespace
+
+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] != kStartByte) {
+        return std::nullopt;
+    }
+    if (bytes[size - 2] != kEndByteFirst || bytes[size - 1] != kEndByteSecond) {
+        return std::nullopt;
+    }
+
+    if (size < 4U) {
+        return std::nullopt;
+    }
+
+    const std::uint8_t length = bytes[3];
+    if (frame_length_from_payload(length) != size) {
+        return std::nullopt;
+    }
+
+    const std::uint8_t address = bytes[1];
+    const FunctionCode function = static_cast<FunctionCode>(bytes[2]);
+    const std::size_t payload_offset = 4U;
+
+    TactileFrame parsed{};
+    parsed.device_address = address;
+    parsed.function = function;
+    parsed.data_length = length;
+    parsed.payload.assign(bytes + payload_offset, bytes + payload_offset + length);
+    return parsed;
+}
+
+std::vector<std::uint16_t> parse_pressure_values(const TactileFrame& frame) {
+    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;
+}
+
+std::optional<MatrixSize> parse_matrix_coordinate_payload(const TactileFrame& frame) {
+    return parse_matrix_size_payload(frame);
+}
+
+const CPCodec* tactile_codec() {
+    return &kTactileCodec;
+}
+
+void register_tactile_codec() {
+    cpcodec_register(&kTactileCodec);
+}
+
+} // namespace ffmsep::tactile