FHSS: Implemented tr51 channel functions

Author: Jarkko Paso

source/Service_Libs/fhss/channel_functions.c

@@ -0,0 +1,79 @@
+#include "nsconfig.h"
+
+static uint32_t global_seed = 1;
+
+void tr51_seed_rand(uint32_t seed)
+{
+    if (!seed) {
+        seed = 1;
+    }
+    global_seed = seed;
+}
+
+int32_t tr51_get_rand(void)
+{
+    uint32_t random_val = ((global_seed * 1103515245) + 12345) & 0x7fffffff;
+    global_seed = random_val;
+    return random_val;
+}
+
+void tr51_calculate_channel_table(uint16_t number_of_channels, uint16_t nearest_prime, int32_t *channel_table)
+{
+    int32_t i,j,k;
+    tr51_seed_rand(1);
+    for (i=0; i < nearest_prime; i++) {
+        channel_table[i] = -1;
+    }
+    for (i=0; i < number_of_channels; i++) {
+        j = tr51_get_rand() % number_of_channels;
+        k=0;
+        while (k <= i) {
+            if (j == channel_table[k]) {
+                j = tr51_get_rand() % number_of_channels;
+                k=0;
+            } else {
+                k=k+1;
+            }
+        }
+        channel_table[i] = j;
+    }
+}
+
+void tr51_compute_cfd(uint8_t *mac, uint8_t *first_element, uint8_t *step_size, uint16_t channel_table_length)
+{
+    *first_element = (mac[5] ^ mac[6] ^ mac[7]) % channel_table_length;
+    *step_size = (mac[7] % (channel_table_length - 1)) + 1;
+}
+
+uint8_t tr51_find_excluded(int32_t channel, uint16_t *excluded_channels, uint16_t number_of_excluded_channels)
+{
+    uint8_t count = 0;
+    if (excluded_channels != NULL) {
+        for (count = 0; count < number_of_excluded_channels; count++) {
+            if (channel == excluded_channels[count]) {
+                return 1;
+            }
+        }
+    }
+    return 0;
+}
+
+uint16_t tr51_calculate_hopping_sequence(int32_t *channel_table, uint16_t channel_table_length, uint8_t first_element, uint8_t step_size, int32_t *output_table, uint16_t *excluded_channels, uint16_t number_of_excluded_channels)
+{
+    uint16_t cntr = channel_table_length;
+    uint8_t index = first_element;
+    uint8_t slot = 0;
+    while (cntr--) {
+        if (channel_table[index] != -1) {
+            if (!tr51_find_excluded(channel_table[index], excluded_channels, number_of_excluded_channels)) {
+                output_table[slot] = channel_table[index];
+                slot++;
+            }
+        }
+        index += step_size;
+        while (index >= channel_table_length) {
+            index -= channel_table_length;
+        }
+    }
+    return slot;
+}