diff --git a/source/Service_Libs/fhss/channel_functions.c b/source/Service_Libs/fhss/channel_functions.c
index 91cc632b3a5..0d16c0e91a6 100644
--- a/source/Service_Libs/fhss/channel_functions.c
+++ b/source/Service_Libs/fhss/channel_functions.c
@@ -41,6 +41,25 @@
 
 static uint32_t global_seed = 1;
 
+
+uint16_t tr51_calc_nearest_prime_number(uint16_t start_value)
+{
+    if (start_value < 2) {
+        return 0;
+    }
+    uint16_t divider = start_value - 1;
+    while (start_value) {
+        if (start_value % divider--) {
+            if (divider == 1) {
+                return start_value;
+            }
+        } else {
+            divider = ++start_value - 1;
+        }
+    }
+    return 0;
+}
+
 void tr51_seed_rand(uint32_t seed)
 {
     if (!seed) {
diff --git a/test/nanostack/unittest/service_libs/channel_functions/channelfunctest.cpp b/test/nanostack/unittest/service_libs/channel_functions/channelfunctest.cpp
index 1843c1e0bf3..20cd2c80168 100644
--- a/test/nanostack/unittest/service_libs/channel_functions/channelfunctest.cpp
+++ b/test/nanostack/unittest/service_libs/channel_functions/channelfunctest.cpp
@@ -58,3 +58,8 @@ TEST(channel_functions, test_dh1cf_get_bc_channel_index)
 {
     CHECK(test_dh1cf_get_bc_channel_index());
 }
+
+TEST(channel_functions, test_tr51_calc_nearest_prime_number)
+{
+    CHECK(test_tr51_calc_nearest_prime_number());
+}
diff --git a/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.c b/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.c
index 84418f98fb8..b873f21f946 100644
--- a/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.c
+++ b/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.c
@@ -36,7 +36,7 @@ const int32_t test_channel_table[131] = {   57,78,25,22,97,90,86,102,
                                             56,71,4,29,52,82,120,113,
                                             8,-1,-1 };
 
-const int32_t test_HopSequenceTable [129] = {   4,93,31,84,55,3,116,123,
+const int32_t test_HopSequenceTable[129] =  {   4,93,31,84,55,3,116,123,
                                                 41,68,105,28,25,71,33,66,
                                                 122,20,112,125,118,11,69,89,
                                                 128,78,56,42,124,30,64,114,
@@ -54,6 +54,23 @@ const int32_t test_HopSequenceTable [129] = {   4,93,31,84,55,3,116,123,
                                                 77,48,100,51,14,21,94,23,
                                                 22 };
 
+const uint16_t prime_number_table[] = { 2, 3, 5, 7, 11, 13, 17, 19, 23, 29,
+                                        31, 37, 41, 43, 47, 53, 59, 61, 67, 71,
+                                        73, 79, 83, 89, 97, 101, 103, 107, 109, 113,
+                                        127, 131, 137, 139, 149, 151, 157, 163, 167, 173,
+                                        179, 181, 191, 193, 197, 199, 211, 223, 227, 229,
+                                        233, 239, 241, 251, 257, 263, 269, 271, 277, 281,
+                                        283, 293, 307, 311, 313, 317, 331, 337, 347, 349,
+                                        353, 359, 367, 373, 379, 383, 389, 397, 401, 409,
+                                        419, 421, 431, 433, 439, 443, 449, 457, 461, 463,
+                                        467, 479, 487, 491, 499, 503, 509, 521, 523, 541,
+                                        547, 557, 563, 569, 571, 577, 587, 593, 599, 601,
+                                        607, 613, 617, 619, 631, 641, 643, 647, 653, 659,
+                                        661, 673, 677, 683, 691, 701, 709, 719, 727, 733,
+                                        739, 743, 751, 757, 761, 769, 773, 787, 797, 809,
+                                        811, 821, 823, 827, 829, 839, 853, 857, 859, 863,
+                                        877, 881, 883, 887, 907, 911, 919, 929, 937, 941,
+                                        947, 953, 967, 971, 977, 983, 991, 997, 1009 };
 
 bool test_tr51_get_rand()
 {
@@ -194,3 +211,21 @@ bool test_dh1cf_get_bc_channel_index()
     }
     return true;
 }
+
+bool test_tr51_calc_nearest_prime_number()
+{
+    uint16_t prime_number;
+    int j;
+    for (int i=2; i<1000; i++) {
+        prime_number = tr51_calc_nearest_prime_number(i);
+        for (j=0; j<sizeof(prime_number_table); j++) {
+            if (prime_number == prime_number_table[j]) {
+                break;
+            }
+        }
+        if (j >= sizeof(prime_number_table)) {
+            return false;
+        }
+    }
+    return true;
+}
diff --git a/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.h b/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.h
index d8132c55975..30f574a8c11 100644
--- a/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.h
+++ b/test/nanostack/unittest/service_libs/channel_functions/test_channel_functions.h
@@ -29,6 +29,7 @@ bool test_tr51_compute_cfd();
 bool test_tr51_calculate_hopping_sequence();
 bool test_dh1cf_get_uc_channel_index();
 bool test_dh1cf_get_bc_channel_index();
+bool test_tr51_calc_nearest_prime_number();
 
 #ifdef __cplusplus
 }