MPX frame support

MPX heeder parser and write support. Module support also fragmentation
which could be supported in future.

Change-Id: Ic91c2d9ae85b955ebe4881b20929c0251657f319

Author: Juha Heiskanen

source/6LoWPAN/ws/ws_mpx_header.c

@@ -0,0 +1,145 @@
+/*
+ * Copyright (c) 2018, Arm Limited 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 <string.h>
+#include "ns_types.h"
+#include "ns_list.h"
+#include "ns_trace.h"
+#include "nsdynmemLIB.h"
+#include "common_functions.h"
+#include "mac_common_defines.h"
+#include "ws_mpx_header.h"
+
+bool ws_llc_mpx_header_frame_parse(uint8_t *ptr, uint16_t length, mpx_msg_t *msg)
+{
+    if (!length) {
+        return false;
+    }
+    memset(msg, 0, sizeof(mpx_msg_t));
+    bool fragmented_number_present = false;
+    bool multiplex_id_present = false;
+    bool fragment_total_size = false;
+
+    msg->transfer_type = *ptr & 7;
+    msg->transaction_id = ((*ptr++ & 0xf8) >> 3);
+    length--;
+
+
+    switch (msg->transfer_type) {
+        case MPX_FT_FULL_FRAME:
+            multiplex_id_present = true;
+            break;
+        case MPX_FT_FULL_FRAME_SMALL_MULTILEX_ID:
+            break;
+        case MPX_FT_FIRST_OR_SUB_FRAGMENT:
+        case MPX_FT_LAST_FRAGMENT:
+            fragmented_number_present = true;
+            if (length < 2) {
+                return false;
+            }
+            break;
+        case MPX_FT_ABORT:
+            if (length == 2) {
+                fragment_total_size = true;
+            } else if (length) {
+                return false;
+            }
+            break;
+        default:
+            return false;
+    }
+
+    if (fragmented_number_present) {
+
+        msg->fragment_number = *ptr++;
+        length--;
+        if (msg->fragment_number == 0) { //First fragment
+            fragment_total_size = true;
+            multiplex_id_present = true;
+        }
+    }
+
+    if (fragment_total_size) {
+        if (length < 2) {
+            return false;
+        }
+        msg->total_upper_layer_size = common_read_16_bit_inverse(ptr);
+        ptr += 2;
+        length -= 2;
+    }
+
+    if (multiplex_id_present) {
+        if (length < 3) {
+            return false;
+        }
+        msg->multiplex_id = common_read_16_bit_inverse(ptr);
+        ptr += 2;
+        length -= 2;
+    }
+
+    msg->frame_ptr = ptr;
+    msg->frame_length = length;
+    return true;
+}
+
+
+uint8_t * ws_llc_mpx_header_write(uint8_t *ptr, const mpx_msg_t *msg)
+{
+
+    bool fragmented_number_present = false;
+    bool multiplex_id_present = false;
+    bool fragment_total_size = false;
+
+    *ptr = msg->transfer_type;
+    *ptr++ |= ((msg->transaction_id << 3) & 0xf8);
+
+    switch (msg->transfer_type) {
+        case MPX_FT_FULL_FRAME:
+            multiplex_id_present = true;
+            break;
+        case MPX_FT_FULL_FRAME_SMALL_MULTILEX_ID:
+            break;
+        case MPX_FT_FIRST_OR_SUB_FRAGMENT:
+        case MPX_FT_LAST_FRAGMENT:
+            fragmented_number_present = true;
+            if (msg->fragment_number == 0) {
+                fragment_total_size = true;
+                multiplex_id_present = true;
+            }
+            break;
+        case MPX_FT_ABORT:
+            if (msg->total_upper_layer_size) {
+                fragment_total_size = true;
+            }
+            break;
+        default:
+           break;
+    }
+
+    if (fragmented_number_present) {
+        *ptr++ = msg->fragment_number;
+    }
+
+    if (fragment_total_size) {
+        ptr = common_write_16_bit_inverse(msg->total_upper_layer_size, ptr);
+    }
+
+    if (multiplex_id_present) {
+        ptr = common_write_16_bit_inverse(msg->multiplex_id,ptr);
+    }
+    return ptr;
+}

source/6LoWPAN/ws/ws_mpx_header.h

@@ -0,0 +1,41 @@
+/*
+ * Copyright (c) 2018, Arm Limited 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.
+ */
+
+#ifndef WS_MPX_HEADER_H_
+#define WS_MPX_HEADER_H_
+
+#define MPX_FT_FULL_FRAME                   0
+#define MPX_FT_FULL_FRAME_SMALL_MULTILEX_ID 1
+#define MPX_FT_FIRST_OR_SUB_FRAGMENT        2
+#define MPX_FT_LAST_FRAGMENT                4
+#define MPX_FT_ABORT                        6
+
+typedef struct {
+    unsigned    transfer_type:3;
+    unsigned    transaction_id:5;
+    uint8_t     fragment_number;
+    uint16_t    total_upper_layer_size;
+    uint16_t    multiplex_id;
+    uint8_t     *frame_ptr;
+    uint16_t    frame_length;
+} mpx_msg_t;
+
+bool ws_llc_mpx_header_frame_parse(uint8_t *ptr, uint16_t length, mpx_msg_t *msg);
+uint8_t * ws_llc_mpx_header_write(uint8_t *ptr, const mpx_msg_t *msg);
+
+
+#endif /* WS_MPX_HEADER_H_ */