#2

1 files changed, 100 insertions, 0 deletions

diff --git a/src/core/rf_tx_pace.c b/src/core/rf_tx_pace.c
new file mode 100644
index 0000000..ec88a0e
--- /dev/null
+++ b/src/core/rf_tx_pace.c
@@ -0,0 +1,100 @@
+#include "hybbx/rf_tx_pace.h"
+#include "hybbx/posix_time.h"
+
+#include <pthread.h>
+
+static pthread_mutex_t g_pace_lock = PTHREAD_MUTEX_INITIALIZER;
+static struct timespec g_last_tx;
+static struct timespec g_burst_start;
+static int g_have_last_tx;
+
+static void timespec_now(struct timespec *ts)
+{
+    clock_gettime(CLOCK_MONOTONIC, ts);
+}
+
+static long timespec_diff_ms(const struct timespec *later,
+                             const struct timespec *earlier)
+{
+    return (later->tv_sec - earlier->tv_sec) * 1000L
+           + (later->tv_nsec - earlier->tv_nsec) / 1000000L;
+}
+
+static long gap_ms_since_last(const struct timespec *now)
+{
+    long elapsed_ms;
+
+    if (!g_have_last_tx) {
+        return (long)HYBBX_RF_TX_MIN_GAP_MS;
+    }
+    elapsed_ms = timespec_diff_ms(now, &g_last_tx);
+    if (elapsed_ms >= (long)HYBBX_RF_TX_MIN_GAP_MS) {
+        return 0;
+    }
+    return (long)HYBBX_RF_TX_MIN_GAP_MS - elapsed_ms;
+}
+
+static void sleep_ms(long wait_ms)
+{
+    if (wait_ms > 0) {
+        usleep((useconds_t)wait_ms * 1000u);
+    }
+}
+
+static void enforce_burst_limit(struct timespec *now)
+{
+    long since_burst;
+    long wait_ms;
+
+    if (!g_have_last_tx) {
+        g_burst_start = *now;
+        return;
+    }
+
+    if (gap_ms_since_last(now) == 0) {
+        g_burst_start = *now;
+        return;
+    }
+
+    since_burst = timespec_diff_ms(now, &g_burst_start);
+    if (since_burst < (long)HYBBX_RF_TX_MAX_BURST_MS) {
+        return;
+    }
+
+    wait_ms = gap_ms_since_last(now);
+    sleep_ms(wait_ms);
+    timespec_now(now);
+    g_burst_start = *now;
+}
+
+static void mark_tx(const struct timespec *now)
+{
+    g_last_tx = *now;
+    g_have_last_tx = 1;
+}
+
+void hybbx_rf_tx_burst_guard(void)
+{
+    struct timespec now;
+
+    pthread_mutex_lock(&g_pace_lock);
+    timespec_now(&now);
+    enforce_burst_limit(&now);
+    mark_tx(&now);
+    pthread_mutex_unlock(&g_pace_lock);
+}
+
+void hybbx_rf_tx_pace(void)
+{
+    struct timespec now;
+    long wait_ms;
+
+    pthread_mutex_lock(&g_pace_lock);
+    timespec_now(&now);
+    enforce_burst_limit(&now);
+    wait_ms = gap_ms_since_last(&now);
+    sleep_ms(wait_ms);
+    timespec_now(&now);
+    mark_tx(&now);
+    pthread_mutex_unlock(&g_pace_lock);
+}