1 files changed, 44 insertions, 42 deletions

diff --git a/src/isochronous.cpp b/src/isochronous.cpp
index 573f05a..62c9703 100644
--- a/src/isochronous.cpp
+++ b/src/isochronous.cpp
@@ -57,7 +57,7 @@ using namespace Isochronous;
 FrameCounter::FrameCounter (double value, const Timestamp& start) : frequency(value) {
     period = static_cast<unsigned int>(1000000 / frequency);
     startTime = start;
-    nextslotTime=start;
+    nextslotTime = start;
     lastcounter = 0;
     slot_counter = 0;
     slip = 0;
@@ -69,6 +69,8 @@ FrameCounter::FrameCounter (double value) : frequency(value) {
     if (!SetThreadPriority(GetCurrentThread(), THREAD_PRIORITY_TIME_CRITICAL))
 	WARN_errno(1, "SetThreadPriority");
 #endif
+    startTime.setnow();
+    nextslotTime = startTime;
     period = static_cast<unsigned int>(1000000 / frequency); // unit us
     lastcounter = 0;
     slot_counter = 0;
@@ -119,23 +121,23 @@ int FrameCounter::mySetWaitableTimer (long delay_time) {
 #endif
 
 #if HAVE_CLOCK_NANOSLEEP
-unsigned int FrameCounter::wait_tick (long *sched_err) {
+unsigned int FrameCounter::wait_tick (long *sched_err, bool sync_strict) {
     Timestamp now;
     int rc = true;
     if (!slot_counter) {
 	slot_counter = 1;
-	now.setnow();
 	nextslotTime = now;
+	startTime = now;
     } else {
-	while (!now.before(nextslotTime)) {
-	    now.setnow();
+	nextslotTime.add(period);
+	slot_counter++;
+	while (now.subUsec(nextslotTime) > static_cast<long>(sync_strict ? 0 : period)) {
 	    nextslotTime.add(period);
-//	    printf("***** next slot %ld.%ld\n",nextslotTime.getSecs(), nextslotTime.getUsecs());
 	    slot_counter++;
 	}
-	if (lastcounter && ((slot_counter - lastcounter) > 1)) {
+        if (lastcounter && ((slot_counter - lastcounter) > 1)) {
 	    slip++;
-	}
+        }
     }
   #ifndef WIN32
     timespec txtime_ts;
@@ -179,42 +181,42 @@ unsigned int FrameCounter::wait_tick (long *sched_err) {
     return(slot_counter);
 }
 #else
-unsigned int FrameCounter::wait_tick (long *sched_err) {
-    long remaining;
-    unsigned int framecounter;
-
-    if (!lastcounter) {
-	reset();
-	framecounter = 1;
+unsigned int FrameCounter::wait_tick (long *sched_err, bool sync_strict) {
+    Timestamp now;
+    if (!slot_counter) {
+	slot_counter = 1;
+	startTime = now;
+	nextslotTime = now;
     } else {
-	framecounter = get(&remaining);
-	if ((framecounter - lastcounter) > 1)
-	    slip++;
-//    	delay_loop(remaining);
-	remaining *= 1000;
-	struct timespec tv0={0,0}, tv1;
-	tv0.tv_sec = (remaining / BILLION);
-	tv0.tv_nsec += (remaining % BILLION);
-	if (tv0.tv_nsec >= BILLION) {
-	    tv0.tv_sec++;
-	    tv0.tv_nsec -= BILLION;
+        long remaining;
+	nextslotTime.add(period);
+	slot_counter++;
+	while (now.subUsec(nextslotTime) > (sync_strict ? 0 : period)) {
+	    nextslotTime.add(period);
+	    slot_counter++;
 	}
-	Timestamp slotstart = startTime;
-	slot_counter = get();
-	// period unit is in microseconds, convert to seconds
-	slotstart.add(slot_counter * period * 1e-6);
-	int rc = nanosleep(&tv0, &tv1);
-	if (sched_err) {
-	    Timestamp actual;
-	    *sched_err = actual.subUsec(slotstart);
-//	    printf("**** slot %ld.%ld actual %ld.%ld %ld\n", slotstart.getSecs(), slotstart.getUsecs(), actual.getSecs(), actual.getUsecs(), *sched_err);
+//	printf("**** sync strict %d now %ld.%ld next %ld.%ld\n", sync_strict, now.getSecs(), now.getUsecs(), nextslotTime.getSecs(), nextslotTime.getUsecs());
+	if (now.before(nextslotTime)) {
+	    struct timespec tv0={0,0}, tv1;
+	    get(&remaining);
+	    remaining *= 1000; // convert to nano seconds
+	    tv0.tv_sec = (remaining / BILLION);
+	    tv0.tv_nsec += (remaining % BILLION);
+	    if (tv0.tv_nsec >= BILLION) {
+	        tv0.tv_sec++;
+		tv0.tv_nsec -= BILLION;
+	    }
+//	    printf("**** wait: nanos %ld remain %ld.%ld\n", remaining, tv0.tv_sec, tv0.tv_nsec);
+	    int rc = nanosleep(&tv0, &tv1);
+	    if (sched_err) {
+	        Timestamp actual;
+		*sched_err = actual.subUsec(nextslotTime);
+		//	printf("**** slot %ld.%ld actual %ld.%ld %ld\n", slotstart.getSecs(), slotstart.getUsecs(), actual.getSecs(), actual.getUsecs(), *sched_err);
+	    }
+	    WARN_errno((rc != 0), "nanosleep wait_tick");
 	}
-	WARN_errno((rc != 0), "nanosleep wait_tick");	    ;
-//	printf("****** rc = %d, remain %ld.%ld\n", rc, tv1.tv_sec, tv1.tv_nsec);
-	framecounter ++;
     }
-    lastcounter = framecounter;
-    return(framecounter);
+    return(slot_counter);
 }
 #endif
 inline unsigned int FrameCounter::get () const {
@@ -226,7 +228,7 @@ inline unsigned int FrameCounter::get (const Timestamp& slot) const {
     return(slot_counter + 1); // Frame counter for packets starts at 1
 }
 
-inline unsigned int FrameCounter::get (long *ticks_remaining) {
+unsigned int FrameCounter::get (long *ticks_remaining) {
     assert(ticks_remaining != NULL);
     Timestamp sampleTime;  // Constructor will initialize timestamp to now
     long usecs = -startTime.subUsec(sampleTime);
@@ -234,7 +236,7 @@ inline unsigned int FrameCounter::get (long *ticks_remaining) {
     // figure out how many usecs before the next frame counter tick
     // the caller can use this to delay until the next tick
     *ticks_remaining = (counter * period) - usecs;
-    return(counter + 1); // Frame counter for packets starts at 1
+    return(counter); // Frame counter for packets starts at 1
 }
 
 inline Timestamp FrameCounter::next_slot () {