EdgeGrblDeviceBase.cpp

/*
 * Copyright (C) 2022 CERN
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 *
 * Created on: 2022-03-08
 *     Author: Michal Mysior <mmysior> <michal.mysior@cern.ch>
 *
 */
 
#include "EdgeGrblDeviceBase.hpp"
 
#include <algorithm>
#include <cerrno>
#include <chrono>
#include <cstring>
#include <thread>
 
#include <ccut/Regex.hpp>
#include <ccut/async.hpp>
#include <dlfcn.h>
#include <fcntl.h>
#include <fmc_mfe/libfmc_mfe.h>
#include <logger/Logger.hpp>
#include <poll.h>
#include <pthread.h>
#include <unistd.h>
 
#include "Exception.hpp"
 
using namespace logger;
using namespace std::chrono;
 
static const size_t s_edgeFifoTxSize{512};
static constexpr uint32_t s_edgeFifoTxNullCharMask{(uint32_t) (1 << 18)};
static constexpr uint32_t s_edgeFifoRxEmptyMask{(uint32_t) (1 << 8)};
static const std::chrono::milliseconds s_edgeFifoRxFreeTimeout{10};
 
namespace edge {
 
const std::string EdgeGrblDeviceBase::loggerCat{"edge:edgegrbldevice"};
 
EdgeGrblDeviceBase::EdgeGrblDeviceBase(int lun, const std::string &driverPath)
{
    init(lun, driverPath);
}
 
EdgeGrblDeviceBase::EdgeGrblDeviceBase() {}
 
void EdgeGrblDeviceBase::init(int lun, const std::string &driverPath)
{
    bool simMode{!driverPath.empty()};
 
    drvOpen(lun, driverPath, simMode);
    m_fifoBytes.resize(m_edgeDrvHdl->reg_map->grbl_fifo.tx_dr->reg_attr->depth);
 
    /* flush fifo */
    EdgeGrblDeviceBase::read();
 
    ::pipe(m_wakeFd);
    ::fcntl(m_wakeFd[0], F_SETFL, ::fcntl(m_wakeFd[0], F_GETFL, 0) | O_NONBLOCK);
    ::fcntl(m_wakeFd[1], F_SETFL, ::fcntl(m_wakeFd[1], F_GETFL, 0) | O_NONBLOCK);
}
 
EdgeGrblDeviceBase::~EdgeGrblDeviceBase()
{
    ::close(m_wakeFd[0]);
    ::close(m_wakeFd[1]);
    drvClose();
}
 
void EdgeGrblDeviceBase::read()
{
    uint32_t txCnt;
    struct edge_reg *txCntReg = m_edgeDrvHdl->reg_map->grbl_fifo.tx_counter;
    if (edge_get(m_edgeDrvHdl->edge_hdl, txCntReg, &txCnt, 1))
    {
        error(loggerCat) << "Failed getting tx_counter register";
        throw smc::Exception{smc::ErrorCode::InternalError,
                             std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)}};
    }
    if (txCnt > 0)
    {
        if (!m_buffer.remaining())
        {
            error(loggerCat) << "flushing buffer";
            m_buffer.reset();
        }
 
        struct edge_reg *txDataReg = m_edgeDrvHdl->reg_map->grbl_fifo.tx_dr;
        size_t bytesToRead = std::min(m_buffer.remaining(),
                                      static_cast<size_t>(txCnt));
        m_fifoBytes.reserve(bytesToRead);
 
        if (edge_get(m_edgeDrvHdl->edge_hdl, txDataReg, m_fifoBytes.data(),
                     static_cast<uint32_t>(bytesToRead)))
        {
            error(loggerCat) << "Failed getting " << bytesToRead
                             << " bytes from tx_dr register";
            throw smc::Exception{
                smc::ErrorCode::InternalError,
                std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)}};
        }
 
        // Add non-null chars to buffer
        for (size_t i = 0; i < bytesToRead; ++i)
        {
            const char byte = *(m_fifoBytes.data() + i);
            if (byte != '\0')
                m_buffer.add(byte);
        }
    }
}
 
void EdgeGrblDeviceBase::write(const std::string &data)
{
    uint32_t rxByteCount;
    if (edge_get(m_edgeDrvHdl->edge_hdl,
                 m_edgeDrvHdl->reg_map->grbl_fifo.rx_counter, &rxByteCount, 1))
    {
        error(loggerCat) << "Failed getting data from rx_count register";
        throw smc::Exception{smc::ErrorCode::InternalError,
                             std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)}};
    }
    struct edge_reg *rxDataReg = m_edgeDrvHdl->reg_map->grbl_fifo.rx_dr;
    size_t rxFreeBytes = rxDataReg->reg_attr->depth - rxByteCount;
 
    size_t bytesWritten = 0;
    while (bytesWritten < data.size())
    {
        size_t bytesToWrite = std::min((data.size() - bytesWritten),
                                       rxFreeBytes);
        if (edge_set(m_edgeDrvHdl->edge_hdl, rxDataReg,
                     const_cast<void *>(reinterpret_cast<const void *>(
                         data.c_str() + bytesWritten)),
                     bytesToWrite))
        {
            error(loggerCat) << "Failed writing " << bytesToWrite
                             << " bytes to rx_dr register";
            throw smc::Exception{
                smc::ErrorCode::InternalError,
                std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)}};
        }
 
        if ((bytesWritten = bytesToWrite) < data.size())
        {
            if (!waitIrq(s_edgeFifoRxFreeTimeout,
                         m_edgeDrvHdl->reg_map->grbl_fifo.intc_sr,
                         s_edgeFifoRxEmptyMask))
            {
                error(loggerCat)
                    << "Failed waiting for Rx FIFO empty interrupt";
                throw smc::Exception{smc::ErrorCode::InternalError,
                                     "Rx FIFO interrupt timeout"};
            }
            rxFreeBytes = rxDataReg->reg_attr->depth;
        }
    }
}
 
bool EdgeGrblDeviceBase::wait(const std::chrono::milliseconds &timeout)
{
    return waitIrq(timeout, m_edgeDrvHdl->reg_map->grbl_fifo.intc_sr,
                   s_edgeFifoTxNullCharMask);
}
 
void EdgeGrblDeviceBase::wake()
{
    ::write(m_wakeFd[1], "X", 1);
}
 
bool EdgeGrblDeviceBase::waitIrq(const std::chrono::milliseconds &timeout,
                                 const struct edge_reg *const expectedReg,
                                 uint32_t expectedBit)
{
    struct pollfd pfds[2] = {{m_wakeFd[0], POLLIN, 0},
                             {m_edgeDrvHdl->edge_hdl->fd, POLLIN | POLLNVAL, 0}};
    const steady_clock::time_point deadline = steady_clock::now() + timeout;
    bool irqMatched{false};
    bool waked{false};
    int remainingIrqs = 0;
    edge_irq irqRes;
 
    while (!irqMatched && !waked)
    {
        milliseconds remaining = duration_cast<milliseconds>(
            deadline - steady_clock::now());
        if (remaining.count() < 0)
            break;
 
        int res = ::poll(pfds, 2, remaining.count() + 1);
        if (res > 0)
        {
            // Check for wake event
            if (pfds[0].revents & POLLIN)
            {
                std::vector<char> buffer(10);
                ::read(m_wakeFd[0], buffer.data(), buffer.size());
                waked = true;
            }
 
            // Check for IRQ event
            if (pfds[1].revents & POLLIN)
            {
                // Read IRQ from device's FD using edge function
                if (edge_wait_irq(m_edgeDrvHdl->edge_hdl, &irqRes) < 0)
                {
                    error(loggerCat)
                        << "edge_wait_irq failed: " << ::strerror(errno);
                    throw make_errno_exception(smc::ErrorCode::IO);
                }
                // Update how many IRQs remain in queue
                remainingIrqs = irqRes.queued;
                // Check if IRQ is equal to expected
                if ((irqRes.irq_src == expectedReg) &&
                    (irqRes.irq_value & expectedBit))
                    irqMatched = true;
            }
            // Check if poll error on edge fd
            if (pfds[1].revents & POLLNVAL)
            {
                error(loggerCat) << "IRQ poll() failed - wrong poll request";
                throw smc::Exception(smc::ErrorCode::InternalError,
                                     "IRQ poll() failed - wrong poll request");
            }
        }
        else if (res == 0)
        {
            /* timeout */
            waked = true;
        }
        else
        {
            // Error
            error(loggerCat) << "poll() failed waiting for IRQ";
            throw smc::Exception(smc::ErrorCode::InternalError,
                                 "IRQ poll() failed");
        }
    }
 
    // Flush remaining IRQs in FD
    while (remainingIrqs--)
    {
        if (edge_wait_irq(m_edgeDrvHdl->edge_hdl, &irqRes) < 0)
        {
            error(loggerCat)
                << "edge_wait_irq (flush) failed: " << ::strerror(errno);
            break;
        }
    }
    return irqMatched;
}
 
void EdgeGrblDeviceBase::read(struct edge_reg *reg,
                              size_t offset,
                              uint32_t &value)
{
    std::string err;
    if (!m_edgeDrvHdl)
        err = "driver not initialized";
    else if (reg && reg->reg_attr->dwidth != 32)
        err = "invalid register size";
    else if (edge_get_range(m_edgeDrvHdl->edge_hdl, reg, offset, &value, 1) != 0)
        err = edge_strerror(m_edgeDrvHdl->edge_hdl);
 
    if (!err.empty())
    {
        err = "Failed to read register: " + err;
        error(loggerCat) << err;
        throw smc::Exception{smc::ErrorCode::IO, err};
    }
}
 
void EdgeGrblDeviceBase::read(struct edge_reg *reg,
                              std::vector<uint32_t> &values)
{
    std::string err;
    if (!m_edgeDrvHdl)
        err = "driver not initialized";
    else if (reg && reg->reg_attr->dwidth != 32)
        err = "invalid register size";
    else if (edge_get_range(m_edgeDrvHdl->edge_hdl, reg, 0, values.data(),
                            values.size()) != 0)
        err = edge_strerror(m_edgeDrvHdl->edge_hdl);
 
    if (!err.empty())
    {
        err = "Failed to read register: " + err;
        error(loggerCat) << err;
        throw smc::Exception{smc::ErrorCode::IO, err};
    }
}
 
void EdgeGrblDeviceBase::write(struct edge_reg *reg,
                               size_t offset,
                               uint32_t value)
{
    std::string err;
    if (!m_edgeDrvHdl)
        err = "driver not initialized";
    else if (reg && reg->reg_attr->dwidth != 32)
        err = "invalid register size";
    else if (edge_set_range(m_edgeDrvHdl->edge_hdl, reg, offset, &value, 1) != 0)
        err = edge_strerror(m_edgeDrvHdl->edge_hdl);
 
    if (!err.empty())
    {
        err = "Failed to write register: " + err;
        error(loggerCat) << err;
        throw smc::Exception{smc::ErrorCode::IO, err};
    }
}
 
void EdgeGrblDeviceBase::drvOpen(int lun, const std::string &simPath, bool isSim)
{
    if (isSim)
    {
        {
            // CentOS7 workarround, see
            // https://gitlab.cern.ch/be-cem-edl/fec/utilities/edge/-/issues/68
            void *lib = dlopen((simPath + "/libfmc_mfe_sim.so").c_str(),
                               RTLD_LAZY | RTLD_NODELETE);
            if (lib)
                dlclose(lib);
        }
        m_edgeDrvHdl = fmc_mfe_sim_open(lun, simPath.c_str());
    }
    else
    {
        m_edgeDrvHdl = fmc_mfe_drv_open(lun);
    }
 
    if (m_edgeDrvHdl == NULL)
    {
        error(loggerCat) << "Opening fmc_mfe driver failed: "
                         << std::string{edge_strerror(NULL)};
        throw smc::Exception(smc::ErrorCode::InternalError,
                             std::string{edge_strerror(NULL)});
    }
 
    struct edge_vers version;
    if (edge_get_version(m_edgeDrvHdl->edge_hdl, &version) == 0)
        info(loggerCat) << "fmc_mfe: " << version.hw_lif_vers;
    else /* unlikely */
        error(loggerCat) << "Failed to retrieve driver version";
 
    // Enable irq queue
    struct edge_irq_cfg cfg;
    cfg.irq_src_reg = m_edgeDrvHdl->reg_map->grbl_fifo.intc_sr;
    cfg.irq_mask = s_edgeFifoTxNullCharMask | s_edgeFifoRxEmptyMask;
    cfg.queue_flg = 1;
    /* very large fallback timeout, should not be used since ::poll is used on
     * the file-descriptor */
    cfg.timeout = 10000;
 
    if (edge_set_irq_cfg(m_edgeDrvHdl->edge_hdl, &cfg,
                         EDGE_IRQ_SRC | EDGE_IRQ_MASK | EDGE_IRQ_QUEUE |
                             EDGE_IRQ_TMO) < 0)
    {
        error(loggerCat) << "Enabling IRQ queue in edge failed: "
                         << std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)};
        throw smc::Exception(smc::ErrorCode::InternalError,
                             std::string{edge_strerror(m_edgeDrvHdl->edge_hdl)});
    }
 
    if (isSim)
        notice(loggerCat)
            << "fmc_mfe driver opened in sim mode on path " << simPath;
    else
        notice(loggerCat) << "fmc_mfe driver opened";
}
 
void EdgeGrblDeviceBase::drvClose()
{
    if (m_edgeDrvHdl != NULL)
    {
        fmc_mfe_close(m_edgeDrvHdl);
        notice(loggerCat) << "fmc_mfe driver closed";
    }
}
 
} /* namespace edge */