TractionModem.hxx

Go to the documentation of this file.

 
#ifndef _TRACTIONMODEM_TRACTIONMODEM_HXX_
#define _TRACTIONMODEM_TRACTIONMODEM_HXX_
 
 
#include "traction_modem/TractionModemDefs.hxx"
#if !defined(GTEST)
#include "hardware.hxx"
#endif
 
#include "executor/StateFlow.hxx"
#include "openlcb/MemoryConfig.hxx"
#include "openlcb/TrainInterface.hxx"
#include "utils/format_utils.hxx"
 
namespace traction_modem
{
 
struct TxMessage
{
    bool valid() const
    {
        return Defs::is_valid(payload);
    }
 
    uint16_t command() const
    {
        return Defs::get_uint16(payload, Defs::OFS_CMD);
    }
 
    uint16_t length() const
    {
        return Defs::get_uint16(payload, Defs::OFS_LEN);
    }
 
    uint16_t response_status()
    {
        return Defs::get_uint16(payload, Defs::OFS_DATA);
    }
 
    string payload;
};
 
using PacketFlowInterface = FlowInterface<Buffer<TxMessage>>;
using TxFlowBase = StateFlow<Buffer<TxMessage>, QList<2>>;
 
class TxFlow : public TxFlowBase
{
public:
    TxFlow(Service *service)
        : TxFlowBase(service)
    {
        LOG(INFO, "[ModemTx] constructor");
    }
 
    void start(int fd)
    {
        LOG(INFO, "[ModemTx] fd");
        fd_ = fd;
    }
 
    Action entry() override
    {
        if (fd_ < 0)
        {
            LOG(INFO, "[ModemTx] no uart");
            return release_and_exit();
        }
        LOG(INFO, "[ModemTx] msg len %d",
            (int)message()->data()->payload.size());
        return write_repeated(&helper_, fd_, message()->data()->payload.data(),
            message()->data()->payload.size(), STATE(write_complete));
    }
 
    Action write_complete()
    {
        unsigned len = message()->data()->payload.size();
        unsigned num_sent = len - helper_.remaining_;
        const uint8_t *d = (const uint8_t *)message()->data()->payload.data();
        LOG(INFO, "[ModemTx] sent E%d len %u done %u %08x %04x...",
            helper_.hasError_, len, num_sent, *(unsigned *)(d + 0),
            (unsigned)be32toh(*(uint16_t *)(d + 4)));
        return release_and_exit();
    }
 
private:
    StateFlowSelectHelper helper_ {this};
    int fd_ = -1;
};
 
class RxFlow : public StateFlowBase
{
public:
    using BufferType = Buffer<TxMessage>;
    using Receiver = PacketFlowInterface;
 
    RxFlow(Service *service)
        : StateFlowBase(service)
        , receiver_(nullptr)
    { }
 
    void start(int fd)
    {
        fd_ = fd;
        start_flow(STATE(reset));
    }
 
    void set_listener(Receiver *rcv)
    {
        receiver_ = rcv;
    }
 
    long long get_character_nsec()
    {
        return CHARACTER_NSEC;
    }
 
#if defined(GTEST)
    bool exit_ = false;
#endif
 
private:
    Action reset()
    {
        // Looking for the start of a new message. The clear is needed to
        // "reset" the string from the std::move() operation that might have 
        // occurred on the prior message, leaving it in an unknown state.
        payload_.clear();
        // Note: We are relying on the fact that the default allocator is used,
        //       which is the heap allocator, and that the heap allocator will
        //       allocate memory on (at least) the native machine size boundary.
        //       This is important in order to be able to cast payload_.data()
        //       to a Defs::Message type for easier decoding. Otherwise if the
        //       architecture supports unaligned accesses, then we are fine for
        //       all allocators.
        payload_.resize(MIN_MESSAGE_SIZE);
        recvCnt_ = 0;
        return call_immediately(STATE(wait_for_base_data));
    }
 
    Action wait_for_base_data()
    {
#if defined(GTEST)
        if (exit_)
        {
            return exit();
        }
#endif
        // Every message is at least a minimum size. There is really no point
        // to waste any cycles processing until at least the minimum count is
        // received.
        if (recvCnt_ >= MIN_MESSAGE_SIZE)
        {
            // We already have enough data to start processing.
            return call_immediately(STATE(base_data_received));
        }
 
        HASSERT(payload_.size() >= MIN_MESSAGE_SIZE);
 
        // This is pre-emptive. We will have received this count by the time we
        // enter the next state because the read blocks until we have all the
        // base data.
        size_t offset = recvCnt_;
        recvCnt_ = MIN_MESSAGE_SIZE;
 
        return read_repeated(&helper_, fd_, &payload_[offset],
            MIN_MESSAGE_SIZE - offset, STATE(base_data_received));
    }
 
    Action base_data_received()
    {
        const Defs::Message *m = (const Defs::Message*)payload_.data();
 
        // Look for the preamble.
        if (m->header_.preamble_ == htobe32(Defs::PREAMBLE))
        {
            // Valid preamble found where it is supposed to be. Assume for now
            // that we also have a valid header that follows.
            LOG(INFO, "[ModemRx] recv cmd: 0x%04x, len: %u",
                be16toh(m->header_.command_), be16toh(m->header_.length_));
            return call_immediately(STATE(header_complete));
        }
 
        // Else valid preamble and/or header not found where it is supposed to
        // be.
        LOG(INFO,
            "[ModemRx] Did not find an expected valid preamble and/or header.");
        return call_immediately(STATE(resync));
    }
 
    Action header_complete()
    {
        const Defs::Message *m = (const Defs::Message*)payload_.data();
        size_t len = be16toh(m->header_.length_);
 
        if (len > MAX_DATA_LEN)
        {
            // Violated the maximum length data allowed by the protocol. We are
            // probably out of sync.
            LOG(INFO, "[ModemRx] Maximum data length violation.");
            return call_immediately(STATE(resync));
        }
 
        size_t total_len = MIN_MESSAGE_SIZE + len;
        if (recvCnt_ >= total_len)
        {
            // We already have enough data for the complete message.
            return call_immediately(STATE(maybe_message_complete));
        }
 
        payload_.resize(total_len);
        size_t needed_len = total_len - recvCnt_;
 
        return read_repeated_with_timeout(&helper_,
            2 * get_character_nsec() * needed_len, fd_, &payload_[recvCnt_],
            needed_len, STATE(maybe_message_complete));
    }
 
    Action maybe_message_complete()
    {
        const Defs::Message *m = (const Defs::Message*)payload_.data();
        size_t len = be16toh(m->header_.length_);
 
        if (recvCnt_ < (MIN_MESSAGE_SIZE + len) && helper_.remaining_)
        {
            // Timeout, we may be out ot sync. Check for a preamble in the data
            // we did receive.
            recvCnt_ += len - helper_.remaining_;
            LOG(INFO, "[ModemRx] Timeout waiting for expected receive data, "
                "remaining: %u", helper_.remaining_);
            return call_immediately(STATE(resync));
        }
 
        Defs::CRC crc_calc;
        Defs::CRC crc_recv = Defs::get_crc(payload_, len);
        crc3_crc16_ccitt(
            payload_.data() + sizeof(uint32_t),
            sizeof(m->header_.command_) + sizeof(m->header_.length_) + len,
            &crc_calc.crc[0]);
        if (crc_calc != crc_recv)
        {
            LOG(INFO, "[ModemRx] CRC Error, received: 0x%04X 0x%04X 0x%04X, "
                "calculated: 0x%04X 0x%04X 0x%04X",
                crc_recv.all_, crc_recv.even_, crc_recv.odd_,
                crc_calc.all_, crc_calc.even_, crc_calc.odd_);
            return call_immediately(STATE(resync));
        }
 
        size_t total_len = MIN_MESSAGE_SIZE + len;
        std::string payload_tmp;
        if (payload_.size() > total_len)
        {
            // We have the start of the next message, which we need to save off.
            payload_tmp =
                payload_.substr(total_len, payload_.size() - total_len);
            payload_.resize(total_len);
        }
        // A this point, we have a valid message.
        LOG(INFO, "[ModemRx] %s", string_to_hex(payload_).c_str());
 
        if (receiver_)
        {
            auto *b = receiver_->alloc();
            b->data()->payload = std::move(payload_);
            receiver_->send(b);
        }
 
        if (payload_tmp.size())
        {
            // Move the start of the next message into payload.
            LOG(INFO, "[ModemRx] Remaining payload size: %zu",
                payload_tmp.size());
            payload_ = std::move(payload_tmp);
            recvCnt_ = payload_.size();
            return call_immediately(STATE(wait_for_base_data));
        }
        return call_immediately(STATE(reset));
    }
 
    Action resync()
    {
        // Message parsing out of sync.
 
 
        for (unsigned idx = 1; idx < recvCnt_; ++idx)
        {
            if (payload_[idx] == Defs::PREAMBLE_FIRST)
            {
                if ((idx + 4) <= recvCnt_)
                {
                    uint32_t p;
                    memcpy(&p, payload_.data() + idx, 4);
                    if (p != htobe32(Defs::PREAMBLE))
                    {
                        continue;
                    }
                }
                // A memmove is more efficient than payload_.erase(0, idx)
                // because it will not shrink the size of the payload_, and it
                // maintains proper alignment by reusing the original heap
                // allocation.
                memmove(&payload_[0], &payload_[idx], recvCnt_ - idx);
                recvCnt_ -= idx;
                LOG(INFO, "[ModemRx] Sync on preamble first, recvCnt_: %zu",
                    recvCnt_);
                return call_immediately(STATE(wait_for_base_data));
            }
        }
        // Now: we're out of sync and never found a viable first byte. Drop
        // all data.
        return call_immediately(STATE(reset));
    }
 
    static constexpr long long CHARACTER_NSEC = 10 * SEC_TO_NSEC(1) / 250000;
 
    static constexpr unsigned MIN_MESSAGE_SIZE = Defs::LEN_BASE;
 
    static constexpr unsigned MAX_DATA_LEN = Defs::MAX_LEN;
 
    StateFlowTimedSelectHelper helper_ {this};
    string payload_;
    size_t recvCnt_;
    Receiver *receiver_;
 
    int fd_ = -1;
};
 
class CvSpace : public openlcb::MemorySpace, public PacketFlowInterface
{
public:
    CvSpace(PacketFlowInterface *tx)
        : pendingRead_(false)
        , doneRead_(false)
        , pendingWrite_(false)
        , doneWrite_(false)
        , txFlow_(tx)
    { }
 
    address_t max_address() override
    {
        return 1023;
    }
 
    bool read_only() override
    {
        return false;
    }
 
    size_t write(address_t destination, const uint8_t *data, size_t len,
        errorcode_t *error, Notifiable *again) override
    {
        if (doneWrite_)
        {
            doneWrite_ = false;
            *error = errorCode_;
            return actualLen_;
        }
 
        actualLen_ = len;
        pendingWrite_ = true;
        doneWrite_ = false;
        done_ = again;
        *error = ERROR_AGAIN;
 
        auto *b = txFlow_->alloc();
        b->data()->payload =
            Defs::get_memw_payload(proxySpace_, destination, data, len);
        txFlow_->send(b);
        return 0;
    }
 
    size_t read(address_t source, uint8_t *dst, size_t len, errorcode_t *error,
        Notifiable *again) override
    {
        if (doneRead_)
        {
            doneRead_ = false;
            *error = errorCode_;
            return actualLen_;
        }
 
        readBuf_ = dst;
        actualLen_ = len;
        pendingRead_ = true;
        doneRead_ = false;
        done_ = again;
        *error = ERROR_AGAIN;
 
        auto *b = txFlow_->alloc();
        b->data()->payload = Defs::get_memr_payload(proxySpace_, source, len);
        txFlow_->send(b);
        return 0;
    }
 
    void send(Buffer<TxMessage>* buf, unsigned prio) override {
        auto rb = get_buffer_deleter(buf);
        auto& txm = *buf->data();
        if (!txm.valid()) {
            return;
        }
        switch (txm.command())
        {
            case Defs::RESP_MEM_R:
            {
                if (pendingRead_)
                {
                    handle_read_response(txm);
                }
                break;
            }
            case Defs::RESP_MEM_W:
            {
                if (pendingWrite_)
                {
                    handle_write_response(txm);
                }
                break;
            }
        }
    }
 
    void handle_read_response(TxMessage &txm)
    {
        doneRead_ = true;
        pendingRead_ = false;
        errorCode_ = txm.response_status();
        unsigned data_bytes = txm.length() - 2;
        if (data_bytes > actualLen_)
        {
            // We should not have received more bytes back than we requested,
            // but we still clip.
            data_bytes = actualLen_;
        }
        memcpy(readBuf_, txm.payload.data() + Defs::OFS_DATA + 2, data_bytes);
        actualLen_ = data_bytes;
        done_->notify();
    }
 
    void handle_write_response(TxMessage& txm) {
        pendingWrite_ = false;
        doneWrite_ = true;
        errorCode_ = txm.response_status();
        if (errorCode_)
        {
            actualLen_ = Defs::get_uint16(txm.payload, Defs::OFS_DATA + 2);
        }
        done_->notify();
    }
 
    static constexpr uint8_t proxySpace_ = openlcb::MemoryConfigDefs::SPACE_DCC_CV;
 
    bool pendingRead_ : 1;
    bool doneRead_ : 1;
    bool pendingWrite_ : 1;
    bool doneWrite_ : 1;
 
    uint16_t errorCode_ = 0;
 
    uint8_t* readBuf_ = nullptr;
    unsigned actualLen_ = 0;
 
    Notifiable* done_ = nullptr;
 
    PacketFlowInterface* txFlow_;
};
 
class ModemTrain : public openlcb::TrainImpl
{
public:
    ModemTrain(Service *service)
        : txFlow_(service)
        , rxFlow_(service)
        , isActive_(false)
    {
        rxFlow_.set_listener(&cvSpace_);
    }
 
    void start(int uart_fd)
    {
        txFlow_.start(uart_fd);
        rxFlow_.start(uart_fd);
    }
 
    void set_is_active(bool is_active)
    {
        if (isActive_ != is_active)
        {
            isActive_ = is_active;
            send_packet(Defs::get_wireless_present_payload(isActive_));
        }
    }
 
    openlcb::MemorySpace *get_cv_space()
    {
        return &cvSpace_;
    }
 
    // ====== Train interface =======
 
    void set_speed(openlcb::SpeedType speed) override
    {
        set_is_active(true);
        inEStop_ = false;
        lastSpeed_ = speed;
        send_packet(Defs::get_speed_set_payload(speed));
    }
 
    openlcb::SpeedType get_speed() override
    {
        return lastSpeed_;
    }
 
    void set_emergencystop() override
    {
        inEStop_ = true;
        lastSpeed_.set_mph(0); // keeps direction
        send_packet(Defs::get_estop_payload());
    }
 
    bool get_emergencystop() override
    {
        return inEStop_;
    }
 
    void set_fn(uint32_t address, uint16_t value) override
    {
        set_is_active(true);
        send_packet(Defs::get_fn_set_payload(address, value));
        switch (address)
        {
            default:
                break;
#if !defined(GTEST)
            case 0:
                if (lastSpeed_.direction() == openlcb::Velocity::REVERSE)
                {
                    LOGIC_F0R_Pin::set(value ? 1 : 0);
                }
                break;
            case 1:
                LOGIC_F1_Pin::set(value ? 1 : 0);
                break;
            case 2:
                LOGIC_F2_Pin::set(value ? 1 : 0);
                break;
            case 3:
                LOGIC_F3_Pin::set(value ? 1 : 0);
                break;
            case 4:
                LOGIC_F4_Pin::set(value ? 1 : 0);
                break;
            // F5 and F6 also overwritten with input1 and input2 values when
            // button 7 or 8 are pressed
            case 5:
                LOGIC_F5_Pin::set(value ? 1 : 0);
                break;
            case 6:
                LOGIC_F6_Pin::set(value ? 1 : 0);
                break;
#endif // !defined(GTEST)
// Commented out since these pins cannot be defined as both inputs
// and outputs.
#if 0
            case 7:
                LOGIC_F7_Pin::set(value ? 1 : 0);
                break;
            case 8:
                LOGIC_F8_Pin::set(value ? 1 : 0);
                break;
#endif
#if !defined(GTEST)
            case 7:
            {
                bool input1 = INPUT1_Pin::get();
                LOGIC_F5_Pin::set(INPUT2_Pin::get());
                LOG(INFO, "INPUT1: %u", input1);
                break;
            }
            case 8:
            {
                bool input2 = INPUT2_Pin::get();
                LOGIC_F6_Pin::set(INPUT2_Pin::get());
                LOG(INFO, "INPUT2: %u", input2);
                break;
            }
#endif // !defined(GTEST)
        }
    }
 
    uint16_t get_fn(uint32_t address) override
    {
        return 0;
    }
 
    uint32_t legacy_address() override
    {
        return 883;
    }
 
    dcc::TrainAddressType legacy_address_type() override
    {
        return dcc::TrainAddressType::DCC_LONG_ADDRESS;
    }
 
private:
    inline void send_packet(Defs::Payload p)
    {
        auto *b = txFlow_.alloc();
        b->data()->payload = std::move(p);
        txFlow_.send(b);
    }
 
    bool isRunning_ = false;
    int fd_;
 
    openlcb::SpeedType lastSpeed_ = 0.0;
    bool inEStop_ = false;
 
    TxFlow txFlow_;
    RxFlow rxFlow_;
    CvSpace cvSpace_{&txFlow_};
    bool isActive_;
};
 
} // namespace traction_modem
 
#endif // _TRACTIONMODEM_TRACTIONMODEM_HXX_