FHSS: DH1CF channel functions implemented

Author: Jarkko Paso

source/Service_Libs/fhss/channel_functions.c

@@ -77,3 +77,71 @@ uint16_t tr51_calculate_hopping_sequence(int32_t *channel_table, uint16_t channe
     }
     return slot;
 }
+
+#define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k))))
+
+#define final(a,b,c) \
+{ \
+    c ^= b; c -= rot(b, 14); \
+    a ^= c; a -= rot(c, 11); \
+    b ^= a; b -= rot(a, 25); \
+    c ^= b; c -= rot(b, 16); \
+    a ^= c; a -= rot(c, 4); \
+    b ^= a; b -= rot(a, 14); \
+    c ^= b; c -= rot(b, 24); \
+}
+
+#define mix(a,b,c) \
+{ \
+    a -= c; a ^= rot(c, 4); c += b; \
+    b -= a; b ^= rot(a, 6); a += c; \
+    c -= b; c ^= rot(b, 8); b += a; \
+    a -= c; a ^= rot(c, 16); c += b; \
+    b -= a; b ^= rot(a, 19); a += c; \
+    c -= b; c ^= rot(b, 4); b += a; \
+}
+
+static uint32_t dh1cf_hashword(const uint32_t *key, size_t key_length, uint32_t init_value)
+{
+    uint32_t a,b,c;
+    a = b = c = 0xdeadbeef + (((uint32_t)key_length) << 2) + init_value;
+    while (key_length > 3) {
+        a += key[0];
+        b += key[1];
+        c += key[2];
+        mix(a, b, c);
+        key_length -= 3;
+        key += 3;
+    }
+    switch(key_length) {
+        case 3 : c += key[2];
+        case 2 : b += key[1];
+        case 1 : a += key[0];
+        final(a, b, c);
+        case 0:
+        break;
+    }
+    return c;
+}
+
+int32_t dh1cf_get_uc_channel_index(uint16_t slot_number, uint8_t *mac, int16_t number_of_channels)
+{
+    int32_t channel_number;
+    uint32_t key[3];
+    key[0] = (uint32_t)slot_number;
+    key[1] = mac[4] << 24 | mac[5] << 16 | mac[6] << 8 | mac[7];
+    key[2] = mac[0] << 24 | mac[1] << 16 | mac[2] << 8 | mac[3];
+    channel_number = dh1cf_hashword(key, 3, 0) % number_of_channels;
+    return channel_number;
+}
+
+int32_t dh1cf_get_bc_channel_index(uint16_t slot_number, uint16_t bsi, int16_t number_of_channels)
+{
+    int32_t channel_number;
+    uint32_t key[3];
+    key[0] = (uint32_t)slot_number;
+    key[1] = bsi << 16;
+    key[2] = 0;
+    channel_number = dh1cf_hashword(key, 3, 0) % number_of_channels;
+    return channel_number;
+}