From c553e8c0fb33348c0258de000d330612d763ef21 Mon Sep 17 00:00:00 2001 From: Juha Heiskanen Date: Tue, 6 Mar 2018 17:12:52 +0200 Subject: [PATCH] WS ie library read update WS Header IE read API: ws_wh_utt_read() & ws_wh_bt_read() WS Nested Payload read API: * ws_wp_nested_us_read() * ws_wp_nested_bs_read() * ws_wp_nested_pan_read() * ws_wp_nested_pan_version_read() * ws_wp_nested_gtkhash_read() Change-Id: Id9c5d9922cc6e7e6b2322eb413491d8e0c20857e --- source/6LoWPAN/ws/ws_common_defines.h | 91 ++++++++ source/6LoWPAN/ws/ws_ie_lib.c | 307 +++++++++++++++++++++++--- source/6LoWPAN/ws/ws_ie_lib.h | 12 + 3 files changed, 383 insertions(+), 27 deletions(-) diff --git a/source/6LoWPAN/ws/ws_common_defines.h b/source/6LoWPAN/ws/ws_common_defines.h index cbdd186d4ff..720ff8d31cf 100644 --- a/source/6LoWPAN/ws/ws_common_defines.h +++ b/source/6LoWPAN/ws/ws_common_defines.h @@ -85,6 +85,97 @@ typedef struct ws_hopping_schedule_s { uint_fast24_t ch0_freq; // Default should be derived from regulatory domain } ws_hopping_schedule_t; +/** + * @brief ws_utt_ie_t WS UTT-IE + */ +typedef struct ws_utt_ie { + uint8_t message_type; + uint_fast24_t ut; +} ws_utt_ie_t; + +/** + * @brief ws_bt_ie_t WS BT-IE read + */ +typedef struct ws_bt_ie { + uint16_t broadcast_slot_number; + uint_fast24_t bsi; +} ws_bt_ie_t; + + +/** + * @brief ws_channel_plan_zero_t WS channel plan 0 define domain and class + */ +typedef struct ws_channel_plan_zero { + uint8_t regulator_domain; + uint8_t operation_class; +} ws_channel_plan_zero_t; + +/** + * @brief ws_channel_plan_one_t WS channel plan 1 define ch0, spasing and channel count + */ +typedef struct ws_channel_plan_one { + uint_fast24_t ch0; + unsigned channel_spacing:4; + uint16_t number_of_channel; +} ws_channel_plan_one_t; + +/** + * @brief ws_channel_function_zero_t WS function 0 fixed channel + */ +typedef struct ws_channel_function_zero { + uint16_t fixed_channel; +} ws_channel_function_zero_t; + +/** + * @brief ws_channel_function_three_t WS function 3 vendor spesific channel hop + */ +typedef struct ws_channel_function_three { + uint8_t channel_hop_count; + uint8_t *channel_list; +} ws_channel_function_three_t; + +/** + * @brief ws_us_ie_t WS US-IE read + */ +typedef struct ws_us_ie { + uint8_t dwell_interval; + uint8_t clock_drift; + uint8_t timing_accurancy; + unsigned channle_plan:3; + unsigned channle_function:3; + unsigned excluded_channel_ctrl:2; + union { + ws_channel_plan_zero_t zero; + ws_channel_plan_one_t one; + } plan; + union { + ws_channel_function_zero_t zero; + ws_channel_function_three_t three; + } function; +} ws_us_ie_t; + +/** + * @brief ws_bs_ie_t WS BS-IE read + */ +typedef struct ws_bs_ie { + uint32_t broadcast_interval; + uint16_t bsi; + uint8_t dwell_interval; + uint8_t clock_drift; + uint8_t timing_accurancy; + unsigned channle_plan:3; + unsigned channle_function:3; + unsigned excluded_channel_ctrl:2; + union { + ws_channel_plan_zero_t zero; + ws_channel_plan_one_t one; + } plan; + union { + ws_channel_function_zero_t zero; + ws_channel_function_three_t three; + } function; +} ws_bs_ie_t; + #define MPX_KEY_MANAGEMENT_ENC_USER_ID 0x0001 /**< MPX Key management user ID */ #define MPX_LOWPAN_ENC_USER_ID 0xA0ED /**< MPX Lowpan User Id */ diff --git a/source/6LoWPAN/ws/ws_ie_lib.c b/source/6LoWPAN/ws/ws_ie_lib.c index 879f2cc37f2..cf5ac076af9 100644 --- a/source/6LoWPAN/ws/ws_ie_lib.c +++ b/source/6LoWPAN/ws/ws_ie_lib.c @@ -32,44 +32,50 @@ static uint8_t *ws_wh_header_base_write(uint8_t *ptr, uint16_t length, uint8_t t return ptr; } -uint16_t ws_wp_nested_hopping_schedule_length(struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule) +static uint16_t ws_channel_plan_length(uint8_t channel_plan) { - uint16_t length; - if (unicast_schedule) { - length = 4; - } else { - length = 10; - } - switch (hopping_schedule->channel_plan) { + switch (channel_plan) { case 0: //Regulator domain and operationg class inline - length += 2; - break; + return 2; case 1: //CHo, Channel spasing and number of channel's inline - length += 6; - break; + return 6; + default: - break; + return 0; } +} - switch (hopping_schedule->channel_function) { +static uint16_t ws_channel_function_length(uint8_t channel_function, uint16_t hop_channel_count) +{ + switch (channel_function) { case 0: //Fixed channel inline - length += 2; - break; + return 2; case 1: case 2: - //No Inline - break; - /*case 3: + return 0; + case 3: //Hop count + channel hop list - length += 1 + hopping_schedule->number_of_channels; - break;*/ + return (1 + hop_channel_count); default: - break; + return 0; } +} + +uint16_t ws_wp_nested_hopping_schedule_length(struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule) +{ + uint16_t length; + if (unicast_schedule) { + length = 4; + } else { + length = 10; + } + + length += ws_channel_plan_length(hopping_schedule->channel_plan); + length += ws_channel_function_length(hopping_schedule->channel_function, 1); //Todo Derive some how exluded channel control return length; @@ -170,10 +176,12 @@ uint8_t *ws_wp_nested_hopping_schedule_write(uint8_t *ptr,struct ws_hopping_sche case 2: //No Inline break; - /*case 3: - //Hop count + channel hop list - length += 1 + hopping_schedule->number_of_channels; - break;*/ + case 3: + //TODO do this properly + //Hop count + channel hop list + *ptr++ = 1; + *ptr++ = 0; + break; default: break; @@ -224,7 +232,7 @@ uint8_t *ws_wp_nested_pan_ver_write(uint8_t *ptr, struct ws_pan_information_s *p return ptr; } ptr = mac_ie_nested_ie_long_base_write(ptr,WP_PAYLOAD_IE_PAN_VER_TYPE, 2); - return common_write_16_bit(pan_congiguration->pan_version, ptr); + 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) @@ -236,3 +244,248 @@ uint8_t *ws_wp_nested_gtkhash_write(uint8_t *ptr, uint8_t *gtkhash, uint8_t gtkh } 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->ut = common_read_24_bit_inverse(data + 1); + 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->bsi = common_read_24_bit_inverse(data + 2); + 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); + ptr += 3; + plan->channel_spacing = (*ptr++ & 0xf0) >> 4; + 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->channle_plan = (*data & 3); + us_ie->channle_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->channle_plan); + if (nested_payload_ie.length < info_length) { + return false; + } + + nested_payload_ie.length -= info_length; + switch ( us_ie->channle_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->channle_function, 0); + + if (nested_payload_ie.length < info_length) { + return false; + } + nested_payload_ie.length -= info_length; + + + switch (us_ie->channle_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; + + } + + 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->bsi = 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->channle_plan = (*data & 3); + bs_ie->channle_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->channle_plan); + if (nested_payload_ie.length < info_length) { + return false; + } + nested_payload_ie.length -= info_length; + switch ( bs_ie->channle_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->channle_function,0); + if (nested_payload_ie.length < info_length) { + return false; + } + nested_payload_ie.length -= info_length; + + switch (bs_ie->channle_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 = true; + 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] & 0x10) == 0x10; + pan_congiguration->rpl_routing_method = (nested_payload_ie.content_ptr[4] & 0x20) == 0x20; + 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 = true; + 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 = true; + if (mac_ie_nested_discover(data , length, &nested_payload_ie) != 32) { + return NULL; + } + + return nested_payload_ie.content_ptr; +} + diff --git a/source/6LoWPAN/ws/ws_ie_lib.h b/source/6LoWPAN/ws/ws_ie_lib.h index 465494e12e8..f85d4d36242 100644 --- a/source/6LoWPAN/ws/ws_ie_lib.h +++ b/source/6LoWPAN/ws/ws_ie_lib.h @@ -19,6 +19,9 @@ #define WS_IE_LIB_H_ struct ws_pan_information_s; +struct ws_utt_ie; +struct ws_bt_ie; +struct ws_us_ie; /* WS_WH HEADER IE */ uint8_t *ws_wh_utt_write(uint8_t *ptr, uint8_t message_type); @@ -27,6 +30,9 @@ uint8_t *ws_wh_fc_write(uint8_t *ptr, uint8_t flow_ctrl); uint8_t *ws_wh_rsl_write(uint8_t *ptr, int8_t rssi); uint8_t *ws_wh_vh_write(uint8_t *ptr, uint8_t *vendor_header, uint8_t vendor_header_length); +bool ws_wh_utt_read(uint8_t *data, uint16_t length, struct ws_utt_ie *utt_ie); +bool ws_wh_bt_read(uint8_t *data, uint16_t length, struct ws_bt_ie * bt_ie); + /* WS_WP_NESTED PAYLOD IE */ uint8_t *ws_wp_base_write(uint8_t *ptr, uint16_t length); uint8_t *ws_wp_nested_hopping_schedule_write(uint8_t *ptr,struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule); @@ -37,4 +43,10 @@ uint8_t *ws_wp_nested_pan_ver_write(uint8_t *ptr, struct ws_pan_information_s *p uint8_t *ws_wp_nested_gtkhash_write(uint8_t *ptr, uint8_t *gtkhash, uint8_t gtkhash_length); uint16_t ws_wp_nested_hopping_schedule_length(struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule); +bool ws_wp_nested_us_read(uint8_t *data, uint16_t length, struct ws_us_ie *us_ie); +bool ws_wp_nested_bs_read(uint8_t *data, uint16_t length, struct ws_bs_ie *bs_ie); +bool ws_wp_nested_pan_read(uint8_t *data, uint16_t length, struct ws_pan_information_s *pan_congiguration); +bool ws_wp_nested_pan_version_read(uint8_t *data, uint16_t length, uint16_t *pan_version); +uint8_t * ws_wp_nested_gtkhash_read(uint8_t *data, uint16_t length); + #endif /* WS_IE_LIB_H_ */