/builddir/build/BUILD/libutil++-1.4.0/src/sella/util/ThreadPool.cpp

/*
** libutil++
** $Id: ThreadPool.cpp 1230 2014-11-16 02:32:01Z sella $
** Copyright (c) 2011-2014 Digital Genesis, LLC. All Rights Reserved.
** Released under the LGPL Version 2.1 License.
** http://www.digitalgenesis.com
*/

static const char rcsid[] __attribute__((used)) = "$Id: ThreadPool.cpp 1230 2014-11-16 02:32:01Z sella $";

#include "ThreadPool.h"
#include "CommonMacro.h"
#include "Time.h"

#include <stdio.h>
#include <assert.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/time.h>
#include <syscall.h>

#include <algorithm>

using namespace sella::util;
using namespace std::chrono;

ThreadPool::ThreadPool(size_t workers) :
        TaskQueue(workers * 2),
        threads(0),
        creating(0),
        actives(0),
        overage(0),
        shutdown(false),
        paused(false),
        allcpus(true),
        maximum(workers),
        minimum(workers),
        spares(DefaultSpareWorkers)
{
        clearCPUAffinity();
        normalize();

        TRACE(PACKAGE, "ThreadPool starting up with %zd workers", this->maximum);

        managerTID = std::thread(&ThreadPool::manager, this);
        tids.insert(managerTID.native_handle());
}

ThreadPool::ThreadPool(size_t maximumWorkers, size_t minimumWorkers, size_t spareWorkers) :
        TaskQueue(maximumWorkers * 2),
        threads(0),
        creating(0),
        actives(0),
        overage(0),
        shutdown(false),
        paused(false),
        allcpus(true),
        maximum(maximumWorkers),
        minimum(minimumWorkers),
        spares(spareWorkers)
{
        clearCPUAffinity();
        normalize();

        TRACE(PACKAGE, "ThreadPool starting up with %zd/%zd workers, %zd spares", this->minimum, this->maximum, this->spares);

        managerTID = std::thread(&ThreadPool::manager, this);
        tids.insert(managerTID.native_handle());
}

ThreadPool::ThreadPool(Log::shared log, size_t workers) :
        TaskQueue(log, workers * 2),
        threads(0),
        creating(0),
        actives(0),
        overage(0),
        shutdown(false),
        paused(false),
        allcpus(true),
        maximum(workers),
        minimum(workers),
        spares(DefaultSpareWorkers)
{
        clearCPUAffinity();
        normalize();

        TRACE(PACKAGE, "ThreadPool starting up with %zd workers", this->maximum);

        managerTID = std::thread(&ThreadPool::manager, this);
        tids.insert(managerTID.native_handle());
}

ThreadPool::ThreadPool(Log::shared log, size_t maximumWorkers, size_t minimumWorkers, size_t spareWorkers) :
        TaskQueue(log, maximumWorkers * 2),
        threads(0),
        creating(0),
        actives(0),
        overage(0),
        shutdown(false),
        paused(false),
        allcpus(true),
        maximum(maximumWorkers),
        minimum(minimumWorkers),
        spares(spareWorkers)
{
        clearCPUAffinity();
        normalize();

        TRACE(PACKAGE, "ThreadPool starting up with %zd/%zd workers, %zd spares", this->minimum, this->maximum, this->spares);

        managerTID = std::thread(&ThreadPool::manager, this);
        tids.insert(managerTID.native_handle());
}

ThreadPool::~ThreadPool() {
        TRACE(PACKAGE, "ThreadPool shutting down with %zd queued, %zd workers, %zd active", heap.size(), (size_t) threads, (size_t) actives);

        shutdown = true;
        managerMutex.lock();
        managerCV.notify_all();
        managerMutex.unlock();

        if (managerTID.joinable()) {
                managerTID.join();
        }

        std::lock_guard<std::mutex> lg(emptyMutex);
        emptyCV.notify_all();
}

void ThreadPool::schedule(Task::shared &task, int priority) {
        assert(task.get() != NULL);

        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Scheduling task %p to run immedately with priority %d", task.get(), task->getPriority());

        heapMutex.lock();
        push(task);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

void ThreadPool::schedule(Task::shared &task, const std::chrono::system_clock::duration &delay, int priority) {
        assert(task.get() != NULL);

        task->setDelay(delay);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Scheduling task %p to run in %lld.%lld sec with priority %d", task.get(), task->getDelay().tv_sec, task->getDelay().tv_usec, task->getPriority());

        heapMutex.lock();
        push(task);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

void ThreadPool::schedule(Task::shared &task, const std::chrono::system_clock::time_point &time, int priority) {
        assert(task.get() != NULL);

        task->setTimer(time);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Scheduling task %p to run at %lld.%lld sec with priority %d", task.get(), task->getTimer().tv_sec, task->getTimer().tv_usec, task->getPriority());

        heapMutex.lock();
        push(task);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

void ThreadPool::schedule(Task::shared &task, time_t sec, suseconds_t usec, int priority) {
        assert(task.get() != NULL);

        task->setDelay(sec, usec);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Scheduling task %p to run in %lld.%lld sec with priority %d", task.get(), sec, usec, task->getPriority());

        heapMutex.lock();
        push(task);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

void ThreadPool::schedule(Task::shared &task, timeval &tv, int priority) {
        assert(task.get() != NULL);

        task->setTimer(tv);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Scheduling task %p to run at %lld.%lld sec with priority %d", task.get(), Time::tvtosec(tv), Time::tvtousec(tv), task->getPriority());

        heapMutex.lock();
        push(task);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

bool ThreadPool::reschedule(Task::shared &task, const std::chrono::system_clock::duration &delay, int priority, bool strict) {
        assert(task.get() != NULL);

        task->setDelay(delay);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Rescheduling task %p to run in %lld.%lld sec with priority %d", task.get(), task->getDelay().tv_sec, task->getDelay().tv_usec, task->getPriority());

        heapMutex.lock();
        bool res = repush(task, delay, priority, strict);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();

        return res;
}

bool ThreadPool::reschedule(Task::shared &task, const std::chrono::system_clock::time_point &time, int priority, bool strict) {
        assert(task.get() != NULL);

        task->setTimer(time);
        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Rescheduling task %p to run at %lld.%lld sec with priority %d", task.get(), task->getTimer().tv_sec, task->getTimer().tv_usec, task->getPriority());

        heapMutex.lock();
        bool res = repush(task, time, priority, strict);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();

        return res;
}

bool ThreadPool::reschedule(Task::shared &task, time_t sec, suseconds_t usec, int priority, bool strict) {
        assert(task.get() != NULL);

        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Rescheduling task %p to run in %lld.%lld sec with priority %d", task.get(), sec, usec, task->getPriority());

        heapMutex.lock();
        bool res = repush(task, sec, usec, priority, strict);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();

        return res;
}

bool ThreadPool::reschedule(Task::shared &task, timeval &tv, int priority, bool strict) {
        assert(task.get() != NULL);

        if (priority) task->setPriority(priority);

        TRACE(PACKAGE "-detail", "Rescheduling task %p to run at %lld.%lld sec with priority %d", task.get(), tv.tv_sec, tv.tv_usec, task->getPriority());

        heapMutex.lock();
        bool res = repush(task, tv, priority, strict);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();

        return res;
}

bool ThreadPool::unschedule(Task::shared &task, bool firstonly) {
        TRACE(PACKAGE "-detail", "Unscheduling task %p%s", task.get(), (firstonly) ? " (firstonly)" : "");

        heapMutex.lock();
        bool res = erase(task, firstonly);
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();

        return res;
}

void ThreadPool::pause(bool on) {
        std::lock_guard<std::mutex> lg(managerMutex);

        if ((paused = on) == false) {
                TRACE(PACKAGE, "Resuming %zd queued, %zd workers, %zd active", heap.size(), (size_t) threads, (size_t) actives);

                managerCV.notify_all();
        } else {
                TRACE(PACKAGE, "Pausing %zd queued, %zd workers, %zd active", heap.size(), (size_t) threads, (size_t) actives);
        }
}

size_t ThreadPool::getCPUCount(void) {
        return (std::thread::hardware_concurrency() > 0) ? std::thread::hardware_concurrency() : sysconf(_SC_NPROCESSORS_ONLN);
}

int ThreadPool::getCPUSetCount(void) {
        return CPU_COUNT(&cpuset);
}

bool ThreadPool::addCPUAffinity(std::set<int> cpus) {
        size_t count = getCPUCount();

        if (allcpus) {
                CPU_ZERO(&cpuset);
        }

        for (auto c = cpus.begin(); c != cpus.end(); ++c) {
                if ((size_t) *c < count) {
                        CPU_SET(*c, &cpuset);
                }
        }

        if (CPU_COUNT(&cpuset) == 0) {
                clearCPUAffinity();
        } else {
                allcpus = false;
        }

        return setThreadsAffinity();
}

bool ThreadPool::addCPUAffinity(int cpu) {
        size_t count = getCPUCount();

        if (allcpus) {
                CPU_ZERO(&cpuset);
        }

        if ((size_t) cpu < count) {
                CPU_SET(cpu, &cpuset);
        }

        if (CPU_COUNT(&cpuset) == 0) {
                clearCPUAffinity();
        } else {
                allcpus = false;
        }

        return setThreadsAffinity();
}

bool ThreadPool::clearCPUAffinity(void) {
        CPU_ZERO(&cpuset);
        allcpus = true;

        size_t cpus = getCPUCount();

        for (size_t i = 0; i < cpus; i++) {
                CPU_SET(i, &cpuset);
        }

        return setThreadsAffinity();
}

bool ThreadPool::clearCPUAffinity(int cpu) {
        if (cpu < CPU_SETSIZE) {
                CPU_CLR(cpu, &cpuset);
        }

        if (CPU_COUNT(&cpuset) == 0) {
                clearCPUAffinity();
        }

        return setThreadsAffinity();
}

bool ThreadPool::clearCPUAffinity(std::set<int> cpus) {
        for (auto c = cpus.begin(); c != cpus.end(); ++c) {
                if (*c < CPU_SETSIZE) {
                        CPU_CLR(*c, &cpuset);
                }
        }

        if (CPU_COUNT(&cpuset) == 0) {
                clearCPUAffinity();
        }

        return setThreadsAffinity();
}

size_t ThreadPool::size(void) const {
        std::lock_guard<std::mutex> lg(heapMutex);

        if (!TaskQueue::empty()) {
                Task::shared task = top();
                timeval delay = task->getDelay();
        }

        return TaskQueue::size() + actives;
}

size_t ThreadPool::workers(void) const {
        return threads;
}

size_t ThreadPool::active(void) const {
        return actives;
}

bool ThreadPool::empty(void) const {
        return (size() == 0);
}

void ThreadPool::clear(void) {
        heapMutex.lock();
        TaskQueue::clear();
        heapMutex.unlock();

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

Task::shared ThreadPool::next(void) {
        std::lock_guard<std::mutex> lg(heapMutex);

        if (!TaskQueue::empty()) {
                Task::shared task = top();

                if (task && Task::compareTimer(task)) {
                        pop();

                        return task;
                }
        }

        return Task::shared();
}

microseconds ThreadPool::duration(void) const {
        if (actives == maximum || UNLIKELY(paused && !shutdown)) {
                return max();
        }

        std::lock_guard<std::mutex> lg(heapMutex);

        if (!TaskQueue::empty()) {
                Task::shared task = top();
                timeval delay = task->getDelay();

                if (delay.tv_usec < 1 && delay.tv_sec < 1) {
                        if (creating) {
                                return milliseconds(10);
                        } else {
                                return milliseconds(2);
                        }
                }

                return seconds(delay.tv_sec) + microseconds(delay.tv_usec);
        } else if (UNLIKELY(shutdown)) {
                return milliseconds(1);
        }

        return max();
}

microseconds ThreadPool::max(void) const {
        return hours(24 * 365 * 100); // 100 years because cv.wait_for(microseconds::max()) broken on GCC 4.4.4.
}

bool ThreadPool::ready(void) const {
        if (TaskQueue::empty()) {
                return false;
        }

        std::lock_guard<std::mutex> lg(heapMutex);

        return Task::compareTimer(top());
}

bool ThreadPool::wait(void) {
        std::unique_lock<std::mutex> lock(emptyMutex);

        if (empty()) {
                managerMutex.lock();
                managerCV.notify_all();
                managerMutex.unlock();

                return true;
        }

        managerMutex.lock();
        managerCV.notify_all();
        managerMutex.unlock();

        emptyCV.wait(lock);

        return empty();
}

bool ThreadPool::wait(const std::chrono::system_clock::duration &delay) {
        std::unique_lock<std::mutex> lock(emptyMutex);

        if (empty()) {
                managerMutex.lock();
                managerCV.notify_all();
                managerMutex.unlock();

                return true;
        }

        managerMutex.lock();
        managerCV.notify_all();
        managerMutex.unlock();
        
        emptyCV.wait_for(lock, delay);

        return empty();
}

bool ThreadPool::wait(const std::chrono::system_clock::time_point &time) {
        std::unique_lock<std::mutex> lock(emptyMutex);

        if (empty()) {
                managerMutex.lock();
                managerCV.notify_all();
                managerMutex.unlock();

                return true;
        }

        managerMutex.lock();
        managerCV.notify_all();
        managerMutex.unlock();

        emptyCV.wait_until(lock, time);

        return empty();
}

void ThreadPool::reserve(size_t capacity) {
        std::lock_guard<std::mutex> lg(heapMutex);
        TaskQueue::reserve(capacity);
}

void ThreadPool::print(void) const {
        std::lock_guard<std::mutex> lg(heapMutex);

        Task::shared task;
        timeval now, timer, delay;

        if (libutilxx__log == NULL) {
                return;
        }

        gettimeofday(&now, NULL);
        INFO("[ThreadPool Heap] - now %10lu.%06lu, %zd, queued, %zd workers, %zd active, %zd overage", now.tv_sec, now.tv_usec, heap.size(), (size_t) threads, (size_t) actives, (size_t) overage);

        if (heap.empty()) {
                INFO(" heap => empty");

                return;
        }

        for (auto it = heap.begin(); it != heap.end(); ++it) {
                timer = (*it)->getTimer();
                if (timerisset(&timer)) {
                        /* The output comes out very wrong when timersub() is used. This output is odd, but accurate. */
                        delay.tv_sec = timer.tv_sec - now.tv_sec;
                        delay.tv_usec = timer.tv_usec - now.tv_usec;
                        if (delay.tv_sec > 0 && delay.tv_usec < 0) { /* Only preform the subtraction when tv_sec is > 0. */
                                --delay.tv_sec;
                                delay.tv_usec += 1000000;
                        }

                        INFO(" heap => task(%10p) obj: %10p pri: %3d timer: %10lu.%06lu delay: %4d.%d", (*it).get(), (*it)->getObject(), (*it)->getPriority(), timer.tv_sec, timer.tv_usec, (signed) delay.tv_sec, (signed) delay.tv_usec);
                } else {
                        INFO(" heap => task(%10p) obj: %10p pri: %3d timer: %10lu.%06lu delay: -inf", (*it).get(), (*it)->getObject(), (*it)->getPriority(), timer.tv_sec, timer.tv_usec);
                }
        }
}

ThreadPool::shared ThreadPool::shared_ptr(size_t maximumWorkers) {
        return std::make_shared<ThreadPool>(maximumWorkers);
}

ThreadPool::shared ThreadPool::shared_ptr(size_t maximumWorkers, size_t minimumWorkers, size_t spareWorkers) {
        return std::make_shared<ThreadPool>(maximumWorkers, minimumWorkers, spareWorkers);
}

ThreadPool::shared ThreadPool::shared_ptr(Log::shared log, size_t maximumWorkers) {
        return std::make_shared<ThreadPool>(log, maximumWorkers);
}

ThreadPool::shared ThreadPool::shared_ptr(Log::shared log, size_t maximumWorkers, size_t minimumWorkers, size_t spareWorkers) {
        return std::make_shared<ThreadPool>(log, maximumWorkers, minimumWorkers, spareWorkers);
}

void ThreadPool::manager(void) {
        size_t cycles = 0;

        for (size_t i = 0; !shutdown && i < minimum; i++) {
                createWorker();
        }

        TRACE(PACKAGE "-detail", "Manager started up with %zd threads", (size_t) threads);

        while (LIKELY(!shutdown)) {
                if (minimum == maximum) { /* Non-scaling manager */
                        while (LIKELY(!shutdown) && threads < minimum) {
                                createWorker();
                        }
                } else { /* Scaling manager */
                        while (LIKELY(!shutdown) && threads < maximum && threads < actives + spares) {
                                createWorker();
                        }

                        if (ready() || (++cycles % maximum) != 0) { /* Don't give up workers when ready tasks exist, or too few cycles have passed. */
                                overage = 0;
                        } else {
                                if (threads > minimum && threads > actives + spares) {
                                        overage = threads - actives + spares;
                                } else if (actives == 0) { /* Drop back to minimum with no active tasks */
                                        overage = threads - minimum;
                                }

                                TRACE(PACKAGE "-detail", "Manager set overage to %zd", (size_t) overage);

                                for (size_t i = 0; LIKELY(!shutdown) && i < overage && i < maximum; i++) { /* i < maximum handles underrun which may occur during shutdown */
                                        TRACE(PACKAGE "-detail", "Manager waking worker for overage");

                                        std::lock_guard<std::mutex> lg(workerMutex);
                                        workerCV.notify_one(); /* Wake required workers */
                                }
                        }
                }

                if (LIKELY(!shutdown)) {
                        TRACE(PACKAGE, "Manager waiting for tasks - %zd queued, %zd active, %zd/%zd workers", heap.size(), (size_t) actives, (size_t) threads, maximum);

                        std::unique_lock<std::mutex> lock(managerMutex);
                        do {
                                emptyMutex.lock();
                                if (empty()) {
                                        emptyCV.notify_all();
                                }
                                emptyMutex.unlock();

                                microseconds delay = duration();
#if 0
                                TRACE(PACKAGE "-detail", "delay: %lld, active: %zd, threads: %zd, overage: %zd, shutdown: %d", microseconds(delay).count(), (size_t) actives, (size_t) threads, (size_t) overage, shutdown);
#endif
                                managerCV.wait_for(lock, delay);
                        } while (UNLIKELY(paused && !shutdown));
                }

                if (LIKELY(!shutdown)) {
                        size_t max = maximum - actives;

                        TRACE(PACKAGE, "Manager waking %zd worker(s) - %zd queued, %zd active, %zd/%zd workers", max, heap.size(), (size_t) actives, (size_t) threads, maximum);

                        std::lock_guard<std::mutex> lg(workerMutex);
                        for (size_t i = 0; ready() && i < max; i++) { /* Loop boosts performance for quick tasks. */
                                workerCV.notify_one();
                        }
                }
        }

        TRACE(PACKAGE "-detail", "Manager waiting on remaining %zd workers to complete", (size_t) threads);

        clear();
        overage = (size_t) threads;
        workerCV.notify_all();

        while (threads > 0) { /* Loop until all threads are complete */
                std::unique_lock<std::mutex> lock(managerMutex);
                managerCV.wait_for(lock, seconds(1));

                workerCV.notify_all();
        }

        std::lock_guard<std::mutex> lg(emptyMutex);
        emptyCV.notify_all();

        TRACE(PACKAGE, "Manager shutdown");
}

void ThreadPool::worker(void) {
        Task::shared task;
        int tid = syscall(SYS_gettid);

        TRACE(PACKAGE, "Worker(%d) started", tid);

        actives++;
        creating--;

        while (LIKELY(!shutdown)) {
                {
                        TRACE(PACKAGE "-detail", "Worker(%d) waiting for next task", tid);

                        std::unique_lock<std::mutex> lock(workerMutex);

                        managerMutex.lock();
                        managerCV.notify_all();
                        managerMutex.unlock();

                        --actives;
                        workerCV.wait(lock);
                        ++actives;
                }

                if (LIKELY(!shutdown)) {
                        if (overage) { /* Check if this thread is unneeded */
                                std::lock_guard<std::mutex> lg(overageMutex);

                                if (overage) {
                                        TRACE(PACKAGE "-detail", "Worker(%d) yielding up the ghost (spare worker)", tid);

                                        --overage;

                                        break;
                                }
                        }

                        workerMutex.lock();

                        if (ready()) {
                                task = next();

                                TRACE(PACKAGE "-detail", "Worker(%d) acquired task %p with priority %d", tid, task.get(), task->getPriority());
                        }

                        workerMutex.unlock();

                        if (task) {
                                TRACE(PACKAGE, "Worker(%d) executing task %p with priority %d", tid, task.get(), task->getPriority());

                                task->execute();
                                task.reset();
                        }
                }
        }

        TRACE(PACKAGE, "Worker(%d) ended", tid);
        
        tidsMutex.lock();
        tids.erase(pthread_self());
        tidsMutex.unlock();

        --actives;
        --threads;

        std::lock_guard<std::mutex> lg(managerMutex);
        managerCV.notify_all();
}

void ThreadPool::normalize(void) {
        if (maximum == 0) {
                maximum = getCPUCount();
        } 

        if (minimum == 0) {
                minimum = getCPUCount();
        }

        if (spares == 0) {
                spares = DefaultSpareWorkers;
        }

        if (minimum > maximum) {
                maximum = minimum;
                spares = 0;
        } else if (maximum == minimum) {
                spares = 0;
        }
}

void ThreadPool::createWorker(void) {
        TRACE(PACKAGE "-detail", "Manager starting up worker %zd/%zd with affinity to %d CPUs", (size_t) threads + 1, maximum, getCPUSetCount());

        std::thread t(&ThreadPool::worker, this);
        setThreadAffinity(t.native_handle());

        tidsMutex.lock();
        tids.insert(t.native_handle());
        tidsMutex.unlock();

        ++threads;
        ++creating;
        t.detach();
}

bool ThreadPool::setThreadsAffinity() {
        std::lock_guard<std::mutex> lg(tidsMutex);

        TRACE(PACKAGE "-detail", "Setting %zd threads affinity to %d CPUs", tids.size(), getCPUSetCount());

        for (auto t = tids.begin(); t != tids.end(); ++t) {
                if (!setThreadAffinity(*t)) {
                        return false;
                }
        }

        return true;
}

bool ThreadPool::setThreadAffinity(pthread_t tid) {
        int error;

        if ((error = pthread_setaffinity_np(tid, sizeof(cpu_set_t), &cpuset)) != 0) {
                char buf[1024] = "message unavailable";
                strerror_r(error, buf, sizeof(buf));

                TRACE(PACKAGE, "pthread_setaffinity_np: %d:%s", error, buf);

                return false;
        }

        return true;
}

/*
** vim: noet ts=3 sw=3
*/

---