grbl-sender/terje/telnet.cpp

#include "telnet.h"
#include "../string_utils.h"
#include "grbl.h"
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <iostream>
#include <fcntl.h>
#include <chrono>

bool iosender::TelnetStream::IsOpen() {
    return fd > 0 && isConnected;
}

int iosender::TelnetStream::OutCount() {
    return 0;
}

std::string iosender::TelnetStream::Reply() {
    return reply;
}

iosender::Comms::StreamType iosender::TelnetStream::StreamType() {
    return Comms::StreamType::Telnet;
}

bool iosender::TelnetStream::EventMode() {
    return eventMode;
}

void iosender::TelnetStream::EventMode(bool mode) {
    eventMode = mode;
}

void iosender::TelnetStream::Close() {
    shouldQuit = true;
    workerThread.join();
}

int iosender::TelnetStream::ReadByte() {
    return 0;
}

void iosender::TelnetStream::WriteByte(uint8_t data) {
    auto result = write(fd, &data, 1);
    if (result == -1) {
        std::cerr << "Failed while writing." << std::endl;
    }
}

void iosender::TelnetStream::WriteBytes(uint8_t *bytes, int len) {
    ssize_t written_bytes = 0;
    for (; written_bytes < len;) {
        auto result = write(fd, &bytes, len);
        if (result == -1) {
            std::cerr << "Failed while writing." << std::endl;
            return;
        }
        written_bytes += result;
    }
}

void iosender::TelnetStream::WriteString(std::string data) {
    WriteBytes(reinterpret_cast<uint8_t *>(data.data()), data.size());
}

void iosender::TelnetStream::WriteCommand(std::string command) {
    CommandState = Comms::State::AwaitAck;

    if (command.size() == 1 && command != GrblConstants::CMD_PROGRAM_DEMARCATION) {
        WriteByte(*command.data());
    } else {
        command += "\r";
        WriteString(command);
    }
}

std::string iosender::TelnetStream::GetReply(std::string command) {
    reply = "";
    AwaitResponse(command);
    return reply;
}

void iosender::TelnetStream::AwaitAck() {
    while (Comms::GetCom()->CommandState == Comms::State::DataReceived ||
           Comms::GetCom()->CommandState == Comms::State::AwaitAck) {
        std::this_thread::sleep_for(std::chrono::microseconds(500));
    }

}

void iosender::TelnetStream::AwaitAck(std::string command) {
    WriteCommand(command);
    AwaitAck();
}

void iosender::TelnetStream::AwaitResponse() {
    while (Comms::GetCom()->CommandState == Comms::State::AwaitAck) {
        std::this_thread::sleep_for(std::chrono::microseconds(500));
    }
}

void iosender::TelnetStream::AwaitResponse(std::string command) {
    WriteCommand(command);
    AwaitResponse();
}

void iosender::TelnetStream::PurgeQueue() {
}

iosender::TelnetStream::TelnetStream(std::string host, iosender::DataReceivedHandler *dataHandler,
                                     iosender::ByteReceivedHandler *byteHandler) {
    Comms::SetCom(this);
    reply = "";
    dataReceivedHandler = dataHandler;
    byteReceivedHandler = byteHandler;

    if (!string_contains(host, ":")) {
        host += ":23";
    }

    auto parameter = split_string(host, ":");
    auto ip = parameter[0];
    auto port = std::stoi(parameter[1]);

    fd = socket(AF_INET, SOCK_STREAM, 0);
    if (fd == -1) {
        std::cerr << "Error creating socket" << std::endl;
    }

    struct sockaddr_in serv_addr{};
    serv_addr.sin_family = AF_INET;
    serv_addr.sin_port = htons(port);

    if (inet_pton(AF_INET, ip.c_str(), &serv_addr.sin_addr) <= 0) {
        std::cerr << "Invalid address/ Address not supported" << std::endl;
        return;
    }

    auto status = connect(fd, (struct sockaddr *) &serv_addr, sizeof(serv_addr));
    if (status < 0) {
        std::cerr << "Connection failed";
        return;
    }

    isConnected = true;

    // set non-blocking please, after we connected
    fcntl(fd, F_SETFL, O_NONBLOCK);

    workerThread = std::thread(&iosender::TelnetStream::readWorker, this);
}

iosender::TelnetStream::~TelnetStream() {
    Close();
}

void iosender::TelnetStream::readWorker() {
    std::string received;
    uint8_t buffer[200];

    while (!shouldQuit) {
        if (isConnected) {

            // anything to read?
            auto readBytes = read(fd, buffer, 200);
            if (readBytes == -1 && (errno == EWOULDBLOCK || errno == EAGAIN)) {
                // do nothing
            } else if (readBytes == -1) {
                std::cerr << "Failed while reading." << std::endl;
                Close();
            } else {
                for (int i = 0; i < readBytes; i++) {
                    auto isEol = buffer[i] == '\n';
                    if (isEol) {
                        reply = received;

                        CommandState = reply == "ok" ?
                                       Comms::State::ACK :
                                       (starts_with(reply, "error") ?
                                        Comms::State::NAK :
                                        Comms::State::DataReceived);

                        if (!reply.empty() && dataReceivedHandler != nullptr) {
                            dataReceivedHandler->DataReceived(reply);
                        }

                        received.clear();
                    } else {
                        received.push_back(static_cast<char>(buffer[i]));
                    }
                }
            }

            // give some time to others
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
    }
}