vrpn  07.33
Virtual Reality Peripheral Network
vrpn_Shared.C
Go to the documentation of this file.
1 #include <math.h> // for floor, fmod
2 #include <stddef.h> // for size_t
3 #include <stdio.h> // for fprintf() and such
4 
5 #ifdef _MSC_VER
6 // Don't tell us about strcpy being dangerous.
7 #define _CRT_SECURE_NO_WARNINGS 1
8 #pragma warning(disable : 4996)
9 #endif
10 
11 #include "vrpn_Shared.h"
12 
13 #if defined(__APPLE__) || defined(__ANDROID__)
14 #include <unistd.h>
15 #endif
16 
17 #if !(defined(_WIN32) && defined(VRPN_USE_WINSOCK_SOCKETS))
18 #include <sys/select.h> // for select
19 #include <netinet/in.h> // for htonl, htons
20 #endif
21 
22 #define CHECK(a) \
23  if (a == -1) return -1
24 
25 // perform normalization of a timeval
26 // XXX this still needs to be checked for errors if the timeval
27 // or the rate is negative
28 static inline void timevalNormalizeInPlace(timeval &in_tv)
29 {
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);
33 }
34 timeval vrpn_TimevalNormalize(const timeval &in_tv)
35 {
36  timeval out_tv = in_tv;
37  timevalNormalizeInPlace(out_tv);
38  return out_tv;
39 }
40 
41 // Calcs the sum of tv1 and tv2. Returns the sum in a timeval struct.
42 // Calcs negative times properly, with the appropriate sign on both tv_sec
43 // and tv_usec (these signs will match unless one of them is 0).
44 // NOTE: both abs(tv_usec)'s must be < 1000000 (ie, normal timeval format)
45 timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
46 {
47  timeval tvSum = tv1;
48 
49  tvSum.tv_sec += tv2.tv_sec;
50  tvSum.tv_usec += tv2.tv_usec;
51 
52  // do borrows, etc to get the time the way i want it: both signs the same,
53  // and abs(usec) less than 1e6
54  if (tvSum.tv_sec > 0) {
55  if (tvSum.tv_usec < 0) {
56  tvSum.tv_sec--;
57  tvSum.tv_usec += 1000000;
58  }
59  else if (tvSum.tv_usec >= 1000000) {
60  tvSum.tv_sec++;
61  tvSum.tv_usec -= 1000000;
62  }
63  }
64  else if (tvSum.tv_sec < 0) {
65  if (tvSum.tv_usec > 0) {
66  tvSum.tv_sec++;
67  tvSum.tv_usec -= 1000000;
68  }
69  else if (tvSum.tv_usec <= -1000000) {
70  tvSum.tv_sec--;
71  tvSum.tv_usec += 1000000;
72  }
73  }
74  else {
75  // == 0, so just adjust usec
76  if (tvSum.tv_usec >= 1000000) {
77  tvSum.tv_sec++;
78  tvSum.tv_usec -= 1000000;
79  }
80  else if (tvSum.tv_usec <= -1000000) {
81  tvSum.tv_sec--;
82  tvSum.tv_usec += 1000000;
83  }
84  }
85 
86  return tvSum;
87 }
88 
89 // Calcs the diff between tv1 and tv2. Returns the diff in a timeval struct.
90 // Calcs negative times properly, with the appropriate sign on both tv_sec
91 // and tv_usec (these signs will match unless one of them is 0)
92 timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
93 {
94  timeval tv;
95 
96  tv.tv_sec = -tv2.tv_sec;
97  tv.tv_usec = -tv2.tv_usec;
98 
99  return vrpn_TimevalSum(tv1, tv);
100 }
101 
102 timeval vrpn_TimevalScale(const timeval &tv, double scale)
103 {
104  timeval result;
105  result.tv_sec = (long)(tv.tv_sec * scale);
106  result.tv_usec =
107  (long)(tv.tv_usec * scale + fmod(tv.tv_sec * scale, 1.0) * 1000000.0);
108  timevalNormalizeInPlace(result);
109  return result;
110 }
111 
112 // returns 1 if tv1 is greater than tv2; 0 otherwise
113 bool vrpn_TimevalGreater(const timeval &tv1, const timeval &tv2)
114 {
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;
117  return 0;
118 }
119 
120 // return 1 if tv1 is equal to tv2; 0 otherwise
121 bool vrpn_TimevalEqual(const timeval &tv1, const timeval &tv2)
122 {
123  if (tv1.tv_sec == tv2.tv_sec && tv1.tv_usec == tv2.tv_usec)
124  return true;
125  else
126  return false;
127 }
128 
129 unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
130 {
131  return (endT.tv_usec - startT.tv_usec) +
132  1000000L * (endT.tv_sec - startT.tv_sec);
133 }
134 
135 double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
136 {
137  return (endT.tv_usec - startT.tv_usec) / 1000000.0 +
138  (endT.tv_sec - startT.tv_sec);
139 }
140 
141 double vrpn_TimevalMsecs(const timeval &tv)
142 {
143  return tv.tv_sec * 1000.0 + tv.tv_usec / 1000.0;
144 }
145 
146 timeval vrpn_MsecsTimeval(const double dMsecs)
147 {
148  timeval tv;
149  tv.tv_sec = (long)floor(dMsecs / 1000.0);
150  tv.tv_usec = (long)((dMsecs / 1000.0 - tv.tv_sec) * 1e6);
151  return tv;
152 }
153 
154 // Sleep for dMsecs milliseconds, freeing up the processor while you
155 // are doing so.
156 
157 void vrpn_SleepMsecs(double dMsecs)
158 {
159 #if defined(_WIN32)
160  Sleep((DWORD)dMsecs);
161 #else
162  timeval timeout;
163 
164  // Convert milliseconds to seconds
165  timeout.tv_sec = (int)(dMsecs / 1000.0);
166 
167  // Subtract of whole number of seconds
168  dMsecs -= timeout.tv_sec * 1000;
169 
170  // Convert remaining milliseconds to microsec
171  timeout.tv_usec = (int)(dMsecs * 1000);
172 
173  // A select() with NULL file descriptors acts like a microsecond
174  // timer.
175  select(0, 0, 0, 0, &timeout); // wait for that long;
176 #endif
177 }
178 
179 // convert vrpn_float64 to/from network order
180 // I have chosen big endian as the network order for vrpn_float64
181 // to match the standard for htons() and htonl().
182 // NOTE: There is an added complexity when we are using an ARM
183 // processor in mixed-endian mode for the doubles, whereby we need
184 // to not just swap all of the bytes but also swap the two 4-byte
185 // words to get things in the right order.
186 #if defined(__arm__)
187 #include <endian.h>
188 #endif
189 
190 vrpn_float64 vrpn_htond(vrpn_float64 d)
191 {
192  if (!vrpn_big_endian) {
193  vrpn_float64 dSwapped;
194  char *pchSwapped = (char *)&dSwapped;
195  char *pchOrig = (char *)&d;
196 
197  // swap to big-endian order.
198  unsigned i;
199  for (i = 0; i < sizeof(vrpn_float64); i++) {
200  pchSwapped[i] = pchOrig[sizeof(vrpn_float64) - i - 1];
201  }
202 
203 #if defined(__arm__) && !defined(__ANDROID__)
204 // On ARM processor, see if we're in mixed mode. If so,
205 // we need to swap the two words after doing the total
206 // swap of bytes.
207 #if __FLOAT_WORD_ORDER != __BYTE_ORDER
208  {
209  /* Fixup mixed endian floating point machines */
210  vrpn_uint32 *pwSwapped = (vrpn_uint32 *)&dSwapped;
211  vrpn_uint32 scratch = pwSwapped[0];
212  pwSwapped[0] = pwSwapped[1];
213  pwSwapped[1] = scratch;
214  }
215 #endif
216 #endif
217 
218  return dSwapped;
219  }
220  else {
221  return d;
222  }
223 }
224 
225 // they are their own inverses, so ...
226 vrpn_float64 vrpn_ntohd(vrpn_float64 d) { return vrpn_htond(d); }
227 
241 VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
242 {
243  vrpn_int32 sec, usec;
244 
245  // tv_sec and usec are 64 bits on some architectures, but we
246  // define them as 32 bit for network transmission
247 
248  sec = t.tv_sec;
249  usec = t.tv_usec;
250 
251  if (vrpn_buffer(insertPt, buflen, sec)) return -1;
252  return vrpn_buffer(insertPt, buflen, usec);
253 }
254 
271 VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen,
272  const char *string, vrpn_int32 length)
273 {
274  if (length > *buflen) {
275  fprintf(stderr, "vrpn_buffer: buffer not long enough for string.\n");
276  return -1;
277  }
278 
279  if (length == -1) {
280  size_t len =
281  strlen(string) + 1; // +1 for the NULL terminating character
282  if (len > (unsigned)*buflen) {
283  fprintf(stderr,
284  "vrpn_buffer: buffer not long enough for string.\n");
285  return -1;
286  }
287  strcpy(*insertPt, string);
288  *insertPt += len;
289  *buflen -= static_cast<vrpn_int32>(len);
290  }
291  else {
292  memcpy(*insertPt, string, length);
293  *insertPt += length;
294  *buflen -= length;
295  }
296 
297  return 0;
298 }
299 
312 VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
313 {
314  vrpn_int32 sec, usec;
315 
316  CHECK(vrpn_unbuffer(buffer, &sec));
317  CHECK(vrpn_unbuffer(buffer, &usec));
318 
319  t->tv_sec = sec;
320  t->tv_usec = usec;
321 
322  return 0;
323 }
324 
348 VRPN_API int vrpn_unbuffer(const char **buffer, char *string, vrpn_int32 length)
349 {
350  if (!string) return -1;
351 
352  if (length < 0) {
353  // Read the string up to maximum length, then check to make sure we
354  // found the null-terminator in the length we read.
355  size_t max_len = static_cast<size_t>(-length);
356  strncpy(string, *buffer, max_len);
357  size_t i;
358  bool found = false;
359  for (i = 0; i < max_len; i++) {
360  if (string[i] == '\0') {
361  found = true;
362  break;
363  }
364  }
365  if (!found) {
366  return -1;
367  }
368  *buffer += strlen(*buffer) + 1; // +1 for NULL terminating character
369  }
370  else {
371  memcpy(string, *buffer, length);
372  *buffer += length;
373  }
374 
375  return 0;
376 }
377 
379 // More accurate gettimeofday() on some Windows operating systems
380 // and machines can be gotten by using the Performance Counter
381 // on the CPU. This doesn't seem to work in NT/2000 for some
382 // computers, so the code to do it is #defined out by default.
383 // To put it back back, #define VRPN_UNSAFE_WINDOWS_CLOCK and
384 // the following code will use the performance counter (which it
385 // takes a second to calibrate at program start-up).
387 
388 #ifndef VRPN_UNSAFE_WINDOWS_CLOCK
389 
390 #if defined(_WIN32) && !defined(__CYGWIN__)
391 #include <math.h> // for floor, fmod
392 
393 #pragma optimize("", on)
394 
395 #ifdef _WIN32
396 void get_time_using_GetLocalTime(unsigned long &sec, unsigned long &usec)
397 {
398  SYSTEMTIME stime; // System time in funky structure
399  FILETIME ftime; // Time in 100-nsec intervals since Jan 1 1601
400  LARGE_INTEGER tics; // ftime stored into a 64-bit quantity
401 
402  GetLocalTime(&stime);
403  SystemTimeToFileTime(&stime, &ftime);
404 
405  // Copy the data into a structure that can be treated as a 64-bit integer
406  tics.HighPart = ftime.dwHighDateTime;
407  tics.LowPart = ftime.dwLowDateTime;
408 
409  // Convert the 64-bit time into seconds and microseconds since July 1 1601
410  sec = (long)(tics.QuadPart / 10000000L);
411  usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
412 
413  // Translate the time to be based on January 1, 1970 (_ftime base)
414  // The offset here is gotten by using the "time_test" program to report the
415  // difference in seconds between the two clocks.
416  sec -= 3054524608;
417 }
418 #endif
419 
421 // Although VC++ doesn't include a gettimeofday
422 // call, Cygnus Solutions Cygwin32 environment does,
423 // so this is not used when compiling with gcc under WIN32.
424 // XXX Note that the cygwin one was wrong in an earlier
425 // version. It is claimed that they have fixed it now, but
426 // better check.
428 int vrpn_gettimeofday(timeval *tp, void *voidp)
429 {
430 #ifndef _STRUCT_TIMEZONE
431 #define _STRUCT_TIMEZONE
432  /* from HP-UX */
433  struct timezone {
434  int tz_minuteswest; /* minutes west of Greenwich */
435  int tz_dsttime; /* type of dst correction */
436  };
437 #endif
438  struct timezone *tzp = (struct timezone *)voidp;
439  if (tp != NULL) {
440 #ifdef _WIN32_WCE
441  unsigned long sec, usec;
442  get_time_using_GetLocalTime(sec, usec);
443  tp->tv_sec = sec;
444  tp->tv_usec = usec;
445 #else
446  struct _timeb t;
447  _ftime(&t);
448  tp->tv_sec = t.time;
449  tp->tv_usec = (long)t.millitm * 1000;
450 #endif
451  }
452  if (tzp != NULL) {
453  TIME_ZONE_INFORMATION tz;
454  GetTimeZoneInformation(&tz);
455  tzp->tz_minuteswest = tz.Bias;
456  tzp->tz_dsttime = (tz.StandardBias != tz.Bias);
457  }
458  return 0;
459 }
460 
461 #endif // defined(_WIN32)
462 
463 #else // In the case that VRPN_UNSAFE_WINDOWS_CLOCK is defined
464 
465 // Although VC++ doesn't include a gettimeofday
466 // call, Cygnus Solutions Cygwin32 environment does.
467 // XXX AND ITS WRONG in the current release 10/11/99, version b20.1
468 // They claim it will be fixed in the next release, version b21
469 // so until then, we will make it right using our solution.
470 #if defined(_WIN32) && !defined(__CYGWIN__)
471 #include <math.h> // for floor, fmod
472 
473 // utility routines to read the pentium time stamp counter
474 // QueryPerfCounter drifts too much -- others have documented this
475 // problem on the net
476 
477 // This is all based on code extracted from the UNC hiball tracker cib lib
478 
479 // 200 mhz pentium default -- we change this based on our calibration
480 static __int64 VRPN_CLOCK_FREQ = 200000000;
481 
482 // Helium to histidine
483 // __int64 FREQUENCY = 199434500;
484 
485 // tori -- but queryperfcounter returns this for us
486 // __int64 FREQUENCY = 198670000;
487 
488 // Read Time Stamp Counter
489 #define rdtsc(li) \
490  { \
491  _asm _emit 0x0f _asm _emit 0x31 _asm mov li.LowPart, \
492  eax _asm mov li.HighPart, edx \
493  }
494 
495 /*
496  * calculate the time stamp counter register frequency (clock freq)
497  */
498 #ifndef VRPN_WINDOWS_CLOCK_V2
499 #pragma optimize("", off)
500 static int vrpn_AdjustFrequency(void)
501 {
502  const int loops = 2;
503  const int tPerLoop = 500; // milliseconds for Sleep()
504  fprintf(stderr, "vrpn vrpn_gettimeofday: determining clock frequency...");
505 
506  LARGE_INTEGER startperf, endperf;
507  LARGE_INTEGER perffreq;
508 
509  // See if the hardware supports the high-resolution performance counter.
510  // If so, get the frequency of it. If not, we can't use it and so return
511  // -1.
512  if (QueryPerformanceFrequency(&perffreq) == 0) {
513  return -1;
514  }
515 
516  // don't optimize away these variables
517  double sum = 0;
518  volatile LARGE_INTEGER liStart, liEnd;
519 
520  DWORD dwPriorityClass = GetPriorityClass(GetCurrentProcess());
521  int iThreadPriority = GetThreadPriority(GetCurrentThread());
522  SetPriorityClass(GetCurrentProcess(), REALTIME_PRIORITY_CLASS);
523  SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL);
524 
525  // pull all into cache and do rough test to see if tsc and perf counter
526  // are one and the same
527  rdtsc(liStart);
528  QueryPerformanceCounter(&startperf);
529  Sleep(100);
530  rdtsc(liEnd);
531  QueryPerformanceCounter(&endperf);
532 
533  double freq = perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
534  ((double)(endperf.QuadPart - startperf.QuadPart));
535 
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);
543  return 0;
544  }
545 
546  // either tcs and perf clock are not the same, or we could not
547  // tell accurately enough with the short test. either way we now
548  // need an accurate frequency measure, so ...
549 
550  fprintf(stderr, " (this will take %lf seconds)...\n",
551  loops * tPerLoop / 1000.0);
552 
553  for (int j = 0; j < loops; j++) {
554  rdtsc(liStart);
555  QueryPerformanceCounter(&startperf);
556  Sleep(tPerLoop);
557  rdtsc(liEnd);
558  QueryPerformanceCounter(&endperf);
559 
560  // perf counter timer ran for one call to Query and one call to
561  // tcs read in addition to the time between the tsc readings
562  // tcs read did the same
563 
564  // endperf - startperf / perf freq = time between perf queries
565  // endtsc - starttsc = clock ticks between perf queries
566  // sum += (endtsc - starttsc) / ((double)(endperf -
567  // startperf)/perffreq);
568  sum += perffreq.QuadPart * (liEnd.QuadPart - liStart.QuadPart) /
569  ((double)(endperf.QuadPart - startperf.QuadPart));
570  }
571 
572  SetPriorityClass(GetCurrentProcess(), dwPriorityClass);
573  SetThreadPriority(GetCurrentThread(), iThreadPriority);
574 
575  // might want last, not sum -- just get into cache and run
576  freq = (sum / loops);
577 
578  // if we are on a uniprocessor system, then use the freq estimate
579  // This used to check against a 200 mhz assumed clock, but now
580  // we assume the routine works and trust the result.
581  // if (fabs(freq - VRPN_CLOCK_FREQ) > 0.05 * VRPN_CLOCK_FREQ) {
582  // cerr << "vrpn vrpn_gettimeofday: measured freq is " << freq/1e6
583  // << " MHz - DOES NOT MATCH" << endl;
584  // return -1;
585  // }
586 
587  // if we are in a system where the perf clock is the tsc, then use the
588  // rate the perf clock returns (or rather, if the freq we measure is
589  // approx the perf clock freq).
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);
595  }
596  else {
597  fprintf(stderr, "vrpn vrpn_gettimeofday: adjusted clock freq to "
598  "measured freq ( %lf MHz )\n",
599  freq / 1e6);
600  }
601  VRPN_CLOCK_FREQ = (__int64)freq;
602  return 0;
603 }
604 #pragma optimize("", on)
605 #endif
606 
607 // The pc has no gettimeofday call, and the closest thing to it is _ftime.
608 // _ftime, however, has only about 6 ms resolution, so we use the peformance
609 // as an offset from a base time which is established by a call to by _ftime.
610 
611 // The first call to vrpn_gettimeofday will establish a new time frame
612 // on which all later calls will be based. This means that the time returned
613 // by vrpn_gettimeofday will not always match _ftime (even at _ftime's
614 // resolution),
615 // but it will be consistent across all vrpn_gettimeofday calls.
616 
618 // Although VC++ doesn't include a gettimeofday
619 // call, Cygnus Solutions Cygwin32 environment does,
620 // so this is not used when compiling with gcc under WIN32
621 
622 // XXX AND ITS WRONG in the current release 10/11/99
623 // They claim it will be fixed in the next release,
624 // so until then, we will make it right using our solution.
626 #ifndef VRPN_WINDOWS_CLOCK_V2
627 int vrpn_gettimeofday(timeval *tp, void *voidp)
628 {
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;
635  timeval tvDiff;
636 
637 #ifndef _STRUCT_TIMEZONE
638 #define _STRUCT_TIMEZONE
639  /* from HP-UX */
640  struct timezone {
641  int tz_minuteswest; /* minutes west of Greenwich */
642  int tz_dsttime; /* type of dst correction */
643  };
644 #endif
645  struct timezone *tzp = (struct timezone *)voidp;
646 
647  if (!fHasPerfCounter) {
648  _ftime(&tbInit);
649  tp->tv_sec = tbInit.time;
650  tp->tv_usec = tbInit.millitm * 1000;
651  return 0;
652  }
653 
654  if (fFirst) {
655  LARGE_INTEGER liTemp;
656  // establish a time base
657  fFirst = 0;
658 
659  // Check to see if we are on a Windows NT machine (as opposed to a
660  // Windows 95/98 machine). If we are not, then use the _ftime code
661  // because the hi-perf clock does not work under Windows 98SE on my
662  // laptop, although the query for one seems to indicate that it is
663  // available.
664 
665  {
666  OSVERSIONINFO osvi;
667 
668  memset(&osvi, 0, sizeof(OSVERSIONINFO));
669  osvi.dwOSVersionInfoSize = sizeof(OSVERSIONINFO);
670  GetVersionEx(&osvi);
671 
672  if (osvi.dwPlatformId != VER_PLATFORM_WIN32_NT) {
673  fprintf(stderr,
674  "\nvrpn_gettimeofday: disabling hi performance clock "
675  "on non-NT system. "
676  "Defaulting to _ftime (~6 ms resolution) ...\n");
677  fHasPerfCounter = 0;
678  vrpn_gettimeofday(tp, tzp);
679  return 0;
680  }
681  }
682 
683  // check that hi-perf clock is available
684  if (!(fHasPerfCounter = QueryPerformanceFrequency(&liTemp))) {
685  fprintf(stderr,
686  "\nvrpn_gettimeofday: no hi performance clock available. "
687  "Defaulting to _ftime (~6 ms resolution) ...\n");
688  fHasPerfCounter = 0;
689  vrpn_gettimeofday(tp, tzp);
690  return 0;
691  }
692 
693  if (vrpn_AdjustFrequency() < 0) {
694  fprintf(stderr,
695  "\nvrpn_gettimeofday: can't verify clock frequency. "
696  "Defaulting to _ftime (~6 ms resolution) ...\n");
697  fHasPerfCounter = 0;
698  vrpn_gettimeofday(tp, tzp);
699  return 0;
700  }
701  // get current time
702  // We assume this machine has a time stamp counter register --
703  // I don't know of an easy way to check for this
704  rdtsc(liInit);
705  _ftime(&tbInit);
706 
707  // we now consider it to be exactly the time _ftime returned
708  // (beyond the resolution of _ftime, down to the perfCounter res)
709  }
710 
711  // now do the regular get time call to get the current time
712  rdtsc(liNow);
713 
714  // find offset from initial value
715  liDiff.QuadPart = liNow.QuadPart - liInit.QuadPart;
716 
717  tvDiff.tv_sec = (long)(liDiff.QuadPart / VRPN_CLOCK_FREQ);
718  tvDiff.tv_usec =
719  (long)(1e6 * ((liDiff.QuadPart - VRPN_CLOCK_FREQ * tvDiff.tv_sec) /
720  (double)VRPN_CLOCK_FREQ));
721 
722  // pack the value and clean it up
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) {
726  tp->tv_sec++;
727  tp->tv_usec -= 1000000;
728  }
729 
730  return 0;
731 }
732 #else // defined(VRPN_WINDOWS_CLOCK_V2)
733 
734 void get_time_using_GetLocalTime(unsigned long &sec, unsigned long &usec)
735 {
736  static LARGE_INTEGER first_count = {0, 0};
737  static unsigned long first_sec, first_usec;
738  static LARGE_INTEGER perf_freq; //< Frequency of the performance counter.
739  SYSTEMTIME stime; // System time in funky structure
740  FILETIME ftime; // Time in 100-nsec intervals since Jan 1 1601
741  LARGE_INTEGER tics; // ftime stored into a 64-bit quantity
742  LARGE_INTEGER perf_counter;
743 
744  // The first_count value will be zero only the first time through; we
745  // rely on this to set up the structures needed to interpret the data
746  // that we get from querying the performance counter.
747  if (first_count.QuadPart == 0) {
748  QueryPerformanceCounter(&first_count);
749 
750  // Find out how fast the performance counter runs. Store this for later
751  // runs.
752  QueryPerformanceFrequency(&perf_freq);
753 
754  // Find out what time it is in a Windows structure.
755  GetLocalTime(&stime);
756  SystemTimeToFileTime(&stime, &ftime);
757 
758  // Copy the data into a structure that can be treated as a 64-bit
759  // integer
760  tics.HighPart = ftime.dwHighDateTime;
761  tics.LowPart = ftime.dwLowDateTime;
762 
763  // Convert the 64-bit time into seconds and microseconds since July 1
764  // 1601
765  sec = (long)(tics.QuadPart / 10000000L);
766  usec = (long)((tics.QuadPart - (((LONGLONG)(sec)) * 10000000L)) / 10);
767  first_sec = sec;
768  first_usec = usec;
769  }
770  else {
771  QueryPerformanceCounter(&perf_counter);
772  if (perf_counter.QuadPart >= first_count.QuadPart) {
773  perf_counter.QuadPart =
774  perf_counter.QuadPart - first_count.QuadPart;
775  }
776  else {
777  // Take care of the case when the counter rolls over.
778  perf_counter.QuadPart = 0x7fffffffffffffffLL -
779  first_count.QuadPart +
780  perf_counter.QuadPart;
781  }
782 
783  // Reinterpret the performance counter into seconds and microseconds
784  // by dividing by the performance counter. Microseconds is placed
785  // into perf_counter by subtracting the seconds value out, then
786  // multiplying by 1 million and re-dividing by the performance counter.
787  sec = (long)(perf_counter.QuadPart / perf_freq.QuadPart);
788  perf_counter.QuadPart -= perf_freq.QuadPart * sec;
789  perf_counter.QuadPart *= 1000000L; //< Turn microseconds into seconds
790  usec = first_usec + (long)(perf_counter.QuadPart / perf_freq.QuadPart);
791  sec += first_sec;
792 
793  // Make sure that we've not got more than a million microseconds.
794  // If so, then shift it into seconds. We don't expect it to be above
795  // more than 1 million because we added two things, each of which were
796  // less than 1 million.
797  if (usec > 1000000L) {
798  usec -= 1000000L;
799  sec++;
800  }
801  }
802 
803  // Translate the time to be based on January 1, 1970 (_ftime base)
804  // The offset here is gotten by using the "time_test" program to report the
805  // difference in seconds between the two clocks.
806  sec -= 3054524608L;
807 }
808 
809 int vrpn_gettimeofday(timeval *tp, void *voidp)
810 {
811 #ifndef _STRUCT_TIMEZONE
812 #define _STRUCT_TIMEZONE
813  /* from HP-UX */
814  struct timezone {
815  int tz_minuteswest; /* minutes west of Greenwich */
816  int tz_dsttime; /* type of dst correction */
817  };
818 #endif
819  struct timezone *tzp = (struct timezone *)voidp;
820 
821  unsigned long sec, usec;
822  get_time_using_GetLocalTime(sec, usec);
823  tp->tv_sec = sec;
824  tp->tv_usec = usec;
825  if (tzp != NULL) {
826  TIME_ZONE_INFORMATION tz;
827  GetTimeZoneInformation(&tz);
828  tzp->tz_minuteswest = tz.Bias;
829  tzp->tz_dsttime = (tz.StandardBias != tz.Bias);
830  }
831  return 0;
832 }
833 
834 #endif // defined(VRPN_WINDOWS_CLOCK_V2)
835 
836 #endif // defined(_WIN32)
837 
838 // do the calibration before the program ever starts up
839 static timeval __tv;
840 static int __iTrash = vrpn_gettimeofday(&__tv, (struct timezone *)NULL);
841 
842 #endif // VRPN_UNSAFE_WINDOWS_CLOCK
843 
844 #include <stdio.h> // for fprintf, stderr, perror, etc
845 #include <string.h> // for memcpy, strlen, strcpy, etc
846 #ifndef _WIN32
847 #include <errno.h> // for EAGAIN, errno
848 #include <signal.h> // for pthread_kill, SIGKILL
849 #endif
850 
851 #define ALL_ASSERT(exp, msg) \
852  if (!(exp)) { \
853  fprintf(stderr, "\nAssertion failed! \n %s (%s, %d)\n", msg, __FILE__, \
854  __LINE__); \
855  }
856 
857 // init all fields in init()
859  : cResources(cNumResources)
860 {
861  init();
862 }
863 
864 // create a new internal structure for the semaphore
865 // (binary copy is not ok)
866 // This does not copy the state of the semaphore
868  : cResources(s.cResources)
869 {
870  init();
871 }
872 
874 {
875 #ifdef sgi
876  if (vrpn_Semaphore::ppaArena == NULL) {
877  vrpn_Semaphore::allocArena();
878  }
879  if (cResources == 1) {
880  fUsingLock = true;
881  ps = NULL;
882  // use lock instead of semaphore
883  if ((l = usnewlock(vrpn_Semaphore::ppaArena)) == NULL) {
884  fprintf(stderr, "vrpn_Semaphore::vrpn_Semaphore: error allocating "
885  "lock from arena.\n");
886  return false;
887  }
888  }
889  else {
890  fUsingLock = false;
891  l = NULL;
892  if ((ps = usnewsema(vrpn_Semaphore::ppaArena, cResources)) == NULL) {
893  fprintf(stderr, "vrpn_Semaphore::vrpn_Semaphore: error allocating "
894  "semaphore from arena.\n");
895  return false;
896  }
897  }
898 #elif defined(_WIN32)
899  // args are security, initial count, max count, and name
900  // TCH 20 Feb 2001 - Make the PC behavior closer to the SGI behavior.
901  int numMax = cResources;
902  if (numMax < 1) {
903  numMax = 1;
904  }
905  hSemaphore = CreateSemaphore(NULL, cResources, numMax, NULL);
906  if (!hSemaphore) {
907  // get error info from windows (from FormatMessage help page)
908  LPVOID lpMsgBuf;
909 
910  FormatMessage(
911  FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
912  GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
913  // Default language
914  (LPTSTR)&lpMsgBuf, 0, NULL);
915  fprintf(stderr,
916  "vrpn_Semaphore::vrpn_Semaphore: error creating semaphore, "
917  "WIN32 CreateSemaphore call caused the following error: %s\n",
918  (LPTSTR)lpMsgBuf);
919  // Free the buffer.
920  LocalFree(lpMsgBuf);
921  return false;
922  }
923 #elif defined(__APPLE__)
924  // We need to use sem_open on the mac because sem_init is not implemented
925  int numMax = cResources;
926  if (numMax < 1) {
927  numMax = 1;
928  }
929  char *tempname = new char[100];
930  sprintf(tempname, "/tmp/vrpn_sem.XXXXXXX");
931  semaphore = sem_open(mktemp(tempname), O_CREAT, 0600, numMax);
932  if (semaphore == SEM_FAILED) {
933  perror("vrpn_Semaphore::vrpn_Semaphore: error opening semaphore");
934  delete[] tempname;
935  return false;
936  }
937  delete[] tempname;
938 #else
939  // Posix threads are the default.
940  // We use sem_init on linux (instead of sem_open).
941  int numMax = cResources;
942  if (numMax < 1) {
943  numMax = 1;
944  }
945  semaphore = new sem_t;
946  if (sem_init(semaphore, 0, numMax) != 0) {
947  perror("vrpn_Semaphore::vrpn_Semaphore: error initializing semaphore");
948  return false;
949  }
950 #endif
951 
952  return true;
953 }
954 
956 {
957 #ifdef sgi
958  if (fUsingLock) {
959  usfreelock(l, vrpn_Semaphore::ppaArena);
960  }
961  else {
962  usfreesema(ps, vrpn_Semaphore::ppaArena);
963  }
964 #elif defined(_WIN32)
965  if (!CloseHandle(hSemaphore)) {
966  // get error info from windows (from FormatMessage help page)
967  LPVOID lpMsgBuf;
968 
969  FormatMessage(
970  FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
971  GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
972  // Default language
973  (LPTSTR)&lpMsgBuf, 0, NULL);
974  fprintf(stderr,
975  "vrpn_Semaphore::destroy: error destroying semaphore, "
976  "WIN32 CloseHandle call caused the following error: %s\n",
977  (LPTSTR)lpMsgBuf);
978  // Free the buffer.
979  LocalFree(lpMsgBuf);
980  return false;
981  }
982 #else
983 // Posix threads are the default.
984 #ifdef __APPLE__
985  if (sem_close(semaphore) != 0) {
986  perror("vrpn_Semaphore::destroy: error destroying semaphore.");
987  return false;
988  }
989 #else
990  if (sem_destroy(semaphore) != 0) {
991  fprintf(stderr,
992  "vrpn_Semaphore::destroy: error destroying semaphore.\n");
993  return false;
994  }
995  delete semaphore;
996 #endif
997  semaphore = NULL;
998 #endif
999  return true;
1000 }
1001 
1003 {
1004  if (!destroy()) {
1005  fprintf(
1006  stderr,
1007  "vrpn_Semaphore::~vrpn_Semaphore: error destroying semaphore.\n");
1008  }
1009 }
1010 
1011 // routine to reset it
1012 bool vrpn_Semaphore::reset(int cNumResources)
1013 {
1014  cResources = cNumResources;
1015 
1016  // Destroy the old semaphore and then create a new one with the correct
1017  // value.
1018  if (!destroy()) {
1019  fprintf(stderr, "vrpn_Semaphore::reset: error destroying semaphore.\n");
1020  return false;
1021  }
1022  if (!init()) {
1023  fprintf(stderr,
1024  "vrpn_Semaphore::reset: error initializing semaphore.\n");
1025  return false;
1026  }
1027  return true;
1028 }
1029 
1030 // routines to use it (p blocks, cond p does not)
1031 // 1 on success, -1 fail
1033 {
1034 #ifdef sgi
1035  if (fUsingLock) {
1036  if (ussetlock(l) != 1) {
1037  perror("vrpn_Semaphore::p: ussetlock:");
1038  return -1;
1039  }
1040  }
1041  else {
1042  if (uspsema(ps) != 1) {
1043  perror("vrpn_Semaphore::p: uspsema:");
1044  return -1;
1045  }
1046  }
1047 #elif defined(_WIN32)
1048  switch (WaitForSingleObject(hSemaphore, INFINITE)) {
1049  case WAIT_OBJECT_0:
1050  // got the resource
1051  break;
1052  case WAIT_TIMEOUT:
1053  ALL_ASSERT(0, "vrpn_Semaphore::p: infinite wait time timed out!");
1054  return -1;
1055  break;
1056  case WAIT_ABANDONED:
1057  ALL_ASSERT(0, "vrpn_Semaphore::p: thread holding resource died");
1058  return -1;
1059  break;
1060  case WAIT_FAILED:
1061  // get error info from windows (from FormatMessage help page)
1062  LPVOID lpMsgBuf;
1063 
1064  FormatMessage(
1065  FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1066  GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1067  // Default language
1068  (LPTSTR)&lpMsgBuf, 0, NULL);
1069  fprintf(stderr,
1070  "vrpn_Semaphore::p: error waiting for resource, "
1071  "WIN32 WaitForSingleObject call caused the following error: %s",
1072  (LPTSTR)lpMsgBuf);
1073  // Free the buffer.
1074  LocalFree(lpMsgBuf);
1075  return -1;
1076  break;
1077  default:
1078  ALL_ASSERT(0, "vrpn_Semaphore::p: unknown return code");
1079  return -1;
1080  }
1081 #else
1082  // Posix by default
1083  if (sem_wait(semaphore) != 0) {
1084  perror("vrpn_Semaphore::p: ");
1085  return -1;
1086  }
1087 #endif
1088  return 1;
1089 }
1090 
1091 // 0 on success, -1 fail
1093 {
1094 #ifdef sgi
1095  if (fUsingLock) {
1096  if (usunsetlock(l)) {
1097  perror("vrpn_Semaphore::v: usunsetlock:");
1098  return -1;
1099  }
1100  }
1101  else {
1102  if (usvsema(ps)) {
1103  perror("vrpn_Semaphore::v: uspsema:");
1104  return -1;
1105  }
1106  }
1107 #elif defined(_WIN32)
1108  if (!ReleaseSemaphore(hSemaphore, 1, NULL)) {
1109  // get error info from windows (from FormatMessage help page)
1110  LPVOID lpMsgBuf;
1111 
1112  FormatMessage(
1113  FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1114  GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1115  // Default language
1116  (LPTSTR)&lpMsgBuf, 0, NULL);
1117  fprintf(stderr,
1118  "vrpn_Semaphore::v: error v'ing semaphore, "
1119  "WIN32 ReleaseSemaphore call caused the following error: %s",
1120  (LPTSTR)lpMsgBuf);
1121  // Free the buffer.
1122  LocalFree(lpMsgBuf);
1123  return -1;
1124  }
1125 #else
1126  // Posix by default
1127  if (sem_post(semaphore) != 0) {
1128  perror("vrpn_Semaphore::p: ");
1129  return -1;
1130  }
1131 #endif
1132  return 0;
1133 }
1134 
1135 // 0 if it can't get the resource, 1 if it can
1136 // -1 if fail
1138 {
1139  int iRetVal = 1;
1140 #ifdef sgi
1141  if (fUsingLock) {
1142  // don't spin at all
1143  iRetVal = uscsetlock(l, 0);
1144  if (iRetVal <= 0) {
1145  perror("vrpn_Semaphore::condP: uscsetlock:");
1146  return -1;
1147  }
1148  }
1149  else {
1150  iRetVal = uscpsema(ps);
1151  if (iRetVal <= 0) {
1152  perror("vrpn_Semaphore::condP: uscpsema:");
1153  return -1;
1154  }
1155  }
1156 #elif defined(_WIN32)
1157  switch (WaitForSingleObject(hSemaphore, 0)) {
1158  case WAIT_OBJECT_0:
1159  // got the resource
1160  break;
1161  case WAIT_TIMEOUT:
1162  // resource not free
1163  iRetVal = 0;
1164  break;
1165  case WAIT_ABANDONED:
1166  ALL_ASSERT(0, "vrpn_Semaphore::condP: thread holding resource died");
1167  return -1;
1168  break;
1169  case WAIT_FAILED:
1170  // get error info from windows (from FormatMessage help page)
1171  LPVOID lpMsgBuf;
1172 
1173  FormatMessage(
1174  FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM, NULL,
1175  GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
1176  // Default language
1177  (LPTSTR)&lpMsgBuf, 0, NULL);
1178  fprintf(stderr,
1179  "Semaphore::condP: error waiting for resource, "
1180  "WIN32 WaitForSingleObject call caused the following error: %s",
1181  (LPTSTR)lpMsgBuf);
1182  // Free the buffer.
1183  LocalFree(lpMsgBuf);
1184  return -1;
1185  break;
1186  default:
1187  ALL_ASSERT(0, "vrpn_Semaphore::p: unknown return code");
1188  return -1;
1189  }
1190 #else
1191  // Posix by default
1192  iRetVal = sem_trywait(semaphore);
1193  if (iRetVal == 0) {
1194  iRetVal = 1;
1195  }
1196  else if (errno == EAGAIN) {
1197  iRetVal = 0;
1198  }
1199  else {
1200  perror("vrpn_Semaphore::condP: ");
1201  iRetVal = -1;
1202  }
1203 #endif
1204  return iRetVal;
1205 }
1206 
1208 
1209 // static var definition
1210 #ifdef sgi
1211 usptr_t *vrpn_Semaphore::ppaArena = NULL;
1212 
1213 #include <sys/stat.h>
1214 // for umask stuff
1215 #include <sys/types.h>
1216 #include <sys/stat.h>
1217 
1218 void vrpn_Semaphore::allocArena()
1219 {
1220  // /dev/zero is a special file which can only be shared between
1221  // processes/threads which share file descriptors.
1222  // It never shows up in the file system.
1223  if ((ppaArena = usinit("/dev/zero")) == NULL) {
1224  perror("vrpn_Thread::allocArena: usinit:");
1225  }
1226 }
1227 #endif
1228 
1229 vrpn_Thread::vrpn_Thread(void (*pfThreadparm)(vrpn_ThreadData &ThreadData),
1230  vrpn_ThreadData tdparm)
1231  : pfThread(pfThreadparm)
1232  , td(tdparm)
1233  , threadID(0)
1234 {
1235 }
1236 
1238 {
1239  if (threadID != 0) {
1240  fprintf(stderr, "vrpn_Thread::go: already running\n");
1241  return false;
1242  }
1243 
1244 #ifdef sgi
1245  if ((threadID = sproc(&threadFuncShell, PR_SALL, this)) ==
1246  ((unsigned long)-1)) {
1247  perror("vrpn_Thread::go: sproc");
1248  return false;
1249  }
1250 // Threads not defined for the CYGWIN environment yet...
1251 #elif defined(_WIN32) && !defined(__CYGWIN__)
1252  // pass in func, let it pick stack size, and arg to pass to thread
1253  if ((threadID = _beginthread(&threadFuncShell, 0, this)) ==
1254  ((unsigned long)-1)) {
1255  perror("vrpn_Thread::go: _beginthread");
1256  return false;
1257  }
1258 #else
1259  // Pthreads by default
1260  if (pthread_create(&threadID, NULL, &threadFuncShellPosix, this) != 0) {
1261  perror("vrpn_Thread::go:pthread_create: ");
1262  return false;
1263  }
1264 #endif
1265  return true;
1266 }
1267 
1269 {
1270 // kill the os thread
1271 #if defined(sgi) || defined(_WIN32)
1272  if (threadID > 0) {
1273 #ifdef sgi
1274  if (::kill((long)threadID, SIGKILL) < 0) {
1275  perror("vrpn_Thread::kill: kill:");
1276  return false;
1277  }
1278 #elif defined(_WIN32)
1279  // Return value of -1 passed to TerminateThread causes a warning.
1280  if (!TerminateThread((HANDLE)threadID, 1)) {
1281  fprintf(stderr,
1282  "vrpn_Thread::kill: problem with terminateThread call.\n");
1283  return false;
1284  }
1285 #endif
1286 #else
1287  if (threadID) {
1288  // Posix by default. Detach so that the thread's resources will be
1289  // freed automatically when it is killed.
1290  if (pthread_detach(threadID) != 0) {
1291  perror("vrpn_Thread::kill:pthread_detach: ");
1292  return false;
1293  }
1294  if (pthread_kill(threadID, SIGKILL) != 0) {
1295  perror("vrpn_Thread::kill:pthread_kill: ");
1296  return false;
1297  }
1298 #endif
1299  }
1300  else {
1301  fprintf(stderr, "vrpn_Thread::kill: thread is not currently alive.\n");
1302  return false;
1303  }
1304  threadID = 0;
1305  return true;
1306 }
1307 
1308 bool vrpn_Thread::running() { return threadID != 0; }
1309 
1311 
1313 {
1314 #ifdef vrpn_THREADS_AVAILABLE
1315  return true;
1316 #else
1317  return false;
1318 #endif
1319 }
1320 
1321 void vrpn_Thread::userData(void *pvNewUserData) { td.pvUD = pvNewUserData; }
1322 
1323 void *vrpn_Thread::userData() { return td.pvUD; }
1324 
1325 void vrpn_Thread::threadFuncShell(void *pvThread)
1326 {
1327  vrpn_Thread *pth = static_cast<vrpn_Thread *>(pvThread);
1328  pth->pfThread(pth->td);
1329  // thread has stopped running
1330 #if !defined(sgi) && !defined(_WIN32)
1331  // Pthreads; need to detach the thread so its resources will be freed.
1332  if (pthread_detach(pth->threadID) != 0) {
1333  perror("vrpn_Thread::threadFuncShell:pthread_detach: ");
1334  }
1335 #endif
1336  pth->threadID = 0;
1337 }
1338 
1339 // This is a Posix-compatible function prototype that
1340 // just calls the other function.
1342 {
1343  threadFuncShell(pvThread);
1344  return NULL;
1345 }
1346 
1348 {
1349  if (running()) {
1350  kill();
1351  }
1352 }
1353 
1354 // For the code to get the number of processor cores.
1355 #ifdef __APPLE__
1356 #include <sys/param.h>
1357 #include <sys/sysctl.h>
1358 #endif
1359 
1361 {
1362 #ifdef _WIN32
1363  // Copy the hardware information to the SYSTEM_INFO structure.
1364  SYSTEM_INFO siSysInfo;
1365  GetSystemInfo(&siSysInfo);
1366  return siSysInfo.dwNumberOfProcessors;
1367 #elif linux
1368  // For Linux, we look at the /proc/cpuinfo file and count up the number
1369  // of "processor :" entries (tab between processor and colon) in
1370  // the file to find out how many we have.
1371  FILE *f = fopen("/proc/cpuinfo", "r");
1372  int count = 0;
1373  if (f == NULL) {
1374  perror("vrpn_Thread::number_of_processors:fopen: ");
1375  return 1;
1376  }
1377 
1378  char line[512];
1379  while (fgets(line, sizeof(line), f) != NULL) {
1380  if (strncmp(line, "processor\t:", strlen("processor\t:")) == 0) {
1381  count++;
1382  }
1383  }
1384 
1385  fclose(f);
1386  if (count == 0) {
1387  fprintf(stderr,
1388  "vrpn_Thread::number_of_processors: Found zero, returning 1\n");
1389  count = 1;
1390  }
1391  return count;
1392 
1393 #elif __APPLE__
1394  int count;
1395  size_t size = sizeof(count);
1396  if (sysctlbyname("hw.ncpu", &count, &size, NULL, 0)) {
1397  return 1;
1398  }
1399  else {
1400  return static_cast<unsigned>(count);
1401  }
1402 
1403 #else
1404  fprintf(stderr, "vrpn_Thread::number_of_processors: Not yet implemented on "
1405  "this architecture.\n");
1406  return 1;
1407 #endif
1408 }
1409 
1410 // Thread function to call from within vrpn_test_threads_and_semaphores().
1411 // In this case, the userdata pointer is a pointer to a semaphore that
1412 // the thread should call v() on so that it will free up the main program
1413 // thread.
1414 static void vrpn_test_thread_body(vrpn_ThreadData &threadData)
1415 {
1416  if (threadData.pvUD == NULL) {
1417  fprintf(stderr, "vrpn_test_thread_body(): pvUD is NULL\n");
1418  return;
1419  }
1420  vrpn_Semaphore *s = static_cast<vrpn_Semaphore *>(threadData.pvUD);
1421  s->v();
1422 
1423  return;
1424 }
1425 
1427 {
1428  //------------------------------------------------------------
1429  // Make a semaphore to test in single-threaded mode. First run its count
1430  // all the way
1431  // down to zero, then bring it back to the full complement and then bring it
1432  // down
1433  // again. Check that all of the semaphores are available and also that
1434  // there are no
1435  // more than expected available.
1436  const unsigned sem_count = 5;
1437  vrpn_Semaphore s(sem_count);
1438  unsigned i;
1439  for (i = 0; i < sem_count; i++) {
1440  if (s.condP() != 1) {
1441  fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore ran "
1442  "out of counts\n");
1443  return false;
1444  }
1445  }
1446  if (s.condP() != 0) {
1447  fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore had too "
1448  "many counts\n");
1449  return false;
1450  }
1451  for (i = 0; i < sem_count; i++) {
1452  if (s.v() != 0) {
1453  fprintf(stderr, "vrpn_test_threads_and_semaphores(): Could not "
1454  "release Semaphore\n");
1455  return false;
1456  }
1457  }
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");
1462  return false;
1463  }
1464  }
1465  if (s.condP() != 0) {
1466  fprintf(stderr, "vrpn_test_threads_and_semaphores(): Semaphore had too "
1467  "many counts, round 2\n");
1468  return false;
1469  }
1470 
1471  //------------------------------------------------------------
1472  // Get a semaphore and use it to construct a thread data structure and then
1473  // a thread. Use that thread to test whether threading is enabled (if not,
1474  // then
1475  // this completes our testing) and to find out how many processors there
1476  // are.
1478  td.pvUD = NULL;
1479  vrpn_Thread t(vrpn_test_thread_body, td);
1480 
1481  // If threading is not enabled, then we're done.
1482  if (!t.available()) {
1483  return true;
1484  }
1485 
1486  // Find out how many processors we have.
1487  unsigned num_procs = t.number_of_processors();
1488  if (num_procs == 0) {
1489  fprintf(stderr, "vrpn_test_threads_and_semaphores(): "
1490  "vrpn_Thread::number_of_processors() returned zero\n");
1491  return false;
1492  }
1493 
1494  //------------------------------------------------------------
1495  // Now make sure that we can actually run a thread. Do this by
1496  // creating a semaphore with one entry and calling p() on it.
1497  // Then make sure we can't p() it again and then run a thread
1498  // that will call v() on it when it runs.
1499  vrpn_Semaphore sem;
1500  if (sem.p() != 1) {
1501  fprintf(stderr, "vrpn_test_threads_and_semaphores(): thread-test "
1502  "Semaphore had no count\n");
1503  return false;
1504  }
1505  if (sem.condP() != 0) {
1506  fprintf(stderr, "vrpn_test_threads_and_semaphores(): thread-test "
1507  "Semaphore had too many counts\n");
1508  return false;
1509  }
1510  t.userData(&sem);
1511  if (!t.go()) {
1512  fprintf(stderr,
1513  "vrpn_test_threads_and_semaphores(): Could not start thread\n");
1514  return false;
1515  }
1516  struct timeval start;
1517  struct timeval now;
1518  vrpn_gettimeofday(&start, NULL);
1519  while (true) {
1520  if (sem.condP() == 1) {
1521  // The thread must have run; we got the semaphore!
1522  break;
1523  }
1524 
1525  // Time out after three seconds if we haven't had the thread run to
1526  // reset the semaphore.
1527  vrpn_gettimeofday(&now, NULL);
1528  struct timeval diff = vrpn_TimevalDiff(now, start);
1529  if (diff.tv_sec >= 3) {
1530  fprintf(stderr,
1531  "vrpn_test_threads_and_semaphores(): Thread didn't run\n");
1532  return false;
1533  }
1534 
1535  vrpn_SleepMsecs(1);
1536  }
1537 
1538  return true;
1539 }
void vrpn_SleepMsecs(double dMsecs)
Definition: vrpn_Shared.C:157
#define ALL_ASSERT(exp, msg)
Definition: vrpn_Shared.C:851
VRPN_API int vrpn_unbuffer(const char **buffer, timeval *t)
Utility routine for taking a struct timeval from a buffer that was sent as a message.
Definition: vrpn_Shared.C:312
bool reset(int cNumResources=1)
Definition: vrpn_Shared.C:1012
timeval vrpn_TimevalNormalize(const timeval &in_tv)
Definition: vrpn_Shared.C:34
void * userData()
Definition: vrpn_Shared.C:1323
vrpn_Semaphore(int cNumResources=1)
Definition: vrpn_Shared.C:858
bool vrpn_test_threads_and_semaphores(void)
Definition: vrpn_Shared.C:1426
timeval vrpn_TimevalDiff(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:92
thread_t threadID
Definition: vrpn_Shared.h:609
#define CHECK(a)
Definition: vrpn_Shared.C:22
vrpn_ThreadData td
Definition: vrpn_Shared.h:599
#define VRPN_API
double vrpn_TimevalDurationSeconds(struct timeval endT, struct timeval startT)
Return the number of seconds between startT and endT as a floating-point value.
Definition: vrpn_Shared.C:135
timeval vrpn_TimevalScale(const timeval &tv, double scale)
Definition: vrpn_Shared.C:102
static unsigned number_of_processors()
Definition: vrpn_Shared.C:1360
double vrpn_TimevalMsecs(const timeval &tv)
Definition: vrpn_Shared.C:141
timeval vrpn_MsecsTimeval(const double dMsecs)
Definition: vrpn_Shared.C:146
bool running()
Definition: vrpn_Shared.C:1308
void userData(void *pvNewUserData)
Definition: vrpn_Shared.C:1321
void(* pfThread)(vrpn_ThreadData &ThreadData)
Definition: vrpn_Shared.h:598
sem_t * semaphore
Definition: vrpn_Shared.h:533
bool vrpn_TimevalGreater(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:113
vrpn_float64 vrpn_htond(vrpn_float64 d)
Definition: vrpn_Shared.C:190
bool vrpn_TimevalEqual(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:121
static void threadFuncShell(void *pvThread)
Definition: vrpn_Shared.C:1325
vrpn_float64 vrpn_ntohd(vrpn_float64 d)
Definition: vrpn_Shared.C:226
#define vrpn_gettimeofday
Definition: vrpn_Shared.h:89
VRPN_API int vrpn_buffer(char **insertPt, vrpn_int32 *buflen, const timeval t)
Utility routine for placing a timeval struct into a buffer that is to be sent as a message.
Definition: vrpn_Shared.C:241
static void * threadFuncShellPosix(void *pvThread)
Definition: vrpn_Shared.C:1341
static bool available()
Definition: vrpn_Shared.C:1312
vrpn_Thread(vrpn_THREAD_FUNC pfThread, vrpn_ThreadData td)
Definition: vrpn_Shared.C:1229
timeval vrpn_TimevalSum(const timeval &tv1, const timeval &tv2)
Definition: vrpn_Shared.C:45
unsigned long vrpn_TimevalDuration(struct timeval endT, struct timeval startT)
Return number of microseconds between startT and endT.
Definition: vrpn_Shared.C:129
pthread_t thread_t
Definition: vrpn_Shared.h:571
thread_t pid()
Definition: vrpn_Shared.C:1310