vrpn_Shared.C

Go to the documentation of this file.

#include <math.h>   // for floor, fmod
#include <stddef.h> // for size_t
#include <stdio.h>  // for fprintf() and such
#include <ctime>
 
#ifdef _MSC_VER
// Don't tell us about strcpy being dangerous.
#define _CRT_SECURE_NO_WARNINGS 1
#pragma warning(disable : 4996)
#endif
 
#include "vrpn_Shared.h"
 
#if defined(__APPLE__) || defined(__ANDROID__)
#include <unistd.h>
#endif
 
#if !(defined(_WIN32) && defined(VRPN_USE_WINSOCK_SOCKETS))
#include <sys/select.h> // for select
#include <netinet/in.h> // for htonl, htons
#endif
 
#define CHECK(a) \
    if (a == -1) return -1
 
#if defined(VRPN_USE_WINSOCK_SOCKETS)
/* from HP-UX */\
/* minGW guards the definition, so we do so here as well.
minGW declares more, so we are not defining _TIMEZONE_DEFINED. */
#ifndef _TIMEZONE_DEFINED /* also in sys/time.h */
struct timezone {
    int tz_minuteswest; /* minutes west of Greenwich */
    int tz_dsttime;     /* type of dst correction */
};
#endif /* _TIMEZONE_DEFINED */
#endif
 
// perform normalization of a timeval
// XXX this still needs to be checked for errors if the timeval
// or the rate is negative
static inline void timevalNormalizeInPlace(timeval &in_tv)
{
    const long div_77777 = (in_tv.tv_usec / 1000000);
    in_tv.tv_sec += div_77777;
    in_tv.tv_usec -= (div_77777 * 1000000);
}
timeval vrpn_TimevalNormalize(const timeval &in_tv)
{
    timeval out_tv = in_tv;
    timevalNormalizeInPlace(out_tv);
    return out_tv;
}
 
// Calcs the sum of tv1 and tv2.  Returns the sum in a timeval struct.
// Calcs negative times properly, with the appropriate sign on both tv_sec
// and tv_usec (these signs will match unless one of them is 0).
// NOTE: both abs(tv_usec)'s must be < 1000000 (ie, normal timeval format)
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
{
    timeval tvSum = tv1;
 
    tvSum.tv_sec += tv2.tv_sec;
    tvSum.tv_usec += tv2.tv_usec;
 
    // do borrows, etc to get the time the way i want it: both signs the same,
    // and abs(usec) less than 1e6
    if (tvSum.tv_sec > 0) {
        if (tvSum.tv_usec < 0) {
            tvSum.tv_sec--;
            tvSum.tv_usec += 1000000;
        }
        else if (tvSum.tv_usec >= 1000000) {
            tvSum.tv_sec++;
            tvSum.tv_usec -= 1000000;
        }
    }
    else if (tvSum.tv_sec < 0) {
        if (tvSum.tv_usec > 0) {
            tvSum.tv_sec++;
            tvSum.tv_usec -= 1000000;
        }
        else if (tvSum.tv_usec <= -1000000) {
            tvSum.tv_sec--;
            tvSum.tv_usec += 1000000;
        }
    }
    else {
        // == 0, so just adjust usec
        if (tvSum.tv_usec >= 1000000) {
            tvSum.tv_sec++;
            tvSum.tv_usec -= 1000000;
        }
        else if (tvSum.tv_usec <= -1000000) {
            tvSum.tv_sec--;
            tvSum.tv_usec += 1000000;
        }
    }
 
    return tvSum;
}
 
// Calcs the diff between tv1 and tv2.  Returns the diff in a timeval struct.
// Calcs negative times properly, with the appropriate sign on both tv_sec
// and tv_usec (these signs will match unless one of them is 0)
timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
{
    timeval tv;
 
    tv.tv_sec = -tv2.tv_sec;
    tv.tv_usec = -tv2.tv_usec;
 
    return vrpn_TimevalSum(tv1, tv);
}
 
timeval vrpn_TimevalScale(const timeval &tv, double scale)
{
    timeval result;
    result.tv_sec = (long)(tv.tv_sec * scale);
    result.tv_usec =
        (long)(tv.tv_usec * scale + fmod(tv.tv_sec * scale, 1.0) * 1000000.0);
    timevalNormalizeInPlace(result);
    return result;
}
 
// returns 1 if tv1 is greater than tv2;  0 otherwise
bool vrpn_TimevalGreater(const timeval &tv1, const timeval &tv2)
{
    if (tv1.tv_sec > tv2.tv_sec) return 1;
    if ((tv1.tv_sec == tv2.tv_sec) && (tv1.tv_usec > tv2.tv_usec)) return 1;
    return 0;
}
 
// return 1 if tv1 is equal to tv2; 0 otherwise
bool vrpn_TimevalEqual(const timeval &tv1, const timeval &tv2)
{
    if (tv1.tv_sec == tv2.tv_sec && tv1.tv_usec == tv2.tv_usec)
        return true;
    else
        return false;
}
 
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
{
    return (endT.tv_usec - startT.tv_usec) +
           1000000L * (endT.tv_sec - startT.tv_sec);
}
 
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
{
    return (endT.tv_usec - startT.tv_usec) / 1000000.0 +
           (endT.tv_sec - startT.tv_sec);
}
 
double vrpn_TimevalMsecs(const timeval &tv)
{
    return tv.tv_sec * 1000.0 + tv.tv_usec / 1000.0;
}
 
timeval vrpn_MsecsTimeval(const double dMsecs)
{
    timeval tv;
    tv.tv_sec = (long)floor(dMsecs / 1000.0);
    tv.tv_usec = (long)((dMsecs / 1000.0 - tv.tv_sec) * 1e6);
    return tv;
}
 
// Sleep for dMsecs milliseconds, freeing up the processor while you
// are doing so.
 
void vrpn_SleepMsecs(double dMilliSecs)
{
#if defined(_WIN32)
    Sleep((DWORD)dMilliSecs);
#else
    timeval timeout;
 
    // Convert milliseconds to seconds
    timeout.tv_sec = (int)(dMilliSecs / 1000.0);
 
    // Subtract of whole number of seconds
    dMilliSecs -= timeout.tv_sec * 1000;
 
    // Convert remaining milliseconds to microsec
    timeout.tv_usec = (int)(dMilliSecs * 1000);
 
    // A select() with NULL file descriptors acts like a microsecond
    // timer.
    select(0, 0, 0, 0, &timeout); // wait for that long;
#endif
}
 
// convert vrpn_float64 to/from network order
// I have chosen big endian as the network order for vrpn_float64
// to match the standard for htons() and htonl().
// NOTE: There is an added complexity when we are using an ARM
// processor in mixed-endian mode for the doubles, whereby we need
// to not just swap all of the bytes but also swap the two 4-byte
// words to get things in the right order.
#if defined(__arm__)
#include <endian.h>
#endif
 
vrpn_float64 vrpn_htond(vrpn_float64 d)
{
    if (!vrpn_big_endian) {
        vrpn_float64 dSwapped;
        char *pchSwapped = (char *)&dSwapped;
        char *pchOrig = (char *)&d;
 
        // swap to big-endian order.
        unsigned i;
        for (i = 0; i < sizeof(vrpn_float64); i++) {
            pchSwapped[i] = pchOrig[sizeof(vrpn_float64) - i - 1];
        }
 
#if defined(__arm__) && !defined(__ANDROID__)
// On ARM processor, see if we're in mixed mode.  If so,
// we need to swap the two words after doing the total
// swap of bytes.
#if __FLOAT_WORD_ORDER != __BYTE_ORDER
        {
            /* Fixup mixed endian floating point machines */
            vrpn_uint32 *pwSwapped = (vrpn_uint32 *)&dSwapped;
            vrpn_uint32 scratch = pwSwapped[0];
            pwSwapped[0] = pwSwapped[1];
            pwSwapped[1] = scratch;
        }
#endif
#endif
 
        return dSwapped;
    }
    else {
        return d;
    }
}
 
// they are their own inverses, so ...
vrpn_float64 vrpn_ntohd(vrpn_float64 d) { return vrpn_htond(d); }

 
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
{
    vrpn_int32 sec, usec;
 
    // tv_sec and usec are 64 bits on some architectures, but we
    // define them as 32 bit for network transmission
 
    sec = t.tv_sec;
    usec = t.tv_usec;
 
    if (vrpn_buffer(insertPt, buflen, sec)) return -1;
    return vrpn_buffer(insertPt, buflen, usec);
}

 
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen,
                         const char *string, vrpn_int32 length)
{
    if (length > *buflen) {
        fprintf(stderr, "vrpn_buffer:  buffer not long enough for string.\n");
        return -1;
    }
 
    if (length == -1) {
        size_t len =
            strlen(string) + 1; // +1 for the NULL terminating character
        if (len > (unsigned)*buflen) {
            fprintf(stderr,
                    "vrpn_buffer:  buffer not long enough for string.\n");
            return -1;
        }
        vrpn_strncpynull(*insertPt, string, len);
        *insertPt += len;
        *buflen -= static_cast<vrpn_int32>(len);
    }
    else {
        memcpy(*insertPt, string, length);
        *insertPt += length;
        *buflen -= length;
    }
 
    return 0;
}

 
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
{
    vrpn_int32 sec, usec;
 
    CHECK(vrpn_unbuffer(buffer, &sec));
    CHECK(vrpn_unbuffer(buffer, &usec));
 
    t->tv_sec = sec;
    t->tv_usec = usec;
 
    return 0;
}

 
VRPN_API int vrpn_unbuffer(const char **buffer, char *string, vrpn_int32 length)
{
    if (!string) return -1;
 
    if (length < 0) {
        // Read the string up to maximum length, then check to make sure we
        // found the null-terminator in the length we read.
        size_t max_len = static_cast<size_t>(-length);
        strncpy(string, *buffer, max_len);
        size_t i;
        bool found = false;
        for (i = 0; i < max_len; i++) {
            if (string[i] == '\0') {
                found = true;
                break;
            }
        }
        if (!found) {
            return -1;
        }
        *buffer += strlen(*buffer) + 1; // +1 for NULL terminating character
    } else {
        memcpy(string, *buffer, length);
        *buffer += length;
    }
 
    return 0;
}
 
//=====================================================================
// This section contains various implementations of vrpn_gettimeofday().
//   Which one is selected depends on various #defines.  There is a second
// section that deals with handling various configurations on Windows.
//   The first section deals with the fact that we may want to use the
// std::chrono classes introduced in C++-11 as a cross-platform (even
// Windows) solution to timing.  If VRPN_USE_STD_CHRONO is defined, then
// we do this -- converting from chrono epoch and interval into the
// gettimeofday() standard tick of microseconds and epoch start of
// midnight, January 1, 1970.

// Implementation with std::chrono follows, and overrides any of
// the Windows-specific definitions if it is present.
 
#ifdef VRPN_USE_STD_CHRONO
#include <chrono>

// With Visual Studio 2013 64-bit, the hires clock produces a clock that has a
// tick interval of around 15.6 MILLIseconds, repeating the same
// time between them.
// With Visual Studio 2015 64-bit, the hires clock produces a good, high-
// resolution clock with no blips.  However, its epoch seems to
// restart when the machine boots, whereas the system clock epoch
// starts at the standard midnight January 1, 1970.

// Helper function to convert from the high-resolution clock
// time to the equivalent system clock time (assuming no clock
// adjustment on the system clock since program start).
//  To make this thread safe, we semaphore the determination of
// the offset to be applied.  To handle a slow-ticking system
// clock, we repeatedly sample it until we get a change.
//  This assumes that the high-resolution clock on different
// threads has the same epoch.
 
static bool hr_offset_determined = false;
static vrpn_Semaphore hr_offset_semaphore;
static struct timeval hr_offset;
 
static struct timeval high_resolution_time_to_system_time(
    struct timeval hi_res_time //< Time computed from high-resolution clock
    )
{
    // If we haven't yet determined the offset between the high-resolution
    // clock and the system clock, do so now.  Avoid a race between threads
    // using the semaphore and checking the boolean both before and after
    // grabbing the semaphore (in case someone beat us to it).
    if (!hr_offset_determined) {
        hr_offset_semaphore.p();
        // Someone else who had the semaphore may have beaten us to this.
        if (!hr_offset_determined) {
            // Watch the system clock until it changes; this will put us
            // at a tick boundary.  On many systems, this will change right
            // away, but on Windows 8 it will only tick every 16ms or so.
            std::chrono::system_clock::time_point pre =
                std::chrono::system_clock::now();
            std::chrono::system_clock::time_point post;
            // On Windows 8.1, this took from 1-16 ticks, and seemed to
            // get offsets to the epoch that were consistent to within
            // around 1ms.
            do {
                post = std::chrono::system_clock::now();
            } while (pre == post);
 
            // Now read the high-resolution timer to find out the time
            // equivalent to the post time on the system clock.
            std::chrono::high_resolution_clock::time_point high =
                std::chrono::high_resolution_clock::now();
 
            // Now convert both the hi-resolution clock time and the
            // post-tick system clock time into struct timevals and
            // store the difference between them as the offset.
            std::time_t high_secs =
                std::chrono::duration_cast<std::chrono::seconds>(
                    high.time_since_epoch())
                    .count();
            std::chrono::high_resolution_clock::time_point
                fractional_high_secs = high - std::chrono::seconds(high_secs);
            struct timeval high_time;
            high_time.tv_sec = static_cast<unsigned long>(high_secs);
            high_time.tv_usec = static_cast<unsigned long>(
                std::chrono::duration_cast<std::chrono::microseconds>(
                    fractional_high_secs.time_since_epoch())
                    .count());
 
            std::time_t post_secs =
                std::chrono::duration_cast<std::chrono::seconds>(
                    post.time_since_epoch())
                    .count();
            std::chrono::system_clock::time_point fractional_post_secs =
                post - std::chrono::seconds(post_secs);
            struct timeval post_time;
            post_time.tv_sec = static_cast<unsigned long>(post_secs);
            post_time.tv_usec = static_cast<unsigned long>(
                std::chrono::duration_cast<std::chrono::microseconds>(
                    fractional_post_secs.time_since_epoch())
                    .count());
 
            hr_offset = vrpn_TimevalDiff(post_time, high_time);
 
            // We've found our offset ... re-use it from here on.
            hr_offset_determined = true;
        }
        hr_offset_semaphore.v();
    }
 
    // The offset has been determined, by us or someone else.  Apply it.
    return vrpn_TimevalSum(hi_res_time, hr_offset);
}
 
int vrpn_gettimeofday(timeval *tp, void *tzp)
{
    // If we have nothing to fill in, don't try.
    if (tp == NULL) {
        return 0;
    }
    struct timezone *timeZone = reinterpret_cast<struct timezone *>(tzp);
 
    // Find out the time, and how long it has been in seconds since the
    // epoch.
    std::chrono::high_resolution_clock::time_point now =
        std::chrono::high_resolution_clock::now();
    std::time_t secs =
        std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch())
            .count();
 
    // Subtract the time in seconds from the full time to get a
    // remainder that is a fraction of a second since the epoch.
    std::chrono::high_resolution_clock::time_point fractional_secs =
        now - std::chrono::seconds(secs);
 
    // Store the seconds and the fractional seconds as microseconds into
    // the timeval structure.  Then convert from the hi-res clock time
    // to system clock time.
    struct timeval hi_res_time;
    hi_res_time.tv_sec = static_cast<unsigned long>(secs);
    hi_res_time.tv_usec = static_cast<unsigned long>(
        std::chrono::duration_cast<std::chrono::microseconds>(
            fractional_secs.time_since_epoch())
            .count());
    *tp = high_resolution_time_to_system_time(hi_res_time);
 
    // @todo Fill in timezone structure with relevant info.
    if (timeZone != NULL) {
        timeZone->tz_minuteswest = 0;
        timeZone->tz_dsttime = 0;
    }
 
    return 0;
}
 
#else // VRPN_USE_STD_CHRONO

// Implementation without std::chrono follows.

// More accurate gettimeofday() on some Windows operating systems
// and machines can be gotten by using the Performance Counter
// on the CPU.  This doesn't seem to work in NT/2000 for some
// computers, so the code to do it is #defined out by default.
// To put it back back, #define VRPN_UNSAFE_WINDOWS_CLOCK and
// the following code will use the performance counter (which it
// takes a second to calibrate at program start-up).
 
#ifndef VRPN_UNSAFE_WINDOWS_CLOCK
 
#if defined(_WIN32) && !defined(__CYGWIN__)
#include <math.h> // for floor, fmod
 
#pragma optimize("", on)
 
#ifdef _WIN32
void get_time_using_GetLocalTime(unsigned long &sec, unsigned long &usec)
{
    SYSTEMTIME stime;   // System time in funky structure
    FILETIME ftime;     // Time in 100-nsec intervals since Jan 1 1601
    ULARGE_INTEGER tics; // ftime stored into a 64-bit quantity
 
    GetLocalTime(&stime);
    SystemTimeToFileTime(&stime, &ftime);
 
    // Copy the data into a structure that can be treated as a 64-bit integer
    tics.HighPart = ftime.dwHighDateTime;
    tics.LowPart = ftime.dwLowDateTime;
 
    // Change units (100 nanoseconds --> microseconds)
    tics.QuadPart /= 10;
    
    // Subtract the offset between the two clock bases (Jan 1, 1601 --> Jan 1, 1970)
    tics.QuadPart -= 11644473600000000ULL;
 
    // Convert the 64-bit time into seconds and microseconds since Jan 1 1970
    sec = (unsigned long)(tics.QuadPart / 1000000UL);
    usec = (unsigned long)(tics.QuadPart % 1000000UL);
}
#endif

// Although VC++ doesn't include a gettimeofday
// call, Cygnus Solutions Cygwin32 environment does,
// so this is not used when compiling with gcc under WIN32.
// XXX Note that the cygwin one was wrong in an earlier
// version.  It is claimed that they have fixed it now, but
// better check.
int vrpn_gettimeofday(timeval *tp, void *tzp)
{
    struct timezone *timeZone = reinterpret_cast<struct timezone *>(tzp);
 
    if (tp != NULL) {
#ifdef _WIN32_WCE
        unsigned long sec, usec;
        get_time_using_GetLocalTime(sec, usec);
        tp->tv_sec = sec;
        tp->tv_usec = usec;
#else
        struct _timeb t;
        _ftime(&t);
        tp->tv_sec = t.time;
        tp->tv_usec = (long)t.millitm * 1000;
#endif
    }
    if (tzp != NULL) {
        TIME_ZONE_INFORMATION tz;
        GetTimeZoneInformation(&tz);
        timeZone->tz_minuteswest = tz.Bias;
        timeZone->tz_dsttime = (tz.StandardBias != tz.Bias);
    }
    return 0;
}
 
#endif // defined(_WIN32)
 
#else // In the case that VRPN_UNSAFE_WINDOWS_CLOCK is defined
 
// Although VC++ doesn't include a gettimeofday
// call, Cygnus Solutions Cygwin32 environment does.
// XXX AND ITS WRONG in the current release 10/11/99, version b20.1
// They claim it will be fixed in the next release, version b21
// so until then, we will make it right using our solution.
#if defined(_WIN32) && !defined(__CYGWIN__)
#include <math.h> // for floor, fmod
 
// utility routines to read the pentium time stamp counter
// QueryPerfCounter drifts too much -- others have documented this
// problem on the net
 
// This is all based on code extracted from the UNC hiball tracker cib lib
 
// 200 mhz pentium default -- we change this based on our calibration
static __int64 VRPN_CLOCK_FREQ = 200000000;
 
// Helium to histidine
// __int64 FREQUENCY = 199434500;
 
// tori -- but queryperfcounter returns this for us
// __int64 FREQUENCY = 198670000;
 
// Read Time Stamp Counter
#define rdtsc(li)                                                              \
    {                                                                          \
        _asm _emit 0x0f _asm _emit 0x31 _asm mov li.LowPart,                   \
            eax _asm mov li.HighPart, edx                                      \
    }
 
/*
 * calculate the time stamp counter register frequency (clock freq)
 */
#ifndef VRPN_WINDOWS_CLOCK_V2
#pragma optimize("", off)
static int vrpn_AdjustFrequency(void)
{
    const int loops = 2;
    const int tPerLoop = 500; // milliseconds for Sleep()
    fprintf(stderr, "vrpn vrpn_gettimeofday: determining clock frequency...");
 
    LARGE_INTEGER startperf, endperf;
    LARGE_INTEGER perffreq;
 
    // See if the hardware supports the high-resolution performance counter.
    // If so, get the frequency of it.  If not, we can't use it and so return
    // -1.
    if (QueryPerformanceFrequency(&perffreq) == 0) {
        return -1;
    }
 
    // don't optimize away these variables
    double sum = 0;
    volatile LARGE_INTEGER liStart, liEnd;
 
    DWORD dwPriorityClass = GetPriorityClass(GetCurrentProcess());
    int iThreadPriority = GetThreadPriority(GetCurrentThread());
    SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
    SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
 
    // pull all into cache and do rough test to see if tsc and perf counter
    // are one and the same
    rdtsc(liStart);
    QueryPerformanceCounter(&startperf);
    Sleep(100);
    rdtsc(liEnd);
    QueryPerformanceCounter(&endperf);
 
    double freq = perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
                  ((double)(endperf.QuadPart - startperf.QuadPart));
 
    if (fabs(perffreq.QuadPart - freq) < 0.05 * freq) {
        VRPN_CLOCK_FREQ = (__int64)perffreq.QuadPart;
        fprintf(stderr, "\nvrpn vrpn_gettimeofday: perf clock is tsc -- using "
                        "perf clock freq ( %lf MHz)\n",
                perffreq.QuadPart / 1e6);
        SetPriorityClass(GetCurrentProcess(), dwPriorityClass);
        SetThreadPriority(GetCurrentThread(), iThreadPriority);
        return 0;
    }
 
    // either tcs and perf clock are not the same, or we could not
    // tell accurately enough with the short test. either way we now
    // need an accurate frequency measure, so ...
 
    fprintf(stderr, " (this will take %lf seconds)...\n",
            loops * tPerLoop / 1000.0);
 
    for (int j = 0; j < loops; j++) {
        rdtsc(liStart);
        QueryPerformanceCounter(&startperf);
        Sleep(tPerLoop);
        rdtsc(liEnd);
        QueryPerformanceCounter(&endperf);
 
        // perf counter timer ran for one call to Query and one call to
        // tcs read in addition to the time between the tsc readings
        // tcs read did the same
 
        // endperf - startperf / perf freq = time between perf queries
        // endtsc - starttsc = clock ticks between perf queries
        //    sum += (endtsc - starttsc) / ((double)(endperf -
        //    startperf)/perffreq);
        sum += perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
               ((double)(endperf.QuadPart - startperf.QuadPart));
    }
 
    SetPriorityClass(GetCurrentProcess(), dwPriorityClass);
    SetThreadPriority(GetCurrentThread(), iThreadPriority);
 
    // might want last, not sum -- just get into cache and run
    freq = (sum / loops);
 
    // if we are on a uniprocessor system, then use the freq estimate
    // This used to check against a 200 mhz assumed clock, but now
    // we assume the routine works and trust the result.
    //  if (fabs(freq - VRPN_CLOCK_FREQ) > 0.05 * VRPN_CLOCK_FREQ) {
    //    cerr << "vrpn vrpn_gettimeofday: measured freq is " << freq/1e6
    //   << " MHz - DOES NOT MATCH" << endl;
    //    return -1;
    //  }
 
    // if we are in a system where the perf clock is the tsc, then use the
    // rate the perf clock returns (or rather, if the freq we measure is
    // approx the perf clock freq).
    if (fabs(perffreq.QuadPart - freq) < 0.05 * freq) {
        VRPN_CLOCK_FREQ = perffreq.QuadPart;
        fprintf(stderr, "vrpn vrpn_gettimeofday: perf clock is tsc -- using "
                        "perf clock freq ( %lf MHz)\n",
                perffreq.QuadPart / 1e6);
    }
    else {
        fprintf(stderr, "vrpn vrpn_gettimeofday: adjusted clock freq to "
                        "measured freq ( %lf MHz )\n",
                freq / 1e6);
    }
    VRPN_CLOCK_FREQ = (__int64)freq;
    return 0;
}
#pragma optimize("", on)
#endif
 
// The pc has no gettimeofday call, and the closest thing to it is _ftime.
// _ftime, however, has only about 6 ms resolution, so we use the peformance
// as an offset from a base time which is established by a call to by _ftime.
 
// The first call to vrpn_gettimeofday will establish a new time frame
// on which all later calls will be based.  This means that the time returned
// by vrpn_gettimeofday will not always match _ftime (even at _ftime's
// resolution),
// but it will be consistent across all vrpn_gettimeofday calls.

// Although VC++ doesn't include a gettimeofday
// call, Cygnus Solutions Cygwin32 environment does,
// so this is not used when compiling with gcc under WIN32
 
// XXX AND ITS WRONG in the current release 10/11/99
// They claim it will be fixed in the next release,
// so until then, we will make it right using our solution.
#ifndef VRPN_WINDOWS_CLOCK_V2
int vrpn_gettimeofday(timeval *tp, void *tzp)
{
    struct timezone *timeZone = reinterpret_cast<struct timezone *>(tzp);
    static int fFirst = 1;
    static int fHasPerfCounter = 1;
    static struct _timeb tbInit;
    static LARGE_INTEGER liInit;
    static LARGE_INTEGER liNow;
    static LARGE_INTEGER liDiff;
    timeval tvDiff;
 
    if (!fHasPerfCounter) {
        _ftime(&tbInit);
        tp->tv_sec = tbInit.time;
        tp->tv_usec = tbInit.millitm * 1000;
        return 0;
    }
 
    if (fFirst) {
        LARGE_INTEGER liTemp;
        // establish a time base
        fFirst = 0;
 
        // Check to see if we are on a Windows NT machine (as opposed to a
        // Windows 95/98 machine).  If we are not, then use the _ftime code
        // because the hi-perf clock does not work under Windows 98SE on my
        // laptop, although the query for one seems to indicate that it is
        // available.
 
        {
            OSVERSIONINFO osvi;
 
            memset(&osvi, 0, sizeof(OSVERSIONINFO));
            osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);
            GetVersionEx(&osvi);
 
            if (osvi.dwPlatformId != VER_PLATFORM_WIN32_NT) {
                fprintf(stderr,
                        "\nvrpn_gettimeofday: disabling hi performance clock "
                        "on non-NT system. "
                        "Defaulting to _ftime (~6 ms resolution) ...\n");
                fHasPerfCounter = 0;
                vrpn_gettimeofday(tp, tzp);
                return 0;
            }
        }
 
        // check that hi-perf clock is available
        if (!(fHasPerfCounter = QueryPerformanceFrequency(&liTemp))) {
            fprintf(stderr,
                    "\nvrpn_gettimeofday: no hi performance clock available. "
                    "Defaulting to _ftime (~6 ms resolution) ...\n");
            fHasPerfCounter = 0;
            vrpn_gettimeofday(tp, tzp);
            return 0;
        }
 
        if (vrpn_AdjustFrequency() < 0) {
            fprintf(stderr,
                    "\nvrpn_gettimeofday: can't verify clock frequency. "
                    "Defaulting to _ftime (~6 ms resolution) ...\n");
            fHasPerfCounter = 0;
            vrpn_gettimeofday(tp, tzp);
            return 0;
        }
        // get current time
        // We assume this machine has a time stamp counter register --
        // I don't know of an easy way to check for this
        rdtsc(liInit);
        _ftime(&tbInit);
 
        // we now consider it to be exactly the time _ftime returned
        // (beyond the resolution of _ftime, down to the perfCounter res)
    }
 
    // now do the regular get time call to get the current time
    rdtsc(liNow);
 
    // find offset from initial value
    liDiff.QuadPart = liNow.QuadPart - liInit.QuadPart;
 
    tvDiff.tv_sec = (long)(liDiff.QuadPart / VRPN_CLOCK_FREQ);
    tvDiff.tv_usec =
        (long)(1e6 * ((liDiff.QuadPart - VRPN_CLOCK_FREQ * tvDiff.tv_sec) /
                      (double)VRPN_CLOCK_FREQ));
 
    // pack the value and clean it up
    tp->tv_sec = tbInit.time + tvDiff.tv_sec;
    tp->tv_usec = tbInit.millitm * 1000 + tvDiff.tv_usec;
    while (tp->tv_usec >= 1000000) {
        tp->tv_sec++;
        tp->tv_usec -= 1000000;
    }
 
    return 0;
}
#else // VRPN_WINDOWS_CLOCK_V2 is defined
 
void get_time_using_GetLocalTime(unsigned long &sec, unsigned long &usec)
{
    static LARGE_INTEGER first_count = {0, 0};
    static unsigned long first_sec, first_usec;
    static LARGE_INTEGER perf_freq; //< Frequency of the performance counter.
    SYSTEMTIME stime;               // System time in funky structure
    FILETIME ftime;     // Time in 100-nsec intervals since Jan 1 1601
    LARGE_INTEGER tics; // ftime stored into a 64-bit quantity
    LARGE_INTEGER perf_counter;
 
    // The first_count value will be zero only the first time through; we
    // rely on this to set up the structures needed to interpret the data
    // that we get from querying the performance counter.
    if (first_count.QuadPart == 0) {
        QueryPerformanceCounter(&first_count);
 
        // Find out how fast the performance counter runs.  Store this for later
        // runs.
        QueryPerformanceFrequency(&perf_freq);
 
        // Find out what time it is in a Windows structure.
        GetLocalTime(&stime);
        SystemTimeToFileTime(&stime, &ftime);
 
        // Copy the data into a structure that can be treated as a 64-bit
        // integer
        tics.HighPart = ftime.dwHighDateTime;
        tics.LowPart = ftime.dwLowDateTime;
 
        // Convert the 64-bit time into seconds and microseconds since July 1
        // 1601
        sec = (long)(tics.QuadPart / 10000000L);
        usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
        first_sec = sec;
        first_usec = usec;
    }
    else {
        QueryPerformanceCounter(&perf_counter);
        if (perf_counter.QuadPart >= first_count.QuadPart) {
            perf_counter.QuadPart =
                perf_counter.QuadPart - first_count.QuadPart;
        }
        else {
            // Take care of the case when the counter rolls over.
            perf_counter.QuadPart = 0x7fffffffffffffffLL -
                                    first_count.QuadPart +
                                    perf_counter.QuadPart;
        }
 
        // Reinterpret the performance counter into seconds and microseconds
        // by dividing by the performance counter.  Microseconds is placed
        // into perf_counter by subtracting the seconds value out, then
        // multiplying by 1 million and re-dividing by the performance counter.
        sec = (long)(perf_counter.QuadPart / perf_freq.QuadPart);
        perf_counter.QuadPart -= perf_freq.QuadPart * sec;
        perf_counter.QuadPart *= 1000000L; //< Turn microseconds into seconds
        usec = first_usec + (long)(perf_counter.QuadPart / perf_freq.QuadPart);
        sec += first_sec;
 
        // Make sure that we've not got more than a million microseconds.
        // If so, then shift it into seconds.  We don't expect it to be above
        // more than 1 million because we added two things, each of which were
        // less than 1 million.
        if (usec > 1000000L) {
            usec -= 1000000L;
            sec++;
        }
    }
 
    // Translate the time to be based on January 1, 1970 (_ftime base)
    // The offset here is gotten by using the "time_test" program to report the
    // difference in seconds between the two clocks.
    sec -= 3054524608L;
}
 
int vrpn_gettimeofday(timeval *tp, void *tzp)
{
    struct timezone *timeZone = reinterpret_cast<struct timezone *>(tzp);
    unsigned long sec, usec;
    get_time_using_GetLocalTime(sec, usec);
    tp->tv_sec = sec;
    tp->tv_usec = usec;
    if (tzp != NULL) {
        TIME_ZONE_INFORMATION tz;
        GetTimeZoneInformation(&tz);
        timeZone->tz_minuteswest = tz.Bias;
        timeZone->tz_dsttime = (tz.StandardBias != tz.Bias);
    }
    return 0;
}
 
#endif // defined(VRPN_WINDOWS_CLOCK_V2)
 
#endif // defined(_WIN32)
 
// do the calibration before the program ever starts up
static timeval __tv;
static int __iTrash = vrpn_gettimeofday(&__tv, (struct timezone *)NULL);
 
#endif // VRPN_UNSAFE_WINDOWS_CLOCK
 
#endif // VRPN_USE_STD_CHRONO
 
// End of the section dealing with vrpn_gettimeofday()
//=====================================================================
 
bool vrpn_test_pack_unpack(void)
{
    // Get a buffer to use that is large enough to test all of the routines.
    vrpn_float64 dbuffer[256];
    vrpn_int32 buflen;
 
    vrpn_float64 in_float64 = 42.1;
    vrpn_int32 in_int32 = 17;
    vrpn_uint16 in_uint16 = 397;
    vrpn_uint8 in_uint8 = 1;
 
    vrpn_float64 out_float64;
    vrpn_int32 out_int32;
    vrpn_uint16 out_uint16;
    vrpn_uint8 out_uint8;
 
    // Test packing using little-endian routines.
    // IMPORTANT: Do these from large to small to get good alignment.
    char *bufptr = (char *)dbuffer;
    buflen = sizeof(dbuffer);
    if (vrpn_buffer_to_little_endian(&bufptr, &buflen, in_float64) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer little endian\n");
        return false;
    }
    if (vrpn_buffer_to_little_endian(&bufptr, &buflen, in_int32) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer little endian\n");
        return false;
    }
    if (vrpn_buffer_to_little_endian(&bufptr, &buflen, in_uint16) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer little endian\n");
        return false;
    }
    if (vrpn_buffer_to_little_endian(&bufptr, &buflen, in_uint8) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer little endian\n");
        return false;
    }
 
    // Test unpacking using little-endian routines.
    bufptr = (char *)dbuffer;
    if (in_float64 !=
        (out_float64 =
             vrpn_unbuffer_from_little_endian<vrpn_float64>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer little endian\n");
        return false;
    }
    if (in_int32 !=
        (out_int32 = vrpn_unbuffer_from_little_endian<vrpn_int32>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer little endian\n");
        return false;
    }
    if (in_uint16 !=
        (out_uint16 = vrpn_unbuffer_from_little_endian<vrpn_uint16>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer little endian\n");
        return false;
    }
    if (in_uint8 !=
        (out_uint8 = vrpn_unbuffer_from_little_endian<vrpn_uint8>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer little endian\n");
        return false;
    }
 
    // Test packing using big-endian routines.
    bufptr = (char *)dbuffer;
    buflen = sizeof(dbuffer);
    if (vrpn_buffer(&bufptr, &buflen, in_float64) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer big endian\n");
        return false;
    }
    if (vrpn_buffer(&bufptr, &buflen, in_int32) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer big endian\n");
        return false;
    }
    if (vrpn_buffer(&bufptr, &buflen, in_uint16) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer big endian\n");
        return false;
    }
    if (vrpn_buffer(&bufptr, &buflen, in_uint8) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer big endian\n");
        return false;
    }
 
    // Test unpacking using big-endian routines.
    bufptr = (char *)dbuffer;
    if (in_float64 != (out_float64 = vrpn_unbuffer<vrpn_float64>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer big endian\n");
        return false;
    }
    if (in_int32 != (out_int32 = vrpn_unbuffer<vrpn_int32>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer big endian\n");
        return false;
    }
    if (in_uint16 != (out_uint16 = vrpn_unbuffer<vrpn_uint16>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer big endian\n");
        return false;
    }
    if (in_uint8 != (out_uint8 = vrpn_unbuffer<vrpn_uint8>(bufptr))) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not unbuffer big endian\n");
        return false;
    }
 
    // XXX Test pack/unpack of all other types.
 
    // Test packing little-endian and unpacking big-endian; they should
    // be different.
    bufptr = (char *)dbuffer;
    buflen = sizeof(dbuffer);
    if (vrpn_buffer_to_little_endian(&bufptr, &buflen, in_float64) != 0) {
        fprintf(stderr,
                "vrpn_test_pack_unpack(): Could not buffer little endian\n");
        return false;
    }
    bufptr = (char *)dbuffer;
    if (in_float64 == (out_float64 = vrpn_unbuffer<vrpn_float64>(bufptr))) {
        fprintf(
            stderr,
            "vrpn_test_pack_unpack(): Cross-packing produced same result\n");
        return false;
    }
 
    return true;
}
 
bool vrpn_test_vrpn_vector(void)
{
  // Test the default constructor and ensure that the destructor doesn't crash.
  {
    vrpn_vector<int> v0;
    if ((v0.size() != 0) || (v0.data() != 0)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Default constructor failed\n");
      return false;
    }
  }
  
  // Test the sized contructor, writing to the vector, the copy
  // constructor, and the [] operator.
  {
    vrpn_uint32 count = 19;
    vrpn_vector<vrpn_uint32> v1(count);
    if (v1.size() != count) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Sized constructor size failed\n");
      return false;
    }
    for (vrpn_uint32 i = 0; i < v1.size(); i++) {
      v1[i] = i;
    }
    vrpn_vector<vrpn_uint32> v2(v1);
    if (v2.size() != count) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Copy constructor size failed\n");
      return false;
    }
    for (vrpn_uint32 i = 0; i < v2.size(); i++) {
      if (v1[i] != i) {
        fprintf(stderr,"vrpn_test_vrpn_vector(): Copy constructor data failed\n");
        return false;
      }
    }
  }
  
  // Test range constructor
  {
    vrpn_uint32 count = 19;
    vrpn_vector<vrpn_uint32> v1(count);
    for (vrpn_uint32 i = 0; i < v1.size(); i++) {
      v1[i] = i;
    }
    vrpn_vector<vrpn_uint32> v2(&v1.data()[1], &v1.data()[10]);
    if (v2.size() != 9) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Range constructor size failed\n");
      return false;
    }
    for (vrpn_uint32 i = 0; i < v2.size(); i++) {
      if (v2[i] != i+1) {
        fprintf(stderr,"vrpn_test_vrpn_vector(): Range constructor data failed\n");
        return false;
      }
    }
  }
  
  // Test data() push_back() and empty().
  {
    vrpn_vector<vrpn_uint32> v1;
    if (!v1.empty()) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Empty failed when empty\n");
      return false;
    }
    v1.push_back(1);
    if (v1.empty()) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): Empty failed when not empty\n");
      return false;
    }
    if (v1.data()[0] != 1) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): data() failed on push_back\n");
      return false;
    }
  }
  
  // Test resize().
  {
    vrpn_vector<vrpn_uint32> v1;
    v1.push_back(1);
    v1.push_back(2);
    v1.resize(2);
    if (v1.size() != 2) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize failed on do nothing\n");
      return false;
    }
    if ((v1[0] != 1) || (v1[1] != 2)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize data failed on do nothing\n");
      return false;
    }
    v1.resize(1);
    if (v1.size() != 1) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize failed on shrink\n");
      return false;
    }
    if ((v1[0] != 1)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize data failed on shrink\n");
      return false;
    }
    v1.resize(10);
    if (v1.size() != 10) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize failed on grow\n");
      return false;
    }
    if ((v1[0] != 1)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): resize data failed on grow\n");
      return false;
    }
  }
  
  // Test front(), back(), assign(), and clear()
  {
    vrpn_vector<vrpn_uint32> v1;
    v1.push_back(1);
    v1.push_back(2);
    if (v1.front() != 1) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): front failed\n");
      return false;
    }
    if (v1.back() != 2) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): back failed\n");
      return false;
    }
    vrpn_vector<vrpn_uint32> v2;
    vrpn_uint32 count = 2;
    v2.assign(count, 7);
    if ((v2[0] != 7) || (v2[1] != 7)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): assign value failed\n");
      return false;
    }
    v2.assign(&v1[0], &v1[2]);
    if ((v2[0] != 1) || (v2[1] != 2)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): assign iterators failed\n");
      return false;
    }
    v1.clear();
    if (v1.size() != 0) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): clear failed\n");
      return false;
    }
  }
  
  // Test begin() and end()
  {
    vrpn_vector<vrpn_uint32> v0, v1(10);
    if ((v0.begin() != 0) || (v0.end() != 0)) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): empty begin/end failed\n");
      return false;
    }
    if ((v1.begin() != v1.data()) || (v1.end() != &v1.data()[10])) {
      fprintf(stderr,"vrpn_test_vrpn_vector(): begin/end failed\n");
      return false;
    }
  }
 
  // Test operator =
  {
    vrpn_vector<vrpn_uint32> v0, v1;
    v0.push_back(1);
    v0.push_back(2);
    v1 = v0;
    v1[0] = 3;
    if ((v1[1] != 2) || (v0[0] != 1)) {
      fprintf(stderr, "vrpn_test_vrpn_vector(): operator = failed (%d, %d)\n", v0[0], v1[1]);
      return false;
    }
  }
 
  return true;
}