GrblSim.cpp

/*
** Copyright (C) 2021 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-02-11T17:16:38+01:00
**     Author: Michal Mysior <mmysior> <michal.mysior@cern.ch>
**
*/
 
#include "GrblSim.hpp"
 
#include <algorithm>
#include <cerrno>
#include <cstring>
#include <thread>
 
#include <ccut/Regex.hpp>
#include <fcntl.h>
#include <logger/Logger.hpp>
#include <poll.h>
#include <regex.h>
#include <signal.h>
#include <spawn.h>
#include <stdlib.h>
#include <sys/wait.h>
#include <unistd.h>
 
#include "Exception.hpp"
#include "local-config.h"
 
using namespace logger;
using namespace std::chrono;
using namespace grbl;
 
// Grbl-sim defines for executable location
#define GRBL_GPIOD_EXE_DEV (BUILDDIR "/instroot/bin/grbl-gpiod")
#define GRBL_GPIOD_EXENAME "grbl-gpiod"
 
static const std::string s_loggerCat{"smc:grbldevice:sim"};
const milliseconds GrblSim::defaultExpectTimeout(500);
static const ccut::Regex s_grblInit("^GrblHAL.*$");
 
static const char *getExe()
{
    if (::access(GRBL_GPIOD_EXE, X_OK) == 0)
        return GRBL_GPIOD_EXE;
    else if (::access(GRBL_GPIOD_EXE_DEV, X_OK) == 0)
        return GRBL_GPIOD_EXE_DEV;
    else
        return "grbl-gpiod";
}
 
GrblSim::GrblSim()
{
    // Launch Grbl-sim, set up pipes
    int pa2s[2];
    int ps2a[2];
 
    ::pipe(pa2s);
    ::pipe(ps2a);
 
    posix_spawn_file_actions_t fileActions;
    posix_spawn_file_actions_init(&fileActions);
    posix_spawn_file_actions_addclose(&fileActions, pa2s[1]);
    posix_spawn_file_actions_adddup2(&fileActions, pa2s[0], STDIN_FILENO);
    posix_spawn_file_actions_addclose(&fileActions, pa2s[0]);
    posix_spawn_file_actions_addclose(&fileActions, ps2a[0]);
    posix_spawn_file_actions_adddup2(&fileActions, ps2a[1], STDOUT_FILENO);
    posix_spawn_file_actions_addclose(&fileActions, ps2a[1]);
 
    char *args[] = {const_cast<char *>(getExe()), (char *) NULL};
    debug(s_loggerCat) << "starting " << args[0];
    int err = posix_spawnp(&m_simPid, args[0], &fileActions, NULL, args, NULL);
    if (err != 0)
    {
        errno = err;
        crit(s_loggerCat) << "failed to spawn " << args[0];
    }
 
    posix_spawn_file_actions_destroy(&fileActions);
 
    ::close(pa2s[0]);
    m_out = pa2s[1];
    ::close(ps2a[1]);
    m_in = ps2a[0];
 
    ::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);
    ::fcntl(m_out, F_SETFL, ::fcntl(m_out, F_GETFL, 0) | O_NONBLOCK);
    ::fcntl(m_in, F_SETFL, ::fcntl(m_in, F_GETFL, 0) | O_NONBLOCK);
 
    if (send(
            std::string(),
            [](const std::string &msg) {
                notice(s_loggerCat) << "<--(init) " << msg;
                if (s_grblInit.search(msg))
                    throw grbl::ErrorCode{0};
                return true;
            },
            std::chrono::milliseconds{3000}) != grbl::ErrorCode{0})
    {
        error(s_loggerCat) << "Grbl welcome banner not found";
        throw smc::Exception{smc::ErrorCode::InternalError,
                             std::string{"Failed to get Grbl banner"}};
    }
}
 
GrblSim::~GrblSim()
{
    // Kill Grbl-sim
    char stopChar = 0x06;
    ::write(m_out, &stopChar, 1);
    ::close(m_out);
    ::close(m_in);
    ::close(m_wakeFd[0]);
    ::close(m_wakeFd[1]);
 
    int status;
    std::this_thread::sleep_for(std::chrono::milliseconds(100));
    if (::waitpid(m_simPid, &status, WNOHANG) == -1)
    {
        ::kill(m_simPid, SIGKILL);
        ::waitpid(m_simPid, &status, 0);
    }
}
 
void GrblSim::flush()
{
    m_buffer.reset();
}
 
void GrblSim::read()
{
    if (!m_buffer.remaining())
    {
        error(s_loggerCat) << "flushing buffer";
        m_buffer.reset();
    }
 
    ssize_t sz = ::read(m_in, m_buffer.end(), m_buffer.remaining());
    if (sz > 0)
    {
        // debug(s_loggerCat) << "bytes read: " << sz;
        m_buffer.fwd(sz);
    }
}
 
void GrblSim::write(const std::string &raw)
{
    for (size_t pos = 0; pos < raw.size();)
    {
        ssize_t ret = ::write(m_out, raw.c_str() + pos, raw.size() - pos);
        if (ret < 0)
        {
            error(s_loggerCat)
                << "failed to send command : " << strerror(errno);
            return;
        }
        pos += ret;
        if (pos != raw.size())
            std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}
 
bool GrblSim::wait(const std::chrono::milliseconds &timeout)
{
    struct pollfd pfds[2] = {{m_in, POLLIN | POLLERR, 0},
                             {m_wakeFd[0], POLLIN | POLLERR, 0}};
    if (::poll(pfds, 2, timeout.count() + 1) >= 0)
    {
        if (pfds[1].revents & POLLIN)
        {
            std::vector<char> buffer(10);
            ::read(m_wakeFd[0], buffer.data(), buffer.size());
        }
        if (pfds[0].revents & POLLERR || pfds[1].revents & POLLERR)
            throw smc::make_errno_exception(smc::ErrorCode::IO);
 
        return (pfds[0].revents & POLLIN);
    }
    else
        throw smc::make_errno_exception(smc::ErrorCode::IO);
    return false;
}
 
void GrblSim::wake()
{
    ::write(m_wakeFd[1], "X", 1);
}