From 9d1a342b0c1af950567d8830c0ef46d223d7fb70 Mon Sep 17 00:00:00 2001
From: Tim Niemeyer <tim@tn-x.org>
Date: Mon, 2 Apr 2018 22:11:07 +0200
Subject: [PATCH] set the prio

---
 macstorage.c | 34 ++++++++++++++++++++++++++++++++--
 tc.c         | 20 ++++++++++----------
 tc.h         |  4 ++--
 3 files changed, 44 insertions(+), 14 deletions(-)

diff --git a/macstorage.c b/macstorage.c
index e88e460..3813b47 100644
--- a/macstorage.c
+++ b/macstorage.c
@@ -16,12 +16,33 @@ struct Entry
 {
     uint8_t mac[6];
     time_t timestamp;
+    uint8_t prio;
 } storage[MAXENTRIES];
 
+uint8_t usedPrio[MAXENTRIES];
+
 size_t count = 0;
 
 pthread_mutex_t mux;
 
+uint8_t _getNextFreePrio()
+{
+    for (size_t i=0; i<MAXENTRIES; i++)
+    {
+        if (usedPrio[i] == 0)
+        {
+            usedPrio[i] = 1;
+            return i;
+        }
+    }
+    return 255;
+}
+
+void _freePrio(uint8_t prio)
+{
+    usedPrio[prio] = 0;
+}
+
 void _addEntry(uint8_t mac[6])
 {
     for (size_t i=0; i<count; i++)
@@ -35,11 +56,13 @@ void _addEntry(uint8_t mac[6])
 
     if (count != MAXENTRIES)
     {
+        uint8_t prio = _getNextFreePrio();
         memcpy(storage[count].mac, mac, 6);
         storage[count].timestamp = time(NULL);
+        storage[count].prio = prio;
         count++;
 
-        tc_allow_mac(mac);
+        tc_allow_mac(mac, prio+1);
     }
 }
 
@@ -60,10 +83,12 @@ void _checkTimeout()
     {
         if (storage[i].timestamp < t)
         {
-            tc_disallow_mac(storage[i].mac);
+            tc_disallow_mac(storage[i].mac, storage[i].prio+1);
+            _freePrio(storage[i].prio);
             count--;
             memcpy(storage[i].mac, storage[count].mac, 6);
             storage[i].timestamp = storage[count].timestamp;
+            i--;
         }
     }
 
@@ -80,6 +105,11 @@ void macStorage_run()
 {
     stop = 0;
 
+    for (size_t i=0; i<MAXENTRIES; i++)
+    {
+        usedPrio[i] = 0;
+    }
+
     while (!stop)
     {
         _checkTimeout();
diff --git a/tc.c b/tc.c
index deb379c..96bb12d 100644
--- a/tc.c
+++ b/tc.c
@@ -52,32 +52,32 @@ void tc_block_all()
     system(cmd);
 }
 
-void tc_allow_mac(const uint8_t mac[])
+void tc_allow_mac(const uint8_t mac[], uint8_t prio)
 {
     char cmd[2048];
     char mac32[9];
     char mac16[5];
-    snprintf(mac32, 9, "%2x%2x%2x%2x", mac[0], mac[1], mac[2], mac[3]);
-    snprintf(mac16, 5, "%2x%2x", mac[4], mac[5]);
-    snprintf(cmd, 2048, "tc filter add dev %s protocol all parent ffff: prio 99 "
+    snprintf(mac32, 9, "%02x%02x%02x%02x", mac[0], mac[1], mac[2], mac[3]);
+    snprintf(mac16, 5, "%02x%02x", mac[4], mac[5]);
+    snprintf(cmd, 2048, "tc filter add dev %s protocol all parent ffff: prio %d "
              "basic match \"u32(u32 0x%s 0x%s at -8)\" "
              "and \"u32(u16 0x%s 0x%s at -4)\" flowid :1 action pass",
-             g_interface, mac32, mac32, mac16, mac16);
+             g_interface, prio, mac32, mac32, mac16, mac16);
     log_debug("CMD: %s\n", cmd);
     system(cmd);
 }
 
-void tc_disallow_mac(const uint8_t mac[])
+void tc_disallow_mac(const uint8_t mac[], uint8_t prio)
 {
     char cmd[2048];
     char mac32[9];
     char mac16[5];
-    snprintf(mac32, 9, "%2x%2x%2x%2x", mac[0], mac[1], mac[2], mac[3]);
-    snprintf(mac16, 5, "%2x%2x", mac[4], mac[5]);
-    snprintf(cmd, 2048, "tc filter delete dev %s protocol all parent ffff: prio 99 "
+    snprintf(mac32, 9, "%02x%02x%02x%02x", mac[0], mac[1], mac[2], mac[3]);
+    snprintf(mac16, 5, "%02x%02x", mac[4], mac[5]);
+    snprintf(cmd, 2048, "tc filter delete dev %s protocol all parent ffff: prio %d "
              "basic match \"u32(u32 0x%s 0x%s at -8)\" "
              "and \"u32(u16 0x%s 0x%s at -4)\" flowid :1 action pass",
-             g_interface, mac32, mac32, mac16, mac16);
+             g_interface, prio, mac32, mac32, mac16, mac16);
     log_debug("CMD: %s\n", cmd);
     system(cmd);
 }
diff --git a/tc.h b/tc.h
index f6088be..46c1739 100644
--- a/tc.h
+++ b/tc.h
@@ -7,8 +7,8 @@ void tc_start();
 
 void tc_stop();
 
-void tc_allow_mac(const uint8_t mac[]);
+void tc_allow_mac(const uint8_t mac[], uint8_t prio);
 
-void tc_disallow_mac(const uint8_t mac[]);
+void tc_disallow_mac(const uint8_t mac[], uint8_t prio);
 
 #endif // _TC_H