Implemented mode switch PHR build and parse (ARMmbed#2665)

Author: Jarkko Paso

source/MAC/IEEE802_15_4/mac_mode_switch.c

@@ -19,30 +19,118 @@
 #include "nsconfig.h"
 #include "ns_types.h"
 #include "ns_trace.h"
+#include "common_functions.h"
 #include "MAC/IEEE802_15_4/mac_defines.h"
 #include "MAC/IEEE802_15_4/mac_mode_switch.h"
 
 #define TRACE_GROUP "mswc"
 
-#define PHR_LEN 2
-
+static uint8_t mac_calculate_mode_switch_parity(uint16_t mode_switch_phr)
+{
+    uint8_t counter = 0;
+    // Calculate number of 1-bits in (15-bit) input and return modulo-2 of the sum
+    for (int i = 0; i < 15; i++) {
+        if (mode_switch_phr & (1 << i)) {
+            counter++;
+        }
+    }
+    return counter % 2;
+}
+/*
+ * 11-bit input must be padded with 4 leading zeroes.
+ * Reverse the 15-bit result and divide with polynomial X⁴ + X + 1 -> 10011
+ * Return remainder as checksum
+ *
+ * Example:
+ * Input:                       xxxxx01000000001
+ * Padded input:                x000001000000001
+ * Reversed and padded input:   100000000100000x
+ * Calculated checksum:         0x0f (00001111)
+ *
+ * Division:
+ *
+ *             10011010101   <- Result
+ *       -----------------
+ * 10011 | 100000000100000   <- Highest bit (10000) is 1, add 1 in result
+ *         10011             <- 10011 * 1
+ *         -----
+ *          00110            <- Highest bit (00110) is 0, add 0 in result
+ *          00000            <- 10011 * 0
+ *          -----
+ *            .
+ *               .
+ *                  .
+ *                   -----
+ *                   11100
+ *                   10011
+ *                    ----
+ *                    1111  <- Remainder
+ *
+ */
+static uint8_t mac_calculate_mode_switch_checksum(uint16_t mode_switch_phr)
+{
+    // X⁴ + X + 1 -> 0b10011 -> 0x13
+    uint8_t polynomial = 0x13;
+    // Pad input with four leading zeroes
+    mode_switch_phr &= ~0x7800;
+    // Reverse input
+    uint16_t phr_reversed = 0;
+    for (int i = 0; i < 16; i++) {
+        if (mode_switch_phr & (1 << i)) {
+            phr_reversed |= 1 << (15 - i);
+        }
+    }
+    // Divide 15-bit padded and reversed input, use polynomial 10011 as the divider
+    uint8_t shift = 11;
+    uint8_t remainder = phr_reversed >> shift;
+    for (int i = 0; i < 11; i++) {
+        // Check highest bit
+        if (remainder & (1 << 4)) {
+            remainder ^= polynomial;
+        } else {
+            remainder ^= 0;
+        }
+        // Division ready, return remainder as checksum
+        if (!(--shift)) {
+            return remainder;
+        }
+        remainder <<= 1;
+        if (phr_reversed & (1 << shift)) {
+            remainder |= 1;
+        }
+    }
+    // Shouldn't go here
+    return 0;
+}
 
-int8_t ms_build_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, uint8_t *data_ptr, uint8_t phy_mode_id)
+/*
+ * Mode switch PHR format:
+ *
+ *  |     0     |   1-2  |      3-10     |  11-14 |  15  |
+ *  |Mode Switch|Reserved|New Phy Mode ID|Checksum|Parity|
+ *
+ */
+int8_t mac_build_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, uint8_t *data_ptr, uint8_t phy_mode_id)
 {
-    (void) rf_ptr, (void) phy_mode_id;
+    (void) rf_ptr;
     if (!data_ptr) {
         return -1;
     }
-
     // - Write mode switch and PHY mode id fields
+    uint16_t mode_switch_phr = 1 << SHIFT_MODE_SWITCH;
+    mode_switch_phr |= phy_mode_id << SHIFT_MODE_SWITCH_PHY_MODE;
     // - Calculate checksum
+    mode_switch_phr |= mac_calculate_mode_switch_checksum(mode_switch_phr) << SHIFT_MODE_SWITCH_CHECKSUM;
     // - Calculate parity
+    mode_switch_phr |= mac_calculate_mode_switch_parity(mode_switch_phr) << SHIFT_MODE_SWITCH_PARITY;
+
+    common_write_16_bit_inverse(mode_switch_phr, data_ptr);
 
     // - With successful return value, MAC should start CSMA-CA for a mode switch PHR
     return 0;
 }
 
-int8_t ms_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const uint8_t *data_ptr, uint16_t data_len)
+int8_t mac_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const uint8_t *data_ptr, uint16_t data_len)
 {
     (void) rf_ptr;
     if (data_len != PHR_LEN) {
@@ -51,11 +139,25 @@ int8_t ms_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const
     if (!data_ptr) {
         return -1;
     }
-    tr_info("Received mode switch PHR %s", trace_array(data_ptr, data_len));
 
+    uint16_t mode_switch_phr = common_read_16_bit_inverse(data_ptr);
+    if (!(mode_switch_phr & MASK_MODE_SWITCH)) {
+        // Mode switch not enabled
+        return -1;
+    }
     // - Validate checksum
+    if (mac_calculate_mode_switch_checksum(mode_switch_phr) != ((mode_switch_phr & MASK_MODE_SWITCH_CHECKSUM) >> SHIFT_MODE_SWITCH_CHECKSUM)) {
+        // Invalid checksum, TODO: error correction
+        return -1;
+    }
     // - Validate parity
+    if (mac_calculate_mode_switch_parity(mode_switch_phr) != (mode_switch_phr & MASK_MODE_SWITCH_PARITY) >> SHIFT_MODE_SWITCH_PARITY) {
+        // Invalid parity
+        return -1;
+    }
     // - Read PHY mode id
+    uint8_t phy_mode_id = (mode_switch_phr & MASK_MODE_SWITCH_PHY_MODE) >> SHIFT_MODE_SWITCH_PHY_MODE;
+    (void) phy_mode_id;
     // - Store current configuration
     // - Resolve new configuration
     // - Set new configuration
@@ -65,7 +167,7 @@ int8_t ms_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const
     return 0;
 }
 
-void ms_update_mode_switch_state(protocol_interface_rf_mac_setup_s *rf_ptr)
+void mac_update_mode_switch_state(protocol_interface_rf_mac_setup_s *rf_ptr)
 {
     (void) rf_ptr;
     // MAC should call this after any transmission (TX done) and reception

source/MAC/IEEE802_15_4/mac_mode_switch.h

@@ -18,14 +18,25 @@
 #ifndef MAC_MODE_SWITCH_H_
 #define MAC_MODE_SWITCH_H_
 
+#define PHR_LEN 2
+
+#define SHIFT_MODE_SWITCH_PARITY    (15)
+#define SHIFT_MODE_SWITCH_CHECKSUM  (11)
+#define SHIFT_MODE_SWITCH_PHY_MODE  (3)
+#define SHIFT_MODE_SWITCH           (0)
+#define MASK_MODE_SWITCH_PARITY     (0x8000)
+#define MASK_MODE_SWITCH_CHECKSUM   (0x7800)
+#define MASK_MODE_SWITCH_PHY_MODE   (0x07F8)
+#define MASK_MODE_SWITCH            (0x0001)
+
 /**
  * @brief Build mode switch PHR.
  * @param rf_ptr Pointer to MAC instance.
  * @param data_ptr Pointer to data buffer.
  * @param phy_mode_id Used PHY mode id.
  * @return 0 - success, -1 - failure.
  */
-int8_t ms_build_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, uint8_t *data_ptr, uint8_t phy_mode_id);
+int8_t mac_build_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, uint8_t *data_ptr, uint8_t phy_mode_id);
 
 /**
  * @brief Parse mode switch PHR.
@@ -34,6 +45,12 @@ int8_t ms_build_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, uint8
  * @param data_len Data length.
  * @return 0 - mode switch PHR found, -1 - mode switch PHR not found.
  */
-int8_t ms_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const uint8_t *data_ptr, uint16_t data_len);
+int8_t mac_parse_mode_switch_phr(protocol_interface_rf_mac_setup_s *rf_ptr, const uint8_t *data_ptr, uint16_t data_len);
+
+/**
+ * @brief Update mode switch state.
+ * @param rf_ptr Pointer to MAC instance.
+ */
+void mac_update_mode_switch_state(protocol_interface_rf_mac_setup_s *rf_ptr);
 
 #endif /* MAC_MODE_SWITCH_H_ */

source/MAC/IEEE802_15_4/mac_pd_sap.c

@@ -1043,7 +1043,7 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
             goto ERROR_HANDLER;
         }
 
-        if (!ms_parse_mode_switch_phr(rf_ptr, pd_data_ind->data_ptr, pd_data_ind->data_len)) {
+        if (!mac_parse_mode_switch_phr(rf_ptr, pd_data_ind->data_ptr, pd_data_ind->data_len)) {
             // TODO: mode switch returned 0, needs some logic to wait frame with new mode
             goto ERROR_HANDLER;
         }

test/nanostack/unittest/mac/mac_mode_switch/Makefile

@@ -0,0 +1,18 @@
+include ../../makefile_defines.txt
+
+COMPONENT_NAME = mac_mode_switch_unit
+
+#This must be changed manually
+SRC_FILES = \
+        ../../../../../source/MAC/IEEE802_15_4/mac_mode_switch.c
+
+TEST_SRC_FILES = \
+	main.cpp \
+        mac_mode_switch_test.cpp \
+        test_mac_mode_switch.c \
+        ../../stub/mbed_trace_stub.c \
+
+include ../../MakefileWorker.mk
+
+CPPUTESTFLAGS += -DFEA_TRACE_SUPPORT
+

test/nanostack/unittest/mac/mac_mode_switch/mac_mode_switch_test.cpp

@@ -0,0 +1,38 @@
+/*
+ * Copyright (c) 2021, Pelion and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "CppUTest/TestHarness.h"
+#include "test_mac_mode_switch.h"
+
+TEST_GROUP(mac_mode_switch)
+{
+    void setup() {
+    }
+
+    void teardown() {
+    }
+};
+
+TEST(mac_mode_switch, test_mac_calculate_mode_switch_checksum)
+{
+    CHECK(test_mac_calculate_mode_switch_checksum());
+}
+
+TEST(mac_mode_switch, test_mac_calculate_mode_switch_parity)
+{
+    CHECK(test_mac_calculate_mode_switch_parity());
+}
+

test/nanostack/unittest/mac/mac_mode_switch/main.cpp

@@ -0,0 +1,28 @@
+/*
+ * Copyright (c) 2021, Pelion and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "CppUTest/CommandLineTestRunner.h"
+#include "CppUTest/TestPlugin.h"
+#include "CppUTest/TestRegistry.h"
+#include "CppUTestExt/MockSupportPlugin.h"
+int main(int ac, char **av)
+{
+    return CommandLineTestRunner::RunAllTests(ac, av);
+}
+
+IMPORT_TEST_GROUP(mac_mode_switch);
+

test/nanostack/unittest/mac/mac_mode_switch/test_mac_mode_switch.c

@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2020, Pelion and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "nsconfig.h"
+#include "test_mac_mode_switch.h"
+#include "mac_defines.h"
+#include "nsdynmemLIB_stub.h"
+#include "MAC/IEEE802_15_4/mac_mode_switch.h"
+
+static protocol_interface_rf_mac_setup_s mac_setup;
+
+uint16_t common_read_16_bit_inverse(const uint8_t data_buf[__static 2])
+{
+    uint16_t temp_16;
+    temp_16 = *data_buf++;
+    temp_16 += (uint16_t)(*data_buf++) << 8;
+    return temp_16;
+}
+
+uint8_t *common_write_16_bit_inverse(uint16_t value, uint8_t ptr[__static 2])
+{
+    *ptr++ = value;
+    *ptr++ = value >> 8;
+    return ptr;
+}
+
+static uint8_t test_parse_checksum(uint16_t mode_switch_phr)
+{
+    return (mode_switch_phr & MASK_MODE_SWITCH_CHECKSUM) >> SHIFT_MODE_SWITCH_CHECKSUM;
+}
+
+static uint8_t test_parse_parity(uint16_t mode_switch_phr)
+{
+    return (mode_switch_phr & MASK_MODE_SWITCH_PARITY) >> SHIFT_MODE_SWITCH_PARITY;
+}
+
+#define TEST_SEQUENCE_LENGTH    10
+
+bool test_mac_calculate_mode_switch_checksum()
+{
+    uint8_t test_data[2];
+    uint8_t phy_modes[TEST_SEQUENCE_LENGTH] = {0x40, 0x54, 0x00, 0x01, 0xff, 0x55, 0x80, 0xa5, 0x5a, 0xda};
+    uint8_t results[TEST_SEQUENCE_LENGTH] = {0x0f, 0x0e, 0x09, 0x07, 0x0d, 0x00, 0x0a, 0x02, 0x06, 0x05};
+    for (int i = 0; i < TEST_SEQUENCE_LENGTH; i++) {
+        mac_build_mode_switch_phr(&mac_setup, &test_data, phy_modes[i]);
+        uint8_t checksum = test_parse_checksum(common_read_16_bit_inverse(test_data));
+        if (checksum != results[i]) {
+            printf("Fail: Mode switch PHR, PHY mode id %x, checksum %x, expected %x\r\n", phy_modes[i], checksum, results[i]);
+            return false;
+        }
+    }
+    return true;
+}
+
+bool test_mac_calculate_mode_switch_parity()
+{
+    uint8_t test_data[2];
+    uint8_t phy_modes[TEST_SEQUENCE_LENGTH] = {0x40, 0x54, 0x00, 0x01, 0xff, 0x55, 0x80, 0xa5, 0x5a, 0xda};
+    uint8_t results[TEST_SEQUENCE_LENGTH] = {0, 1, 1, 1, 0, 1, 0, 0, 1, 0};
+    for (int i = 0; i < TEST_SEQUENCE_LENGTH; i++) {
+        mac_build_mode_switch_phr(&mac_setup, &test_data, phy_modes[i]);
+        uint8_t parity = test_parse_parity(common_read_16_bit_inverse(test_data));
+        if (parity != results[i]) {
+            printf("Fail: Mode switch PHR, PHY mode id %x, parity %x, expected %x\r\n", phy_modes[i], parity, results[i]);
+            return false;
+        }
+    }
+    return true;
+}
+