7 #define _CRT_SECURE_NO_WARNINGS 1
8 #pragma warning(disable : 4996)
13 #if defined(__APPLE__) || defined(__ANDROID__)
17 #if !(defined(_WIN32) && defined(VRPN_USE_WINSOCK_SOCKETS))
18 #include <sys/select.h>
19 #include <netinet/in.h>
23 if (a == -1) return -1
28 static inline void timevalNormalizeInPlace(timeval &in_tv)
30 const long div_77777 = (in_tv.tv_usec / 1000000);
31 in_tv.tv_sec += div_77777;
32 in_tv.tv_usec -= (div_77777 * 1000000);
36 timeval out_tv = in_tv;
37 timevalNormalizeInPlace(out_tv);
49 tvSum.tv_sec += tv2.tv_sec;
50 tvSum.tv_usec += tv2.tv_usec;
54 if (tvSum.tv_sec > 0) {
55 if (tvSum.tv_usec < 0) {
57 tvSum.tv_usec += 1000000;
59 else if (tvSum.tv_usec >= 1000000) {
61 tvSum.tv_usec -= 1000000;
64 else if (tvSum.tv_sec < 0) {
65 if (tvSum.tv_usec > 0) {
67 tvSum.tv_usec -= 1000000;
69 else if (tvSum.tv_usec <= -1000000) {
71 tvSum.tv_usec += 1000000;
76 if (tvSum.tv_usec >= 1000000) {
78 tvSum.tv_usec -= 1000000;
80 else if (tvSum.tv_usec <= -1000000) {
82 tvSum.tv_usec += 1000000;
96 tv.tv_sec = -tv2.tv_sec;
97 tv.tv_usec = -tv2.tv_usec;
105 result.tv_sec = (long)(tv.tv_sec * scale);
107 (long)(tv.tv_usec * scale + fmod(tv.tv_sec * scale, 1.0) * 1000000.0);
108 timevalNormalizeInPlace(result);
115 if (tv1.tv_sec > tv2.tv_sec)
return 1;
116 if ((tv1.tv_sec == tv2.tv_sec) && (tv1.tv_usec > tv2.tv_usec))
return 1;
123 if (tv1.tv_sec == tv2.tv_sec && tv1.tv_usec == tv2.tv_usec)
131 return (endT.tv_usec - startT.tv_usec) +
132 1000000L * (endT.tv_sec - startT.tv_sec);
137 return (endT.tv_usec - startT.tv_usec) / 1000000.0 +
138 (endT.tv_sec - startT.tv_sec);
143 return tv.tv_sec * 1000.0 + tv.tv_usec / 1000.0;
149 tv.tv_sec = (long)floor(dMsecs / 1000.0);
150 tv.tv_usec = (long)((dMsecs / 1000.0 - tv.tv_sec) * 1e6);
160 Sleep((DWORD)dMsecs);
165 timeout.tv_sec = (int)(dMsecs / 1000.0);
168 dMsecs -= timeout.tv_sec * 1000;
171 timeout.tv_usec = (int)(dMsecs * 1000);
175 select(0, 0, 0, 0, &timeout);
192 if (!vrpn_big_endian) {
193 vrpn_float64 dSwapped;
194 char *pchSwapped = (
char *)&dSwapped;
195 char *pchOrig = (
char *)&d;
199 for (i = 0; i <
sizeof(vrpn_float64); i++) {
200 pchSwapped[i] = pchOrig[
sizeof(vrpn_float64) - i - 1];
203 #if defined(__arm__) && !defined(__ANDROID__)
207 #if __FLOAT_WORD_ORDER != __BYTE_ORDER
210 vrpn_uint32 *pwSwapped = (vrpn_uint32 *)&dSwapped;
211 vrpn_uint32 scratch = pwSwapped[0];
212 pwSwapped[0] = pwSwapped[1];
213 pwSwapped[1] = scratch;
243 vrpn_int32 sec, usec;
272 const char *
string, vrpn_int32 length)
274 if (length > *buflen) {
275 fprintf(stderr,
"vrpn_buffer: buffer not long enough for string.\n");
282 if (len > (
unsigned)*buflen) {
284 "vrpn_buffer: buffer not long enough for string.\n");
287 strcpy(*insertPt,
string);
289 *buflen -=
static_cast<vrpn_int32
>(len);
292 memcpy(*insertPt,
string, length);
314 vrpn_int32 sec, usec;
350 if (!
string)
return -1;
355 size_t max_len =
static_cast<size_t>(-length);
356 strncpy(
string, *buffer, max_len);
359 for (i = 0; i < max_len; i++) {
360 if (
string[i] ==
'\0') {
368 *buffer += strlen(*buffer) + 1;
371 memcpy(
string, *buffer, length);
388 #ifndef VRPN_UNSAFE_WINDOWS_CLOCK
390 #if defined(_WIN32) && !defined(__CYGWIN__)
393 #pragma optimize("", on)
396 void get_time_using_GetLocalTime(
unsigned long &sec,
unsigned long &usec)
402 GetLocalTime(&stime);
403 SystemTimeToFileTime(&stime, &ftime);
406 tics.HighPart = ftime.dwHighDateTime;
407 tics.LowPart = ftime.dwLowDateTime;
410 sec = (long)(tics.QuadPart / 10000000L);
411 usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
430 #ifndef _STRUCT_TIMEZONE
431 #define _STRUCT_TIMEZONE
438 struct timezone *tzp = (
struct timezone *)voidp;
441 unsigned long sec, usec;
442 get_time_using_GetLocalTime(sec, usec);
449 tp->tv_usec = (long)t.millitm * 1000;
453 TIME_ZONE_INFORMATION tz;
454 GetTimeZoneInformation(&tz);
455 tzp->tz_minuteswest = tz.Bias;
456 tzp->tz_dsttime = (tz.StandardBias != tz.Bias);
461 #endif // defined(_WIN32)
463 #else // In the case that VRPN_UNSAFE_WINDOWS_CLOCK is defined
470 #if defined(_WIN32) && !defined(__CYGWIN__)
480 static __int64 VRPN_CLOCK_FREQ = 200000000;
491 _asm _emit 0x0f _asm _emit 0x31 _asm mov li.LowPart, \
492 eax _asm mov li.HighPart, edx \
498 #ifndef VRPN_WINDOWS_CLOCK_V2
499 #pragma optimize("", off)
500 static int vrpn_AdjustFrequency(
void)
503 const int tPerLoop = 500;
504 fprintf(stderr,
"vrpn vrpn_gettimeofday: determining clock frequency...");
506 LARGE_INTEGER startperf, endperf;
507 LARGE_INTEGER perffreq;
512 if (QueryPerformanceFrequency(&perffreq) == 0) {
518 volatile LARGE_INTEGER liStart, liEnd;
520 DWORD dwPriorityClass = GetPriorityClass(GetCurrentProcess());
521 int iThreadPriority = GetThreadPriority(GetCurrentThread());
522 SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
523 SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
528 QueryPerformanceCounter(&startperf);
531 QueryPerformanceCounter(&endperf);
533 double freq = perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
534 ((
double)(endperf.QuadPart - startperf.QuadPart));
536 if (fabs(perffreq.QuadPart - freq) < 0.05 * freq) {
537 VRPN_CLOCK_FREQ = (__int64)perffreq.QuadPart;
538 fprintf(stderr,
"\nvrpn vrpn_gettimeofday: perf clock is tsc -- using "
539 "perf clock freq ( %lf MHz)\n",
540 perffreq.QuadPart / 1e6);
541 SetPriorityClass(GetCurrentProcess(), dwPriorityClass);
542 SetThreadPriority(GetCurrentThread(), iThreadPriority);
550 fprintf(stderr,
" (this will take %lf seconds)...\n",
551 loops * tPerLoop / 1000.0);
553 for (
int j = 0; j < loops; j++) {
555 QueryPerformanceCounter(&startperf);
558 QueryPerformanceCounter(&endperf);
568 sum += perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
569 ((
double)(endperf.QuadPart - startperf.QuadPart));
572 SetPriorityClass(GetCurrentProcess(), dwPriorityClass);
573 SetThreadPriority(GetCurrentThread(), iThreadPriority);
576 freq = (sum / loops);
590 if (fabs(perffreq.QuadPart - freq) < 0.05 * freq) {
591 VRPN_CLOCK_FREQ = perffreq.QuadPart;
592 fprintf(stderr,
"vrpn vrpn_gettimeofday: perf clock is tsc -- using "
593 "perf clock freq ( %lf MHz)\n",
594 perffreq.QuadPart / 1e6);
597 fprintf(stderr,
"vrpn vrpn_gettimeofday: adjusted clock freq to "
598 "measured freq ( %lf MHz )\n",
601 VRPN_CLOCK_FREQ = (__int64)freq;
604 #pragma optimize("", on)
626 #ifndef VRPN_WINDOWS_CLOCK_V2
629 static int fFirst = 1;
630 static int fHasPerfCounter = 1;
631 static struct _timeb tbInit;
632 static LARGE_INTEGER liInit;
633 static LARGE_INTEGER liNow;
634 static LARGE_INTEGER liDiff;
637 #ifndef _STRUCT_TIMEZONE
638 #define _STRUCT_TIMEZONE
645 struct timezone *tzp = (
struct timezone *)voidp;
647 if (!fHasPerfCounter) {
649 tp->tv_sec = tbInit.time;
650 tp->tv_usec = tbInit.millitm * 1000;
655 LARGE_INTEGER liTemp;
668 memset(&osvi, 0,
sizeof(OSVERSIONINFO));
669 osvi.dwOSVersionInfoSize =
sizeof(OSVERSIONINFO);
672 if (osvi.dwPlatformId != VER_PLATFORM_WIN32_NT) {
674 "\nvrpn_gettimeofday: disabling hi performance clock "
676 "Defaulting to _ftime (~6 ms resolution) ...\n");
684 if (!(fHasPerfCounter = QueryPerformanceFrequency(&liTemp))) {
686 "\nvrpn_gettimeofday: no hi performance clock available. "
687 "Defaulting to _ftime (~6 ms resolution) ...\n");
693 if (vrpn_AdjustFrequency() < 0) {
695 "\nvrpn_gettimeofday: can't verify clock frequency. "
696 "Defaulting to _ftime (~6 ms resolution) ...\n");
715 liDiff.QuadPart = liNow.QuadPart - liInit.QuadPart;
717 tvDiff.tv_sec = (long)(liDiff.QuadPart / VRPN_CLOCK_FREQ);
719 (long)(1e6 * ((liDiff.QuadPart - VRPN_CLOCK_FREQ * tvDiff.tv_sec) /
720 (double)VRPN_CLOCK_FREQ));
723 tp->tv_sec = tbInit.time + tvDiff.tv_sec;
724 tp->tv_usec = tbInit.millitm * 1000 + tvDiff.tv_usec;
725 while (tp->tv_usec >= 1000000) {
727 tp->tv_usec -= 1000000;
732 #else // defined(VRPN_WINDOWS_CLOCK_V2)
734 void get_time_using_GetLocalTime(
unsigned long &sec,
unsigned long &usec)
736 static LARGE_INTEGER first_count = {0, 0};
737 static unsigned long first_sec, first_usec;
738 static LARGE_INTEGER perf_freq;
742 LARGE_INTEGER perf_counter;
747 if (first_count.QuadPart == 0) {
748 QueryPerformanceCounter(&first_count);
752 QueryPerformanceFrequency(&perf_freq);
755 GetLocalTime(&stime);
756 SystemTimeToFileTime(&stime, &ftime);
760 tics.HighPart = ftime.dwHighDateTime;
761 tics.LowPart = ftime.dwLowDateTime;
765 sec = (long)(tics.QuadPart / 10000000L);
766 usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
771 QueryPerformanceCounter(&perf_counter);
772 if (perf_counter.QuadPart >= first_count.QuadPart) {
773 perf_counter.QuadPart =
774 perf_counter.QuadPart - first_count.QuadPart;
778 perf_counter.QuadPart = 0x7fffffffffffffffLL -
779 first_count.QuadPart +
780 perf_counter.QuadPart;
787 sec = (long)(perf_counter.QuadPart / perf_freq.QuadPart);
788 perf_counter.QuadPart -= perf_freq.QuadPart * sec;
789 perf_counter.QuadPart *= 1000000L;
790 usec = first_usec + (long)(perf_counter.QuadPart / perf_freq.QuadPart);
797 if (usec > 1000000L) {
811 #ifndef _STRUCT_TIMEZONE
812 #define _STRUCT_TIMEZONE
819 struct timezone *tzp = (
struct timezone *)voidp;
821 unsigned long sec, usec;
822 get_time_using_GetLocalTime(sec, usec);
826 TIME_ZONE_INFORMATION tz;
827 GetTimeZoneInformation(&tz);
828 tzp->tz_minuteswest = tz.Bias;
829 tzp->tz_dsttime = (tz.StandardBias != tz.Bias);
834 #endif // defined(VRPN_WINDOWS_CLOCK_V2)
836 #endif // defined(_WIN32)
842 #endif // VRPN_UNSAFE_WINDOWS_CLOCK
851 #define ALL_ASSERT(exp, msg) \
853 fprintf(stderr, "\nAssertion failed! \n %s (%s, %d)\n", msg, __FILE__, \
859 : cResources(cNumResources)
868 : cResources(s.cResources)
876 if (vrpn_Semaphore::ppaArena == NULL) {
877 vrpn_Semaphore::allocArena();
883 if ((l = usnewlock(vrpn_Semaphore::ppaArena)) == NULL) {
884 fprintf(stderr,
"vrpn_Semaphore::vrpn_Semaphore: error allocating "
885 "lock from arena.\n");
892 if ((ps = usnewsema(vrpn_Semaphore::ppaArena,
cResources)) == NULL) {
893 fprintf(stderr,
"vrpn_Semaphore::vrpn_Semaphore: error allocating "
894 "semaphore from arena.\n");
898 #elif defined(_WIN32)
905 hSemaphore = CreateSemaphore(NULL,
cResources, numMax, NULL);
911 FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
912 GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
914 (LPTSTR)&lpMsgBuf, 0, NULL);
916 "vrpn_Semaphore::vrpn_Semaphore: error creating semaphore, "
917 "WIN32 CreateSemaphore call caused the following error: %s\n",
923 #elif defined(__APPLE__)
929 char *tempname =
new char[100];
930 sprintf(tempname,
"/tmp/vrpn_sem.XXXXXXX");
931 semaphore = sem_open(mktemp(tempname), O_CREAT, 0600, numMax);
933 perror(
"vrpn_Semaphore::vrpn_Semaphore: error opening semaphore");
946 if (sem_init(
semaphore, 0, numMax) != 0) {
947 perror(
"vrpn_Semaphore::vrpn_Semaphore: error initializing semaphore");
959 usfreelock(l, vrpn_Semaphore::ppaArena);
962 usfreesema(ps, vrpn_Semaphore::ppaArena);
964 #elif defined(_WIN32)
965 if (!CloseHandle(hSemaphore)) {
970 FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
971 GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
973 (LPTSTR)&lpMsgBuf, 0, NULL);
975 "vrpn_Semaphore::destroy: error destroying semaphore, "
976 "WIN32 CloseHandle call caused the following error: %s\n",
986 perror(
"vrpn_Semaphore::destroy: error destroying semaphore.");
992 "vrpn_Semaphore::destroy: error destroying semaphore.\n");
1007 "vrpn_Semaphore::~vrpn_Semaphore: error destroying semaphore.\n");
1019 fprintf(stderr,
"vrpn_Semaphore::reset: error destroying semaphore.\n");
1024 "vrpn_Semaphore::reset: error initializing semaphore.\n");
1036 if (ussetlock(l) != 1) {
1037 perror(
"vrpn_Semaphore::p: ussetlock:");
1042 if (uspsema(ps) != 1) {
1043 perror(
"vrpn_Semaphore::p: uspsema:");
1047 #elif defined(_WIN32)
1048 switch (WaitForSingleObject(hSemaphore, INFINITE)) {
1053 ALL_ASSERT(0,
"vrpn_Semaphore::p: infinite wait time timed out!");
1056 case WAIT_ABANDONED:
1057 ALL_ASSERT(0,
"vrpn_Semaphore::p: thread holding resource died");
1065 FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1066 GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1068 (LPTSTR)&lpMsgBuf, 0, NULL);
1070 "vrpn_Semaphore::p: error waiting for resource, "
1071 "WIN32 WaitForSingleObject call caused the following error: %s",
1074 LocalFree(lpMsgBuf);
1078 ALL_ASSERT(0,
"vrpn_Semaphore::p: unknown return code");
1084 perror(
"vrpn_Semaphore::p: ");
1096 if (usunsetlock(l)) {
1097 perror(
"vrpn_Semaphore::v: usunsetlock:");
1103 perror(
"vrpn_Semaphore::v: uspsema:");
1107 #elif defined(_WIN32)
1108 if (!ReleaseSemaphore(hSemaphore, 1, NULL)) {
1113 FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1114 GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1116 (LPTSTR)&lpMsgBuf, 0, NULL);
1118 "vrpn_Semaphore::v: error v'ing semaphore, "
1119 "WIN32 ReleaseSemaphore call caused the following error: %s",
1122 LocalFree(lpMsgBuf);
1128 perror(
"vrpn_Semaphore::p: ");
1143 iRetVal = uscsetlock(l, 0);
1145 perror(
"vrpn_Semaphore::condP: uscsetlock:");
1150 iRetVal = uscpsema(ps);
1152 perror(
"vrpn_Semaphore::condP: uscpsema:");
1156 #elif defined(_WIN32)
1157 switch (WaitForSingleObject(hSemaphore, 0)) {
1165 case WAIT_ABANDONED:
1166 ALL_ASSERT(0,
"vrpn_Semaphore::condP: thread holding resource died");
1174 FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1175 GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1177 (LPTSTR)&lpMsgBuf, 0, NULL);
1179 "Semaphore::condP: error waiting for resource, "
1180 "WIN32 WaitForSingleObject call caused the following error: %s",
1183 LocalFree(lpMsgBuf);
1187 ALL_ASSERT(0,
"vrpn_Semaphore::p: unknown return code");
1196 else if (errno == EAGAIN) {
1200 perror(
"vrpn_Semaphore::condP: ");
1211 usptr_t *vrpn_Semaphore::ppaArena = NULL;
1213 #include <sys/stat.h>
1215 #include <sys/types.h>
1216 #include <sys/stat.h>
1218 void vrpn_Semaphore::allocArena()
1223 if ((ppaArena = usinit(
"/dev/zero")) == NULL) {
1224 perror(
"vrpn_Thread::allocArena: usinit:");
1231 : pfThread(pfThreadparm)
1240 fprintf(stderr,
"vrpn_Thread::go: already running\n");
1246 ((
unsigned long)-1)) {
1247 perror(
"vrpn_Thread::go: sproc");
1251 #elif defined(_WIN32) && !defined(__CYGWIN__)
1254 ((
unsigned long)-1)) {
1255 perror(
"vrpn_Thread::go: _beginthread");
1261 perror(
"vrpn_Thread::go:pthread_create: ");
1271 #if defined(sgi) || defined(_WIN32)
1275 perror(
"vrpn_Thread::kill: kill:");
1278 #elif defined(_WIN32)
1280 if (!TerminateThread((HANDLE)
threadID, 1)) {
1282 "vrpn_Thread::kill: problem with terminateThread call.\n");
1290 if (pthread_detach(
threadID) != 0) {
1291 perror(
"vrpn_Thread::kill:pthread_detach: ");
1294 if (pthread_kill(
threadID, SIGKILL) != 0) {
1295 perror(
"vrpn_Thread::kill:pthread_kill: ");
1301 fprintf(stderr,
"vrpn_Thread::kill: thread is not currently alive.\n");
1314 #ifdef vrpn_THREADS_AVAILABLE
1330 #if !defined(sgi) && !defined(_WIN32)
1332 if (pthread_detach(pth->
threadID) != 0) {
1333 perror(
"vrpn_Thread::threadFuncShell:pthread_detach: ");
1356 #include <sys/param.h>
1357 #include <sys/sysctl.h>
1364 SYSTEM_INFO siSysInfo;
1365 GetSystemInfo(&siSysInfo);
1366 return siSysInfo.dwNumberOfProcessors;
1371 FILE *f = fopen(
"/proc/cpuinfo",
"r");
1374 perror(
"vrpn_Thread::number_of_processors:fopen: ");
1379 while (fgets(line,
sizeof(line), f) != NULL) {
1380 if (strncmp(line,
"processor\t:", strlen(
"processor\t:")) == 0) {
1388 "vrpn_Thread::number_of_processors: Found zero, returning 1\n");
1395 size_t size =
sizeof(count);
1396 if (sysctlbyname(
"hw.ncpu", &count, &size, NULL, 0)) {
1400 return static_cast<unsigned>(count);
1404 fprintf(stderr,
"vrpn_Thread::number_of_processors: Not yet implemented on "
1405 "this architecture.\n");
1416 if (threadData.
pvUD == NULL) {
1417 fprintf(stderr,
"vrpn_test_thread_body(): pvUD is NULL\n");
1436 const unsigned sem_count = 5;
1439 for (i = 0; i < sem_count; i++) {
1440 if (s.
condP() != 1) {
1441 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): Semaphore ran "
1446 if (s.
condP() != 0) {
1447 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): Semaphore had too "
1451 for (i = 0; i < sem_count; i++) {
1453 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): Could not "
1454 "release Semaphore\n");
1458 for (i = 0; i < sem_count; i++) {
1459 if (s.
condP() != 1) {
1460 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): Semaphore ran "
1461 "out of counts, round 2\n");
1465 if (s.
condP() != 0) {
1466 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): Semaphore had too "
1467 "many counts, round 2\n");
1488 if (num_procs == 0) {
1489 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): "
1490 "vrpn_Thread::number_of_processors() returned zero\n");
1501 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): thread-test "
1502 "Semaphore had no count\n");
1505 if (sem.
condP() != 0) {
1506 fprintf(stderr,
"vrpn_test_threads_and_semaphores(): thread-test "
1507 "Semaphore had too many counts\n");
1513 "vrpn_test_threads_and_semaphores(): Could not start thread\n");
1516 struct timeval start;
1520 if (sem.
condP() == 1) {
1529 if (diff.tv_sec >= 3) {
1531 "vrpn_test_threads_and_semaphores(): Thread didn't run\n");