+ static struct timeval base_time = { 0, 0 };
+ struct timeval current_time;
+ unsigned long counter_ms;
+
+ gettimeofday(¤t_time, NULL);
+
+ if (mode == INIT_COUNTER || current_time.tv_sec < base_time.tv_sec)
+ base_time = current_time;
+
+ counter_ms = (current_time.tv_sec - base_time.tv_sec) * 1000
+ + (current_time.tv_usec - base_time.tv_usec) / 1000;
+
+ return counter_ms; /* return milliseconds since last init */
+}
+
+void InitCounter() /* set counter back to zero */
+{
+ mainCounter(INIT_COUNTER);
+}
+
+unsigned long Counter() /* get milliseconds since last call of InitCounter() */
+{
+ return(mainCounter(READ_COUNTER));
+}
+
+static void sleep_milliseconds(unsigned long milliseconds_delay)
+{
+ if (milliseconds_delay < 5)