<commit_before>/*
 * Copyright (C) 2004-2008 Marc Boris Duerner
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * As a special exception, you may use this file as part of a free
 * software library without restriction. Specifically, if other files
 * instantiate templates or use macros or inline functions from this
 * file, or you compile this file and link it with other files to
 * produce an executable, this file does not by itself cause the
 * resulting executable to be covered by the GNU General Public
 * License. This exception does not however invalidate any other
 * reasons why the executable file might be covered by the GNU Library
 * General Public License.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "cxxtools/streambuffer.h"
#include <algorithm>
#include <stdexcept>
#include <cstring>
#include <cxxtools/log.h>

log_define("cxxtools.streambuffer")

namespace cxxtools {

StreamBuffer::StreamBuffer(IODevice& ioDevice, size_t bufferSize, bool extend)
: _ioDevice(&ioDevice),
  _ibufferSize(bufferSize+4),
  _ibuffer(0),
  _obufferSize(bufferSize),
  _obuffer(0),
  _pbmax(4),
  _oextend(extend)
{
    setg(0, 0, 0);
    setp(0, 0);

    attach(ioDevice);
}

StreamBuffer::StreamBuffer(size_t bufferSize, bool extend)
: _ioDevice(0),
  _ibufferSize(bufferSize+4),
  _ibuffer(0),
  _obufferSize(bufferSize),
  _obuffer(0),
  _pbmax(4),
  _oextend(extend)
{
    setg(0, 0, 0);
    setp(0, 0);
}

StreamBuffer::~StreamBuffer()
{
    delete[] _ibuffer;
    delete[] _obuffer;
}

void StreamBuffer::attach(IODevice& ioDevice)
{
    if (ioDevice.busy())
        throw IOPending("IODevice in use");

    if (_ioDevice)
    {
        if (_ioDevice->busy())
            throw IOPending("IODevice in use");

        disconnect(ioDevice.inputReady, *this, &StreamBuffer::onRead);
        disconnect(ioDevice.outputReady, *this, &StreamBuffer::onWrite);
    }

    _ioDevice = &ioDevice;
    connect(ioDevice.inputReady, *this, &StreamBuffer::onRead);
    connect(ioDevice.outputReady, *this, &StreamBuffer::onWrite);
}

void StreamBuffer::beginRead()
{
    if (_ioDevice == 0 || _ioDevice->reading())
        return;

    if (! _ibuffer)
    {
        _ibuffer = new char[_ibufferSize];
    }

    size_t putback = _pbmax;
    size_t leftover = 0;

    // keep chars for putback
    if (gptr())
    {
        putback = std::min<size_t>( gptr() - eback(), _pbmax);
        char* to = _ibuffer + _pbmax - putback;
        char* from = gptr() - putback;

        leftover = egptr() - gptr();
        std::memmove( to, from, putback + leftover );
    }

    size_t used = _pbmax + leftover;

    if (_ibufferSize == used)
        throw std::logic_error("StreamBuffer is full");

    _ioDevice->beginRead( _ibuffer + used, _ibufferSize - used );

    setg( _ibuffer + (_pbmax - putback), // start of get area
                _ibuffer + putback, // gptr position
                _ibuffer + used ); // end of get area
}

void StreamBuffer::onRead(IODevice& /*dev*/)
{
    inputReady.send(*this);
}

void StreamBuffer::endRead()
{
    size_t readSize = _ioDevice->endRead();

    setg(eback(), // start of get area
         gptr(),  // gptr position
         egptr() + readSize); // end of get area
}

StreamBuffer::int_type StreamBuffer::underflow()
{
    log_trace("underflow");

    if (!_ioDevice)
        return traits_type::eof();

    if (_ioDevice->reading())
        endRead();

    if (gptr() < egptr())
        return traits_type::to_int_type( *(gptr()) );

    if (_ioDevice->eof())
        return traits_type::eof();

    if (! _ibuffer)
    {
        _ibuffer = new char[_ibufferSize];
    }

    size_t putback = _pbmax;

    if (gptr())
    {
        putback = std::min<size_t>(gptr() - eback(), _pbmax);
        std::memmove( _ibuffer + (_pbmax - putback),
                      gptr() - putback,
                      putback );
    }

    size_t readSize = _ioDevice->read( _ibuffer + _pbmax, _ibufferSize - _pbmax );

    setg( _ibuffer + _pbmax - putback,    // start of get area
                _ibuffer + _pbmax,              // gptr position
                _ibuffer + _pbmax + readSize ); // end of get area

    if (_ioDevice->eof())
        return traits_type::eof();

    return traits_type::to_int_type( *(gptr()) );
}

size_t StreamBuffer::beginWrite()
{
    log_trace("beginWrite; out_avail=" << out_avail());

    if (_ioDevice == 0)
        return 0;

    if (_ioDevice->writing())
    {
        log_debug("device already in write mode - nothing done");
        return 0;
    }

    if (pptr())
    {
        size_t avail = pptr() - pbase();
        if (avail > 0)
        {
            return _ioDevice->beginWrite(_obuffer, avail);
        }
    }

    return 0;
}

void StreamBuffer::discard()
{
    if (_ioDevice && (_ioDevice->reading() || _ioDevice->writing()))
        throw IOPending("discard failed - streambuffer is in use");

    if (gptr())
        setg(_ibuffer, _ibuffer + _ibufferSize, _ibuffer + _ibufferSize);

    if (pptr())
        setp(_obuffer, _obuffer + _obufferSize);
}

void StreamBuffer::onWrite(IODevice& /*dev*/)
{
    outputReady.send(*this);
}

size_t StreamBuffer::endWrite()
{
    log_trace("endWrite; out_avail=" << out_avail());

    size_t leftover = 0;
    size_t written = 0;

    if (pptr())
    {
        size_t avail = pptr() - pbase();
        written = _ioDevice->endWrite();

        leftover = avail - written;

        log_debug(written << " bytes written; " << leftover << " left in buffer");

        if (leftover > 0)
        {
            traits_type::move(_obuffer, _obuffer + written, leftover);
        }
    }

    setp(_obuffer, _obuffer + _obufferSize);
    pbump( leftover );

    return written;
}

StreamBuffer::int_type StreamBuffer::overflow(int_type ch)
{
    log_trace("overflow(" << ch << ')');

    if (!_ioDevice)
        return traits_type::eof();

    if (!_obuffer)
    {
        _obuffer = new char[_obufferSize];
        setp(_obuffer, _obuffer + _obufferSize);
    }
    else if (_oextend && !traits_type::eq_int_type( ch, traits_type::eof() ))
    {
        // break asyncronous I/O if any active
        bool pendingWrite = _ioDevice->writing();
        bool pendingRead = _ioDevice->reading();
        if (pendingWrite)
            _ioDevice->cancel();

        // if the buffer area is extensible and overflow is not called by
        // sync/flush we copy the output buffer to a larger one
        size_t bufsize = _obufferSize + (_obufferSize/2);
        log_debug("extend buffer from " << _obufferSize << " to " << bufsize);
        char* buf = new char[ bufsize ];
        traits_type::copy(buf, _obuffer, _obufferSize);
        std::swap(_obuffer, buf);
        setp(_obuffer, _obuffer + bufsize);
        pbump( _obufferSize );
        _obufferSize = bufsize;
        delete [] buf;

        // restart asyncronous I/O
        if (pendingWrite)
        {
            beginWrite();
            if (pendingRead)
                beginRead();
        }
    }
    else if (_ioDevice->writing()) // beginWrite is unfinished
    {
        log_debug("finish writing");
        endWrite();
    }
    else
    {
        // normal blocking overflow case
        log_debug("blocking overflow");
        size_t avail = pptr() - _obuffer;
        size_t written = _ioDevice->write(_obuffer, avail);
        size_t leftover = avail - written;

        if (leftover > 0)
        {
            traits_type::move(_obuffer, _obuffer + written, leftover);
        }

        setp(_obuffer, _obuffer + _obufferSize);
        pbump( leftover );
    }

    // if the overflow char is not EOF put it in buffer
    if (traits_type::eq_int_type(ch, traits_type::eof()) ==  false)
    {
        *pptr() = traits_type::to_char_type(ch);
        pbump(1);
    }

    return traits_type::not_eof(ch);
}

StreamBuffer::int_type StreamBuffer::pbackfail(StreamBuffer::int_type)
{
    return traits_type::eof();
}

int StreamBuffer::sync()
{
    log_trace("sync");

    if (! _ioDevice)
        return 0;

    if (pptr())
    {
        while (pptr() > pbase())
        {
            const int_type ch = overflow( traits_type::eof() );
            if (ch == traits_type::eof())
            {
                return -1;
            }

            _ioDevice->sync();
        }
    }

    return 0;
}

std::streamsize StreamBuffer::xspeekn(char* buffer, std::streamsize size)
{
    if (traits_type::eof() == underflow())
        return 0;

    const std::streamsize avail = egptr() - gptr();
    size = std::min(avail, size);
    if (size == 0)
        return 0;

    std::memcpy(buffer, gptr(), sizeof(char) * size);
    return size;
}

StreamBuffer::pos_type
StreamBuffer::seekoff(off_type off, std::ios::seekdir dir, std::ios::openmode)
{
    pos_type ret =  pos_type( off_type(-1) );

    if (! _ioDevice || ! _ioDevice->enabled() ||
         ! _ioDevice->seekable() || off == 0)
    {
        return ret;
    }

    if (_ioDevice->writing())
    {
        endWrite();
    }

    if (_ioDevice->reading())
    {
        endRead();
    }

    ret = _ioDevice->seek(off, dir);

    // eliminate currently buffered sequence
    discard();

    return ret;
}

StreamBuffer::pos_type
StreamBuffer::seekpos(pos_type p, std::ios::openmode mode)
{
    return seekoff(p, std::ios::beg, mode);
}

}
<commit_msg>fix reading from streambuffer Under high load the streambuffer may reinitialize the write buffer wrongly. This happens when data was pending on overflow. The write buffer was initialized with the old size although some data was already written.<commit_after>/*
 * Copyright (C) 2004-2008 Marc Boris Duerner
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * As a special exception, you may use this file as part of a free
 * software library without restriction. Specifically, if other files
 * instantiate templates or use macros or inline functions from this
 * file, or you compile this file and link it with other files to
 * produce an executable, this file does not by itself cause the
 * resulting executable to be covered by the GNU General Public
 * License. This exception does not however invalidate any other
 * reasons why the executable file might be covered by the GNU Library
 * General Public License.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include "cxxtools/streambuffer.h"
#include <algorithm>
#include <stdexcept>
#include <cstring>
#include <cxxtools/log.h>

log_define("cxxtools.streambuffer")

namespace cxxtools {

StreamBuffer::StreamBuffer(IODevice& ioDevice, size_t bufferSize, bool extend)
: _ioDevice(&ioDevice),
  _ibufferSize(bufferSize+4),
  _ibuffer(0),
  _obufferSize(bufferSize),
  _obuffer(0),
  _pbmax(4),
  _oextend(extend)
{
    setg(0, 0, 0);
    setp(0, 0);

    attach(ioDevice);
}

StreamBuffer::StreamBuffer(size_t bufferSize, bool extend)
: _ioDevice(0),
  _ibufferSize(bufferSize+4),
  _ibuffer(0),
  _obufferSize(bufferSize),
  _obuffer(0),
  _pbmax(4),
  _oextend(extend)
{
    setg(0, 0, 0);
    setp(0, 0);
}

StreamBuffer::~StreamBuffer()
{
    delete[] _ibuffer;
    delete[] _obuffer;
}

void StreamBuffer::attach(IODevice& ioDevice)
{
    if (ioDevice.busy())
        throw IOPending("IODevice in use");

    if (_ioDevice)
    {
        if (_ioDevice->busy())
            throw IOPending("IODevice in use");

        disconnect(ioDevice.inputReady, *this, &StreamBuffer::onRead);
        disconnect(ioDevice.outputReady, *this, &StreamBuffer::onWrite);
    }

    _ioDevice = &ioDevice;
    connect(ioDevice.inputReady, *this, &StreamBuffer::onRead);
    connect(ioDevice.outputReady, *this, &StreamBuffer::onWrite);
}

void StreamBuffer::beginRead()
{
    if (_ioDevice == 0 || _ioDevice->reading())
        return;

    if (! _ibuffer)
    {
        _ibuffer = new char[_ibufferSize];
    }

    size_t putback = _pbmax;
    size_t leftover = 0;

    // keep chars for putback
    if (gptr())
    {
        putback = std::min<size_t>( gptr() - eback(), _pbmax);
        char* to = _ibuffer + _pbmax - putback;
        char* from = gptr() - putback;

        leftover = egptr() - gptr();
        std::memmove( to, from, putback + leftover );
    }

    size_t used = _pbmax + leftover;

    if (_ibufferSize == used)
        throw std::logic_error("StreamBuffer is full");

    _ioDevice->beginRead( _ibuffer + used, _ibufferSize - used );

    setg( _ibuffer + (_pbmax - putback), // start of get area
                _ibuffer + putback, // gptr position
                _ibuffer + used ); // end of get area
}

void StreamBuffer::onRead(IODevice& /*dev*/)
{
    inputReady.send(*this);
}

void StreamBuffer::endRead()
{
    size_t readSize = _ioDevice->endRead();

    setg(eback(), // start of get area
         gptr(),  // gptr position
         egptr() + readSize); // end of get area
}

StreamBuffer::int_type StreamBuffer::underflow()
{
    log_trace("underflow");

    if (!_ioDevice)
        return traits_type::eof();

    if (_ioDevice->reading())
        endRead();

    if (gptr() < egptr())
        return traits_type::to_int_type( *(gptr()) );

    if (_ioDevice->eof())
        return traits_type::eof();

    if (! _ibuffer)
    {
        _ibuffer = new char[_ibufferSize];
    }

    size_t putback = _pbmax;

    if (gptr())
    {
        putback = std::min<size_t>(gptr() - eback(), _pbmax);
        std::memmove( _ibuffer + (_pbmax - putback),
                      gptr() - putback,
                      putback );
    }

    size_t readSize = _ioDevice->read( _ibuffer + _pbmax, _ibufferSize - _pbmax );

    setg( _ibuffer + _pbmax - putback,    // start of get area
                _ibuffer + _pbmax,              // gptr position
                _ibuffer + _pbmax + readSize ); // end of get area

    if (_ioDevice->eof())
        return traits_type::eof();

    return traits_type::to_int_type( *(gptr()) );
}

size_t StreamBuffer::beginWrite()
{
    log_trace("beginWrite; out_avail=" << out_avail());

    if (_ioDevice == 0)
        return 0;

    if (_ioDevice->writing())
    {
        log_debug("device already in write mode - nothing done");
        return 0;
    }

    if (pptr())
    {
        size_t avail = pptr() - pbase();
        if (avail > 0)
        {
            return _ioDevice->beginWrite(_obuffer, avail);
        }
    }

    return 0;
}

void StreamBuffer::discard()
{
    if (_ioDevice && (_ioDevice->reading() || _ioDevice->writing()))
        throw IOPending("discard failed - streambuffer is in use");

    if (gptr())
        setg(_ibuffer, _ibuffer + _ibufferSize, _ibuffer + _ibufferSize);

    if (pptr())
        setp(_obuffer, _obuffer + _obufferSize);
}

void StreamBuffer::onWrite(IODevice& /*dev*/)
{
    outputReady.send(*this);
}

size_t StreamBuffer::endWrite()
{
    log_trace("endWrite; out_avail=" << out_avail());

    size_t leftover = 0;
    size_t written = 0;

    if (pptr())
    {
        size_t avail = pptr() - pbase();
        written = _ioDevice->endWrite();

        leftover = avail - written;

        log_debug(written << " bytes written; " << leftover << " left in buffer");

        if (leftover > 0)
        {
            traits_type::move(_obuffer, _obuffer + written, leftover);
        }
    }

    setp(_obuffer, _obuffer + _obufferSize);
    pbump( leftover );

    return written;
}

StreamBuffer::int_type StreamBuffer::overflow(int_type ch)
{
    log_trace("overflow(" << ch << ')');

    if (!_ioDevice)
        return traits_type::eof();

    if (!_obuffer)
    {
        _obuffer = new char[_obufferSize];
        setp(_obuffer, _obuffer + _obufferSize);
    }
    else if (_oextend && !traits_type::eq_int_type( ch, traits_type::eof() ))
    {
        // break asyncronous I/O if any active
        bool pendingWrite = _ioDevice->writing();
        bool pendingRead = false;

        if (pendingWrite)
        {
            if (_ioDevice->wavail())
            {
                log_debug("finish writing");
                endWrite();
            }
            else
            {
                pendingRead = _ioDevice->reading();
                log_debug("cancel asyncronous operations");
                _ioDevice->cancel();
            }
        }

        // `endWrite` may have freed some data in the buffer, in which case a
        // expand is not needed any more.

        if (epptr() == pptr())
        {
            // if the buffer area is extensible and overflow is not called by
            // sync/flush we copy the output buffer to a larger one
            size_t bufsize = _obufferSize + (_obufferSize/2);
            log_debug("extend buffer from " << _obufferSize << " to " << bufsize);
            char* buf = new char[ bufsize ];
            traits_type::copy(buf, _obuffer, _obufferSize);
            std::swap(_obuffer, buf);
            setp(_obuffer, _obuffer + bufsize);
            pbump( _obufferSize );
            _obufferSize = bufsize;
            delete [] buf;
        }

        // restart asyncronous I/O
        if (pendingWrite)
        {
            beginWrite();
            if (pendingRead)
                beginRead();
        }
    }
    else if (_ioDevice->writing()) // beginWrite is unfinished
    {
        log_debug("finish writing");
        endWrite();
    }
    else
    {
        // normal blocking overflow case
        log_debug("blocking overflow");
        size_t avail = pptr() - _obuffer;
        size_t written = _ioDevice->write(_obuffer, avail);
        size_t leftover = avail - written;

        if (leftover > 0)
        {
            traits_type::move(_obuffer, _obuffer + written, leftover);
        }

        setp(_obuffer, _obuffer + _obufferSize);
        pbump( leftover );
    }

    // if the overflow char is not EOF put it in buffer
    if (traits_type::eq_int_type(ch, traits_type::eof()) ==  false)
    {
        *pptr() = traits_type::to_char_type(ch);
        pbump(1);
    }

    return traits_type::not_eof(ch);
}

StreamBuffer::int_type StreamBuffer::pbackfail(StreamBuffer::int_type)
{
    return traits_type::eof();
}

int StreamBuffer::sync()
{
    log_trace("sync");

    if (! _ioDevice)
        return 0;

    if (pptr())
    {
        while (pptr() > pbase())
        {
            const int_type ch = overflow( traits_type::eof() );
            if (ch == traits_type::eof())
            {
                return -1;
            }

            _ioDevice->sync();
        }
    }

    return 0;
}

std::streamsize StreamBuffer::xspeekn(char* buffer, std::streamsize size)
{
    if (traits_type::eof() == underflow())
        return 0;

    const std::streamsize avail = egptr() - gptr();
    size = std::min(avail, size);
    if (size == 0)
        return 0;

    std::memcpy(buffer, gptr(), sizeof(char) * size);
    return size;
}

StreamBuffer::pos_type
StreamBuffer::seekoff(off_type off, std::ios::seekdir dir, std::ios::openmode)
{
    pos_type ret =  pos_type( off_type(-1) );

    if (! _ioDevice || ! _ioDevice->enabled() ||
         ! _ioDevice->seekable() || off == 0)
    {
        return ret;
    }

    if (_ioDevice->writing())
    {
        endWrite();
    }

    if (_ioDevice->reading())
    {
        endRead();
    }

    ret = _ioDevice->seek(off, dir);

    // eliminate currently buffered sequence
    discard();

    return ret;
}

StreamBuffer::pos_type
StreamBuffer::seekpos(pos_type p, std::ios::openmode mode)
{
    return seekoff(p, std::ios::beg, mode);
}

}
<|endoftext|>


<commit_before>/*
 * A queue that manages work for worker threads.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "thread_queue.hxx"
#include "thread_job.hxx"
#include "notify.hxx"

#include <glib.h>

#include <pthread.h>
#include <assert.h>

class ThreadQueue {
public:
    pthread_mutex_t mutex;
    pthread_cond_t cond;

    bool alive;

    /**
     * Was the #wakeup_event triggered?  This avoids duplicate events.
     */
    bool pending;

    struct list_head waiting, busy, done;

    Notify *notify;
};

static void
thread_queue_wakeup_callback(void *ctx)
{
    ThreadQueue *q = (ThreadQueue *)ctx;
    pthread_mutex_lock(&q->mutex);

    q->pending = false;

    while (!list_empty(&q->done)) {
        struct thread_job *job = (struct thread_job *)q->done.next;
        assert(job->state == THREAD_JOB_DONE);

        list_remove(&job->siblings);

        if (job->again) {
            /* schedule this job again */
            job->state = THREAD_JOB_WAITING;
            job->again = false;
            list_add(&job->siblings, &q->waiting);
            pthread_cond_signal(&q->cond);
        } else {
            job->state = THREAD_JOB_NULL;
            pthread_mutex_unlock(&q->mutex);
            job->done(job);
            pthread_mutex_lock(&q->mutex);
        }
    }

    pthread_mutex_unlock(&q->mutex);

    if (list_empty(&q->waiting) && list_empty(&q->busy) &&
        list_empty(&q->done))
        notify_disable(q->notify);
}

ThreadQueue *
thread_queue_new()
{
    auto q = new ThreadQueue();

    pthread_mutex_init(&q->mutex, nullptr);
    pthread_cond_init(&q->cond, nullptr);

    q->alive = true;
    q->pending = false;

    list_init(&q->waiting);
    list_init(&q->busy);
    list_init(&q->done);

    GError *error = nullptr;
    q->notify = notify_new(thread_queue_wakeup_callback, q, &error);
    if (q->notify == nullptr)
        g_printerr("%s\n", error->message);

    return q;
}

void
thread_queue_stop(ThreadQueue *q)
{
    pthread_mutex_lock(&q->mutex);
    q->alive = false;
    pthread_cond_broadcast(&q->cond);
    pthread_mutex_unlock(&q->mutex);
}

void
thread_queue_free(ThreadQueue *q)
{
    assert(!q->alive);

    notify_free(q->notify);

    pthread_mutex_destroy(&q->mutex);
    pthread_cond_destroy(&q->cond);
    delete q;
}

void
thread_queue_add(ThreadQueue *q, struct thread_job *job)
{
    pthread_mutex_lock(&q->mutex);
    assert(q->alive);

    if (job->state == THREAD_JOB_NULL) {
        job->state = THREAD_JOB_WAITING;
        job->again = false;
        list_add(&job->siblings, &q->waiting);
        pthread_cond_signal(&q->cond);
    } else if (job->state != THREAD_JOB_WAITING) {
        job->again = true;
    }

    pthread_mutex_unlock(&q->mutex);

    notify_enable(q->notify);
}

struct thread_job *
thread_queue_wait(ThreadQueue *q)
{
    pthread_mutex_lock(&q->mutex);

    while (q->alive && list_empty(&q->waiting))
        /* queue is empty, wait for a new job to be added */
        pthread_cond_wait(&q->cond, &q->mutex);

    struct thread_job *job = nullptr;
    if (q->alive && !list_empty(&q->waiting)) {
        job = (struct thread_job *)q->waiting.next;
        assert(job->state == THREAD_JOB_WAITING);

        job->state = THREAD_JOB_BUSY;
        list_remove(&job->siblings);
        list_add(&job->siblings, &q->busy);
    }

    pthread_mutex_unlock(&q->mutex);

    return job;
}

void
thread_queue_done(ThreadQueue *q, struct thread_job *job)
{
    assert(job->state == THREAD_JOB_BUSY);

    pthread_mutex_lock(&q->mutex);

    job->state = THREAD_JOB_DONE;
    list_remove(&job->siblings);
    list_add(&job->siblings, &q->done);

    q->pending = true;

    pthread_mutex_unlock(&q->mutex);

    notify_signal(q->notify);
}

bool
thread_queue_cancel(ThreadQueue *q, struct thread_job *job)
{
    pthread_mutex_lock(&q->mutex);

    bool result = false;
    switch (job->state) {
    case THREAD_JOB_NULL:
        /* already idle */
        result = true;
        break;

    case THREAD_JOB_WAITING:
        /* cancel it */
        list_remove(&job->siblings);
        job->state = THREAD_JOB_NULL;
        result = true;
        break;

    case THREAD_JOB_BUSY:
        /* no chance */
        break;

    case THREAD_JOB_DONE:
        /* TODO: the callback hasn't been invoked yet - do that now?
           anyway, with this pending state, we can't return success */
        break;
    }

    pthread_mutex_unlock(&q->mutex);
    return result;
}
<commit_msg>thread_queue: use fprintf() instead of g_printerr()<commit_after>/*
 * A queue that manages work for worker threads.
 *
 * author: Max Kellermann <mk@cm4all.com>
 */

#include "thread_queue.hxx"
#include "thread_job.hxx"
#include "notify.hxx"

#include <glib.h>

#include <pthread.h>
#include <assert.h>
#include <stdio.h>

class ThreadQueue {
public:
    pthread_mutex_t mutex;
    pthread_cond_t cond;

    bool alive;

    /**
     * Was the #wakeup_event triggered?  This avoids duplicate events.
     */
    bool pending;

    struct list_head waiting, busy, done;

    Notify *notify;
};

static void
thread_queue_wakeup_callback(void *ctx)
{
    ThreadQueue *q = (ThreadQueue *)ctx;
    pthread_mutex_lock(&q->mutex);

    q->pending = false;

    while (!list_empty(&q->done)) {
        struct thread_job *job = (struct thread_job *)q->done.next;
        assert(job->state == THREAD_JOB_DONE);

        list_remove(&job->siblings);

        if (job->again) {
            /* schedule this job again */
            job->state = THREAD_JOB_WAITING;
            job->again = false;
            list_add(&job->siblings, &q->waiting);
            pthread_cond_signal(&q->cond);
        } else {
            job->state = THREAD_JOB_NULL;
            pthread_mutex_unlock(&q->mutex);
            job->done(job);
            pthread_mutex_lock(&q->mutex);
        }
    }

    pthread_mutex_unlock(&q->mutex);

    if (list_empty(&q->waiting) && list_empty(&q->busy) &&
        list_empty(&q->done))
        notify_disable(q->notify);
}

ThreadQueue *
thread_queue_new()
{
    auto q = new ThreadQueue();

    pthread_mutex_init(&q->mutex, nullptr);
    pthread_cond_init(&q->cond, nullptr);

    q->alive = true;
    q->pending = false;

    list_init(&q->waiting);
    list_init(&q->busy);
    list_init(&q->done);

    GError *error = nullptr;
    q->notify = notify_new(thread_queue_wakeup_callback, q, &error);
    if (q->notify == nullptr)
        fprintf(stderr, "%s\n", error->message);

    return q;
}

void
thread_queue_stop(ThreadQueue *q)
{
    pthread_mutex_lock(&q->mutex);
    q->alive = false;
    pthread_cond_broadcast(&q->cond);
    pthread_mutex_unlock(&q->mutex);
}

void
thread_queue_free(ThreadQueue *q)
{
    assert(!q->alive);

    notify_free(q->notify);

    pthread_mutex_destroy(&q->mutex);
    pthread_cond_destroy(&q->cond);
    delete q;
}

void
thread_queue_add(ThreadQueue *q, struct thread_job *job)
{
    pthread_mutex_lock(&q->mutex);
    assert(q->alive);

    if (job->state == THREAD_JOB_NULL) {
        job->state = THREAD_JOB_WAITING;
        job->again = false;
        list_add(&job->siblings, &q->waiting);
        pthread_cond_signal(&q->cond);
    } else if (job->state != THREAD_JOB_WAITING) {
        job->again = true;
    }

    pthread_mutex_unlock(&q->mutex);

    notify_enable(q->notify);
}

struct thread_job *
thread_queue_wait(ThreadQueue *q)
{
    pthread_mutex_lock(&q->mutex);

    while (q->alive && list_empty(&q->waiting))
        /* queue is empty, wait for a new job to be added */
        pthread_cond_wait(&q->cond, &q->mutex);

    struct thread_job *job = nullptr;
    if (q->alive && !list_empty(&q->waiting)) {
        job = (struct thread_job *)q->waiting.next;
        assert(job->state == THREAD_JOB_WAITING);

        job->state = THREAD_JOB_BUSY;
        list_remove(&job->siblings);
        list_add(&job->siblings, &q->busy);
    }

    pthread_mutex_unlock(&q->mutex);

    return job;
}

void
thread_queue_done(ThreadQueue *q, struct thread_job *job)
{
    assert(job->state == THREAD_JOB_BUSY);

    pthread_mutex_lock(&q->mutex);

    job->state = THREAD_JOB_DONE;
    list_remove(&job->siblings);
    list_add(&job->siblings, &q->done);

    q->pending = true;

    pthread_mutex_unlock(&q->mutex);

    notify_signal(q->notify);
}

bool
thread_queue_cancel(ThreadQueue *q, struct thread_job *job)
{
    pthread_mutex_lock(&q->mutex);

    bool result = false;
    switch (job->state) {
    case THREAD_JOB_NULL:
        /* already idle */
        result = true;
        break;

    case THREAD_JOB_WAITING:
        /* cancel it */
        list_remove(&job->siblings);
        job->state = THREAD_JOB_NULL;
        result = true;
        break;

    case THREAD_JOB_BUSY:
        /* no chance */
        break;

    case THREAD_JOB_DONE:
        /* TODO: the callback hasn't been invoked yet - do that now?
           anyway, with this pending state, we can't return success */
        break;
    }

    pthread_mutex_unlock(&q->mutex);
    return result;
}
<|endoftext|>


<commit_before>#include <math.h>
#include <stdio.h>
#include <string.h>
#include <algorithm>
#include "timeEstimate.h"
#include "settings/settings.h"

namespace cura
{

#define MINIMUM_PLANNER_SPEED 0.05// (mm/sec)

void TimeEstimateCalculator::setFirmwareDefaults(const SettingsBaseVirtual* settings_base)
{
    max_feedrate[X_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_x");
    max_feedrate[Y_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_y");
    max_feedrate[Z_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_z");
    max_feedrate[E_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_e");
    max_acceleration[X_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_x");
    max_acceleration[Y_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_y");
    max_acceleration[Z_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_z");
    max_acceleration[E_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_e");
    max_xy_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_xy");
    max_z_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_z");
    max_e_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_e");
    minimumfeedrate = settings_base->getSettingInMillimetersPerSecond("machine_minimum_feedrate");
    acceleration = settings_base->getSettingInMillimetersPerSecond("machine_acceleration");
}

template<typename T> const T square(const T& a) { return a * a; }

void TimeEstimateCalculator::setPosition(Position newPos)
{
    currentPosition = newPos;
}

void TimeEstimateCalculator::addTime(double time)
{
    extra_time += time;
}

void TimeEstimateCalculator::setAcceleration(double acc)
{
    acceleration = acc;
}

void TimeEstimateCalculator::setMaxXyJerk(double jerk)
{
    max_xy_jerk = jerk;
}

void TimeEstimateCalculator::reset()
{
    extra_time = 0.0;
    blocks.clear();
}

// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
static inline double max_allowable_speed(double acceleration, double target_velocity, double distance)
{
  return sqrt(target_velocity*target_velocity-2*acceleration*distance);
}

// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the given acceleration:
static inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
{
    if (acceleration == 0)
        return 0.0;
    return (square(target_rate)-square(initial_rate)) / (2.0*acceleration);
}

// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
static inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance)
{
    if (acceleration == 0.0)
        return 0.0;
    return (2.0*acceleration*distance-square(initial_rate)+square(final_rate)) / (4.0*acceleration);
}

// This function gives the time it needs to accelerate from an initial speed to reach a final distance.
static inline double acceleration_time_from_distance(double initial_feedrate, double distance, double acceleration)
{
    double discriminant = square(initial_feedrate) - 2 * acceleration * -distance;
    //If discriminant is negative, we're moving in the wrong direction.
    //Making the discriminant 0 then gives the extremum of the parabola instead of the intersection.
    discriminant = std::max(0.0, discriminant);
    return (-initial_feedrate + sqrt(discriminant)) / acceleration;
}

// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
void TimeEstimateCalculator::calculate_trapezoid_for_block(Block *block, double entry_factor, double exit_factor)
{
    double initial_feedrate = block->nominal_feedrate*entry_factor;
    double final_feedrate = block->nominal_feedrate*exit_factor;

    double acceleration = block->acceleration;
    double accelerate_distance = estimate_acceleration_distance(initial_feedrate, block->nominal_feedrate, acceleration);
    double decelerate_distance = estimate_acceleration_distance(block->nominal_feedrate, final_feedrate, -acceleration);

    // Calculate the size of Plateau of Nominal Rate.
    double plateau_distance = block->distance-accelerate_distance - decelerate_distance;

    // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
    // have to use intersection_distance() to calculate when to abort acceleration and start braking
    // in order to reach the final_rate exactly at the end of this block.
    if (plateau_distance < 0)
    {
        accelerate_distance = intersection_distance(initial_feedrate, final_feedrate, acceleration, block->distance);
        accelerate_distance = std::max(accelerate_distance, 0.0); // Check limits due to numerical round-off
        accelerate_distance = std::min(accelerate_distance, block->distance);//(We can cast here to unsigned, because the above line ensures that we are above zero)
        plateau_distance = 0;
    }

    block->accelerate_until = accelerate_distance;
    block->decelerate_after = accelerate_distance+plateau_distance;
    block->initial_feedrate = initial_feedrate;
    block->final_feedrate = final_feedrate;
}

void TimeEstimateCalculator::plan(Position newPos, double feedrate)
{
    Block block;
    memset(&block, 0, sizeof(block));

    block.maxTravel = 0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        block.delta[n] = newPos[n] - currentPosition[n];
        block.absDelta[n] = fabs(block.delta[n]);
        block.maxTravel = std::max(block.maxTravel, block.absDelta[n]);
    }
    if (block.maxTravel <= 0)
        return;
    if (feedrate < minimumfeedrate)
        feedrate = minimumfeedrate;
    block.distance = sqrtf(square(block.absDelta[0]) + square(block.absDelta[1]) + square(block.absDelta[2]));
    if (block.distance == 0.0)
        block.distance = block.absDelta[3];
    block.nominal_feedrate = feedrate;

    Position current_feedrate;
    Position current_abs_feedrate;
    double feedrate_factor = 1.0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        current_feedrate[n] = block.delta[n] * feedrate / block.distance;
        current_abs_feedrate[n] = fabs(current_feedrate[n]);
        if (current_abs_feedrate[n] > max_feedrate[n])
            feedrate_factor = std::min(feedrate_factor, max_feedrate[n] / current_abs_feedrate[n]);
    }
    //TODO: XY_FREQUENCY_LIMIT

    if(feedrate_factor < 1.0)
    {
        for(unsigned int n=0; n<NUM_AXIS; n++)
        {
            current_feedrate[n] *= feedrate_factor;
            current_abs_feedrate[n] *= feedrate_factor;
        }
        block.nominal_feedrate *= feedrate_factor;
    }

    block.acceleration = acceleration;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        if (block.acceleration * (block.absDelta[n] / block.distance) > max_acceleration[n])
            block.acceleration = max_acceleration[n];
    }

    double vmax_junction = max_xy_jerk/2;
    double vmax_junction_factor = 1.0;
    if(current_abs_feedrate[Z_AXIS] > max_z_jerk/2)
        vmax_junction = std::min(vmax_junction, max_z_jerk/2);
    if(current_abs_feedrate[E_AXIS] > max_e_jerk/2)
        vmax_junction = std::min(vmax_junction, max_e_jerk/2);
    vmax_junction = std::min(vmax_junction, block.nominal_feedrate);
    double safe_speed = vmax_junction;

    if ((blocks.size() > 0) && (previous_nominal_feedrate > 0.0001))
    {
        double xy_jerk = sqrt(square(current_feedrate[X_AXIS]-previous_feedrate[X_AXIS])+square(current_feedrate[Y_AXIS]-previous_feedrate[Y_AXIS]));
        vmax_junction = block.nominal_feedrate;
        if (xy_jerk > max_xy_jerk) {
            vmax_junction_factor = (max_xy_jerk/xy_jerk);
        }
        if(fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS]) > max_z_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_z_jerk/fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS])));
        }
        if(fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS]) > max_e_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_e_jerk/fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS])));
        }
        vmax_junction = std::min(previous_nominal_feedrate, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }

    block.max_entry_speed = vmax_junction;

    double v_allowable = max_allowable_speed(-block.acceleration, MINIMUM_PLANNER_SPEED, block.distance);
    block.entry_speed = std::min(vmax_junction, v_allowable);
    block.nominal_length_flag = block.nominal_feedrate <= v_allowable;
    block.recalculate_flag = true; // Always calculate trapezoid for new block

    previous_feedrate = current_feedrate;
    previous_nominal_feedrate = block.nominal_feedrate;

    currentPosition = newPos;

    calculate_trapezoid_for_block(&block, block.entry_speed/block.nominal_feedrate, safe_speed/block.nominal_feedrate);

    blocks.push_back(block);
}

double TimeEstimateCalculator::calculate()
{
    reverse_pass();
    forward_pass();
    recalculate_trapezoids();

    double totalTime = extra_time;
    for(unsigned int n=0; n<blocks.size(); n++)
    {
        Block& block = blocks[n];
        double plateau_distance = block.decelerate_after - block.accelerate_until;

        totalTime += acceleration_time_from_distance(block.initial_feedrate, block.accelerate_until, block.acceleration);
        totalTime += plateau_distance / block.nominal_feedrate;
        totalTime += acceleration_time_from_distance(block.final_feedrate, (block.distance - block.decelerate_after), block.acceleration);
    }
    return totalTime;
}

// The kernel called by accelerationPlanner::calculate() when scanning the plan from last to first entry.
void TimeEstimateCalculator::planner_reverse_pass_kernel(Block *previous, Block *current, Block *next)
{
    (void)previous;
    if(!current || !next)
        return;

    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
    if (current->entry_speed != current->max_entry_speed)
    {
        // If nominal length true, max junction speed is guaranteed to be reached. Only compute
        // for max allowable speed if block is decelerating and nominal length is false.
        if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed))
        {
            current->entry_speed = std::min(current->max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->distance));
        } else {
            current->entry_speed = current->max_entry_speed;
        }
        current->recalculate_flag = true;
    }
}

void TimeEstimateCalculator::reverse_pass()
{
    Block* block[3] = {nullptr, nullptr, nullptr};
    for(unsigned int n=blocks.size()-1; int(n)>=0; n--)
    {
        block[2]= block[1];
        block[1]= block[0];
        block[0] = &blocks[n];
        planner_reverse_pass_kernel(block[0], block[1], block[2]);
    }
}

// The kernel called by accelerationPlanner::calculate() when scanning the plan from first to last entry.
void TimeEstimateCalculator::planner_forward_pass_kernel(Block *previous, Block *current, Block *next)
{
    (void)next;
    if(!previous)
        return;

    // If the previous block is an acceleration block, but it is not long enough to complete the
    // full speed change within the block, we need to adjust the entry speed accordingly. Entry
    // speeds have already been reset, maximized, and reverse planned by reverse planner.
    // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
    if (!previous->nominal_length_flag)
    {
        if (previous->entry_speed < current->entry_speed)
        {
            double entry_speed = std::min(current->entry_speed, max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->distance) );

            // Check for junction speed change
            if (current->entry_speed != entry_speed)
            {
                current->entry_speed = entry_speed;
                current->recalculate_flag = true;
            }
        }
    }
}

void TimeEstimateCalculator::forward_pass()
{
    Block* block[3] = {nullptr, nullptr, nullptr};
    for(unsigned int n=0; n<blocks.size(); n++)
    {
        block[0]= block[1];
        block[1]= block[2];
        block[2] = &blocks[n];
        planner_forward_pass_kernel(block[0], block[1], block[2]);
    }
    planner_forward_pass_kernel(block[1], block[2], nullptr);
}

// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
// entry_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
void TimeEstimateCalculator::recalculate_trapezoids()
{
    Block *current;
    Block *next = nullptr;

    for(unsigned int n=0; n<blocks.size(); n++)
    {
        current = next;
        next = &blocks[n];
        if (current)
        {
            // Recalculate if current block entry or exit junction speed has changed.
            if (current->recalculate_flag || next->recalculate_flag)
            {
                // NOTE: Entry and exit factors always > 0 by all previous logic operations.
                calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_feedrate, next->entry_speed/current->nominal_feedrate);
                current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
            }
        }
    }
    // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
    if(next != nullptr)
    {
        calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_feedrate, MINIMUM_PLANNER_SPEED/next->nominal_feedrate);
        next->recalculate_flag = false;
    }
}

}//namespace cura<commit_msg>replace square function by math::square (CURA-1013)<commit_after>#include <math.h>
#include <stdio.h>
#include <string.h>
#include <algorithm>

#include "utils/math.h"
#include "timeEstimate.h"
#include "settings/settings.h"

namespace cura
{

#define MINIMUM_PLANNER_SPEED 0.05// (mm/sec)

void TimeEstimateCalculator::setFirmwareDefaults(const SettingsBaseVirtual* settings_base)
{
    max_feedrate[X_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_x");
    max_feedrate[Y_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_y");
    max_feedrate[Z_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_z");
    max_feedrate[E_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_feedrate_e");
    max_acceleration[X_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_x");
    max_acceleration[Y_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_y");
    max_acceleration[Z_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_z");
    max_acceleration[E_AXIS] = settings_base->getSettingInMillimetersPerSecond("machine_max_acceleration_e");
    max_xy_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_xy");
    max_z_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_z");
    max_e_jerk = settings_base->getSettingInMillimetersPerSecond("machine_max_jerk_e");
    minimumfeedrate = settings_base->getSettingInMillimetersPerSecond("machine_minimum_feedrate");
    acceleration = settings_base->getSettingInMillimetersPerSecond("machine_acceleration");
}

void TimeEstimateCalculator::setPosition(Position newPos)
{
    currentPosition = newPos;
}

void TimeEstimateCalculator::addTime(double time)
{
    extra_time += time;
}

void TimeEstimateCalculator::setAcceleration(double acc)
{
    acceleration = acc;
}

void TimeEstimateCalculator::setMaxXyJerk(double jerk)
{
    max_xy_jerk = jerk;
}

void TimeEstimateCalculator::reset()
{
    extra_time = 0.0;
    blocks.clear();
}

// Calculates the maximum allowable speed at this point when you must be able to reach target_velocity using the
// acceleration within the allotted distance.
static inline double max_allowable_speed(double acceleration, double target_velocity, double distance)
{
  return sqrt(target_velocity*target_velocity-2*acceleration*distance);
}

// Calculates the distance (not time) it takes to accelerate from initial_rate to target_rate using the given acceleration:
static inline float estimate_acceleration_distance(float initial_rate, float target_rate, float acceleration)
{
    if (acceleration == 0)
        return 0.0;
    return (square(target_rate)-square(initial_rate)) / (2.0*acceleration);
}

// This function gives you the point at which you must start braking (at the rate of -acceleration) if
// you started at speed initial_rate and accelerated until this point and want to end at the final_rate after
// a total travel of distance. This can be used to compute the intersection point between acceleration and
// deceleration in the cases where the trapezoid has no plateau (i.e. never reaches maximum speed)
static inline double intersection_distance(double initial_rate, double final_rate, double acceleration, double distance)
{
    if (acceleration == 0.0)
        return 0.0;
    return (2.0*acceleration*distance-square(initial_rate)+square(final_rate)) / (4.0*acceleration);
}

// This function gives the time it needs to accelerate from an initial speed to reach a final distance.
static inline double acceleration_time_from_distance(double initial_feedrate, double distance, double acceleration)
{
    double discriminant = square(initial_feedrate) - 2 * acceleration * -distance;
    //If discriminant is negative, we're moving in the wrong direction.
    //Making the discriminant 0 then gives the extremum of the parabola instead of the intersection.
    discriminant = std::max(0.0, discriminant);
    return (-initial_feedrate + sqrt(discriminant)) / acceleration;
}

// Calculates trapezoid parameters so that the entry- and exit-speed is compensated by the provided factors.
void TimeEstimateCalculator::calculate_trapezoid_for_block(Block *block, double entry_factor, double exit_factor)
{
    double initial_feedrate = block->nominal_feedrate*entry_factor;
    double final_feedrate = block->nominal_feedrate*exit_factor;

    double acceleration = block->acceleration;
    double accelerate_distance = estimate_acceleration_distance(initial_feedrate, block->nominal_feedrate, acceleration);
    double decelerate_distance = estimate_acceleration_distance(block->nominal_feedrate, final_feedrate, -acceleration);

    // Calculate the size of Plateau of Nominal Rate.
    double plateau_distance = block->distance-accelerate_distance - decelerate_distance;

    // Is the Plateau of Nominal Rate smaller than nothing? That means no cruising, and we will
    // have to use intersection_distance() to calculate when to abort acceleration and start braking
    // in order to reach the final_rate exactly at the end of this block.
    if (plateau_distance < 0)
    {
        accelerate_distance = intersection_distance(initial_feedrate, final_feedrate, acceleration, block->distance);
        accelerate_distance = std::max(accelerate_distance, 0.0); // Check limits due to numerical round-off
        accelerate_distance = std::min(accelerate_distance, block->distance);//(We can cast here to unsigned, because the above line ensures that we are above zero)
        plateau_distance = 0;
    }

    block->accelerate_until = accelerate_distance;
    block->decelerate_after = accelerate_distance+plateau_distance;
    block->initial_feedrate = initial_feedrate;
    block->final_feedrate = final_feedrate;
}

void TimeEstimateCalculator::plan(Position newPos, double feedrate)
{
    Block block;
    memset(&block, 0, sizeof(block));

    block.maxTravel = 0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        block.delta[n] = newPos[n] - currentPosition[n];
        block.absDelta[n] = fabs(block.delta[n]);
        block.maxTravel = std::max(block.maxTravel, block.absDelta[n]);
    }
    if (block.maxTravel <= 0)
        return;
    if (feedrate < minimumfeedrate)
        feedrate = minimumfeedrate;
    block.distance = sqrtf(square(block.absDelta[0]) + square(block.absDelta[1]) + square(block.absDelta[2]));
    if (block.distance == 0.0)
        block.distance = block.absDelta[3];
    block.nominal_feedrate = feedrate;

    Position current_feedrate;
    Position current_abs_feedrate;
    double feedrate_factor = 1.0;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        current_feedrate[n] = block.delta[n] * feedrate / block.distance;
        current_abs_feedrate[n] = fabs(current_feedrate[n]);
        if (current_abs_feedrate[n] > max_feedrate[n])
            feedrate_factor = std::min(feedrate_factor, max_feedrate[n] / current_abs_feedrate[n]);
    }
    //TODO: XY_FREQUENCY_LIMIT

    if(feedrate_factor < 1.0)
    {
        for(unsigned int n=0; n<NUM_AXIS; n++)
        {
            current_feedrate[n] *= feedrate_factor;
            current_abs_feedrate[n] *= feedrate_factor;
        }
        block.nominal_feedrate *= feedrate_factor;
    }

    block.acceleration = acceleration;
    for(unsigned int n=0; n<NUM_AXIS; n++)
    {
        if (block.acceleration * (block.absDelta[n] / block.distance) > max_acceleration[n])
            block.acceleration = max_acceleration[n];
    }

    double vmax_junction = max_xy_jerk/2;
    double vmax_junction_factor = 1.0;
    if(current_abs_feedrate[Z_AXIS] > max_z_jerk/2)
        vmax_junction = std::min(vmax_junction, max_z_jerk/2);
    if(current_abs_feedrate[E_AXIS] > max_e_jerk/2)
        vmax_junction = std::min(vmax_junction, max_e_jerk/2);
    vmax_junction = std::min(vmax_junction, block.nominal_feedrate);
    double safe_speed = vmax_junction;

    if ((blocks.size() > 0) && (previous_nominal_feedrate > 0.0001))
    {
        double xy_jerk = sqrt(square(current_feedrate[X_AXIS]-previous_feedrate[X_AXIS])+square(current_feedrate[Y_AXIS]-previous_feedrate[Y_AXIS]));
        vmax_junction = block.nominal_feedrate;
        if (xy_jerk > max_xy_jerk) {
            vmax_junction_factor = (max_xy_jerk/xy_jerk);
        }
        if(fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS]) > max_z_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_z_jerk/fabs(current_feedrate[Z_AXIS] - previous_feedrate[Z_AXIS])));
        }
        if(fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS]) > max_e_jerk) {
            vmax_junction_factor = std::min(vmax_junction_factor, (max_e_jerk/fabs(current_feedrate[E_AXIS] - previous_feedrate[E_AXIS])));
        }
        vmax_junction = std::min(previous_nominal_feedrate, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
    }

    block.max_entry_speed = vmax_junction;

    double v_allowable = max_allowable_speed(-block.acceleration, MINIMUM_PLANNER_SPEED, block.distance);
    block.entry_speed = std::min(vmax_junction, v_allowable);
    block.nominal_length_flag = block.nominal_feedrate <= v_allowable;
    block.recalculate_flag = true; // Always calculate trapezoid for new block

    previous_feedrate = current_feedrate;
    previous_nominal_feedrate = block.nominal_feedrate;

    currentPosition = newPos;

    calculate_trapezoid_for_block(&block, block.entry_speed/block.nominal_feedrate, safe_speed/block.nominal_feedrate);

    blocks.push_back(block);
}

double TimeEstimateCalculator::calculate()
{
    reverse_pass();
    forward_pass();
    recalculate_trapezoids();

    double totalTime = extra_time;
    for(unsigned int n=0; n<blocks.size(); n++)
    {
        Block& block = blocks[n];
        double plateau_distance = block.decelerate_after - block.accelerate_until;

        totalTime += acceleration_time_from_distance(block.initial_feedrate, block.accelerate_until, block.acceleration);
        totalTime += plateau_distance / block.nominal_feedrate;
        totalTime += acceleration_time_from_distance(block.final_feedrate, (block.distance - block.decelerate_after), block.acceleration);
    }
    return totalTime;
}

// The kernel called by accelerationPlanner::calculate() when scanning the plan from last to first entry.
void TimeEstimateCalculator::planner_reverse_pass_kernel(Block *previous, Block *current, Block *next)
{
    (void)previous;
    if(!current || !next)
        return;

    // If entry speed is already at the maximum entry speed, no need to recheck. Block is cruising.
    // If not, block in state of acceleration or deceleration. Reset entry speed to maximum and
    // check for maximum allowable speed reductions to ensure maximum possible planned speed.
    if (current->entry_speed != current->max_entry_speed)
    {
        // If nominal length true, max junction speed is guaranteed to be reached. Only compute
        // for max allowable speed if block is decelerating and nominal length is false.
        if ((!current->nominal_length_flag) && (current->max_entry_speed > next->entry_speed))
        {
            current->entry_speed = std::min(current->max_entry_speed, max_allowable_speed(-current->acceleration, next->entry_speed, current->distance));
        } else {
            current->entry_speed = current->max_entry_speed;
        }
        current->recalculate_flag = true;
    }
}

void TimeEstimateCalculator::reverse_pass()
{
    Block* block[3] = {nullptr, nullptr, nullptr};
    for(unsigned int n=blocks.size()-1; int(n)>=0; n--)
    {
        block[2]= block[1];
        block[1]= block[0];
        block[0] = &blocks[n];
        planner_reverse_pass_kernel(block[0], block[1], block[2]);
    }
}

// The kernel called by accelerationPlanner::calculate() when scanning the plan from first to last entry.
void TimeEstimateCalculator::planner_forward_pass_kernel(Block *previous, Block *current, Block *next)
{
    (void)next;
    if(!previous)
        return;

    // If the previous block is an acceleration block, but it is not long enough to complete the
    // full speed change within the block, we need to adjust the entry speed accordingly. Entry
    // speeds have already been reset, maximized, and reverse planned by reverse planner.
    // If nominal length is true, max junction speed is guaranteed to be reached. No need to recheck.
    if (!previous->nominal_length_flag)
    {
        if (previous->entry_speed < current->entry_speed)
        {
            double entry_speed = std::min(current->entry_speed, max_allowable_speed(-previous->acceleration,previous->entry_speed,previous->distance) );

            // Check for junction speed change
            if (current->entry_speed != entry_speed)
            {
                current->entry_speed = entry_speed;
                current->recalculate_flag = true;
            }
        }
    }
}

void TimeEstimateCalculator::forward_pass()
{
    Block* block[3] = {nullptr, nullptr, nullptr};
    for(unsigned int n=0; n<blocks.size(); n++)
    {
        block[0]= block[1];
        block[1]= block[2];
        block[2] = &blocks[n];
        planner_forward_pass_kernel(block[0], block[1], block[2]);
    }
    planner_forward_pass_kernel(block[1], block[2], nullptr);
}

// Recalculates the trapezoid speed profiles for all blocks in the plan according to the
// entry_factor for each junction. Must be called by planner_recalculate() after
// updating the blocks.
void TimeEstimateCalculator::recalculate_trapezoids()
{
    Block *current;
    Block *next = nullptr;

    for(unsigned int n=0; n<blocks.size(); n++)
    {
        current = next;
        next = &blocks[n];
        if (current)
        {
            // Recalculate if current block entry or exit junction speed has changed.
            if (current->recalculate_flag || next->recalculate_flag)
            {
                // NOTE: Entry and exit factors always > 0 by all previous logic operations.
                calculate_trapezoid_for_block(current, current->entry_speed/current->nominal_feedrate, next->entry_speed/current->nominal_feedrate);
                current->recalculate_flag = false; // Reset current only to ensure next trapezoid is computed
            }
        }
    }
    // Last/newest block in buffer. Exit speed is set with MINIMUM_PLANNER_SPEED. Always recalculated.
    if(next != nullptr)
    {
        calculate_trapezoid_for_block(next, next->entry_speed/next->nominal_feedrate, MINIMUM_PLANNER_SPEED/next->nominal_feedrate);
        next->recalculate_flag = false;
    }
}

}//namespace cura<|endoftext|>


<commit_before>/*
* TLS Record Reading
* (C) 2004-2010 Jack Lloyd
*
* Released under the terms of the Botan license
*/

#include <botan/tls_record.h>
#include <botan/lookup.h>
#include <botan/loadstor.h>

namespace Botan {

/*
* Reset the state
*/
void Record_Reader::reset()
   {
   cipher.reset();

   delete mac;
   mac = 0;

   mac_size = 0;
   block_size = 0;
   iv_size = 0;
   major = minor = 0;
   seq_no = 0;
   }

/*
* Set the version to use
*/
void Record_Reader::set_version(Version_Code version)
   {
   if(version != SSL_V3 && version != TLS_V10 && version != TLS_V11)
      throw Invalid_Argument("Record_Reader: Invalid protocol version");

   major = (version >> 8) & 0xFF;
   minor = (version & 0xFF);
   }

/*
* Set the keys for reading
*/
void Record_Reader::set_keys(const CipherSuite& suite, const SessionKeys& keys,
                             Connection_Side side)
   {
   cipher.reset();
   delete mac;
   mac = 0;

   SymmetricKey mac_key, cipher_key;
   InitializationVector iv;

   if(side == CLIENT)
      {
      cipher_key = keys.server_cipher_key();
      iv = keys.server_iv();
      mac_key = keys.server_mac_key();
      }
   else
      {
      cipher_key = keys.client_cipher_key();
      iv = keys.client_iv();
      mac_key = keys.client_mac_key();
      }

   const std::string cipher_algo = suite.cipher_algo();
   const std::string mac_algo = suite.mac_algo();

   if(have_block_cipher(cipher_algo))
      {
      cipher.append(get_cipher(
                       cipher_algo + "/CBC/NoPadding",
                       cipher_key, iv, DECRYPTION)
         );
      block_size = block_size_of(cipher_algo);

      if(major > 3 || (major == 3 && minor >= 2))
         iv_size = block_size;
      else
         iv_size = 0;
      }
   else if(have_stream_cipher(cipher_algo))
      {
      cipher.append(get_cipher(cipher_algo, cipher_key, DECRYPTION));
      block_size = 0;
      iv_size = 0;
      }
   else
      throw Invalid_Argument("Record_Reader: Unknown cipher " + cipher_algo);

   if(have_hash(mac_algo))
      {
      Algorithm_Factory& af = global_state().algorithm_factory();

      if(major == 3 && minor == 0)
         mac = af.make_mac("SSL3-MAC(" + mac_algo + ")");
      else
         mac = af.make_mac("HMAC(" + mac_algo + ")");

      mac->set_key(mac_key);
      mac_size = mac->output_length();
      }
   else
      throw Invalid_Argument("Record_Reader: Unknown hash " + mac_algo);
   }

void Record_Reader::add_input(const byte input[], size_t input_size)
   {
   input_queue.write(input, input_size);
   }

/*
* Retrieve the next record
*/
size_t Record_Reader::get_record(byte& msg_type,
                                 MemoryRegion<byte>& output)
   {
   byte header[5] = { 0 };

   const size_t have_in_queue = input_queue.size();

   if(have_in_queue < sizeof(header))
      return (sizeof(header) - have_in_queue);

   /*
   * We peek first to make sure we have the full record
   */
   input_queue.peek(header, sizeof(header));

   // SSLv2-format client hello?
   if(header[0] & 0x80 && header[2] == 1 && header[3] == 3)
      {
      size_t record_len = make_u16bit(header[0], header[1]) & 0x7FFF;

      if(have_in_queue < record_len + 2)
         return (record_len + 2 - have_in_queue);

      msg_type = HANDSHAKE;
      output.resize(record_len + 4);

      input_queue.read(&output[2], record_len + 2);
      output[0] = CLIENT_HELLO_SSLV2;
      output[1] = 0;
      output[2] = header[0] & 0x7F;
      output[3] = header[1];

      return 0;
      }

   if(header[0] != CHANGE_CIPHER_SPEC &&
      header[0] != ALERT &&
      header[0] != HANDSHAKE &&
      header[0] != APPLICATION_DATA)
      {
      throw TLS_Exception(UNEXPECTED_MESSAGE,
                          "Record_Reader: Unknown record type");
      }

   const u16bit version    = make_u16bit(header[1], header[2]);
   const u16bit record_len = make_u16bit(header[3], header[4]);

   if(major && (header[1] != major || header[2] != minor))
      throw TLS_Exception(PROTOCOL_VERSION,
                          "Record_Reader: Got unexpected version");

   // If insufficient data, return without doing anything
   if(have_in_queue < (sizeof(header) + record_len))
      return (sizeof(header) + record_len - have_in_queue);

   SecureVector<byte> buffer(record_len);

   input_queue.read(header, sizeof(header)); // pull off the header
   input_queue.read(&buffer[0], buffer.size());

   /*
   * We are handshaking, no crypto to do so return as-is
   * TODO: Check msg_type to confirm a handshake?
   */
   if(mac_size == 0)
      {
      msg_type = header[0];
      output = buffer;
      return 0; // got a full record
      }

   // Otherwise, decrypt, check MAC, return plaintext

   cipher.process_msg(buffer);
   SecureVector<byte> plaintext = cipher.read_all(Pipe::LAST_MESSAGE);

   size_t pad_size = 0;

   if(block_size)
      {
      byte pad_value = plaintext[plaintext.size()-1];
      pad_size = pad_value + 1;

      /*
      * Check the padding; if it is wrong, then say we have 0 bytes of
      * padding, which should ensure that the MAC check below does not
      * suceed. This hides a timing channel.
      *
      * This particular countermeasure is recommended in the TLS 1.2
      * spec (RFC 5246) in section 6.2.3.2
      */
      if(version == SSL_V3)
         {
         if(pad_value > block_size)
            pad_size = 0;
         }
      else
         {
         for(size_t i = 0; i != pad_size; ++i)
            if(plaintext[plaintext.size()-i-1] != pad_value)
               pad_size = 0;
         }
      }

   if(plaintext.size() < mac_size + pad_size + iv_size)
      throw Decoding_Error("Record_Reader: Record truncated");

   const size_t mac_offset = plaintext.size() - (mac_size + pad_size);
   SecureVector<byte> received_mac(&plaintext[mac_offset],
                                   mac_size);

   const u16bit plain_length = plaintext.size() - (mac_size + pad_size + iv_size);

   mac->update_be(seq_no);
   mac->update(header[0]); // msg_type

   if(version != SSL_V3)
      for(size_t i = 0; i != 2; ++i)
         mac->update(get_byte(i, version));

   mac->update_be(plain_length);
   mac->update(&plaintext[iv_size], plain_length);

   ++seq_no;

   SecureVector<byte> computed_mac = mac->final();

   if(received_mac != computed_mac)
      throw TLS_Exception(BAD_RECORD_MAC, "Record_Reader: MAC failure");

   msg_type = header[0];

   output.resize(plain_length);
   copy_mem(&output[0], &plaintext[iv_size], plain_length);
   return 0;
   }

}
<commit_msg>Fix crash<commit_after>/*
* TLS Record Reading
* (C) 2004-2010 Jack Lloyd
*
* Released under the terms of the Botan license
*/

#include <botan/tls_record.h>
#include <botan/lookup.h>
#include <botan/loadstor.h>

namespace Botan {

/*
* Reset the state
*/
void Record_Reader::reset()
   {
   cipher.reset();

   delete mac;
   mac = 0;

   mac_size = 0;
   block_size = 0;
   iv_size = 0;
   major = minor = 0;
   seq_no = 0;
   }

/*
* Set the version to use
*/
void Record_Reader::set_version(Version_Code version)
   {
   if(version != SSL_V3 && version != TLS_V10 && version != TLS_V11)
      throw Invalid_Argument("Record_Reader: Invalid protocol version");

   major = (version >> 8) & 0xFF;
   minor = (version & 0xFF);
   }

/*
* Set the keys for reading
*/
void Record_Reader::set_keys(const CipherSuite& suite, const SessionKeys& keys,
                             Connection_Side side)
   {
   cipher.reset();
   delete mac;
   mac = 0;

   SymmetricKey mac_key, cipher_key;
   InitializationVector iv;

   if(side == CLIENT)
      {
      cipher_key = keys.server_cipher_key();
      iv = keys.server_iv();
      mac_key = keys.server_mac_key();
      }
   else
      {
      cipher_key = keys.client_cipher_key();
      iv = keys.client_iv();
      mac_key = keys.client_mac_key();
      }

   const std::string cipher_algo = suite.cipher_algo();
   const std::string mac_algo = suite.mac_algo();

   if(have_block_cipher(cipher_algo))
      {
      cipher.append(get_cipher(
                       cipher_algo + "/CBC/NoPadding",
                       cipher_key, iv, DECRYPTION)
         );
      block_size = block_size_of(cipher_algo);

      if(major > 3 || (major == 3 && minor >= 2))
         iv_size = block_size;
      else
         iv_size = 0;
      }
   else if(have_stream_cipher(cipher_algo))
      {
      cipher.append(get_cipher(cipher_algo, cipher_key, DECRYPTION));
      block_size = 0;
      iv_size = 0;
      }
   else
      throw Invalid_Argument("Record_Reader: Unknown cipher " + cipher_algo);

   if(have_hash(mac_algo))
      {
      Algorithm_Factory& af = global_state().algorithm_factory();

      if(major == 3 && minor == 0)
         mac = af.make_mac("SSL3-MAC(" + mac_algo + ")");
      else
         mac = af.make_mac("HMAC(" + mac_algo + ")");

      mac->set_key(mac_key);
      mac_size = mac->output_length();
      }
   else
      throw Invalid_Argument("Record_Reader: Unknown hash " + mac_algo);
   }

void Record_Reader::add_input(const byte input[], size_t input_size)
   {
   input_queue.write(input, input_size);
   }

/*
* Retrieve the next record
*/
size_t Record_Reader::get_record(byte& msg_type,
                                 MemoryRegion<byte>& output)
   {
   byte header[5] = { 0 };

   const size_t have_in_queue = input_queue.size();

   if(have_in_queue < sizeof(header))
      return (sizeof(header) - have_in_queue);

   /*
   * We peek first to make sure we have the full record
   */
   input_queue.peek(header, sizeof(header));

   // SSLv2-format client hello?
   if(header[0] & 0x80 && header[2] == 1 && header[3] == 3)
      {
      size_t record_len = make_u16bit(header[0], header[1]) & 0x7FFF;

      if(have_in_queue < record_len + 2)
         return (record_len + 2 - have_in_queue);

      msg_type = HANDSHAKE;
      output.resize(record_len + 4);

      input_queue.read(&output[2], record_len + 2);
      output[0] = CLIENT_HELLO_SSLV2;
      output[1] = 0;
      output[2] = header[0] & 0x7F;
      output[3] = header[1];

      return 0;
      }

   if(header[0] != CHANGE_CIPHER_SPEC &&
      header[0] != ALERT &&
      header[0] != HANDSHAKE &&
      header[0] != APPLICATION_DATA)
      {
      throw TLS_Exception(UNEXPECTED_MESSAGE,
                          "Record_Reader: Unknown record type");
      }

   const u16bit version    = make_u16bit(header[1], header[2]);
   const u16bit record_len = make_u16bit(header[3], header[4]);

   if(major && (header[1] != major || header[2] != minor))
      throw TLS_Exception(PROTOCOL_VERSION,
                          "Record_Reader: Got unexpected version");

   // If insufficient data, return without doing anything
   if(have_in_queue < (sizeof(header) + record_len))
      return (sizeof(header) + record_len - have_in_queue);

   SecureVector<byte> buffer(record_len);

   input_queue.read(header, sizeof(header)); // pull off the header
   input_queue.read(&buffer[0], buffer.size());

   /*
   * We are handshaking, no crypto to do so return as-is
   * TODO: Check msg_type to confirm a handshake?
   */
   if(mac_size == 0)
      {
      msg_type = header[0];
      output = buffer;
      return 0; // got a full record
      }

   // Otherwise, decrypt, check MAC, return plaintext

   cipher.process_msg(buffer);
   SecureVector<byte> plaintext = cipher.read_all(Pipe::LAST_MESSAGE);

   size_t pad_size = 0;

   if(block_size)
      {
      byte pad_value = plaintext[plaintext.size()-1];
      pad_size = pad_value + 1;

      /*
      * Check the padding; if it is wrong, then say we have 0 bytes of
      * padding, which should ensure that the MAC check below does not
      * suceed. This hides a timing channel.
      *
      * This particular countermeasure is recommended in the TLS 1.2
      * spec (RFC 5246) in section 6.2.3.2
      */
      if(version == SSL_V3)
         {
         if(pad_value > block_size)
            pad_size = 0;
         }
      else
         {
         bool padding_good = true;

         for(size_t i = 0; i != pad_size; ++i)
            if(plaintext[plaintext.size()-i-1] != pad_value)
               padding_good = false;

         if(!padding_good)
            pad_size = 0;
         }
      }

   if(plaintext.size() < mac_size + pad_size + iv_size)
      throw Decoding_Error("Record_Reader: Record truncated");

   const size_t mac_offset = plaintext.size() - (mac_size + pad_size);
   SecureVector<byte> received_mac(&plaintext[mac_offset],
                                   mac_size);

   const u16bit plain_length = plaintext.size() - (mac_size + pad_size + iv_size);

   mac->update_be(seq_no);
   mac->update(header[0]); // msg_type

   if(version != SSL_V3)
      for(size_t i = 0; i != 2; ++i)
         mac->update(get_byte(i, version));

   mac->update_be(plain_length);
   mac->update(&plaintext[iv_size], plain_length);

   ++seq_no;

   SecureVector<byte> computed_mac = mac->final();

   if(received_mac != computed_mac)
      throw TLS_Exception(BAD_RECORD_MAC, "Record_Reader: MAC failure");

   msg_type = header[0];

   output.resize(plain_length);
   copy_mem(&output[0], &plaintext[iv_size], plain_length);
   return 0;
   }

}
<|endoftext|>