diff --git a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
index c5c85b5..653eb95 100644
--- a/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
+++ b/ptxdist/local_src/ecu01-codesys/io_driver/ecu01softcompcap.cpp
@@ -45,26 +45,29 @@
 
 // ****************************************************************************
 // function: diffUSec
 // ****************************************************************************
 
 static inline int diffUSec(struct timespec t1, struct timespec t2)
 {
    int diff;
 
    // calculate and return the difference of t1 and t2 in microseconds
 
    diff = USEC_PER_SEC * ((int) t1.tv_sec - (int) t2.tv_sec);
-   diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
-
+   if (t2.tv_sec > t1.tv_sec)
+       diff += ((int) t1.tv_nsec - (int) t2.tv_nsec) / 1000;
+   else
+       diff += (((int) t1.tv_nsec + (int) t2.tv_nsec) / 1000)+USEC_PER_SEC;
+   
    return diff;
 }
 
 // ****************************************************************************
 // ****************************************************************************
 // class: gpioCallBack
 // ****************************************************************************
 // ****************************************************************************
 
 // ****************************************************************************
 // constructor: gpioCallBack
 // ****************************************************************************