wirenboard / wb-mqtt-gpio / 93

Committed 31 Jul 2026 10:43AM UTC coverage: 44.281% (+4.5%) from 39.772%

Build # 93

Build Type

push

github

Committed by

web-flow

Commit Message

Use exported vars instead of MAKEFLAGS for coverage options (#82)

Coverage Stats

493 of 1086 branches covered (45.4%)

875 of 1976 relevant lines covered (44.28%)

3.96 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

13.92

/src/gpio_chip_driver.cpp

#include "gpio_chip_driver.h"
#include "exceptions.h"
#include "gpio_chip.h"
#include "gpio_counter.h"
#include "gpio_line.h"
#include "interruption_context.h"
#include "log.h"
#include "utils.h"

#include <wblib/utils.h>

#include <algorithm>
#include <cassert>
#include <fstream>
#include <string.h>
#include <sys/epoll.h>
#include <sys/ioctl.h>
#include <sys/timerfd.h>
#include <unistd.h>

#define LOG(logger) ::logger.Log() << "[gpio chip driver] "

using namespace std;

const auto CONSUMER = "wb-mqtt-gpio";

namespace
{
    uint32_t GetFlagsFromConfig(const TGpioLineConfig& config, bool asIs = false)
    {
        uint32_t flags = 0;

        if (!asIs) {
            if (config.Direction == EGpioDirection::Input)
                flags |= GPIOHANDLE_REQUEST_INPUT;
            else if (config.Direction == EGpioDirection::Output)
                flags |= GPIOHANDLE_REQUEST_OUTPUT;
        }
        if (config.IsOpenDrain)
            flags |= GPIOHANDLE_REQUEST_OPEN_DRAIN;
        if (config.IsOpenSource)
            flags |= GPIOHANDLE_REQUEST_OPEN_SOURCE;
        if (config.IsActiveLow)
            flags |= GPIOHANDLE_REQUEST_ACTIVE_LOW;

        return flags;
    }
} // namespace

TGpioChipDriver::TGpioChipDriver(const TGpioChipConfig& config): AddedToEpoll(false)
{
    Chip = make_shared<TGpioChip>(config.Path);

    unordered_map<uint32_t, vector<vector<PGpioLine>>> pollLines;
    auto addToPoll = [&pollLines](const PGpioLine& line) {
        auto& lineBulks = pollLines[GetFlagsFromConfig(*line->GetConfig())];

        if (lineBulks.empty() || lineBulks.back().size() == GPIOHANDLES_MAX) {
            lineBulks.emplace_back();
        }

        lineBulks.back().push_back(line);
    };

    if (!Chip->IsValid()) {
        for (const auto& lineConfig: config.Lines) {
            auto line = make_shared<TGpioLine>(Chip, lineConfig);
            LOG(Error) << "Add " << line->DescribeShort() << " as initially disconnected";
            InitiallyDisconnectedLines[line->GetOffset()] = line;
        }
        return;
    }

    for (const auto& line: Chip->LoadLines(config.Lines)) {
        if (!ReleaseLineIfUsed(line)) {
            LOG(Error) << "Skipping " << line->DescribeShort();
        }
        switch (line->GetConfig()->Direction) {
            case EGpioDirection::Input: {
                if (!InitInputInterrupts(line)) {
                    addToPoll(line);
                }
                break;
            }
            case EGpioDirection::Output: {
                if (!InitOutput(line, line->GetConfig()->InitialState)) {
                    LOG(Error) << "Failed to init output " << line->DescribeShort()
                               << ". Treating as initially disconnected";
                    InitiallyDisconnectedLines[line->GetOffset()] = line;
                }
                break;
            }
        }
    }

    /* Initialize polling if line doesn't support interrupts */
    for (const auto& flagsLines: pollLines) {
        const auto flags = flagsLines.first;
        const auto& lineBulks = flagsLines.second;

        for (const auto& lines: lineBulks) {
            if (!InitLinesPolling(flags, lines)) {
                for (const auto& line: lines) {
                    LOG(Error) << "Failed to init polling " << line->DescribeShort()
                               << ". Treating as initially disconnected";
                    InitiallyDisconnectedLines[line->GetOffset()] = line;
                }
            }
        }
    }

    if (Lines.empty() && InitiallyDisconnectedLines.empty()) {
        wb_throw(TGpioDriverException, "Failed to initialize any line");
    }

    AutoDetectInterruptEdges();
    ReadInputValues();
}

TGpioChipDriver::TGpioChipDriver(): AddedToEpoll(false)
{}

TGpioChipDriver::~TGpioChipDriver()
{
    for (const auto& fdLines: Lines) {
        auto fd = fdLines.first;
        const auto& lines = fdLines.second;

        {
            auto logDebug = move(LOG(Debug) << "Close fd for:");
            for (const auto& line: lines) {
                logDebug << "\n\t" << line->DescribeShort();
            }
        }

        close(fd);
    }
}

int TGpioChipDriver::CreateIntervalTimer()
{
    int tfd = timerfd_create(CLOCK_MONOTONIC, 0);
    if (tfd == -1) {
        LOG(Error) << "timerfd_create failed: " << strerror(errno);
        wb_throw(TGpioDriverException, "unable to create timer: timerfd_create failed with " + string(strerror(errno)));
    }
    return tfd;
}

void TGpioChipDriver::SetIntervalTimer(int tfd, std::chrono::microseconds intervalUs)
{
    auto sec = std::chrono::floor<std::chrono::seconds>(intervalUs);
    auto nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(intervalUs - sec);

    struct itimerspec ts;
    ts.it_value.tv_sec = sec.count();
    ts.it_value.tv_nsec = nsec.count();
    ts.it_interval.tv_sec = 0;
    ts.it_interval.tv_nsec = 0;

    if (timerfd_settime(tfd, 0, &ts, NULL) < 0) {
        LOG(Error) << "timerfd_settime failed: " << strerror(errno);
        close(tfd);
        wb_throw(TGpioDriverException, "unable to setup timer: timerfd_settime failed with " + string(strerror(errno)));
    }
}

TGpioChipDriver::TGpioLinesByOffsetMap TGpioChipDriver::MapLinesByOffset() const
{
    TGpioLinesByOffsetMap linesByOffset;

    FOR_EACH_LINE(this, line)
    {
        linesByOffset[line->GetOffset()] = line;
    });

    return linesByOffset;
}

const TGpioChipDriver::TGpioLinesByOffsetMap& TGpioChipDriver::MapInitiallyDisconnectedLinesByOffset() const
{
    return InitiallyDisconnectedLines;
}

void TGpioChipDriver::AddToEpoll(int epfd)
{
    AddedToEpoll = true;

    FOR_EACH_LINE(this, line)
    {
        if (line->IsOutput() || line->GetInterruptSupport() != EInterruptSupport::YES) {
            return;
        }

        struct epoll_event ep_event{};

        ep_event.events = EPOLLIN | EPOLLPRI;
        ep_event.data.fd = line->GetFd();

        if (epoll_ctl(epfd, EPOLL_CTL_ADD, line->GetFd(), &ep_event) < 0) {
            LOG(Error) << "epoll_ctl error: '" << strerror(errno) << "' at " << line->DescribeShort();
        }

        auto timerFd = line->GetTimerFd();
        ep_event.events = EPOLLIN;
        ep_event.data.fd = timerFd;
        if (epoll_ctl(epfd, EPOLL_CTL_ADD, timerFd, &ep_event) < 0) {
            LOG(Error) << "epoll_ctl error: '" << strerror(errno);
            wb_throw(TGpioDriverException,
                     "unable to add timer to epoll: epoll_ctl failed with " + string(strerror(errno)));
        }
    });
}

bool TGpioChipDriver::HandleGpioInterrupt(const PGpioLine& line, const TInterruptionContext& ctx)
{
    bool isHandled = false;
    auto fd = line->GetFd();

    fd_set rfds;
    FD_ZERO(&rfds);
    FD_SET(fd, &rfds);
    struct timeval tv{0}; // do not block

    while (auto retVal = select(fd + 1, &rfds, nullptr, nullptr, &tv)) {
        if (retVal < 0) {
            LOG(Error) << "select failed: " << strerror(errno);
            wb_throw(TGpioDriverException,
                     "unable to read line event data: select failed with " + string(strerror(errno)));
        }

        gpioevent_data data{};
        if (read(fd, &data, sizeof(data)) < 0) {
            LOG(Error) << "Read gpioevent_data failed: " << strerror(errno);
            wb_throw(TGpioDriverException,
                     "unable to read line event data: gpioevent_data failed with " + string(strerror(errno)));
        }

        auto time = ctx.ToSteadyClock(data.timestamp);

        gpiohandle_data values;
        if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &values) < 0) {
            LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);
            line->SetError("r");
            return false;
        }

        line->SetCachedValueUnfiltered(values.values[0]);
        line->HandleInterrupt(time); // record interrupt time, (re)arm debounce window
        SetIntervalTimer(line->GetTimerFd(), line->GetConfig()->DebounceTimeout);
        isHandled = true;
    }
    return isHandled;
}

bool TGpioChipDriver::HandleTimerInterrupt(const PGpioLine& line)
{
    bool isHandled = false;

    if (line->UpdateIfStable(chrono::steady_clock::now())) {
        isHandled = true;
        SetIntervalTimer(line->GetTimerFd(),
                         std::chrono::microseconds(0)); // disarm timer
    }
    return isHandled;
}

bool TGpioChipDriver::HandleInterrupt(const TInterruptionContext& ctx)
{
    bool isHandled = false;

    for (int i = 0; i < ctx.Count; i++) {
        auto fd = ctx.Events[i].data.fd;

        // gpio interrupt event fired: set stable-val-check timer
        auto itFdLines = Lines.find(fd);
        if (itFdLines != Lines.end()) {
            const auto& lines = itFdLines->second;
            assert(lines.size() == 1);
            const auto& line = lines.front();
            HandleGpioInterrupt(line, ctx);

            // timer event fired: check, is value stable or bouncing
        } else {
            auto itFdTimers = Timers.find(fd);
            if (itFdTimers != Timers.end()) {
                const auto& line = itFdTimers->second.front();
                isHandled |= HandleTimerInterrupt(line);
            }
        }
    }
    return isHandled;
}

bool TGpioChipDriver::PollLines()
{
    bool isHandled = false;

    for (const auto& fdLines: Lines) {
        const auto& lines = fdLines.second;
        assert(!lines.empty());

        isHandled = true;

        PollLinesValues(lines);
    }

    return isHandled;
}

void TGpioChipDriver::ForEachLine(const TGpioLineHandler& handler) const
{
    for (const auto& fdLines: Lines) {
        const auto& lines = fdLines.second;
        assert(!lines.empty());

        for (const auto& line: lines) {
            handler(line);
        }
    }
}

bool TGpioChipDriver::ReleaseLineIfUsed(const PGpioLine& line)
{
    if (!line->IsUsed())
        return true;

    LOG(Debug) << line->Describe() << " is used by '" << line->GetConsumer() << "'.";
    if (line->GetConsumer() == "sysfs") {
        ofstream unexportGpio("/sys/class/gpio/unexport");
        if (unexportGpio.is_open()) {
            LOG(Debug) << "Trying to unexport...";
            try {
                unexportGpio << Utils::ToSysfsGpio(line);
            } catch (const TGpioDriverException& e) {
                LOG(Error) << line->Describe() << " is used by '" << line->GetConsumer() << "',"
                           << " during unexport: " << e.what();
            }
        }
    }

    line->UpdateInfo();

    if (line->IsUsed()) {
        LOG(Error) << "Failed to release " << line->DescribeShort();
        return false;
    }

    LOG(Debug) << line->DescribeShort() << " successfully released";
    return true;
}

bool TGpioChipDriver::TryListenLine(const PGpioLine& line)
{
    const auto& config = line->GetConfig();
    assert(config->Direction == EGpioDirection::Input);

    gpioevent_request req{};

    strcpy(req.consumer_label, CONSUMER);
    req.lineoffset = line->GetOffset();
    req.handleflags = GetFlagsFromConfig(*config);

    req.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES;

    errno = 0;
    if (ioctl(Chip->GetFd(), GPIO_GET_LINEEVENT_IOCTL, &req) < 0) {
        auto error = errno;
        LOG(Warn) << "GPIO_GET_LINEEVENT_IOCTL failed: " << strerror(error) << " at " << line->DescribeShort();
        return false;
    }

    Lines[req.fd].push_back(line);
    assert(Lines[req.fd].size() == 1);
    line->SetFd(req.fd);

    auto timerFd = CreateIntervalTimer();
    line->SetTimerFd(timerFd);
    Timers[timerFd].push_back(line);

    LOG(Debug) << "Listening to " << line->DescribeShort();
    return true;
}

bool TGpioChipDriver::FlushMcp23xState(const PGpioLine& line)
{
    /*
        MCP's POR state is input => we need to init gpio-extender module as output on physicall reconnect.

        "pinctrl_mcp23s08" kernel driver has internal cache => once init module as input
        and then init as output to trigger needed i2c communication with mcp.
    */
    if (line->GetConfig()->Direction != EGpioDirection::Output) {
        wb_throw(TGpioDriverException, "Only output lines need flushing-state magic after physical reconnect");
    }

    LOG(Debug) << "Flush state of " << line->DescribeShort() << " to guarantee, it is alive after any disconnects";

    gpioevent_request req{};
    req.lineoffset = line->GetOffset();
    req.handleflags |= GPIOHANDLE_REQUEST_INPUT;
    req.eventflags |= GPIOEVENT_REQUEST_RISING_EDGE;
    strcpy(req.consumer_label, CONSUMER);

    if (ioctl(Chip->GetFd(), GPIO_GET_LINEEVENT_IOCTL, &req) < 0) {
        LOG(Error) << "Temporary init " << line->DescribeShort()
                   << " as input failed. GPIO_GET_LINEEVENT_IOCTL: " << strerror(errno);
        return false;
    }
    line->UpdateInfo();
    close(req.fd);
    return true;
}

bool TGpioChipDriver::InitOutput(const PGpioLine& line, uint8_t val)
{
    const auto& config = line->GetConfig();
    assert(config->Direction == EGpioDirection::Output);

    gpiohandle_request req;
    memset(&req, 0, sizeof(gpiohandle_request));
    req.lines = 1;
    req.lineoffsets[0] = line->GetOffset();
    req.default_values[0] = val;
    req.flags = GetFlagsFromConfig(*config, line->IsOutput());
    strcpy(req.consumer_label, CONSUMER);

    if (ioctl(Chip->GetFd(), GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {
        LOG(Error) << "GPIO_GET_LINEHANDLE_IOCTL failed: " << strerror(errno) << " at " << line->DescribeShort();
        return false;
    }

    Lines[req.fd].push_back(line);
    assert(Lines[req.fd].size() == 1);
    line->SetFd(req.fd);

    if (Debug.IsEnabled()) {
        gpiohandle_data data;
        if (ioctl(line->GetFd(), GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) >= 0) {
            LOG(Debug) << "Initialized output " << line->DescribeShort() << " = " << static_cast<int>(data.values[0]);
        }
    } else {
        LOG(Info) << "Initialized output " << line->DescribeShort();
    }

    return true;
}

bool TGpioChipDriver::InitInputInterrupts(const PGpioLine& line)
{
    switch (line->GetInterruptSupport()) {
        case EInterruptSupport::UNKNOWN:
        case EInterruptSupport::YES: {
            if (TryListenLine(line)) {
                line->SetInterruptSupport(EInterruptSupport::YES);
                return true;
            }
            LOG(Info) << line->Describe() << " does not support interrupts. Polling will be used instead.";
            line->SetInterruptSupport(EInterruptSupport::NO);
            return false;
        }

        case EInterruptSupport::NO: {
            return false;
        }
    }
}

bool TGpioChipDriver::InitLinesPolling(uint32_t flags, const vector<PGpioLine>& lines)
{
    assert(lines.size() <= GPIOHANDLES_MAX);

    gpiohandle_request req;
    req.lines = 0;
    req.flags = flags;
    strcpy(req.consumer_label, CONSUMER);

    for (auto& line: lines) {
        req.lineoffsets[req.lines] = line->GetOffset();
        req.default_values[req.lines] = line->IsActiveLow();
        ++req.lines;
    }

    if (ioctl(Chip->GetFd(), GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {
        LOG(Error) << "GPIO_GET_LINEHANDLE_IOCTL failed: " << strerror(errno);
        return false;
    }

    auto& initialized = Lines[req.fd];

    assert(initialized.empty());
    initialized.reserve(lines.size());

    for (const auto& line: lines) {
        line->SetFd(req.fd);
        initialized.push_back(line);
    }

    return true;
}

void TGpioChipDriver::PollLinesValues(const TGpioLines& lines)
{
    assert(!lines.empty());

    auto fd = lines.front()->GetFd();
    gpiohandle_data data;
    if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) < 0) {
        if (lines.front()->GetError().empty()) {
            LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);
            for (const auto& line: lines) {
                LOG(Error) << "Treating " << line->DescribeShort() << " as disconnected";
                line->SetError("r");
            }
        }
        return;
    }

    auto now = chrono::steady_clock::now();
    for (uint32_t i = 0; i < lines.size(); ++i) {
        const auto& line = lines[i];
        assert(line->GetFd() == fd);

        bool oldValue = line->GetValue();
        bool newValue = data.values[i];

        bool recovery = !line->GetError().empty();
        if (recovery) {
            line->ClearError();
            LOG(Info) << "Treating " << line->DescribeShort() << " as alive again";
            if (line->GetConfig()->Direction == EGpioDirection::Output) {
                ReInitOutput(line);
            }
        }

        LOG(Debug) << "Poll " << line->DescribeShort() << " old value: " << oldValue << " new value: " << newValue;

        if (!line->IsOutput()) {
            /* if value changed for input we simulate interrupt */
            if (recovery || oldValue != newValue) {
                line->HandleInterrupt(now);
                line->SetCachedValue(newValue);
            }
        } else { /* for output just set value to cache: it will publish it if
                    changed */
            line->SetCachedValue(newValue);
        }
    }
}

void TGpioChipDriver::ReadLinesValues(const TGpioLines& lines)
{
    if (lines.empty()) {
        return;
    }
    auto fd = lines.front()->GetFd();

    gpiohandle_data data;
    if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) < 0) {
        LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);
        for (const auto& line: lines) {
            line->SetError("r");
        }
        return;
    }

    uint32_t i = 0;
    for (const auto& line: lines) {
        assert(line->GetFd() == fd);

        line->SetCachedValue(data.values[i++]);
    }
}

void TGpioChipDriver::ReListenLine(PGpioLine line)
{
    assert(!AddedToEpoll);

    auto oldFd = line->GetFd();
    auto oldTimerFd = line->GetTimerFd();

    assert(oldFd > -1);

    Lines.erase(oldFd);
    Timers.erase(oldTimerFd);
    close(oldFd);

    bool ok = TryListenLine(line);
    assert(ok);
    if (!ok) {
        LOG(Error) << "Unable to re-listen to " << line->DescribeShort();
    }
}

void TGpioChipDriver::ReInitOutput(PGpioLine line)
{
    auto oldfd = line->GetFd();
    Lines.erase(oldfd);
    close(oldfd);

    if (Chip->GetLabel() == "mcp23017" || Chip->GetLabel() == "mcp23008")
        if (!FlushMcp23xState(line)) {
            LOG(Error) << "Unable to re-init output " << line->DescribeShort();
            return;
        }

    auto lastSuccessfulVal = line->GetValue();
    if (!InitOutput(line, lastSuccessfulVal)) {
        LOG(Error) << "Unable to re-init output " << line->DescribeShort();
    }
}

void TGpioChipDriver::AutoDetectInterruptEdges()
{
    static auto doesNeedAutoDetect = [](const PGpioLine& line) {
        if (line->IsHandled() && !line->IsOutput()) {
            if (const auto& counter = line->GetCounter()) {
                return counter->GetInterruptEdge() == EGpioEdge::AUTO;
            }
        }

        return false;
    };

    vector<TGpioLines> pollingLines;
    unordered_map<PGpioLine, int> linesSum;

    for (const auto& fdLines: Lines) {
        const auto& lines = fdLines.second;

        if (any_of(lines.begin(), lines.end(), doesNeedAutoDetect)) {
            pollingLines.push_back(lines);
        }
    }

    const auto testCount = 10;

    for (auto i = 0; i < testCount; ++i) {
        for (const auto& lines: pollingLines) {
            ReadLinesValues(lines);

            for (const auto& line: lines) {
                if (doesNeedAutoDetect(line)) {
                    linesSum[line] += line->GetValue();
                }
            }
        }
    }

    for (auto& lineSum: linesSum) {
        const auto& line = lineSum.first;
        auto& sum = lineSum.second;

        auto edge = sum < testCount ? EGpioEdge::RISING : EGpioEdge::FALLING;

        LOG(Info) << "Auto detected edge for line: " << line->DescribeShort() << ": " << GpioEdgeToString(edge);

        line->GetCounter()->SetInterruptEdge(edge);
        line->GetConfig()->InterruptEdge = edge;

        // Re-listen only for lines that are actually handled via interrupts.
        // A counter line on a chip without interrupt support (e.g. wbec-gpio
        // MOD inputs) falls back to polling and shares a single fd with all the
        // other polled lines. ReListenLine() would erase that shared fd, killing
        // every line behind it, and then fail to re-establish the event request.
        // Polling already resolves the now-concrete edge, so nothing to re-listen.
        if (line->GetInterruptSupport() == EInterruptSupport::YES) {
            ReListenLine(line);
        }
    }
}

void TGpioChipDriver::ReadInputValues()
{
    for (const auto& fdLines: Lines) {
        TGpioLines linesToRead;
        for (auto line: fdLines.second) {
            if (!line->IsOutput() && !line->GetCounter()) {
                linesToRead.push_back(line);
            }
        }
        ReadLinesValues(linesToRead);
    }
}

1	#include "gpio_chip_driver.h"
2	#include "exceptions.h"
3	#include "gpio_chip.h"
4	#include "gpio_counter.h"
5	#include "gpio_line.h"
6	#include "interruption_context.h"
7	#include "log.h"
8	#include "utils.h"
9
10	#include <wblib/utils.h>
11
12	#include <algorithm>
13	#include <cassert>
14	#include <fstream>
15	#include <string.h>
16	#include <sys/epoll.h>
17	#include <sys/ioctl.h>
18	#include <sys/timerfd.h>
19	#include <unistd.h>
20
21	#define LOG(logger) ::logger.Log() << "[gpio chip driver] "
22
23	using namespace std;
24
25	const auto CONSUMER = "wb-mqtt-gpio";
26
27	namespace
28	{
29	uint32_t GetFlagsFromConfig(const TGpioLineConfig& config, bool asIs = false)	×
30	{
31	uint32_t flags = 0;	×
32
33	if (!asIs) {	×
34	if (config.Direction == EGpioDirection::Input)	×
35	flags \|= GPIOHANDLE_REQUEST_INPUT;	×
36	else if (config.Direction == EGpioDirection::Output)	×
37	flags \|= GPIOHANDLE_REQUEST_OUTPUT;	×
38	}
39	if (config.IsOpenDrain)	×
40	flags \|= GPIOHANDLE_REQUEST_OPEN_DRAIN;	×
41	if (config.IsOpenSource)	×
42	flags \|= GPIOHANDLE_REQUEST_OPEN_SOURCE;	×
43	if (config.IsActiveLow)	×
44	flags \|= GPIOHANDLE_REQUEST_ACTIVE_LOW;	×
45
46	return flags;	×
47	}
48	} // namespace
49
50	TGpioChipDriver::TGpioChipDriver(const TGpioChipConfig& config): AddedToEpoll(false)	1✔
51	{
52	Chip = make_shared<TGpioChip>(config.Path);	1✔
53
54	unordered_map<uint32_t, vector<vector<PGpioLine>>> pollLines;	1✔
55	auto addToPoll = [&pollLines](const PGpioLine& line) {	×
56	auto& lineBulks = pollLines[GetFlagsFromConfig(*line->GetConfig())];	×
57
58	if (lineBulks.empty() \|\| lineBulks.back().size() == GPIOHANDLES_MAX) {	×
59	lineBulks.emplace_back();	×
60	}
61
62	lineBulks.back().push_back(line);	×
63	};	×
64
65	if (!Chip->IsValid()) {	1✔
66	for (const auto& lineConfig: config.Lines) {	2✔
67	auto line = make_shared<TGpioLine>(Chip, lineConfig);	1✔
68	LOG(Error) << "Add " << line->DescribeShort() << " as initially disconnected";	1✔
69	InitiallyDisconnectedLines[line->GetOffset()] = line;	1✔
70	}	1✔
71	return;	1✔
72	}
73
74	for (const auto& line: Chip->LoadLines(config.Lines)) {	×
75	if (!ReleaseLineIfUsed(line)) {	×
76	LOG(Error) << "Skipping " << line->DescribeShort();	×
77	}
78	switch (line->GetConfig()->Direction) {	×
79	case EGpioDirection::Input: {	×
80	if (!InitInputInterrupts(line)) {	×
81	addToPoll(line);	×
82	}
83	break;	×
84	}
85	case EGpioDirection::Output: {	×
86	if (!InitOutput(line, line->GetConfig()->InitialState)) {	×
87	LOG(Error) << "Failed to init output " << line->DescribeShort()	×
88	<< ". Treating as initially disconnected";	×
89	InitiallyDisconnectedLines[line->GetOffset()] = line;	×
90	}
91	break;	×
92	}
93	}
94	}	×
95
96	/* Initialize polling if line doesn't support interrupts */
97	for (const auto& flagsLines: pollLines) {	×
98	const auto flags = flagsLines.first;	×
99	const auto& lineBulks = flagsLines.second;	×
100
101	for (const auto& lines: lineBulks) {	×
102	if (!InitLinesPolling(flags, lines)) {	×
103	for (const auto& line: lines) {	×
104	LOG(Error) << "Failed to init polling " << line->DescribeShort()	×
105	<< ". Treating as initially disconnected";	×
106	InitiallyDisconnectedLines[line->GetOffset()] = line;	×
107	}
108	}
109	}
110	}
111
112	if (Lines.empty() && InitiallyDisconnectedLines.empty()) {	×
113	wb_throw(TGpioDriverException, "Failed to initialize any line");	×
114	}
115
116	AutoDetectInterruptEdges();	×
117	ReadInputValues();	×
118	}	1✔
119
120	TGpioChipDriver::TGpioChipDriver(): AddedToEpoll(false)	7✔
121	{}	7✔
122
123	TGpioChipDriver::~TGpioChipDriver()	8✔
124	{
125	for (const auto& fdLines: Lines) {	15✔
126	auto fd = fdLines.first;	7✔
127	const auto& lines = fdLines.second;	7✔
128
129	{
130	auto logDebug = move(LOG(Debug) << "Close fd for:");	7✔
131	for (const auto& line: lines) {	15✔
132	logDebug << "\n\t" << line->DescribeShort();	8✔
133	}
134	}	7✔
135
136	close(fd);	7✔
137	}
138	}	8✔
139
140	int TGpioChipDriver::CreateIntervalTimer()	×
141	{
142	int tfd = timerfd_create(CLOCK_MONOTONIC, 0);	×
143	if (tfd == -1) {	×
144	LOG(Error) << "timerfd_create failed: " << strerror(errno);	×
145	wb_throw(TGpioDriverException, "unable to create timer: timerfd_create failed with " + string(strerror(errno)));	×
146	}
147	return tfd;	×
148	}
149
150	void TGpioChipDriver::SetIntervalTimer(int tfd, std::chrono::microseconds intervalUs)	×
151	{
152	auto sec = std::chrono::floor<std::chrono::seconds>(intervalUs);	×
153	auto nsec = std::chrono::duration_cast<std::chrono::nanoseconds>(intervalUs - sec);	×
154
155	struct itimerspec ts;
156	ts.it_value.tv_sec = sec.count();	×
157	ts.it_value.tv_nsec = nsec.count();	×
158	ts.it_interval.tv_sec = 0;	×
159	ts.it_interval.tv_nsec = 0;	×
160
161	if (timerfd_settime(tfd, 0, &ts, NULL) < 0) {	×
162	LOG(Error) << "timerfd_settime failed: " << strerror(errno);	×
163	close(tfd);	×
164	wb_throw(TGpioDriverException, "unable to setup timer: timerfd_settime failed with " + string(strerror(errno)));	×
165	}
166	}	×
167
168	TGpioChipDriver::TGpioLinesByOffsetMap TGpioChipDriver::MapLinesByOffset() const	×
169	{
170	TGpioLinesByOffsetMap linesByOffset;	×
171
172	FOR_EACH_LINE(this, line)	×
173	{
174	linesByOffset[line->GetOffset()] = line;	×
175	});	×
176
177	return linesByOffset;	×
178	}	×
179
180	const TGpioChipDriver::TGpioLinesByOffsetMap& TGpioChipDriver::MapInitiallyDisconnectedLinesByOffset() const	1✔
181	{
182	return InitiallyDisconnectedLines;	1✔
183	}
184
185	void TGpioChipDriver::AddToEpoll(int epfd)	×
186	{
187	AddedToEpoll = true;	×
188
189	FOR_EACH_LINE(this, line)	×
190	{
191	if (line->IsOutput() \|\| line->GetInterruptSupport() != EInterruptSupport::YES) {	×
192	return;	×
193	}
194
195	struct epoll_event ep_event{};	×
196
197	ep_event.events = EPOLLIN \| EPOLLPRI;	×
198	ep_event.data.fd = line->GetFd();	×
199
200	if (epoll_ctl(epfd, EPOLL_CTL_ADD, line->GetFd(), &ep_event) < 0) {	×
201	LOG(Error) << "epoll_ctl error: '" << strerror(errno) << "' at " << line->DescribeShort();	×
202	}
203
204	auto timerFd = line->GetTimerFd();	×
205	ep_event.events = EPOLLIN;	×
206	ep_event.data.fd = timerFd;	×
207	if (epoll_ctl(epfd, EPOLL_CTL_ADD, timerFd, &ep_event) < 0) {	×
208	LOG(Error) << "epoll_ctl error: '" << strerror(errno);	×
209	wb_throw(TGpioDriverException,	×
210	"unable to add timer to epoll: epoll_ctl failed with " + string(strerror(errno)));
211	}
212	});
213	}	×
214
215	bool TGpioChipDriver::HandleGpioInterrupt(const PGpioLine& line, const TInterruptionContext& ctx)	×
216	{
217	bool isHandled = false;	×
218	auto fd = line->GetFd();	×
219
220	fd_set rfds;
221	FD_ZERO(&rfds);	×
222	FD_SET(fd, &rfds);	×
223	struct timeval tv{0}; // do not block	×
224
225	while (auto retVal = select(fd + 1, &rfds, nullptr, nullptr, &tv)) {	×
226	if (retVal < 0) {	×
227	LOG(Error) << "select failed: " << strerror(errno);	×
228	wb_throw(TGpioDriverException,	×
229	"unable to read line event data: select failed with " + string(strerror(errno)));
230	}
231
232	gpioevent_data data{};	×
233	if (read(fd, &data, sizeof(data)) < 0) {	×
234	LOG(Error) << "Read gpioevent_data failed: " << strerror(errno);	×
235	wb_throw(TGpioDriverException,	×
236	"unable to read line event data: gpioevent_data failed with " + string(strerror(errno)));
237	}
238
239	auto time = ctx.ToSteadyClock(data.timestamp);	×
240
241	gpiohandle_data values;
242	if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &values) < 0) {	×
243	LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);	×
244	line->SetError("r");	×
245	return false;	×
246	}
247
248	line->SetCachedValueUnfiltered(values.values[0]);	×
249	line->HandleInterrupt(time); // record interrupt time, (re)arm debounce window	×
250	SetIntervalTimer(line->GetTimerFd(), line->GetConfig()->DebounceTimeout);	×
251	isHandled = true;	×
252	}	×
253	return isHandled;	×
254	}
255
256	bool TGpioChipDriver::HandleTimerInterrupt(const PGpioLine& line)	×
257	{
258	bool isHandled = false;	×
259
260	if (line->UpdateIfStable(chrono::steady_clock::now())) {	×
261	isHandled = true;	×
262	SetIntervalTimer(line->GetTimerFd(),	×
263	std::chrono::microseconds(0)); // disarm timer	×
264	}
265	return isHandled;	×
266	}
267
268	bool TGpioChipDriver::HandleInterrupt(const TInterruptionContext& ctx)	×
269	{
270	bool isHandled = false;	×
271
272	for (int i = 0; i < ctx.Count; i++) {	×
273	auto fd = ctx.Events[i].data.fd;	×
274
275	// gpio interrupt event fired: set stable-val-check timer
276	auto itFdLines = Lines.find(fd);	×
277	if (itFdLines != Lines.end()) {	×
278	const auto& lines = itFdLines->second;	×
279	assert(lines.size() == 1);	×
280	const auto& line = lines.front();	×
281	HandleGpioInterrupt(line, ctx);	×
282
283	// timer event fired: check, is value stable or bouncing
284	} else {
285	auto itFdTimers = Timers.find(fd);	×
286	if (itFdTimers != Timers.end()) {	×
287	const auto& line = itFdTimers->second.front();	×
288	isHandled \|= HandleTimerInterrupt(line);	×
289	}
290	}
291	}
292	return isHandled;	×
293	}
294
295	bool TGpioChipDriver::PollLines()	×
296	{
297	bool isHandled = false;	×
298
299	for (const auto& fdLines: Lines) {	×
300	const auto& lines = fdLines.second;	×
301	assert(!lines.empty());	×
302
303	isHandled = true;	×
304
305	PollLinesValues(lines);	×
306	}
307
308	return isHandled;	×
309	}
310
311	void TGpioChipDriver::ForEachLine(const TGpioLineHandler& handler) const	×
312	{
313	for (const auto& fdLines: Lines) {	×
314	const auto& lines = fdLines.second;	×
315	assert(!lines.empty());	×
316
317	for (const auto& line: lines) {	×
318	handler(line);	×
319	}
320	}
321	}	×
322
323	bool TGpioChipDriver::ReleaseLineIfUsed(const PGpioLine& line)	×
324	{
325	if (!line->IsUsed())	×
326	return true;	×
327
328	LOG(Debug) << line->Describe() << " is used by '" << line->GetConsumer() << "'.";	×
329	if (line->GetConsumer() == "sysfs") {	×
330	ofstream unexportGpio("/sys/class/gpio/unexport");	×
331	if (unexportGpio.is_open()) {	×
332	LOG(Debug) << "Trying to unexport...";	×
333	try {
334	unexportGpio << Utils::ToSysfsGpio(line);	×
335	} catch (const TGpioDriverException& e) {	×
336	LOG(Error) << line->Describe() << " is used by '" << line->GetConsumer() << "',"	×
337	<< " during unexport: " << e.what();	×
338	}	×
339	}
340	}	×
341
342	line->UpdateInfo();	×
343
344	if (line->IsUsed()) {	×
345	LOG(Error) << "Failed to release " << line->DescribeShort();	×
346	return false;	×
347	}
348
349	LOG(Debug) << line->DescribeShort() << " successfully released";	×
350	return true;	×
351	}
352
353	bool TGpioChipDriver::TryListenLine(const PGpioLine& line)	×
354	{
355	const auto& config = line->GetConfig();	×
356	assert(config->Direction == EGpioDirection::Input);	×
357
358	gpioevent_request req{};	×
359
360	strcpy(req.consumer_label, CONSUMER);	×
361	req.lineoffset = line->GetOffset();	×
362	req.handleflags = GetFlagsFromConfig(*config);	×
363
364	req.eventflags = GPIOEVENT_REQUEST_BOTH_EDGES;	×
365
366	errno = 0;	×
367	if (ioctl(Chip->GetFd(), GPIO_GET_LINEEVENT_IOCTL, &req) < 0) {	×
368	auto error = errno;	×
369	LOG(Warn) << "GPIO_GET_LINEEVENT_IOCTL failed: " << strerror(error) << " at " << line->DescribeShort();	×
370	return false;	×
371	}
372
373	Lines[req.fd].push_back(line);	×
374	assert(Lines[req.fd].size() == 1);	×
375	line->SetFd(req.fd);	×
376
377	auto timerFd = CreateIntervalTimer();	×
378	line->SetTimerFd(timerFd);	×
379	Timers[timerFd].push_back(line);	×
380
381	LOG(Debug) << "Listening to " << line->DescribeShort();	×
382	return true;	×
383	}
384
385	bool TGpioChipDriver::FlushMcp23xState(const PGpioLine& line)	×
386	{
387	/*
388	MCP's POR state is input => we need to init gpio-extender module as output on physicall reconnect.
389
390	"pinctrl_mcp23s08" kernel driver has internal cache => once init module as input
391	and then init as output to trigger needed i2c communication with mcp.
392	*/
393	if (line->GetConfig()->Direction != EGpioDirection::Output) {	×
394	wb_throw(TGpioDriverException, "Only output lines need flushing-state magic after physical reconnect");	×
395	}
396
397	LOG(Debug) << "Flush state of " << line->DescribeShort() << " to guarantee, it is alive after any disconnects";	×
398
399	gpioevent_request req{};	×
400	req.lineoffset = line->GetOffset();	×
401	req.handleflags \|= GPIOHANDLE_REQUEST_INPUT;	×
402	req.eventflags \|= GPIOEVENT_REQUEST_RISING_EDGE;	×
403	strcpy(req.consumer_label, CONSUMER);	×
404
405	if (ioctl(Chip->GetFd(), GPIO_GET_LINEEVENT_IOCTL, &req) < 0) {	×
406	LOG(Error) << "Temporary init " << line->DescribeShort()	×
407	<< " as input failed. GPIO_GET_LINEEVENT_IOCTL: " << strerror(errno);	×
408	return false;	×
409	}
410	line->UpdateInfo();	×
411	close(req.fd);	×
412	return true;	×
413	}
414
415	bool TGpioChipDriver::InitOutput(const PGpioLine& line, uint8_t val)	×
416	{
417	const auto& config = line->GetConfig();	×
418	assert(config->Direction == EGpioDirection::Output);	×
419
420	gpiohandle_request req;
421	memset(&req, 0, sizeof(gpiohandle_request));	×
422	req.lines = 1;	×
423	req.lineoffsets[0] = line->GetOffset();	×
424	req.default_values[0] = val;	×
425	req.flags = GetFlagsFromConfig(*config, line->IsOutput());	×
426	strcpy(req.consumer_label, CONSUMER);	×
427
428	if (ioctl(Chip->GetFd(), GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {	×
429	LOG(Error) << "GPIO_GET_LINEHANDLE_IOCTL failed: " << strerror(errno) << " at " << line->DescribeShort();	×
430	return false;	×
431	}
432
433	Lines[req.fd].push_back(line);	×
434	assert(Lines[req.fd].size() == 1);	×
435	line->SetFd(req.fd);	×
436
437	if (Debug.IsEnabled()) {	×
438	gpiohandle_data data;
439	if (ioctl(line->GetFd(), GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) >= 0) {	×
440	LOG(Debug) << "Initialized output " << line->DescribeShort() << " = " << static_cast<int>(data.values[0]);	×
441	}
442	} else {
443	LOG(Info) << "Initialized output " << line->DescribeShort();	×
444	}
445
446	return true;	×
447	}
448
449	bool TGpioChipDriver::InitInputInterrupts(const PGpioLine& line)	×
450	{
451	switch (line->GetInterruptSupport()) {	×
452	case EInterruptSupport::UNKNOWN:	×
453	case EInterruptSupport::YES: {
454	if (TryListenLine(line)) {	×
455	line->SetInterruptSupport(EInterruptSupport::YES);	×
456	return true;	×
457	}
458	LOG(Info) << line->Describe() << " does not support interrupts. Polling will be used instead.";	×
459	line->SetInterruptSupport(EInterruptSupport::NO);	×
460	return false;	×
461	}
462
463	case EInterruptSupport::NO: {	×
464	return false;	×
465	}
466	}
467	}
468
469	bool TGpioChipDriver::InitLinesPolling(uint32_t flags, const vector<PGpioLine>& lines)	×
470	{
471	assert(lines.size() <= GPIOHANDLES_MAX);	×
472
473	gpiohandle_request req;
474	req.lines = 0;	×
475	req.flags = flags;	×
476	strcpy(req.consumer_label, CONSUMER);	×
477
478	for (auto& line: lines) {	×
479	req.lineoffsets[req.lines] = line->GetOffset();	×
480	req.default_values[req.lines] = line->IsActiveLow();	×
481	++req.lines;	×
482	}
483
484	if (ioctl(Chip->GetFd(), GPIO_GET_LINEHANDLE_IOCTL, &req) < 0) {	×
485	LOG(Error) << "GPIO_GET_LINEHANDLE_IOCTL failed: " << strerror(errno);	×
486	return false;	×
487	}
488
489	auto& initialized = Lines[req.fd];	×
490
491	assert(initialized.empty());	×
492	initialized.reserve(lines.size());	×
493
494	for (const auto& line: lines) {	×
495	line->SetFd(req.fd);	×
496	initialized.push_back(line);	×
497	}
498
499	return true;	×
500	}
501
502	void TGpioChipDriver::PollLinesValues(const TGpioLines& lines)	×
503	{
504	assert(!lines.empty());	×
505
506	auto fd = lines.front()->GetFd();	×
507	gpiohandle_data data;
508	if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) < 0) {	×
509	if (lines.front()->GetError().empty()) {	×
510	LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);	×
511	for (const auto& line: lines) {	×
512	LOG(Error) << "Treating " << line->DescribeShort() << " as disconnected";	×
513	line->SetError("r");	×
514	}
515	}
516	return;	×
517	}
518
519	auto now = chrono::steady_clock::now();	×
520	for (uint32_t i = 0; i < lines.size(); ++i) {	×
521	const auto& line = lines[i];	×
522	assert(line->GetFd() == fd);	×
523
524	bool oldValue = line->GetValue();	×
525	bool newValue = data.values[i];	×
526
527	bool recovery = !line->GetError().empty();	×
528	if (recovery) {	×
529	line->ClearError();	×
530	LOG(Info) << "Treating " << line->DescribeShort() << " as alive again";	×
531	if (line->GetConfig()->Direction == EGpioDirection::Output) {	×
532	ReInitOutput(line);	×
533	}
534	}
535
536	LOG(Debug) << "Poll " << line->DescribeShort() << " old value: " << oldValue << " new value: " << newValue;	×
537
538	if (!line->IsOutput()) {	×
539	/* if value changed for input we simulate interrupt */
540	if (recovery \|\| oldValue != newValue) {	×
541	line->HandleInterrupt(now);	×
542	line->SetCachedValue(newValue);	×
543	}
544	} else { /* for output just set value to cache: it will publish it if
545	changed */
546	line->SetCachedValue(newValue);	×
547	}
548	}
549	}
550
551	void TGpioChipDriver::ReadLinesValues(const TGpioLines& lines)	×
552	{
553	if (lines.empty()) {	×
554	return;	×
555	}
556	auto fd = lines.front()->GetFd();	×
557
558	gpiohandle_data data;
559	if (ioctl(fd, GPIOHANDLE_GET_LINE_VALUES_IOCTL, &data) < 0) {	×
560	LOG(Error) << "GPIOHANDLE_GET_LINE_VALUES_IOCTL failed: " << strerror(errno);	×
561	for (const auto& line: lines) {	×
562	line->SetError("r");	×
563	}
564	return;	×
565	}
566
567	uint32_t i = 0;	×
568	for (const auto& line: lines) {	×
569	assert(line->GetFd() == fd);	×
570
571	line->SetCachedValue(data.values[i++]);	×
572	}
573	}
574
575	void TGpioChipDriver::ReListenLine(PGpioLine line)	×
576	{
577	assert(!AddedToEpoll);	×
578
579	auto oldFd = line->GetFd();	×
580	auto oldTimerFd = line->GetTimerFd();	×
581
582	assert(oldFd > -1);	×
583
584	Lines.erase(oldFd);	×
585	Timers.erase(oldTimerFd);	×
586	close(oldFd);	×
587
588	bool ok = TryListenLine(line);	×
589	assert(ok);	×
590	if (!ok) {	×
591	LOG(Error) << "Unable to re-listen to " << line->DescribeShort();	×
592	}
593	}	×
594
595	void TGpioChipDriver::ReInitOutput(PGpioLine line)	×
596	{
597	auto oldfd = line->GetFd();	×
598	Lines.erase(oldfd);	×
599	close(oldfd);	×
600
601	if (Chip->GetLabel() == "mcp23017" \|\| Chip->GetLabel() == "mcp23008")	×
602	if (!FlushMcp23xState(line)) {	×
603	LOG(Error) << "Unable to re-init output " << line->DescribeShort();	×
604	return;	×
605	}
606
607	auto lastSuccessfulVal = line->GetValue();	×
608	if (!InitOutput(line, lastSuccessfulVal)) {	×
609	LOG(Error) << "Unable to re-init output " << line->DescribeShort();	×
610	}
611	}
612
613	void TGpioChipDriver::AutoDetectInterruptEdges()	7✔
614	{
615	static auto doesNeedAutoDetect = [](const PGpioLine& line) {	57✔
616	if (line->IsHandled() && !line->IsOutput()) {	57✔
617	if (const auto& counter = line->GetCounter()) {	57✔
618	return counter->GetInterruptEdge() == EGpioEdge::AUTO;	47✔
619	}
620	}
621
622	return false;	10✔
623	};
624
625	vector<TGpioLines> pollingLines;	7✔
626	unordered_map<PGpioLine, int> linesSum;	7✔
627
628	for (const auto& fdLines: Lines) {	14✔
629	const auto& lines = fdLines.second;	7✔
630
631	if (any_of(lines.begin(), lines.end(), doesNeedAutoDetect)) {	7✔
632	pollingLines.push_back(lines);	4✔
633	}
634	}
635
636	const auto testCount = 10;	7✔
637
638	for (auto i = 0; i < testCount; ++i) {	77✔
639	for (const auto& lines: pollingLines) {	110✔
640	ReadLinesValues(lines);	40✔
641
642	for (const auto& line: lines) {	90✔
643	if (doesNeedAutoDetect(line)) {	50✔
644	linesSum[line] += line->GetValue();	40✔
645	}
646	}
647	}
648	}
649
650	for (auto& lineSum: linesSum) {	11✔
651	const auto& line = lineSum.first;	4✔
652	auto& sum = lineSum.second;	4✔
653
654	auto edge = sum < testCount ? EGpioEdge::RISING : EGpioEdge::FALLING;	4✔
655
656	LOG(Info) << "Auto detected edge for line: " << line->DescribeShort() << ": " << GpioEdgeToString(edge);	4✔
657
658	line->GetCounter()->SetInterruptEdge(edge);	4✔
659	line->GetConfig()->InterruptEdge = edge;	4✔
660
661	// Re-listen only for lines that are actually handled via interrupts.
662	// A counter line on a chip without interrupt support (e.g. wbec-gpio
663	// MOD inputs) falls back to polling and shares a single fd with all the
664	// other polled lines. ReListenLine() would erase that shared fd, killing
665	// every line behind it, and then fail to re-establish the event request.
666	// Polling already resolves the now-concrete edge, so nothing to re-listen.
667	if (line->GetInterruptSupport() == EInterruptSupport::YES) {	4✔
668	ReListenLine(line);	1✔
669	}
670	}
671	}	7✔
672
673	void TGpioChipDriver::ReadInputValues()	×
674	{
675	for (const auto& fdLines: Lines) {	×
676	TGpioLines linesToRead;	×
677	for (auto line: fdLines.second) {	×
678	if (!line->IsOutput() && !line->GetCounter()) {	×
679	linesToRead.push_back(line);	×
680	}
681	}	×
682	ReadLinesValues(linesToRead);	×
683	}	×
684	}	×

wirenboard / wb-mqtt-gpio / 93

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous