rts/umlrt/umlrttimerqueue.cc

// umlrttimerqueue.cc

/*******************************************************************************
* Copyright (c) 2014-2015 Zeligsoft (2009) Limited  and others.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the Eclipse Public License v1.0
* which accompanies this distribution, and is available at
* http://www.eclipse.org/legal/epl-v10.html
*******************************************************************************/

#include "basefatal.hh"
#include "basedebug.hh"
#include "umlrtapi.hh"
#include "umlrtguard.hh"
#include "umlrttimer.hh"
#include "umlrttimerqueue.hh"
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <sys/socket.h>

// See umlrttimerqueue.hh for documentation.

UMLRTTimerQueue::UMLRTTimerQueue() : UMLRTQueue()
{
    // Appenders write the pipe. Controllers 'wait' on a select() on the pipe with
    // a timeout associated with any running timer. A write on the pipe wakes the controller.
    if (pipe(notifyFd) < 0)
    {
        FATAL_ERRNO("pipe");
    }
    // Bytes available in the pipe wake up the controller as 'notify'. The controller
    // has to be able to clean out the pipe (read) without blocking.
    int flags = fcntl(notifyFd[0], F_GETFL, 0);
    if (flags < 0)
    {
        FATAL_ERRNO("fcntl F_GETFL");
    }
    if (fcntl(notifyFd[0], F_SETFL, flags | O_NONBLOCK) < 0)
    {
        FATAL_ERRNO("fcntl F_SETFL");
    }
}

// Remove the first timer on the queue. Returns NULL if first timer has not yet expired.
UMLRTTimer * UMLRTTimerQueue::dequeue()
{
    UMLRTGuard(getMutex());

    UMLRTTimer * first = (UMLRTTimer *)head;

    if (first)
    {
        UMLRTTimespec now;
        UMLRTTimespec::getClock(&now);

        if (now >= first->due)
        {
            // First timer is due - dequeue it and return it.
            head = first->next;
            if (head == NULL)
            {
                tail = NULL; // Not required, but cleaner.
            }
            BDEBUG(BD_TIMER, "this(%p) dequeue found first timer due.\n", this);
        }
        else
        {
            // First timer still running - leave it there.
            first = NULL;
            BDEBUG(BD_TIMER, "this(%p) dequeue found first timer still running.\n", this);
        }
    }
    else
    {
        BDEBUG(BD_TIMER, "this(%p) dequeue found no timers.\n", this);
    }
    return first;
}

// Add a timer to the queue in the order of when they will expire.
void UMLRTTimerQueue::enqueue( const UMLRTTimer * timer )
{
    UMLRTGuard(getMutex());

    UMLRTTimer * next = (UMLRTTimer *)head;
    UMLRTTimer * previous = NULL;

    char tmbuf[UMLRTTimespec::TIMESPEC_TOSTRING_SZ];
    BDEBUG(BD_TIMER, "this(%p) timer-enqueue due(%s)\n", this, timer->due.toString(tmbuf,sizeof(tmbuf)));

    timer->next = NULL; // Initialize as last-in-queue.

    // Only need to notify the controller if the new timer ends up at the head of the queue.
    // The wait mechanism is only interested in the time remaining for the timer at the head of the queue.
    if (!head)
    {
        // List was empty. Put it in there as only element.
        head = tail = timer;
        sendNotification();
    }
    else
    {
        // Skip ahead until we meet a timer due after this one.
        while (next && (next->due <= timer->due))
        {
            previous = next;
            next = (UMLRTTimer *)next->next;
        }
        if (!next)
        {
            // We're appending this timer to the end of the queue.
            tail->next = timer;
            tail = timer;
        }
        else if (!previous)
        {
            // This timer is before the first element in the queue - prepend it.
            timer->next = head;
            head = timer;
            sendNotification();
        }
        else
        {
            // This timer goes after 'previous' and before 'next'.
            previous->next = timer;
            timer->next = next;
        }
    }
}

// Calculate how much time left before timer on the head of the queue is due.
UMLRTTimespec UMLRTTimerQueue::timeRemaining() const
{
    UMLRTGuard(getMutex());

    // NOTE: Intended only for the consumer of the queue elements which has confirmed
    // the queue was non-empty. An alternate implementation is required if an empty queue
    // is possible.
    if (isEmpty())
    {
        FATAL("timer queue was empty in timeRemaining()");
    }
    UMLRTTimespec now;
    UMLRTTimespec::getClock(&now);

    BDEBUG(BD_TIMER, "this(%p) head(%p) tail(%p)\n", this, head, tail);

    UMLRTTimespec remain = ((UMLRTTimer*)head)->due - now;

    char tmbuf[UMLRTTimespec::TIMESPEC_TOSTRING_SZ];
    BDEBUG(BD_TIMER, "timeRemaining %s\n", remain.toStringRelative(tmbuf, sizeof(tmbuf)));

    return remain;
}

// Add a timer to the queue in the order of when they will expire.
bool UMLRTTimerQueue::cancel( UMLRTTimerId id )
{
    UMLRTGuard(getMutex());

    bool ok = false;
    UMLRTTimer * next = (UMLRTTimer *)head;
    UMLRTTimer * previous = NULL;

    while (next && (next != id.getTimer()))
    {
        previous = next;
        next = (UMLRTTimer *)next->next;
    }
    // Only need to notify the controller if the cancelled timer was at the head of the queue.
    if (next)
    {
        // Found the one to delete.
        if (!previous)
        {
            // This timer was at the head of the queue.
            head = (UMLRTTimer *)next->next;
            if (head == NULL)
            {
                tail = NULL; // Not strictly required, but cleaner.
            }
            sendNotification();
        }
        else
        {
            // Unlink the timer being deallocated by setting next of previous to be
            // the cancelled timer's next.
            previous->next = next->next;

            // If the timer was last in the queue, the tail has to be updated.
            if (tail == next)
            {
                tail = previous;
            }
        }
        // Return it to the pool.
        umlrt::TimerPutToPool(next);

        ok = true;
    }
    return ok;
}

void UMLRTTimerQueue::sendNotification()
{
    int bytes_ready;

    if (ioctl(notifyFd[0], FIONREAD, &bytes_ready) < 0)
    {
        FATAL_ERRNO("ioctl");
    }
    if (!bytes_ready)
    {
        // Write a byte to the notification-pipe as a notification of a pending message.
        uint8_t notifyByte = 0;

        if (write(notifyFd[1], &notifyByte, 1) < 0)
        {
            FATAL_ERRNO("write");
        }
    }
}

// See umlrtprioritymessagequeue.hh for documentation.
void UMLRTTimerQueue::clearNotifyFd()
{
    uint8_t ignore;
    int bytes;
    if ((bytes = read(notifyFd[0], &ignore, 1)) < 0)
    {
        FATAL_ERRNO("initial read - synchronization mechanism implies a byte is waiting");
    }
    else if (!bytes)
    {
        // Indicates synchronization logic error. Should always get at least one byte.
        FATAL("Should never find pipe empty when clearing a notification.");
    }
    // Clear out the notification-pipe.
    // Can shrink this after notify is debugged.
    bool done = false;
    while (!done)
    {
        int bytes = read(notifyFd[0], &ignore, 1);
        if (bytes < 0)
        {
            if (errno != EAGAIN)
            {
                FATAL_ERRNO("read");
            }
            else
            {
                done = true;
            }
        }
        if (bytes > 0)
        {
            // BDEBUG(0, "read returned bytes(%d)\n", bytes);
        }
        else
        {
            done = true;
        }
    }
}

// See umlrtprioritymessagequeue.hh for documentation.
int UMLRTTimerQueue::getNotifyFd()
{
    return notifyFd[0];
}