channel_functions.c

00001 /*
00002  * Copyright (c) 2018-2019, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 #include "nsconfig.h"
00018 #include "common_functions.h"
00019 
00020 #define rot(x,k) (((x)<<(k)) | ((x)>>(32-(k))))
00021 
00022 #define final(a,b,c) \
00023 { \
00024     c ^= b; c -= rot(b, 14); \
00025     a ^= c; a -= rot(c, 11); \
00026     b ^= a; b -= rot(a, 25); \
00027     c ^= b; c -= rot(b, 16); \
00028     a ^= c; a -= rot(c, 4); \
00029     b ^= a; b -= rot(a, 14); \
00030     c ^= b; c -= rot(b, 24); \
00031 }
00032 
00033 #define mix(a,b,c) \
00034 { \
00035     a -= c; a ^= rot(c, 4); c += b; \
00036     b -= a; b ^= rot(a, 6); a += c; \
00037     c -= b; c ^= rot(b, 8); b += a; \
00038     a -= c; a ^= rot(c, 16); c += b; \
00039     b -= a; b ^= rot(a, 19); a += c; \
00040     c -= b; c ^= rot(b, 4); b += a; \
00041 }
00042 
00043 static uint32_t global_seed = 1;
00044 
00045 
00046 uint16_t tr51_calc_nearest_prime_number(uint16_t start_value)
00047 {
00048     if (start_value < 2) {
00049         return 0;
00050     }
00051     uint16_t divider = start_value - 1;
00052     while (start_value) {
00053         if (start_value % divider--) {
00054             if (divider == 1) {
00055                 break;
00056             }
00057         } else {
00058             divider = ++start_value - 1;
00059         }
00060     }
00061     return start_value;
00062 }
00063 
00064 static void tr51_seed_rand(uint32_t seed)
00065 {
00066     if (!seed) {
00067         seed = 1;
00068     }
00069     global_seed = seed;
00070 }
00071 
00072 static int32_t tr51_get_rand(void)
00073 {
00074     uint32_t random_val = ((global_seed * 1103515245) + 12345) & 0x7fffffff;
00075     global_seed = random_val;
00076     return random_val;
00077 }
00078 
00079 /**
00080  * @brief Calculate channel table based on TR51 channel function.
00081  * @param number_of_channels Number of channels in table.
00082  * @param nearest_prime Nearest prime number. Must be equal to or larger than number_of_channels.
00083  * @param channel_table Output channel table. Has to be at least nearest_prime in length.
00084  */
00085 static void tr51_calculate_channel_table(uint16_t number_of_channels, uint16_t nearest_prime, int16_t *channel_table)
00086 {
00087     int32_t i, j, k;
00088     tr51_seed_rand(1);
00089     for (i = 0; i < nearest_prime; i++) {
00090         channel_table[i] = -1;
00091     }
00092     for (i = 0; i < number_of_channels; i++) {
00093         j = tr51_get_rand() % number_of_channels;
00094         k = 0;
00095         while (k <= i) {
00096             if (j == channel_table[k]) {
00097                 j = tr51_get_rand() % number_of_channels;
00098                 k = 0;
00099             } else {
00100                 k = k + 1;
00101             }
00102         }
00103         channel_table[i] = j;
00104     }
00105 }
00106 
00107 static void tr51_compute_cfd(uint8_t *mac, uint8_t *first_element, uint8_t *step_size, uint16_t channel_table_length)
00108 {
00109     *first_element = (mac[5] ^ mac[6] ^ mac[7]) % channel_table_length;
00110     *step_size = (mac[7] % (channel_table_length - 1)) + 1;
00111 }
00112 
00113 static uint8_t tr51_find_excluded(int32_t channel, uint32_t *excluded_channels)
00114 {
00115     if (excluded_channels != NULL) {
00116         uint8_t index = channel / 32;
00117         channel %= 32;
00118         if (excluded_channels[index] & ((uint32_t)1 << channel)) {
00119             return true;
00120         }
00121     }
00122     return false;
00123 }
00124 
00125 /**
00126  * @brief Calculate hopping sequence for a specific peer using tr51 channel function.
00127  * @param channel_table Used channel table.
00128  * @param channel_table_length Length of the used channel table.
00129  * @param first_element Start generated by CFD function.
00130  * @param step_size Step size generated by CFD function.
00131  * @param output_table Output hopping sequence table.
00132  * @param excluded_channels Bit mask where excluded channels are set to 1.
00133  * @return Number of channels in sequence.
00134  */
00135 static uint16_t tr51_calculate_hopping_sequence(int16_t *channel_table, uint16_t channel_table_length, uint8_t first_element, uint8_t step_size, uint8_t *output_table, uint32_t *excluded_channels)
00136 {
00137     uint16_t cntr = channel_table_length;
00138     uint8_t index = first_element;
00139     uint8_t slot = 0;
00140     while (cntr--) {
00141         if (channel_table[index] != -1) {
00142             if (tr51_find_excluded(channel_table[index], excluded_channels) == false) {
00143                 output_table[slot] = channel_table[index];
00144                 slot++;
00145             }
00146         }
00147         index += step_size;
00148         index %= channel_table_length;
00149     }
00150     return slot;
00151 }
00152 
00153 static uint32_t dh1cf_hashword(const uint32_t *key, size_t key_length, uint32_t init_value)
00154 {
00155     uint32_t a, b, c;
00156     a = b = c = 0xdeadbeef + (((uint32_t)key_length) << 2) + init_value;
00157     while (key_length > 3) {
00158         a += key[0];
00159         b += key[1];
00160         c += key[2];
00161         mix(a, b, c);
00162         key_length -= 3;
00163         key += 3;
00164     }
00165     switch (key_length) {
00166         case 3:
00167             c += key[2];
00168         /* fall through */
00169         case 2:
00170             b += key[1];
00171         /* fall through */
00172         case 1:
00173             a += key[0];
00174             final(a, b, c);
00175         /* fall through */
00176         case 0:
00177             break;
00178     }
00179     return c;
00180 }
00181 
00182 int32_t dh1cf_get_uc_channel_index(uint16_t slot_number, uint8_t *mac, int16_t number_of_channels)
00183 {
00184     int32_t channel_number;
00185     uint32_t key[3];
00186     key[0] = (uint32_t) slot_number;
00187     key[1] = common_read_32_bit(&mac[4]);
00188     key[2] = common_read_32_bit(&mac[0]);
00189     channel_number = dh1cf_hashword(key, 3, 0) % number_of_channels;
00190     return channel_number;
00191 }
00192 
00193 int32_t dh1cf_get_bc_channel_index(uint16_t slot_number, uint16_t bsi, int16_t number_of_channels)
00194 {
00195     int32_t channel_number;
00196     uint32_t key[3];
00197     key[0] = (uint32_t) slot_number;
00198     key[1] = (uint32_t) bsi << 16;
00199     key[2] = 0;
00200     channel_number = dh1cf_hashword(key, 3, 0) % number_of_channels;
00201     return channel_number;
00202 }
00203 
00204 int tr51_init_channel_table(int16_t *channel_table, int16_t number_of_channels)
00205 {
00206     uint16_t nearest_prime = tr51_calc_nearest_prime_number(number_of_channels);
00207     tr51_calculate_channel_table(number_of_channels, nearest_prime, channel_table);
00208     return 0;
00209 }
00210 
00211 int32_t tr51_get_uc_channel_index(int16_t *channel_table, uint8_t *output_table, uint16_t slot_number, uint8_t *mac, int16_t number_of_channels, uint32_t *excluded_channels)
00212 {
00213     uint16_t nearest_prime = tr51_calc_nearest_prime_number(number_of_channels);
00214     uint8_t first_element;
00215     uint8_t step_size;
00216     tr51_compute_cfd(mac, &first_element, &step_size, nearest_prime);
00217     tr51_calculate_hopping_sequence(channel_table, nearest_prime, first_element, step_size, output_table, excluded_channels);
00218     return output_table[slot_number];
00219 }
00220 
00221 int32_t tr51_get_bc_channel_index(int16_t *channel_table, uint8_t *output_table, uint16_t slot_number, uint16_t bsi, int16_t number_of_channels, uint32_t *excluded_channels)
00222 {
00223     uint16_t nearest_prime = tr51_calc_nearest_prime_number(number_of_channels);
00224     uint8_t mac[8] = {0, 0, 0, 0, 0, 0, (uint8_t)(bsi >> 8), (uint8_t)bsi};
00225     uint8_t first_element;
00226     uint8_t step_size;
00227     tr51_compute_cfd(mac, &first_element, &step_size, nearest_prime);
00228     tr51_calculate_hopping_sequence(channel_table, nearest_prime, first_element, step_size, output_table, excluded_channels);
00229     return output_table[slot_number];
00230 }