lokinet/llarp/util/codel.hpp

#ifndef LLARP_CODEL_QUEUE_HPP
#define LLARP_CODEL_QUEUE_HPP

#include <util/logging/logger.hpp>
#include <util/mem.hpp>
#include <util/thread/threading.hpp>
#include <util/time.hpp>

#include <algorithm>
#include <array>
#include <cmath>
#include <functional>
#include <string>
#include <utility>

namespace llarp
{
  namespace util
  {
    struct GetNowSyscall
    {
      llarp_time_t
      operator()() const
      {
        return llarp::time_now_ms();
      }
    };

    template < typename T, typename GetTime, typename PutTime, typename Compare,
               typename GetNow = GetNowSyscall, typename Mutex_t = util::Mutex,
               typename Lock_t = std::lock_guard< Mutex_t >,
               size_t MaxSize  = 1024 >
    struct CoDelQueue
    {
      CoDelQueue(std::string name, PutTime put, GetNow now)
          : m_QueueIdx(0)
          , m_name(std::move(name))
          , _putTime(std::move(put))
          , _getNow(std::move(now))
      {
      }

      size_t
      Size() EXCLUDES(m_QueueMutex)
      {
        Lock_t lock(m_QueueMutex);
        return m_QueueIdx;
      }

      template < typename... Args >
      bool
      EmplaceIf(std::function< bool(T&) > pred, Args&&... args)
          EXCLUDES(m_QueueMutex)
      {
        Lock_t lock(m_QueueMutex);
        if(m_QueueIdx == MaxSize)
          return false;
        T* t = &m_Queue[m_QueueIdx];
        new(t) T(std::forward< Args >(args)...);
        if(!pred(*t))
        {
          t->~T();
          return false;
        }

        _putTime(m_Queue[m_QueueIdx]);
        if(firstPut == 0s)
          firstPut = _getTime(m_Queue[m_QueueIdx]);
        ++m_QueueIdx;

        return true;
      }

      template < typename... Args >
      void
      Emplace(Args&&... args) EXCLUDES(m_QueueMutex)
      {
        Lock_t lock(m_QueueMutex);
        if(m_QueueIdx == MaxSize)
          return;
        T* t = &m_Queue[m_QueueIdx];
        new(t) T(std::forward< Args >(args)...);
        _putTime(m_Queue[m_QueueIdx]);
        if(firstPut == 0s)
          firstPut = _getTime(m_Queue[m_QueueIdx]);
        ++m_QueueIdx;
      }

      template < typename Visit >
      void
      Process(Visit v)
      {
        return Process(v, [](T&) -> bool { return false; });
      }

      template < typename Visit, typename Filter >
      void
      Process(Visit visitor, Filter f) EXCLUDES(m_QueueMutex)
      {
        llarp_time_t lowest = std::numeric_limits< llarp_time_t >::max();
        if(_getNow() < nextTickAt)
          return;
        // llarp::LogInfo("CoDelQueue::Process - start at ", start);
        Lock_t lock(m_QueueMutex);
        auto start = firstPut;

        if(m_QueueIdx == 1)
        {
          visitor(m_Queue[0]);
          T* t = &m_Queue[0];
          t->~T();
          m_QueueIdx = 0;
          firstPut   = 0s;
          return;
        }
        size_t idx = 0;
        while(m_QueueIdx)
        {
          llarp::LogDebug(m_name, " - queue has ", m_QueueIdx);
          T* item = &m_Queue[idx++];
          if(f(*item))
            break;
          --m_QueueIdx;
          const llarp_time_t dlt = start - _getTime(*item);
          // llarp::LogInfo("CoDelQueue::Process - dlt ", dlt);
          lowest = std::min(dlt, lowest);
          if(m_QueueIdx == 0)
          {
            // llarp::LogInfo("CoDelQueue::Process - single item: lowest ",
            // lowest, " dropMs: ", dropMs);
            if(lowest > dropMs)
            {
              item->~T();
              nextTickInterval +=
                  initialIntervalMs / uint64_t(std::sqrt(++dropNum));
              firstPut   = 0s;
              nextTickAt = start + nextTickInterval;
              return;
            }

            nextTickInterval = initialIntervalMs;
            dropNum          = 0;
          }
          visitor(*item);
          item->~T();
        }
        firstPut   = 0s;
        nextTickAt = start + nextTickInterval;
      }

      const llarp_time_t initialIntervalMs = 5ms;
      const llarp_time_t dropMs            = 100ms;
      llarp_time_t firstPut                = 0s;
      size_t dropNum                       = 0;
      llarp_time_t nextTickInterval        = initialIntervalMs;
      llarp_time_t nextTickAt              = 0s;
      Mutex_t m_QueueMutex;
      size_t m_QueueIdx GUARDED_BY(m_QueueMutex);
      std::array< T, MaxSize > m_Queue GUARDED_BY(m_QueueMutex);
      std::string m_name;
      GetTime _getTime;
      PutTime _putTime;
      GetNow _getNow;
    };  // namespace util
  }     // namespace util
}  // namespace llarp

#endif