diff --git a/src/cmdu_ackq.c b/src/cmdu_ackq.c index 4e970bc32cffc944b9d548daa520880a1bc2d02e..30d233f31223b81f0d403ec6c9b4c46e3abaa067 100644 --- a/src/cmdu_ackq.c +++ b/src/cmdu_ackq.c @@ -1,5 +1,5 @@ /* - * 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));