Initial commit for GitHub migration based on spice2x-25-03-03

src/spice2x/acio2emu/packet.cpp

@@ -0,0 +1,259 @@
+#include "packet.h"
+
+#include "util/logging.h"
+
+#include "acio2emu/internal/crc.h"
+
+namespace acio2emu {
+    static constexpr uint8_t SOF = 0xAA;
+    static constexpr uint8_t ESC = 0xFF;
+
+    static void encode_payload_(std::queue<uint8_t> &out, const std::vector<uint8_t> &payload) {
+        for (auto b : payload) {
+            if (b == SOF || b == ESC) {
+                out.push(ESC);
+                b = ~b;
+            }
+            out.push(b); 
+        }
+        // compute and write the payload's CRC
+        out.push(detail::crc7_lgp_48(0x7F, payload.data(), payload.size()) ^ 0x7F);
+    }
+
+    bool encode_packet(std::queue<uint8_t> &out, uint8_t node, uint8_t tag, const std::vector<uint8_t> &payload) {
+        auto size = payload.size();
+        if (size > 127) {
+            log_warning("acio2emu", "cannot encode packet: payload too large: {} > 127", payload.size());
+            return false;
+        }
+
+        // build the header
+        uint8_t header[5] = {
+            SOF,
+            static_cast<uint8_t>(node * 3),
+            tag,
+            static_cast<uint8_t>(size),
+            0,
+        };
+        // compute the header's CRC
+        header[4] = detail::crc4_lgp_c(0x0F, &header[1], sizeof(header) - 1) ^ 0x0F;
+        // push the header to the output queue
+        for (size_t i = 0; i < sizeof(header); i++) {
+            out.push(header[i]);
+        }
+
+        encode_payload_(out, payload);
+        return true;
+    }
+
+    void PacketDecoder::set_step_(readStep s) {
+#ifndef NDEBUG
+    auto valid = true;
+    switch (s) {
+        case readStep::idle:
+        case readStep::readNode:
+            // transition from any step/state allowed
+            break;
+
+        case readStep::readTag:
+            if (step_ != readStep::readNode) {
+                valid = false;
+            }
+            break;
+
+        case readStep::readPayloadSize:
+            if (step_ != readStep::readTag) {
+                valid = false;
+            }
+            break;
+
+        case readStep::readPayloadFlags:
+            if (step_ != readStep::readPayloadSize) {
+                valid = false;
+            }
+            break;
+
+        case readStep::readReplacementByte:
+            if (step_ != readStep::readPayloadFlags) {
+                valid = false;
+            }
+            break;
+
+        case readStep::readPayload:
+            if (step_ != readStep::readPayloadFlags && 
+                step_ != readStep::readReplacementByte &&
+                step_ != readStep::readEscaped
+            ) {
+                valid = false;
+            }
+            break;
+
+        case readStep::readEscaped:
+            if (step_ != readStep::readPayload) {
+                valid = false;
+            }
+            break;
+
+        default:
+            log_fatal("acio2emu", "cannot set step: unknown value: {}", s);
+            break;
+        }
+
+        if (!valid) {
+            log_fatal("acio2emu", "illegal transition detected: {} -> {}", step_, s);
+        }
+#endif
+        step_ = s;
+    }
+
+    int PacketDecoder::update_payload_size_(uint8_t b) {
+        if ((b & 0x80) == 0) {
+            payload_size_ = (payload_size_ << 7) | (b & 0x7F);
+            // finished
+            return 0;
+        }
+        else if ((b & 0x40) != 0 && payload_size_count_ < 5) {
+            payload_size_count_++;
+            payload_size_count_ = (payload_size_count_ << 6) | (b & 0x3F);
+            // continuation required
+            return 1;
+        }
+        else {
+            // invalid value or invalid state
+            return -1;
+        }
+    }
+
+    uint8_t PacketDecoder::deobfuscate_(uint8_t b) {
+        if ((b ^ 0xAA) == 0) {
+            return b;
+        }
+
+        auto mask = 0x55;
+        if ((b & 0x80) == 0) {
+            mask = 0x7F;
+        }
+
+        return (b ^ lcg_()) & mask;
+    }
+
+    void PacketDecoder::reset_(readStep s) {
+        set_step_(s);
+        packet_ = {};
+        payload_size_ = 0;
+        payload_size_count_ = 0;
+    }
+
+    bool PacketDecoder::update(uint8_t b) {
+        // is this the start of a packet?
+        if (b == SOF) {
+            reset_(readStep::readNode);
+            return false;
+        }
+
+        switch (step_) {
+        case readStep::readNode:
+            packet_.node = b;
+
+            set_step_(readStep::readTag);
+            break;
+
+        case readStep::readTag:
+            packet_.tag = b;
+
+            set_step_(readStep::readPayloadSize);
+            break;
+
+        case readStep::readPayloadSize: {
+            auto status = update_payload_size_(b);
+            if (status == 0) {
+                // finished reading payload size
+                packet_.payload.reserve(payload_size_);
+                set_step_(readStep::readPayloadFlags);
+            }  
+            else if (status == -1) {
+                // reset on error
+                reset_(readStep::idle);
+            }
+            break;
+        }
+
+        case readStep::readPayloadFlags:
+            obfuscated_ = (b & (1 << 4)) != 0;
+            encoding_ = static_cast<payloadEncoding>(b >> 5);
+        
+            if (obfuscated_) {
+                lcg_.seed(packet_.tag ^ 0x55);
+            }
+
+            if (encoding_ == payloadEncoding::replace) {
+                set_step_(readStep::readReplacementByte);
+            }
+            else {
+                set_step_(readStep::readPayload);
+
+                if (encoding_ == payloadEncoding::lz) {
+                    // reset the InflateTransformer
+                    inflate_ = {};
+                }
+            }
+            break;
+
+        case readStep::readReplacementByte:
+            substitute_ = b;
+
+            set_step_(readStep::readPayload);
+            break;
+
+        case readStep::readPayload:
+            // do we need to deobfuscate?
+            if (obfuscated_) {
+                b = deobfuscate_(b);
+            }
+
+            if (encoding_ == payloadEncoding::lz) {
+                inflate_.put(b);
+                for (int i = inflate_.get(); i >= 0; i = inflate_.get()) {
+                    packet_.payload.push_back(i);
+                }
+            }
+            else if (encoding_ == payloadEncoding::replace && b == substitute_) {
+                packet_.payload.push_back(SOF);
+            }
+            else if (encoding_ == payloadEncoding::byteStuffing && b == ESC) {
+                set_step_(readStep::readEscaped);
+                break;
+            }
+            else {
+                packet_.payload.push_back(b);
+            }
+            break;
+
+        case readStep::readEscaped:
+            b = ~b;
+            if (obfuscated_) {
+                b = deobfuscate_(b);
+            }
+            packet_.payload.push_back(b);
+
+            set_step_(readStep::readPayload);
+            break;
+
+        default:
+            break;
+        }
+
+        if ((step_ == readStep::readPayload || step_ == readStep::readPayloadFlags) && 
+            (packet_.payload.size() >= payload_size_)) {
+            set_step_(readStep::idle);
+            // finished reading packet
+            return true;
+        }
+
+        return false;
+    }
+
+    const Packet &PacketDecoder::packet() {
+        return packet_;
+    }
+}