cmdu_ackq: use getcurrtime and change timercmp(<=) -> timercmp(>)

src/cmdu_ackq.c

 /*
- * delm_cmdu_ackq.c
+ * cmdu_ackq.c
  * CMDU response and ack queue management
  *
  * Copyright (C) 2020 IOPSYS Software Solutions AB. All rights reserved.
@@ -12,7 +12,7 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <pthread.h>
+#include <time.h>
 
 #include <easy/easy.h>
 
@@ -43,6 +43,27 @@ static int timeradd_msecs(struct timeval *a, unsigned long msecs,
 	return -1;
 }
 
+static int getcurrtime(struct timeval *out)
+{
+	struct timespec nowts = { 0 };
+	struct timeval now = { 0 };
+	int ret;
+
+	ret = clock_gettime(CLOCK_MONOTONIC_RAW, &nowts);
+	if (!ret) {
+		now.tv_sec = nowts.tv_sec;
+		now.tv_usec = nowts.tv_nsec / 1000;
+	} else {
+		ret = gettimeofday(&now, NULL);
+	}
+
+	now.tv_usec = (now.tv_usec / 1000) * 1000;
+	out->tv_sec = now.tv_sec;
+	out->tv_usec = now.tv_usec;
+
+	return ret;
+}
+
 struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
 					     uint8_t *dest, uint32_t timeout,
 					     int resend_cnt, void *cookie)
@@ -58,7 +79,7 @@ struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
 
 	msg->type = type;
 	msg->mid = mid;
-	gettimeofday(&tsp, NULL);
+	getcurrtime(&tsp);
 	msg->ageing_time = timeout;
 	timeradd_msecs(&tsp, msg->ageing_time, &msg->ageing_tmo);
 
@@ -67,7 +88,7 @@ struct cmdu_ackq_entry *cmdu_ackq_create_msg(uint16_t type, uint16_t mid,
 	memcpy(msg->origin, dest, 6);
 	msg->resend_cnt = resend_cnt;
 	msg->cookie = cookie;
-	dbg("    CREATE msg: type = 0x%04x  mid = %hu origin = " MACFMT " timeout = { %u (%lu:%lu) }\n",
+	dbg("    CREATE msg: type = 0x%04x  mid = %hu origin = " MACFMT " timeout = { %u ms (%lu:%lu) }\n",
 		type, mid, MAC2STR(dest), msg->ageing_time,
 		msg->ageing_tmo.tv_sec, msg->ageing_tmo.tv_usec / 1000);
 
@@ -97,21 +118,20 @@ static void cmdu_ackq_ageout_entry(struct cmdu_ackq *st, struct hlist_head *head
 	struct timeval now = { 0 };
 
 
-	gettimeofday(&now, NULL);
-	now.tv_usec = (now.tv_usec / 1000) * 1000;
-
+	getcurrtime(&now);
 
 	hlist_for_each_entry_safe(msg, tmp, head, hlist) {
 		int action = CMDU_ACKQ_TMO_NONE;
 		struct timeval new_next_tmo = { 0 };
 
-		dbg("%s(): Entry msg->ageout = (%lu.%lu), now = (%lu.%lu)\n",
+		loud("%s(): check entry msg->ageout? (%lu.%lu), now = (%lu.%lu)\n",
 		     __func__, msg->ageing_tmo.tv_sec, msg->ageing_tmo.tv_usec,
 		     now.tv_sec, now.tv_usec);
 
-		if (timercmp(&msg->ageing_tmo, &now, <=)) {
+		if (!timercmp(&msg->ageing_tmo, &now, >)) {
 			dbg("No response from " MACFMT " with CMDU 0x%x mid = %hu\n",
 				MAC2STR(msg->origin), msg->type, msg->mid);
+
 			if (st->timeout_cb) {
 				action = st->timeout_cb(st, msg);
 				if (action == CMDU_ACKQ_TMO_REARM) {
@@ -136,7 +156,7 @@ static void cmdu_ackq_ageout_entry(struct cmdu_ackq *st, struct hlist_head *head
 			if (!timercmp(min_next_tmo, &new_next_tmo, <)) {
 				min_next_tmo->tv_sec = new_next_tmo.tv_sec;
 				min_next_tmo->tv_usec = new_next_tmo.tv_usec;
-				loud("next-tmo = (%lu.%lu)\n",
+				loud("Adjusted next-tmo = (%lu.%lu)\n",
 					min_next_tmo->tv_sec,
 					min_next_tmo->tv_usec);
 			}
@@ -151,20 +171,19 @@ static void cmdu_ackq_ageing_timer_run(atimer_t *t)
 	//struct timeval *next_tmo = &st->next_tmo;
 	struct timeval min_next_tmo = { .tv_sec = 999999 };
 	int remain_cnt = 0;
-	int i;
 	struct timeval nu;
+	int i;
 
-	gettimeofday(&nu, NULL);
 
-	loud("\n ----Timer now = %lu.%lu  --- cnt = %d---\n",
-		nu.tv_sec, nu.tv_usec, st->pending_cnt);
+	getcurrtime(&nu);
+	loud("\n ----In timer ---- time now = %lu.%lu,  msg-cnt = %d\n",
+	     nu.tv_sec, nu.tv_usec, st->pending_cnt);
 
-	//spin_lock(&st->hash_lock);
 	for (i = 0; i < CMDU_BACKLOG_MAX; i++) {
 		if (hlist_empty(&st->table[i]))
 			continue;
 
-		loud("i = %d\t", i);
+		loud("cmdu_ackq row %d has msg\n", i);
 		cmdu_ackq_ageout_entry(st, &st->table[i], &min_next_tmo);
 	}
 
@@ -172,16 +191,17 @@ static void cmdu_ackq_ageing_timer_run(atimer_t *t)
 	st->next_tmo.tv_sec = min_next_tmo.tv_sec;
 	st->next_tmo.tv_usec = min_next_tmo.tv_usec;
 
-	//spin_unlock(&st->hash_lock);
+	loud("\n ----Next timer adjusted ---- next_tmo = %lu.%lu,  msg-cnt = %d\n",
+	     st->next_tmo.tv_sec, st->next_tmo.tv_usec, remain_cnt);
+
 	if (remain_cnt) {
 		uint32_t tmo_msecs =
 			st->next_tmo.tv_sec * 1000 + st->next_tmo.tv_usec / 1000;
 
-		 loud("%s(): remain = %d next_tmo = {(%lu s, %lu us) %u}\n",
-		       __func__, remain_cnt, st->next_tmo.tv_sec, st->next_tmo.tv_usec, tmo_msecs); 
-
-		if (tmo_msecs > 0)
+		if (tmo_msecs > 0) {
+			loud("Set timer after %u ms, msg-cnt = %d\n", tmo_msecs, remain_cnt);
 			timer_set(&st->ageing_timer, tmo_msecs);
+		}
 	}
 }
 
@@ -190,7 +210,6 @@ int cmdu_ackq_init(void *cmdu_q)
 	struct cmdu_ackq *q = (struct cmdu_ackq *)cmdu_q;
 
 	memset(q, 0, sizeof(*q));
-	pthread_mutex_init(&q->qlock, NULL);
 	timer_init(&q->ageing_timer, cmdu_ackq_ageing_timer_run);
 
 	return 0;
@@ -203,16 +222,13 @@ struct cmdu_ackq_entry *cmdu_ackq_lookup(void *cmdu_q, uint16_t type,
 	int idx = cmdu_ackq_hash(type, mid, dest);
 	struct cmdu_ackq_entry *msg = NULL;
 
-	pthread_mutex_lock(&q->qlock);
 	hlist_for_each_entry(msg, &q->table[idx], hlist) {
 		if (msg->type == type && msg->mid == mid &&
 		    !memcmp(msg->origin, dest, 6)) {
 
-			pthread_mutex_unlock(&q->qlock);
 			return msg;
 		}
 	}
-	pthread_mutex_unlock(&q->qlock);
 
 	return NULL;
 }
@@ -223,7 +239,6 @@ void cmdu_ackq_flush(void *cmdu_q)
 	struct cmdu_ackq_entry *msg = NULL;
 	int idx = 0;
 
-	pthread_mutex_lock(&q->qlock);
 	for (idx = 0; idx < CMDU_BACKLOG_MAX; idx++) {
 		hlist_for_each_entry(msg, &q->table[idx], hlist)
 			cmdu_ackq_delete_msg(q, msg);
@@ -232,7 +247,6 @@ void cmdu_ackq_flush(void *cmdu_q)
 	}
 
 	q->pending_cnt = 0;
-	pthread_mutex_unlock(&q->qlock);
 }
 
 void cmdu_ackq_free(void *cmdu_q)
@@ -241,7 +255,6 @@ void cmdu_ackq_free(void *cmdu_q)
 
 	cmdu_ackq_flush(q);
 	timer_del(&q->ageing_timer);
-	pthread_mutex_destroy(&q->qlock);
 }
 
 /* In this function, type = cmdutype that is expected with 'mid' from 'dest' */
@@ -262,21 +275,24 @@ int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
 	if (msg) {
 		int idx = cmdu_ackq_hash(type, mid, dest);
 
-		pthread_mutex_lock(&q->qlock);
 		hlist_add_head(&msg->hlist, &q->table[idx]);
 
 		q->pending_cnt++;
-		dbg("    ENQ:        type = 0x%04x  mid = %hu origin = " MACFMT "\n",
-		     type, mid, MAC2STR(dest));
+		dbg("    ENQ:        type = 0x%04x  mid = %hu origin = " MACFMT " (pending msg-cnt = %d)\n",
+		     type, mid, MAC2STR(dest), q->pending_cnt);
 
 		if (timer_pending(&q->ageing_timer)) {
-			loud("Timer pending ===\n");
+			loud("Pending timer === next_tmo = %lu.%lu,  msg-ageing_tmo = %lu.%lu\n",
+			     q->next_tmo.tv_sec, q->next_tmo.tv_usec,
+			     msg->ageing_tmo.tv_sec, msg->ageing_tmo.tv_usec);
+
 			if (timercmp(&q->next_tmo, &msg->ageing_tmo, >)) {
 				q->next_tmo.tv_sec = msg->ageing_tmo.tv_sec;
 				q->next_tmo.tv_usec = msg->ageing_tmo.tv_usec;
 
 				timer_set(&q->ageing_timer, msg->ageing_time);
-				loud("Adjust ageout timer ===\n");
+				loud("Adjusted next_tmo = %lu.%lu,  msg-cnt = %d\n",
+				     q->next_tmo.tv_sec, q->next_tmo.tv_usec, q->pending_cnt);
 			}
 		} else {
 			loud("Start ageout timer ===\n");
@@ -285,7 +301,6 @@ int cmdu_ackq_enqueue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *dest,
 			timer_set(&q->ageing_timer, msg->ageing_time);
 		}
 
-		pthread_mutex_unlock(&q->qlock);
 		return 0;
 	}
 
@@ -307,10 +322,8 @@ int cmdu_ackq_dequeue(void *cmdu_q, uint16_t type, uint16_t mid, uint8_t *src, v
 
 	idx = cmdu_ackq_hash(type, mid, src);
 
-	pthread_mutex_lock(&q->qlock);
 	hlist_del(&msg->hlist, &q->table[idx]);
 	q->pending_cnt--;
-	pthread_mutex_unlock(&q->qlock);
 
 	dbg("DEQ: type = 0x%04x  mid = %hu origin = " MACFMT "\n",
 	     type, mid, MAC2STR(src));