RTUtility.cpp

Go to the documentation of this file.

#include "RTUtility.h"
 
#include <errno.h>
 
#include <thread>
 
#include <fcntl.h>
#include <sched.h>
#include <sys/stat.h>
#include <sys/syscall.h>
 
#include <ArmarXCore/core/logging/Logging.h>
 
namespace armarx::control::ethercat
{
    bool
    RTUtility::elevateThreadPriority(int priority)
    {
        long pid = syscall(SYS_gettid);
        ARMARX_INFO << "Elevating priority of thread #" << pid << " to " << priority;
        ARMARX_INFO << "Priority before: " << sched_getscheduler(static_cast<int>(pid));
        struct sched_param param;
        param.sched_priority = priority;
        if (sched_setscheduler(static_cast<int>(pid), SCHED_FIFO | SCHED_RESET_ON_FORK, &param) ==
            -1)
        {
            int error = errno;
            ARMARX_WARNING << "sched_setscheduler failed: " << std::string(strerror(error));
            return false;
        }
        if (sched_getparam(static_cast<int>(pid), &param) == -1)
        {
            int error = errno;
            ARMARX_WARNING << "sched_getparam failed: " << std::string(strerror(error));
            return false;
        }
        int new_priority = param.sched_priority;
        if (new_priority == priority)
        {
            ARMARX_IMPORTANT << "Successfully elevated priority of thread #" << pid
                             << " to new priority " << new_priority;
        }
        else
        {
            ARMARX_ERROR << "Failed to elevate priority of thread #" << pid;
        }
 
        return true;
    }
 
    bool
    RTUtility::pinThreadToCPU(unsigned int cpu)
    {
        long pid = syscall(SYS_gettid);
        unsigned int availableCores = std::thread::hardware_concurrency();
        if (availableCores < cpu)
        {
            ARMARX_ERROR << "Trying to pin thread #" << pid << " to CPU #" << cpu << ", but only "
                         << availableCores << " cores are available";
            return false;
        }
 
        ARMARX_INFO << "Pinning thread #" << pid << " to CPU #" << cpu;
        cpu_set_t mask;
        CPU_ZERO(&mask);
        CPU_SET(cpu, &mask);
        int retval = sched_setaffinity(static_cast<int>(pid), sizeof(mask), &mask);
        if (retval != 0)
        {
            ARMARX_ERROR << "Failed to pin thread #" << pid << " to CPU #" << cpu;
            return false;
        }
        cpu_set_t mask2;
        CPU_ZERO(&mask2);
        CPU_SET(cpu, &mask2);
        sched_getaffinity(static_cast<int>(pid), sizeof(mask2), &mask2);
        bool matches = CPU_EQUAL(&mask, &mask2);
        if (matches)
        {
            ARMARX_IMPORTANT << "Successfully pinned thread #" << pid << " to CPU #" << cpu;
            return true;
        }
        else
        {
            ARMARX_ERROR << "Failed to pin thread #" << pid << " to CPU #" << cpu;
            return false;
        }
    }
 
    static int fd_low_latency_target = -1;
    static constexpr std::int32_t LOW_LATENCY_MAX_RESPONSE_TIME = 0;
 
    bool
    RTUtility::startLowLatencyMode()
    {
        ARMARX_INFO << "Starting low latency mode by writing " << LOW_LATENCY_MAX_RESPONSE_TIME
                    << " to /dev/cpu_dma_latency and keeping the file open";
 
        struct stat s;
        int err;
        err = stat("/dev/cpu_dma_latency", &s);
        if (err == -1)
        {
            ARMARX_WARNING << "stat /dev/cpu_dma_latency failed: " << strerror(errno);
            return false;
        }
        fd_low_latency_target = open("/dev/cpu_dma_latency", O_RDWR);
        if (fd_low_latency_target == -1)
        {
            ARMARX_WARNING << "open /dev/cpu_dma_latency failed: " << strerror(errno);
            return false;
        }
 
        err = static_cast<int>(write(fd_low_latency_target,
                                     &LOW_LATENCY_MAX_RESPONSE_TIME,
                                     sizeof(LOW_LATENCY_MAX_RESPONSE_TIME)));
        if (err < 1)
        {
            ARMARX_WARNING << "error writing " << LOW_LATENCY_MAX_RESPONSE_TIME
                           << " to /dev/cpu_dma_latency";
            close(fd_low_latency_target);
            return false;
        }
        ARMARX_IMPORTANT << "Successfully set /dev/cpu_dma_latency to "
                         << LOW_LATENCY_MAX_RESPONSE_TIME << " µs\n";
        return true;
    }
 
    bool
    RTUtility::stopLowLatencyMode()
    {
        ARMARX_INFO << "Stopping low latency mode by closing file /dev/cpu_dma_latency";
        if (fd_low_latency_target >= 0)
        {
            close(fd_low_latency_target);
            return true;
        }
        else
        {
            ARMARX_WARNING << "Low latency mode not active!";
            return false;
        }
    }
 
 
} // namespace armarx::control::ethercat