diff --git a/include/dap_common.h b/include/dap_common.h
index e3212ca1ba55ce7b1950941e254fdb97c3bcbb70..ca885dc8f8725906a4e0155197fc71f520a133a2 100755
--- a/include/dap_common.h
+++ b/include/dap_common.h
@@ -352,6 +352,7 @@ void dap_set_log_tag_width(size_t width);
 char *dap_itoa(int i);
 
 int dap_time_to_str_rfc822(char * out, size_t out_size_max, time_t t);
+int timespec_diff(struct timespec *a_start, struct timespec *a_stop, struct timespec *a_result);
 
 int get_select_breaker(void);
 int send_select_break(void);
diff --git a/src/dap_common.c b/src/dap_common.c
index 56bc9fd1627628f4a7886b6f95df5d4209e8eafa..1f6988152caa1b07ac2dbbd197d553f66156a342 100755
--- a/src/dap_common.c
+++ b/src/dap_common.c
@@ -370,6 +370,32 @@ int dap_time_to_str_rfc822(char * out, size_t out_size_max, time_t t)
   return ret;
 }
 
+/**
+ * @brief Calculate diff of two struct timespec
+ * @param[in] a_start - first time
+ * @param[in] a_stop - second time
+ * @param[out] a_result -  diff time, may be NULL
+ * @return diff time in millisecond
+ */
+int timespec_diff(struct timespec *a_start, struct timespec *a_stop, struct timespec *a_result)
+{
+    if(!a_start || !a_stop)
+        return 0;
+    if(!a_result) {
+        struct timespec l_time_tmp = { 0 };
+        a_result = &l_time_tmp;
+    }
+    if((a_stop->tv_nsec - a_start->tv_nsec) < 0) {
+        a_result->tv_sec = a_stop->tv_sec - a_start->tv_sec - 1;
+        a_result->tv_nsec = a_stop->tv_nsec - a_start->tv_nsec + 1000000000;
+    } else {
+        a_result->tv_sec = a_stop->tv_sec - a_start->tv_sec;
+        a_result->tv_nsec = a_stop->tv_nsec - a_start->tv_nsec;
+    }
+
+    return (a_result->tv_sec * 1000 + a_result->tv_nsec / 1000000);
+}
+
 #define BREAK_LATENCY   1
 
 static int breaker_set[2] = { -1, -1 };