path: root/src/timespec.c

#include "common.h"

#include "timespec.h"

#if !HAVE_CLOCK_GETTIME
# include <sys/time.h>
#endif

#if HAVE_CLOCK_GETTIME
static bool support_monotonic = true;
#elif defined(__MACH__)
#include <mach/mach_time.h>
static struct mach_timebase_info timebase_info = { 0, 0 };

/*
 * The methods below to muldiv128 have the listed copyright:
 *
 * Copyright (c) 1999, 2003, 2006, 2007 Apple Inc. All rights reserved.
 *
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this
 * file.
 *
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 */

typedef struct
{
    uint64_t high;
    uint64_t low;
} uint128_t;

/* acc += add */
static inline void add128_128(uint128_t *acc, uint128_t *add)
{
    acc->high += add->high;
    acc->low += add->low;
    if (acc->low < add->low)
    {
        acc->high++; // carry
    }
}

/* acc -= sub */
static inline void sub128_128(uint128_t *acc, uint128_t *sub)
{
    acc->high -= sub->high;
    if (acc->low < sub->low)
    {
        acc->high--; // borrow
    }
    acc->low -= sub->low;
}

static inline double uint128_double(uint128_t *u)
{
    return (((double)(1ULL << 32)) * ((double)(1ULL << 32))) *
        u->high + u->low; // may loses precision
 }

/* 64x64 -> 128 bit multiplication */
static inline void mul64x64(uint64_t x, uint64_t y, uint128_t *prod)
{
    uint128_t add;
    /*
     * Split the two 64-bit multiplicands into 32-bit parts:
     * x => 2^32 * x1 + x2
     * y => 2^32 * y1 + y2
     */
    uint32_t x1 = (uint32_t)(x >> 32);
    uint32_t x2 = (uint32_t)x;
    uint32_t y1 = (uint32_t)(y >> 32);
    uint32_t y2 = (uint32_t)y;
    /*
     * direct multiplication:
     * x * y => 2^64 * (x1 * y1) + 2^32 (x1 * y2 + x2 * y1) + (x2 * y2)
     * The first and last terms are direct assignmenet into the uint128_t
     * structure.  Then we add the middle two terms separately, to avoid
     * 64-bit overflow.  (We could use the Karatsuba algorithm to save
     * one multiply, but it is harder to deal with 64-bit overflows.)
     */
    prod->high = (uint64_t)x1 * (uint64_t)y1;
    prod->low = (uint64_t)x2 * (uint64_t)y2;
    add.low = (uint64_t)x1 * (uint64_t)y2;
    add.high = (add.low >> 32);
    add.low <<= 32;
    add128_128(prod, &add);
    add.low = (uint64_t)x2 * (uint64_t)y1;
    add.high = (add.low >> 32);
    add.low <<= 32;
    add128_128(prod, &add);
}

/* (x * y / divisor) */
static uint64_t muldiv128(uint64_t x, uint64_t y, uint64_t divisor)
{
    uint128_t temp;
    uint128_t divisor128 = {0, divisor};
    uint64_t result = 0;
    double recip;

    mul64x64(x, y, &temp);

    /*
     * Now divide by the divisor.  We use floating point to calculate an
     * approximate answer and update the results.  Then we iterate and
     * calculate a correction from the difference.
     */
    recip = 1.0 / (double)divisor;
    while (temp.high || temp.low >= divisor)
    {
        uint128_t backmul;
        uint64_t uapprox;

        uapprox = (uint64_t)(uint128_double(&temp) * recip);
        mul64x64(uapprox, divisor, &backmul);
        /*
         * Because we are using unsigned integers, we need to approach the
         * answer from the lesser side.  So if our estimate is too large
         * we need to decrease it until it is smaller.
         */
        while (backmul.high > temp.high ||
               (backmul.high == temp.high && backmul.low > temp.low))
        {
            sub128_128(&backmul, &divisor128);
            uapprox--;
        }
        sub128_128(&temp, &backmul);
        result += uapprox;
    }
    return result;
}
#endif

void timespec_now(struct timespec *ts)
{
    assert(ts);
#if HAVE_CLOCK_GETTIME
    if (support_monotonic)
    {
        if (clock_gettime(CLOCK_MONOTONIC, ts) == 0)
        {
            return;
        }
        support_monotonic = false;
    }
    clock_gettime(CLOCK_REALTIME, ts);
#elif defined( __MACH__)
    if (timebase_info.denom == 0)
    {
        mach_timebase_info(&timebase_info);
    }
    {
        uint64_t time;
        if (timebase_info.denom == timebase_info.numer)
        {
            time = mach_absolute_time();
        }
        else
        {
            time = muldiv128(timebase_info.numer, mach_absolute_time(),
                             timebase_info.denom);
        }
        ts->tv_sec = time / 1000000000;
        ts->tv_nsec = time % 1000000000;
    }
#else
    {
        struct timeval tv;
        gettimeofday(&tv, NULL);
        ts->tv_sec = tv.tv_sec;
        ts->tv_nsec = tv.tv_usec * 1000;
    }
#endif
}

void timespec_addms(struct timespec *ts, unsigned long ms)
{
    const unsigned int sec = ms / 1000;
    assert(ts);
    ms -= sec * 1000;
    ts->tv_nsec += ms * 1000000;
    ts->tv_sec += ts->tv_nsec / 1000000000 + sec;
    ts->tv_nsec = ts->tv_nsec % 1000000000;
}

void timespec_add(struct timespec *ts, const struct timespec *add)
{
    assert(ts && add);
    ts->tv_nsec += add->tv_nsec;
    ts->tv_sec += ts->tv_nsec / 1000000000 + add->tv_sec;
    ts->tv_nsec = ts->tv_nsec % 1000000000;
}

int timespec_sub(struct timespec *ts, const struct timespec *sub)
{
    assert(ts && sub);
    if (ts->tv_sec < sub->tv_sec)
    {
        ts->tv_sec = sub->tv_sec - ts->tv_sec;
        if (ts->tv_nsec <= sub->tv_nsec)
        {
            ts->tv_nsec = sub->tv_nsec - ts->tv_nsec;
        }
        else
        {
            ts->tv_sec--;
            ts->tv_nsec = 1000000000 + sub->tv_nsec - ts->tv_nsec;
        }
        return -1;
    }
    else if (ts->tv_sec > sub->tv_sec)
    {
        ts->tv_sec -= sub->tv_sec;
        if (ts->tv_nsec < sub->tv_nsec)
        {
            ts->tv_sec--;
            ts->tv_nsec = 1000000000 + ts->tv_nsec - sub->tv_nsec;
        }
        else
        {
            ts->tv_nsec -= sub->tv_nsec;
        }
        return 1;
    }
    else /* if (ts->tv_sec == sub->tv_sec) */
    {
        ts->tv_sec = 0;
        if (ts->tv_nsec < sub->tv_nsec)
        {
            ts->tv_nsec = sub->tv_nsec - ts->tv_nsec;
            return -1;
        }
        else if (ts->tv_nsec > sub->tv_nsec)
        {
            ts->tv_nsec -= sub->tv_nsec;
            return 1;
        }
        else
        {
            ts->tv_nsec = 0;
            return 0;
        }
    }
}

int timespec_cmp(const struct timespec *x, const struct timespec *y)
{
    assert(x && y);
    if (x->tv_sec < y->tv_sec)
    {
        return -1;
    }
    else if (x->tv_sec > y->tv_sec)
    {
        return 1;
    }
    else /* if (x->tv_sec == y->tv_sec) */
    {
        if (x->tv_nsec < y->tv_nsec)
        {
            return -1;
        }
        else if (x->tv_nsec > y->tv_nsec)
        {
            return 1;
        }
        else
        {
            return 0;
        }
    }
}