Monk’s Blog

/*-
 * Copyright (c) 2009 Gary "Monk" Stanley <gstanley@colocrossing.com>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the author nor the names of any co-contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
 
/*
 * x86 based adaptive usleep
 */
 
#include <stdio.h>
#include <time.h>
#include <unistd.h>
#include <sys/time.h>
 
#ifdef DEBUG
#define DEBUG_ONEMILLI          /* debug gameserver for 1000 us */
#endif
 
/* conversion */
#define ONE_SECOND      1000000000      /* 1 second in nanoseconds      */
#define ONE_MSECOND     1000000         /* 1 millisecond in nanoseconds */
#define ONE_USECOND     1000            /* 1 microsecond in nanoseconds */
#define ONE_MILLI       1000            /* 1 millisecond in microseconds*/
 
/* usleep */
#define USLEEP_CLATENCY 100000          /* check latency in usleep      */
#define USLEEP_LATENCY  50000           /* default latency in usleep    */
#define USLEEP_OVERHEAD 10500           /* overhead of usleep function  */
#define DELAY_TEST      3000            /* delay loop test              */
#define USLEEP_DELAY    50              /* usleep will use dealy        */
#define USLEEP_NONE     2               /* usleep is irrelevant         */
#define USLEEP_COUNT    1               /* default usleep count         */
 
static unsigned long usleep_avg_lat = USLEEP_LATENCY;
static unsigned long usleep_count = USLEEP_COUNT;
 
#ifdef DEBUG_ONEMILLI
unsigned long long debug_usleep_count = 0;
unsigned long long debug_usleep_last = 0;
unsigned long long debug_usleep_last_last = 0;
#endif
 
static unsigned long long get_nsecs(void)
{
        unsigned long long nsec;
        struct timespec ts;
 
        clock_gettime(CLOCK_MONOTONIC, &ts);
 
        nsec = ts.tv_sec;
        nsec = (nsec * ONE_SECOND) + ts.tv_nsec;
 
        return nsec;
}
 
static inline void loopdelay(unsigned long loops)
{
        /* This code is NOT portable */
        asm volatile(
                "test %0,%0\n"
                "jz 3f\n"
                "jmp 1f\n"
                ".align 16\n"
                "1:jmp 2f\n"
                ".align 16\n"
                "2:dec %0\n"
                "jnz 2b\n"
                "3:dec %0\n"
                : : "a" (loops)
        );
}
 
static void microdelay(unsigned long usecs)
{
        unsigned long long nsecs, nsecs_delay;
 
        nsecs = get_nsecs();
        usecs = usecs * ONE_USECOND;
        nsecs_delay = get_nsecs() - nsecs;
        /* If already timeout then return */
        if (nsecs_delay >= usecs)
                return;
 
        loopdelay(DELAY_TEST);
        nsecs = get_nsecs() - nsecs;
        /* if already timeout then return */
        if (nsecs >= usecs)
                return;
 
        nsecs_delay = nsecs - nsecs_delay - nsecs_delay;
 
        loopdelay((DELAY_TEST * (usecs - nsecs)) / nsecs_delay);
}
 
#ifdef DEBUG_ONEMILLI
void debug_onemill(unsigned long long nsecs)
{
        if (debug_usleep_count % ONE_MILLI == 0) {
                if (!debug_usleep_count) {
                        debug_usleep_last = nsecs;
                        debug_usleep_count++;
                        return;
                }
                debug_usleep_last_last = debug_usleep_last;
                debug_usleep_last = nsecs;
                printf("called usleep(%d) %d times in %10llu milliseconds\n",
                        ONE_MILLI, ONE_MILLI,
                        (debug_usleep_last - debug_usleep_last_last)/ONE_MSECOND);
        }
        debug_usleep_count++;
}
#endif
 
int usleep(__useconds_t usecs)
{
        unsigned long long nsecs;
        struct timespec ts;
        int ret = 0;
 
        /*
         * no usleep is required for less then USLEEP_DELAY
         * because even if we return it will fulfill usleep.
         */
        if (usecs < USLEEP_NONE)
                return ret;
 
        /*
         * usleep will use delay for less then USLEEP_DELAY
         * because if we go for sleep we will never come back before 70-80 usecs (even with SCHED_FIFO)
         */
        if (usecs < USLEEP_DELAY) {
                microdelay(usecs);
                return ret;
        }
 
        nsecs = get_nsecs();
#ifdef DEBUG_ONEMILLI
        if (usecs == ONE_MILLI)
                debug_onemill(nsecs);
#endif
        usecs = usecs * ONE_USECOND;
        /* 
         * CLOCK_MONOTONIC is faster to read than REALTIME, but REALTIME is more 
         * accurate.
         */
        clock_gettime(CLOCK_MONOTONIC, &ts);
        if (usecs > ONE_SECOND)
                ts.tv_sec += usecs / ONE_SECOND;
 
        /*
         * calculate real sleep value based on overhead and latency of usleep()
         */
        ts.tv_nsec += usecs - USLEEP_OVERHEAD - (usleep_avg_lat / usleep_count);
        if (ts.tv_nsec > ONE_SECOND) {
                ts.tv_sec++;
                ts.tv_nsec -= ONE_SECOND;
        }
 
        ret = clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts, NULL);
        nsecs = get_nsecs() - nsecs - usecs;
 
        /*
         * check for valid latency
         */
        if (nsecs < USLEEP_CLATENCY) {
                usleep_avg_lat += nsecs;
                usleep_count++;
        }
 
        return ret;
}