ARMmbed
/
mbed-os
mirror of https://github.com/ARMmbed/mbed-os.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
mbed-os/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_ie_lib.c

649 lines
21 KiB
C
Raw Blame History

/*
* Copyright (c) 2018-2019, 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 "common_functions.h"
#include "mac_common_defines.h"
#include "6LoWPAN/MAC/mac_ie_lib.h"
#include "6LoWPAN/ws/ws_common_defines.h"
#include "6LoWPAN/ws/ws_ie_lib.h"
static uint8_t *ws_wh_header_base_write(uint8_t *ptr, uint16_t length, uint8_t type)
{
ptr = mac_ie_header_base_write(ptr, MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID, length + 1);
*ptr++ = type;
return ptr;
}
static uint16_t ws_channel_plan_length(uint8_t channel_plan)
{
switch (channel_plan) {
case 0:
//Regulator domain and operationg class inline
return 2;
case 1:
//CHo, Channel spasing and number of channel's inline
return 6;
default:
return 0;
}
}
static uint16_t ws_channel_function_length(uint8_t channel_function, uint16_t hop_channel_count)
{
switch (channel_function) {
case 0:
//Fixed channel inline
return 2;
case 1:
case 2:
return 0;
case 3:
//Hop count + channel hop list
return (1 + hop_channel_count);
default:
return 0;
}
}
uint16_t ws_wp_nested_hopping_schedule_length(struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule)
{
uint16_t length;
uint8_t channel_function;
if (unicast_schedule) {
length = 4;
channel_function = hopping_schedule->uc_channel_function;
} else {
length = 10;
channel_function = hopping_schedule->bc_channel_function;
}
length += ws_channel_plan_length(hopping_schedule->channel_plan);
length += ws_channel_function_length(channel_function, 1);
if (unicast_schedule && hopping_schedule->excluded_channels.excuded_channel_ctrl) {
if (hopping_schedule->excluded_channels.excuded_channel_ctrl == WS_EXC_CHAN_CTRL_RANGE) {
length += (hopping_schedule->excluded_channels.excluded_range_length * 4) + 1;
} else {
length += hopping_schedule->excluded_channels.channel_mask_bytes_inline;
}
}
return length;
}
uint8_t *ws_wh_utt_write(uint8_t *ptr, uint8_t message_type)
{
ptr = ws_wh_header_base_write(ptr, 4, WH_IE_UTT_TYPE);
*ptr++ = message_type;
memset(ptr, 0, 3);
ptr += 3;
return ptr;
}
uint8_t *ws_wh_bt_write(uint8_t *ptr)
{
ptr = ws_wh_header_base_write(ptr, 5, WH_IE_BT_TYPE);
memset(ptr, 0, 5);
ptr += 5;
return ptr;
}
uint8_t *ws_wh_fc_write(uint8_t *ptr, ws_fc_ie_t *fc_ie)
{
ptr = ws_wh_header_base_write(ptr, 2, WH_IE_FC_TYPE);
*ptr++ = fc_ie->tx_flow_ctrl;
*ptr++ = fc_ie->rx_flow_ctrl;
return ptr;
}
uint8_t *ws_wh_rsl_write(uint8_t *ptr, uint8_t rsl)
{
ptr = ws_wh_header_base_write(ptr, 1, WH_IE_RSL_TYPE);
*ptr++ = rsl;
return ptr;
}
uint8_t *ws_wh_ea_write(uint8_t *ptr, uint8_t *eui64)
{
ptr = ws_wh_header_base_write(ptr, 8, WH_IE_EA_TYPE);
memcpy(ptr, eui64, 8);
ptr += 8;
return ptr;
}
uint8_t *ws_wh_vh_write(uint8_t *ptr, uint8_t *vendor_header, uint8_t vendor_header_length)
{
ptr = ws_wh_header_base_write(ptr, vendor_header_length, WH_IE_VH_TYPE);
if (vendor_header_length) {
memcpy(ptr, vendor_header, vendor_header_length);
ptr += vendor_header_length;
}
return ptr;
}
uint8_t *ws_wp_base_write(uint8_t *ptr, uint16_t length)
{
return mac_ie_payload_base_write(ptr, WS_WP_NESTED_IE, length);
}
uint8_t *ws_wp_nested_hopping_schedule_write(uint8_t *ptr, struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule)
{
//Calculate length
uint16_t length = ws_wp_nested_hopping_schedule_length(hopping_schedule, unicast_schedule);
if (!unicast_schedule) {
ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_BS_TYPE, length);
ptr = common_write_32_bit_inverse(hopping_schedule->fhss_broadcast_interval, ptr);
ptr = common_write_16_bit_inverse(hopping_schedule->fhss_bsi, ptr);
*ptr++ = hopping_schedule->fhss_bc_dwell_interval;
} else {
ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_US_TYPE, length);
*ptr++ = hopping_schedule->fhss_uc_dwell_interval;
}
*ptr++ = hopping_schedule->clock_drift;
*ptr++ = hopping_schedule->timing_accurancy;
uint8_t channel_info_base = 0;
channel_info_base = (hopping_schedule->channel_plan);
if (unicast_schedule) {
channel_info_base |= (hopping_schedule->uc_channel_function << 3);
//Set Excluded Channel control part
channel_info_base |= (hopping_schedule->excluded_channels.excuded_channel_ctrl << 6);
} else {
channel_info_base |= (hopping_schedule->bc_channel_function << 3);
}
*ptr++ = channel_info_base;
switch (hopping_schedule->channel_plan) {
case 0:
//Regulator domain and operationg class inline
*ptr++ = hopping_schedule->regulatory_domain;
*ptr++ = hopping_schedule->operating_class;
break;
case 1:
//CHo, Channel spasing and number of channel's inline
ptr = common_write_24_bit_inverse(hopping_schedule->ch0_freq * 100, ptr);
*ptr++ = hopping_schedule->channel_spacing;
ptr = common_write_16_bit_inverse(hopping_schedule->number_of_channels, ptr);
break;
default:
break;
}
uint8_t cf = hopping_schedule->uc_channel_function;
uint16_t fixed_channel = hopping_schedule->uc_fixed_channel;
if (!unicast_schedule) {
cf = hopping_schedule->bc_channel_function;
}
switch (cf) {
case 0:
//Fixed channel inline
if (!unicast_schedule) {
fixed_channel = hopping_schedule->bc_fixed_channel;
}
ptr = common_write_16_bit_inverse(fixed_channel, ptr);
break;
case 1:
case 2:
//No Inline
break;
case 3:
//TODO do this properly
//Hop count + channel hop list
*ptr++ = 1;
*ptr++ = 0;
break;
default:
break;
}
if (unicast_schedule && hopping_schedule->excluded_channels.excuded_channel_ctrl) {
if (hopping_schedule->excluded_channels.excuded_channel_ctrl == WS_EXC_CHAN_CTRL_RANGE) {
uint8_t range_length = hopping_schedule->excluded_channels.excluded_range_length;
ws_excluded_channel_range_data_t *range_ptr = hopping_schedule->excluded_channels.exluded_range;
*ptr++ = range_length;
while (range_length) {
ptr = common_write_16_bit_inverse(range_ptr->range_start, ptr);
ptr = common_write_16_bit_inverse(range_ptr->range_end, ptr);
range_length--;
range_ptr++;
}
} else if (hopping_schedule->excluded_channels.excuded_channel_ctrl == WS_EXC_CHAN_CTRL_BITMASK) {
//Set Mask
uint16_t channel_mask_length = hopping_schedule->excluded_channels.channel_mask_bytes_inline * 8;
for (uint8_t i = 0; i < 8; i++) {
uint32_t mask_value = hopping_schedule->excluded_channels.channel_mask[i];
if (channel_mask_length >= 32) {
ptr = common_write_32_bit(mask_value, ptr);
channel_mask_length -= 32;
} else {
//Write MSB Bits from mask 24-8 top bits
uint8_t move_mask = 0;
while (channel_mask_length) {
*ptr++ = (uint8_t)(mask_value >> (24 - move_mask));
channel_mask_length -= 8;
move_mask += 8;
}
}
if (channel_mask_length == 0) {
break;
}
}
}
}
return ptr;
}
uint8_t *ws_wp_nested_vp_write(uint8_t *ptr, uint8_t *vendor_payload, uint16_t vendor_payload_length)
{
if (vendor_payload_length) {
ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_VP_TYPE, vendor_payload_length);
memcpy(ptr, vendor_payload, vendor_payload_length);
ptr += vendor_payload_length;
}
return ptr;
}
uint8_t *ws_wp_nested_pan_info_write(uint8_t *ptr, struct ws_pan_information_s *pan_congiguration)
{
if (!pan_congiguration) {
return mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_TYPE, 0);
}
ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_TYPE, 5);
ptr = common_write_16_bit_inverse(pan_congiguration->pan_size, ptr);
ptr = common_write_16_bit_inverse(pan_congiguration->routing_cost, ptr);
uint8_t temp8 = 0;
temp8 |= (pan_congiguration->use_parent_bs << 0);
temp8 |= (pan_congiguration->rpl_routing_method << 1);
temp8 |= pan_congiguration->version << 5;
*ptr++ = temp8;
return ptr;
}
uint8_t *ws_wp_nested_netname_write(uint8_t *ptr, uint8_t *network_name, uint8_t network_name_length)
{
ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_NETNAME_TYPE, network_name_length);
if (network_name_length) {
memcpy(ptr, network_name, network_name_length);
ptr += network_name_length;
}
return ptr;
}
uint8_t *ws_wp_nested_pan_ver_write(uint8_t *ptr, struct ws_pan_information_s *pan_congiguration)
{
if (!pan_congiguration) {
return ptr;
}
ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_VER_TYPE, 2);
return common_write_16_bit_inverse(pan_congiguration->pan_version, ptr);
}
uint8_t *ws_wp_nested_gtkhash_write(uint8_t *ptr, uint8_t *gtkhash, uint8_t gtkhash_length)
{
ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_GTKHASH_TYPE, gtkhash_length);
if (gtkhash_length) {
memcpy(ptr, gtkhash, 32);
ptr += 32;
}
return ptr;
}
bool ws_wh_utt_read(uint8_t *data, uint16_t length, struct ws_utt_ie *utt_ie)
{
mac_header_IE_t utt_ie_data;
utt_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
if (4 != mac_ie_header_sub_id_discover(data, length, &utt_ie_data, WH_IE_UTT_TYPE)) {
// NO UTT header
return false;
}
data = utt_ie_data.content_ptr;
utt_ie->message_type = *data++;
utt_ie->ufsi = common_read_24_bit_inverse(data);
return true;
}
bool ws_wh_bt_read(uint8_t *data, uint16_t length, struct ws_bt_ie *bt_ie)
{
mac_header_IE_t btt_ie_data;
btt_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
if (5 != mac_ie_header_sub_id_discover(data, length, &btt_ie_data, WH_IE_BT_TYPE)) {
return false;
}
data = btt_ie_data.content_ptr;
bt_ie->broadcast_slot_number = common_read_16_bit_inverse(data);
bt_ie->broadcast_interval_offset = common_read_24_bit_inverse(data + 2);
return true;
}
bool ws_wh_fc_read(uint8_t *data, uint16_t length, struct ws_fc_ie *fc_ie)
{
mac_header_IE_t fc_ie_data;
fc_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
if (2 != mac_ie_header_sub_id_discover(data, length, &fc_ie_data, WH_IE_FC_TYPE)) {
return false;
}
data = fc_ie_data.content_ptr;
fc_ie->tx_flow_ctrl = *data++;
fc_ie->rx_flow_ctrl = *data;
return true;
}
bool ws_wh_rsl_read(uint8_t *data, uint16_t length, int8_t *rsl)
{
mac_header_IE_t rsl_ie_data;
rsl_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
if (1 != mac_ie_header_sub_id_discover(data, length, &rsl_ie_data, WH_IE_RSL_TYPE)) {
return false;
}
*rsl = *rsl_ie_data.content_ptr;
return true;
}
bool ws_wh_ea_read(uint8_t *data, uint16_t length, uint8_t *eui64)
{
mac_header_IE_t rsl_ie_data;
rsl_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
if (8 != mac_ie_header_sub_id_discover(data, length, &rsl_ie_data, WH_IE_EA_TYPE)) {
return false;
}
memcpy(eui64, rsl_ie_data.content_ptr, 8);
return true;
}
static uint8_t *ws_channel_plan_zero_read(uint8_t *ptr, ws_channel_plan_zero_t *plan)
{
plan->regulator_domain = *ptr++;
plan->operation_class = *ptr++;
return ptr;
}
static uint8_t *ws_channel_plan_one_read(uint8_t *ptr, ws_channel_plan_one_t *plan)
{
plan->ch0 = common_read_24_bit_inverse(ptr);
plan->ch0 /= 100;
ptr += 3;
plan->channel_spacing = *ptr++;
plan->number_of_channel = common_read_16_bit_inverse(ptr);
ptr += 2;
return ptr;
}
static uint8_t *ws_channel_function_zero_read(uint8_t *ptr, ws_channel_function_zero_t *plan)
{
plan->fixed_channel = common_read_16_bit_inverse(ptr);
return ptr + 2;
}
static uint8_t *ws_channel_function_three_read(uint8_t *ptr, ws_channel_function_three_t *plan)
{
plan->channel_hop_count = *ptr++;
plan->channel_list = ptr++;
return ptr;
}
bool ws_wp_nested_us_read(uint8_t *data, uint16_t length, struct ws_us_ie *us_ie)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_US_TYPE;
nested_payload_ie.type_long = true;
if (mac_ie_nested_discover(data, length, &nested_payload_ie) < 4) {
return false;
}
data = nested_payload_ie.content_ptr;
us_ie->dwell_interval = *data++;
us_ie->clock_drift = *data++;
us_ie->timing_accurancy = *data++;
us_ie->channel_plan = (*data & 3);
us_ie->channel_function = (*data & 0x38) >> 3;
us_ie->excluded_channel_ctrl = (*data & 0xc0) >> 6;
data++;
uint16_t info_length = 0;
nested_payload_ie.length -= 4;
info_length = ws_channel_plan_length(us_ie->channel_plan);
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
switch (us_ie->channel_plan) {
case 0:
data = ws_channel_plan_zero_read(data, &us_ie->plan.zero);
break;
case 1:
data = ws_channel_plan_one_read(data, &us_ie->plan.one);
break;
default:
return false;
}
info_length = ws_channel_function_length(us_ie->channel_function, 0);
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
switch (us_ie->channel_function) {
case 0:
data = ws_channel_function_zero_read(data, &us_ie->function.zero);
break;
case 1:
case 2:
break;
case 3:
data = ws_channel_function_three_read(data, &us_ie->function.three);
info_length = us_ie->function.three.channel_hop_count;
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
data += info_length;
break;
default:
return false;
}
switch (us_ie->excluded_channel_ctrl) {
case WS_EXC_CHAN_CTRL_NONE:
break;
case WS_EXC_CHAN_CTRL_RANGE:
us_ie->excluded_channels.range.number_of_range = *data;
if (nested_payload_ie.length < (us_ie->excluded_channels.range.number_of_range * 4) + 1) {
return false;
}
//Set Range start after validation
us_ie->excluded_channels.range.range_start = data + 1;
break;
case WS_EXC_CHAN_CTRL_BITMASK:
if (us_ie->channel_plan == 1) {
us_ie->excluded_channels.mask.mask_len_inline = ((us_ie->plan.one.number_of_channel + 7) / 8);
if (us_ie->excluded_channels.mask.mask_len_inline != nested_payload_ie.length) {
//Channel mask length is not correct
return false;
}
} else {
us_ie->excluded_channels.mask.mask_len_inline = nested_payload_ie.length;
}
us_ie->excluded_channels.mask.channel_mask = data;
break;
default:
return false;
}
return true;
}
bool ws_wp_nested_bs_read(uint8_t *data, uint16_t length, struct ws_bs_ie *bs_ie)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_BS_TYPE;
nested_payload_ie.type_long = true;
if (mac_ie_nested_discover(data, length, &nested_payload_ie) < 10) {
return false;
}
data = nested_payload_ie.content_ptr;
bs_ie->broadcast_interval = common_read_32_bit_inverse(data);
bs_ie->broadcast_schedule_identifier = common_read_16_bit_inverse(data + 4);
data += 6;
bs_ie->dwell_interval = *data++;
bs_ie->clock_drift = *data++;
bs_ie->timing_accurancy = *data++;
bs_ie->channel_plan = (*data & 3);
bs_ie->channel_function = (*data & 0x38) >> 3;
bs_ie->excluded_channel_ctrl = (*data & 0xc0) >> 6;
data++;
nested_payload_ie.length -= 10;
uint16_t info_length = 0;
info_length = ws_channel_plan_length(bs_ie->channel_plan);
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
switch (bs_ie->channel_plan) {
case 0:
data = ws_channel_plan_zero_read(data, &bs_ie->plan.zero);
break;
case 1:
data = ws_channel_plan_one_read(data, &bs_ie->plan.one);
break;
default:
return false;
}
info_length = ws_channel_function_length(bs_ie->channel_function, 0);
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
switch (bs_ie->channel_function) {
case 0:
data = ws_channel_function_zero_read(data, &bs_ie->function.zero);
break;
case 1:
case 2:
break;
case 3:
data = ws_channel_function_three_read(data, &bs_ie->function.three);
info_length = bs_ie->function.three.channel_hop_count;
if (nested_payload_ie.length < info_length) {
return false;
}
nested_payload_ie.length -= info_length;
data += info_length;
break;
default:
return false;
}
return true;
}
bool ws_wp_nested_pan_read(uint8_t *data, uint16_t length, struct ws_pan_information_s *pan_congiguration)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_PAN_TYPE;
nested_payload_ie.type_long = false;
if (mac_ie_nested_discover(data, length, &nested_payload_ie) != 5) {
return false;
}
pan_congiguration->pan_size = common_read_16_bit_inverse(nested_payload_ie.content_ptr);
pan_congiguration->routing_cost = common_read_16_bit_inverse(nested_payload_ie.content_ptr + 2);
pan_congiguration->use_parent_bs = (nested_payload_ie.content_ptr[4] & 0x01) == 0x01;
pan_congiguration->rpl_routing_method = (nested_payload_ie.content_ptr[4] & 0x02) == 0x02;
pan_congiguration->version = (nested_payload_ie.content_ptr[4] & 0xe0) >> 5;
return true;
}
bool ws_wp_nested_pan_version_read(uint8_t *data, uint16_t length, uint16_t *pan_version)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_PAN_VER_TYPE;
nested_payload_ie.type_long = false;
if (mac_ie_nested_discover(data, length, &nested_payload_ie) != 2) {
return false;
}
*pan_version = common_read_16_bit_inverse(nested_payload_ie.content_ptr);
return true;
}
uint8_t *ws_wp_nested_gtkhash_read(uint8_t *data, uint16_t length)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_GTKHASH_TYPE;
nested_payload_ie.type_long = false;
if (mac_ie_nested_discover(data, length, &nested_payload_ie) != 32) {
return NULL;
}
return nested_payload_ie.content_ptr;
}
bool ws_wp_nested_network_name_read(uint8_t *data, uint16_t length, ws_wp_network_name_t *network_name)
{
mac_nested_payload_IE_t nested_payload_ie;
nested_payload_ie.id = WP_PAYLOAD_IE_NETNAME_TYPE;
nested_payload_ie.type_long = false;
if (0 == mac_ie_nested_discover(data, length, &nested_payload_ie)) {
return false;
} else if (nested_payload_ie.length > 32) {
//Too long name
return false;
}
network_name->network_name = nested_payload_ie.content_ptr;
network_name->network_name_length = nested_payload_ie.length;
return true;
}
Reference in New Issue View Git Blame Copy Permalink