time_stamp_count.hpp

// Copyright Take Vos 2021.
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
 
#pragma once
 
#include "../utility/utility.hpp"
#include "../concurrency/concurrency.hpp"
#include "../numeric/module.hpp"
#include "../macros.hpp"
#include <atomic>
#include <array>
#include <cstdint>
 
#if HI_OPERATING_SYSTEM == HI_OS_WINDOWS
#include <intrin.h>
#elif HI_OPERATING_SYSTEM == HI_OS_LINUX
#include <x86intrin.h>
#endif
 
 
 
namespace hi::inline v1 {
 
class time_stamp_count {
public:
    struct inplace {};
    struct inplace_with_cpu_id {};
    struct inplace_with_thread_id {};
 
    constexpr time_stamp_count() noexcept : _count(0), _aux(0), _thread_id(0) {}
 
    constexpr time_stamp_count(uint64_t count, uint32_t aux) noexcept : _count(count), _aux(aux), _thread_id(0) {}
 
    explicit time_stamp_count(time_stamp_count::inplace) noexcept : _aux(0), _thread_id(0)
    {
#if HI_PROCESSOR == HI_CPU_X64
        uint32_t tmp;
        _count = __rdtscp(&tmp);
#else
#error "Not Implemented"
#endif
    }
 
    explicit time_stamp_count(time_stamp_count::inplace_with_cpu_id) noexcept : _thread_id(0)
    {
#if HI_PROCESSOR == HI_CPU_X64
            _count = __rdtscp(&_aux);
#else
#error "Not Implemented"
#endif
    }
 
    explicit time_stamp_count(time_stamp_count::inplace_with_thread_id) noexcept
    {
#if HI_PROCESSOR == HI_CPU_X64 and HI_OPERATING_SYSTEM == HI_OS_WINDOWS
        constexpr uint64_t NT_TIB_CurrentThreadID = 0x48;
 
        _count = __rdtscp(&_aux);
        _thread_id = __readgsdword(NT_TIB_CurrentThreadID);
#else
#error "Not Implemented"
#endif
    }
 
    [[nodiscard]] static time_stamp_count now() noexcept
    {
        return time_stamp_count{time_stamp_count::inplace_with_cpu_id{}};
    }
 
    [[nodiscard]] ssize_t cpu_id() const noexcept
    {
        if (_aux_is_cpu_id.load(std::memory_order::relaxed)) {
            // On Linux the upper bits are used for a node-id.
            return _aux & 0xfff;
        } else {
            return cpu_id_fallback();
        }
    }
 
    [[nodiscard]] constexpr uint32_t thread_id() const noexcept
    {
        return _thread_id;
    }
 
    [[nodiscard]] constexpr uint64_t count() const noexcept
    {
        return _count;
    }
 
    [[nodiscard]] static std::chrono::nanoseconds duration_from_count(uint64_t count) noexcept
    {
        using namespace std::chrono_literals;
 
        hilet[lo, hi] = mul_carry(count, _period.load(std::memory_order::relaxed));
        return 1ns * static_cast<int64_t>((hi << 32) | (lo >> 32));
    }
 
    [[nodiscard]] std::chrono::nanoseconds time_since_epoch() const noexcept
    {
        return duration_from_count(_count);
    }
 
    constexpr time_stamp_count& operator+=(uint64_t rhs) noexcept
    {
        _count += rhs;
        return *this;
    }
 
    [[nodiscard]] constexpr time_stamp_count operator+(uint64_t rhs) const noexcept
    {
        auto tmp = *this;
        tmp += rhs;
        return tmp;
    }
 
    [[nodiscard]] static std::pair<utc_nanoseconds, time_stamp_count> time_stamp_utc_sample()
    {
        auto shortest_diff = std::numeric_limits<uint64_t>::max();
        time_stamp_count shortest_tsc;
        utc_nanoseconds shortest_tp;
 
        // With three samples gathered on the same CPU we should
        // have a TSC/UTC/TSC combination that was run inside a single time-slice.
        for (auto i = 0; i != 10; ++i) {
            hilet tmp_tsc1 = time_stamp_count::now();
            hilet tmp_tp = std::chrono::utc_clock::now();
            hilet tmp_tsc2 = time_stamp_count::now();
 
            if (tmp_tsc1.cpu_id() != tmp_tsc2.cpu_id()) {
                throw os_error("CPU Switch detected during get_sample(), which should never happen");
            }
 
            if (tmp_tsc1.count() > tmp_tsc2.count()) {
                // TSC skipped backwards, this may happen when the TSC of multiple
                // CPUs get synchronized with each other.
                // For example when waking up from sleep.
                continue;
            }
 
            hilet diff = tmp_tsc2.count() - tmp_tsc1.count();
 
            if (diff < shortest_diff) {
                shortest_diff = diff;
                shortest_tp = tmp_tp;
                shortest_tsc = tmp_tsc1 + (diff / 2);
            }
        }
 
        if (shortest_diff == std::numeric_limits<uint64_t>::max()) {
            throw os_error("Unable to get TSC sample.");
        }
 
        return {shortest_tp, shortest_tsc};
    }
 
    [[nodiscard]] static uint64_t measure_frequency(std::chrono::milliseconds sample_duration)
    {
        using namespace std::chrono_literals;
 
        // Only sample the frequency of one of the TSC clocks.
        hilet prev_mask = set_thread_affinity(current_cpu_id());
 
        hilet [tp1, tsc1] = time_stamp_utc_sample();
        std::this_thread::sleep_for(sample_duration);
        hilet [tp2, tsc2] = time_stamp_utc_sample();
 
        // Reset the mask back.
        set_thread_affinity_mask(prev_mask);
 
        if (tsc1._aux != tsc2._aux) {
            // This must never happen, as we set the thread affinity to a single CPU
            // if this happens something is seriously wrong.
            throw os_error("CPU Switch detected when measuring the TSC frequency.");
        }
 
        if (tsc1.count() >= tsc2.count()) {
            // The TSC should only be reset during the very early boot sequence when
            // the CPUs are started and synchronized. It may also happen to a CPU that
            // was hot-swapped while the computer is running, in that case the CPU
            // should not be running applications yet.
            throw os_error("TSC Did not advance during measuring its frequency.");
        }
 
        if (tp1 >= tp2) {
            // The UTC clock did not advance, maybe a time server changed the clock.
            return 0;
        }
 
        // Calculate the frequency by dividing the delta-tsc by the duration.
        // We scale both the delta-tsc and duration by 1'000'000'000 before the
        // division. The duration is scaled by 1'000'000'000 by dividing by 1ns.
        hilet[delta_tsc_lo, delta_tsc_hi] = mul_carry(tsc2.count() - tsc1.count(), uint64_t{1'000'000'000});
        auto duration = narrow_cast<uint64_t>((tp2 - tp1) / 1ns);
        return wide_div(delta_tsc_lo, delta_tsc_hi, duration);
    }
 
    static void set_frequency(uint64_t frequency) noexcept
    {
        hilet period = (uint64_t{1'000'000'000} << 32) / frequency;
        _period.store(period, std::memory_order_relaxed);
    }
 
    static std::pair<uint64_t, bool> start_subsystem()
    {
        hilet frequency = configure_frequency();
        hilet aux_is_cpu_id = populate_aux_values();
        return {frequency, aux_is_cpu_id};
    }
 
private:
    uint64_t _count;
 
    uint32_t _aux;
    
    uint32_t _thread_id;
 
    inline static std::atomic<uint64_t> _period = 0;
 
    inline static std::atomic<bool> _aux_is_cpu_id = false;
 
    inline static std::atomic<std::size_t> _num_aux_values = 0;
 
    inline static std::array<uint32_t, maximum_num_cpus> _aux_values;
 
    inline static std::array<std::size_t, maximum_num_cpus> _cpu_ids;
 
    [[nodiscard]] ssize_t cpu_id_fallback() const noexcept
    {
        auto aux_value_ = _mm_set1_epi32(_aux);
 
        hilet num_aux_values = _num_aux_values.load(std::memory_order_acquire);
        hi_assert(_aux_values.size() == _cpu_ids.size());
        hi_assert_bounds(num_aux_values, _aux_values);
 
        for (std::size_t i = 0; i < num_aux_values; i += 4) {
            hilet row = _mm_loadu_si128(reinterpret_cast<__m128i const *>(_aux_values.data() + i));
            hilet row_result = _mm_cmpeq_epi32(row, aux_value_);
            hilet row_result_ = _mm_castsi128_ps(row_result);
            hilet row_result_mask = _mm_movemask_ps(row_result_);
            if (to_bool(row_result_mask)) {
                hilet j = i + std::countr_zero(narrow_cast<unsigned int>(row_result_mask));
                if (j < num_aux_values) {
                    return _cpu_ids[j];
                }
 
                return -1;
            }
        }
 
        return -1;
    }
 
    static bool populate_aux_values()
    {
        // Keep track of the original thread affinity of the main thread.
        auto prev_mask = set_thread_affinity(current_cpu_id());
 
        // Create a table of cpu_ids.
        std::size_t next_cpu = 0;
        std::size_t current_cpu = 0;
        bool aux_is_cpu_id = true;
        do {
            current_cpu = advance_thread_affinity(next_cpu);
 
            auto i = _num_aux_values.load(std::memory_order::acquire);
            auto tsc = time_stamp_count::now();
            _aux_values[i] = tsc._aux;
            _cpu_ids[i] = current_cpu;
            _num_aux_values.store(i + 1, std::memory_order::release);
 
            if ((tsc._aux & 0xfff) != current_cpu) {
                aux_is_cpu_id = false;
            }
 
        } while (next_cpu > current_cpu);
 
        _aux_is_cpu_id.store(aux_is_cpu_id, std::memory_order_relaxed);
 
        // Set the thread affinity back to the original.
        set_thread_affinity_mask(prev_mask);
        return aux_is_cpu_id;
    }
    static uint64_t configure_frequency()
    {
        using namespace std::chrono_literals;
 
        // This function is called from the crt and must therefor be quick as we do not
        // want to keep the user waiting. We are satisfied if the measured frequency is
        // to within 1% accuracy.
 
        // We take an average over 4 times in case the hires_utc_clock gets reset by a time server.
        uint64_t frequency = 0;
        uint64_t num_samples = 0;
        for (int i = 0; i != 4; ++i) {
            hilet f = time_stamp_count::measure_frequency(25ms);
            if (f != 0) {
                frequency += f;
                ++num_samples;
            }
        }
        if (num_samples == 0) {
            throw os_error("Unable the measure the frequency of the TSC. The UTC time did not advance.");
        }
        frequency /= num_samples;
 
        time_stamp_count::set_frequency(frequency);
        return frequency;
    }
};
 
} // namespace hi::inline v1