ws_ie_lib.c

00001 /*
00002  * Copyright (c) 2018-2019, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 #include "nsconfig.h"
00019 #include <string.h>
00020 #include "ns_types.h"
00021 #include "ns_list.h"
00022 #include "ns_trace.h"
00023 #include "common_functions.h"
00024 #include "mac_common_defines.h"
00025 #include "6LoWPAN/MAC/mac_ie_lib.h"
00026 #include "6LoWPAN/ws/ws_common_defines.h"
00027 #include "6LoWPAN/ws/ws_ie_lib.h"
00028 
00029 static uint8_t *ws_wh_header_base_write(uint8_t *ptr, uint16_t length, uint8_t type)
00030 {
00031     ptr = mac_ie_header_base_write(ptr, MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID, length + 1);
00032     *ptr++ = type;
00033     return ptr;
00034 }
00035 
00036 static uint16_t ws_channel_plan_length(uint8_t channel_plan)
00037 {
00038     switch (channel_plan) {
00039         case 0:
00040             //Regulator domain and operationg class inline
00041             return 2;
00042         case 1:
00043             //CHo, Channel spasing and number of channel's inline
00044             return 6;
00045 
00046         default:
00047             return 0;
00048     }
00049 }
00050 
00051 static uint16_t ws_channel_function_length(uint8_t channel_function, uint16_t hop_channel_count)
00052 {
00053     switch (channel_function) {
00054         case 0:
00055             //Fixed channel inline
00056             return 2;
00057         case 1:
00058         case 2:
00059             return 0;
00060         case 3:
00061             //Hop count + channel hop list
00062             return (1 + hop_channel_count);
00063         default:
00064             return 0;
00065 
00066     }
00067 }
00068 
00069 uint16_t ws_wp_nested_hopping_schedule_length(struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule)
00070 {
00071     uint16_t length;
00072     uint8_t channel_function;
00073     if (unicast_schedule) {
00074         length = 4;
00075         channel_function = hopping_schedule->uc_channel_function;
00076     } else {
00077         length = 10;
00078         channel_function = hopping_schedule->bc_channel_function;
00079     }
00080 
00081     length += ws_channel_plan_length(hopping_schedule->channel_plan);
00082 
00083     length += ws_channel_function_length(channel_function, 1);
00084 
00085     //Todo Derive some how exluded channel control
00086     return length;
00087 }
00088 
00089 uint8_t *ws_wh_utt_write(uint8_t *ptr, uint8_t message_type)
00090 {
00091     ptr = ws_wh_header_base_write(ptr, 4, WH_IE_UTT_TYPE);
00092     *ptr++ = message_type;
00093     memset(ptr, 0, 3);
00094     ptr += 3;
00095     return ptr;
00096 }
00097 
00098 uint8_t *ws_wh_bt_write(uint8_t *ptr)
00099 {
00100     ptr = ws_wh_header_base_write(ptr, 5, WH_IE_BT_TYPE);
00101     memset(ptr, 0, 5);
00102     ptr += 5;
00103     return ptr;
00104 }
00105 
00106 
00107 uint8_t *ws_wh_fc_write(uint8_t *ptr, uint8_t flow_ctrl)
00108 {
00109     ptr = ws_wh_header_base_write(ptr, 1, WH_IE_FC_TYPE);
00110     *ptr++ = flow_ctrl;
00111     return ptr;
00112 }
00113 
00114 uint8_t *ws_wh_rsl_write(uint8_t *ptr, uint8_t rsl)
00115 {
00116     ptr = ws_wh_header_base_write(ptr, 1, WH_IE_RSL_TYPE);
00117     *ptr++ = rsl;
00118     return ptr;
00119 }
00120 
00121 uint8_t *ws_wh_ea_write(uint8_t *ptr, uint8_t *eui64)
00122 {
00123     ptr = ws_wh_header_base_write(ptr, 8, WH_IE_EA_TYPE);
00124     memcpy(ptr, eui64, 8);
00125     ptr += 8;
00126     return ptr;
00127 }
00128 
00129 uint8_t *ws_wh_vh_write(uint8_t *ptr, uint8_t *vendor_header, uint8_t vendor_header_length)
00130 {
00131     ptr = ws_wh_header_base_write(ptr, vendor_header_length, WH_IE_VH_TYPE);
00132     if (vendor_header_length) {
00133         memcpy(ptr, vendor_header, vendor_header_length);
00134         ptr += vendor_header_length;
00135     }
00136     return ptr;
00137 }
00138 
00139 uint8_t *ws_wp_base_write(uint8_t *ptr, uint16_t length)
00140 {
00141     return mac_ie_payload_base_write(ptr, WS_WP_NESTED_IE, length);
00142 }
00143 
00144 uint8_t *ws_wp_nested_hopping_schedule_write(uint8_t *ptr, struct ws_hopping_schedule_s *hopping_schedule, bool unicast_schedule)
00145 {
00146     //Calculate length
00147     uint16_t length = ws_wp_nested_hopping_schedule_length(hopping_schedule, unicast_schedule);
00148     if (!unicast_schedule) {
00149         ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_BS_TYPE, length);
00150         ptr = common_write_32_bit_inverse(hopping_schedule->fhss_broadcast_interval, ptr);
00151         ptr = common_write_16_bit_inverse(hopping_schedule->fhss_bsi, ptr);
00152         *ptr++ = hopping_schedule->fhss_bc_dwell_interval;
00153     } else {
00154         ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_US_TYPE, length);
00155         *ptr++ =  hopping_schedule->fhss_uc_dwell_interval;
00156     }
00157 
00158     *ptr++ =  hopping_schedule->clock_drift;
00159     *ptr++ =  hopping_schedule->timing_accurancy;
00160     uint8_t channel_info_base = 0;
00161     channel_info_base = (hopping_schedule->channel_plan);
00162     if (unicast_schedule) {
00163         channel_info_base |= (hopping_schedule->uc_channel_function << 3);
00164     } else {
00165         channel_info_base |= (hopping_schedule->bc_channel_function << 3);
00166     }
00167     //Todo define excluded channel ctrl
00168 
00169     *ptr++ = channel_info_base;
00170 
00171     switch (hopping_schedule->channel_plan) {
00172         case 0:
00173             //Regulator domain and operationg class inline
00174             *ptr++ = hopping_schedule->regulatory_domain;
00175             *ptr++ = hopping_schedule->operating_class;
00176             break;
00177         case 1:
00178             //CHo, Channel spasing and number of channel's inline
00179             ptr = common_write_24_bit(hopping_schedule->fhss_uc_dwell_interval, ptr);
00180             *ptr++ = ((hopping_schedule->channel_spacing << 4) & 0xf0);
00181             ptr = common_write_16_bit(hopping_schedule->number_of_channels, ptr);
00182             break;
00183         default:
00184             break;
00185     }
00186     uint8_t cf = hopping_schedule->uc_channel_function;
00187     uint16_t fixed_channel = hopping_schedule->uc_fixed_channel;
00188     if (!unicast_schedule) {
00189         cf = hopping_schedule->bc_channel_function;
00190     }
00191     switch (cf) {
00192         case 0:
00193             //Fixed channel inline
00194             if (!unicast_schedule) {
00195                 fixed_channel = hopping_schedule->bc_fixed_channel;
00196             }
00197             ptr = common_write_16_bit_inverse(fixed_channel, ptr);
00198             break;
00199         case 1:
00200         case 2:
00201             //No Inline
00202             break;
00203         case 3:
00204             //TODO do this properly
00205             //Hop count + channel hop list
00206             *ptr++ = 1;
00207             *ptr++ = 0;
00208             break;
00209         default:
00210             break;
00211 
00212     }
00213     return ptr;
00214 }
00215 
00216 uint8_t *ws_wp_nested_vp_write(uint8_t *ptr, uint8_t *vendor_payload, uint16_t vendor_payload_length)
00217 {
00218     if (vendor_payload_length) {
00219         ptr = mac_ie_nested_ie_long_base_write(ptr, WP_PAYLOAD_IE_VP_TYPE, vendor_payload_length);
00220         memcpy(ptr, vendor_payload, vendor_payload_length);
00221         ptr += vendor_payload_length;
00222     }
00223     return ptr;
00224 }
00225 
00226 uint8_t *ws_wp_nested_pan_info_write(uint8_t *ptr, struct ws_pan_information_s *pan_congiguration)
00227 {
00228     if (!pan_congiguration) {
00229         return mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_TYPE, 0);
00230     }
00231     ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_TYPE, 5);
00232     ptr = common_write_16_bit_inverse(pan_congiguration->pan_size, ptr);
00233     ptr = common_write_16_bit_inverse(pan_congiguration->routing_cost, ptr);
00234     uint8_t temp8 = 0;
00235     temp8 |= (pan_congiguration->use_parent_bs << 0);
00236     temp8 |= (pan_congiguration->rpl_routing_method << 1);
00237     temp8 |= pan_congiguration->version << 5;
00238     *ptr++ = temp8;
00239     return ptr;
00240 }
00241 
00242 
00243 uint8_t *ws_wp_nested_netname_write(uint8_t *ptr, uint8_t *network_name, uint8_t network_name_length)
00244 {
00245     ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_NETNAME_TYPE, network_name_length);
00246     if (network_name_length) {
00247         memcpy(ptr, network_name, network_name_length);
00248         ptr += network_name_length;
00249     }
00250     return ptr;
00251 }
00252 
00253 uint8_t *ws_wp_nested_pan_ver_write(uint8_t *ptr, struct ws_pan_information_s *pan_congiguration)
00254 {
00255     if (!pan_congiguration) {
00256         return ptr;
00257     }
00258     ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_PAN_VER_TYPE, 2);
00259     return common_write_16_bit_inverse(pan_congiguration->pan_version, ptr);
00260 }
00261 
00262 uint8_t *ws_wp_nested_gtkhash_write(uint8_t *ptr, uint8_t *gtkhash, uint8_t gtkhash_length)
00263 {
00264     ptr = mac_ie_nested_ie_short_base_write(ptr, WP_PAYLOAD_IE_GTKHASH_TYPE, gtkhash_length);
00265     if (gtkhash_length) {
00266         memcpy(ptr, gtkhash, 32);
00267         ptr += 32;
00268     }
00269     return ptr;
00270 }
00271 
00272 bool ws_wh_utt_read(uint8_t *data, uint16_t length, struct ws_utt_ie *utt_ie)
00273 {
00274     mac_header_IE_t utt_ie_data;
00275     utt_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
00276     if (4 != mac_ie_header_sub_id_discover(data, length, &utt_ie_data, WH_IE_UTT_TYPE)) {
00277         // NO UTT header
00278         return false;
00279     }
00280     data = utt_ie_data.content_ptr;
00281     utt_ie->message_type = *data++;
00282     utt_ie->ufsi = common_read_24_bit_inverse(data);
00283     return true;
00284 }
00285 
00286 bool ws_wh_bt_read(uint8_t *data, uint16_t length, struct ws_bt_ie *bt_ie)
00287 {
00288     mac_header_IE_t btt_ie_data;
00289     btt_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
00290     if (5 != mac_ie_header_sub_id_discover(data, length, &btt_ie_data, WH_IE_BT_TYPE)) {
00291         return false;
00292     }
00293     data = btt_ie_data.content_ptr;
00294     bt_ie->broadcast_slot_number = common_read_16_bit_inverse(data);
00295     bt_ie->broadcast_interval_offset = common_read_24_bit_inverse(data + 2);
00296     return true;
00297 }
00298 
00299 bool ws_wh_rsl_read(uint8_t *data, uint16_t length, int8_t *rsl)
00300 {
00301     mac_header_IE_t rsl_ie_data;
00302     rsl_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
00303     if (1 != mac_ie_header_sub_id_discover(data, length, &rsl_ie_data, WH_IE_RSL_TYPE)) {
00304         return false;
00305     }
00306     *rsl = *rsl_ie_data.content_ptr;
00307 
00308     return true;
00309 }
00310 
00311 bool ws_wh_ea_read(uint8_t *data, uint16_t length, uint8_t *eui64)
00312 {
00313     mac_header_IE_t rsl_ie_data;
00314     rsl_ie_data.id = MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID;
00315     if (8 != mac_ie_header_sub_id_discover(data, length, &rsl_ie_data, WH_IE_EA_TYPE)) {
00316         return false;
00317     }
00318     memcpy(eui64, rsl_ie_data.content_ptr, 8);
00319 
00320     return true;
00321 }
00322 
00323 static uint8_t *ws_channel_plan_zero_read(uint8_t *ptr, ws_channel_plan_zero_t *plan)
00324 {
00325     plan->regulator_domain = *ptr++;
00326     plan->operation_class = *ptr++;
00327     return ptr;
00328 }
00329 
00330 static uint8_t *ws_channel_plan_one_read(uint8_t *ptr, ws_channel_plan_one_t *plan)
00331 {
00332     plan->ch0 = common_read_24_bit_inverse(ptr);
00333     ptr += 3;
00334     plan->channel_spacing = (*ptr++ & 0xf0) >> 4;
00335     plan->number_of_channel = common_read_16_bit_inverse(ptr);
00336     ptr += 2;
00337     return ptr;
00338 }
00339 
00340 static uint8_t *ws_channel_function_zero_read(uint8_t *ptr, ws_channel_function_zero_t *plan)
00341 {
00342     plan->fixed_channel = common_read_16_bit_inverse(ptr);
00343     return ptr + 2;
00344 }
00345 
00346 static uint8_t *ws_channel_function_three_read(uint8_t *ptr, ws_channel_function_three_t *plan)
00347 {
00348     plan->channel_hop_count = *ptr++;
00349     plan->channel_list = ptr++;
00350     return ptr;
00351 }
00352 
00353 bool ws_wp_nested_us_read(uint8_t *data, uint16_t length, struct ws_us_ie *us_ie)
00354 {
00355     mac_nested_payload_IE_t nested_payload_ie;
00356     nested_payload_ie.id = WP_PAYLOAD_IE_US_TYPE;
00357     nested_payload_ie.type_long = true;
00358     if (mac_ie_nested_discover(data, length, &nested_payload_ie) < 4) {
00359         return false;
00360     }
00361     data = nested_payload_ie.content_ptr;
00362     us_ie->dwell_interval = *data++;
00363     us_ie->clock_drift = *data++;
00364     us_ie->timing_accurancy = *data++;
00365     us_ie->channel_plan = (*data & 3);
00366     us_ie->channel_function = (*data & 0x38) >> 3;
00367     us_ie->excluded_channel_ctrl = (*data & 0xc0) >> 6;
00368     data++;
00369     uint16_t info_length = 0;
00370     nested_payload_ie.length -= 4;
00371     info_length = ws_channel_plan_length(us_ie->channel_plan);
00372     if (nested_payload_ie.length < info_length) {
00373         return false;
00374     }
00375 
00376     nested_payload_ie.length -= info_length;
00377     switch (us_ie->channel_plan) {
00378         case 0:
00379             data = ws_channel_plan_zero_read(data, &us_ie->plan.zero);
00380             break;
00381 
00382         case 1:
00383             data = ws_channel_plan_one_read(data, &us_ie->plan.one);
00384             break;
00385         default:
00386             return false;
00387 
00388     }
00389 
00390     info_length = ws_channel_function_length(us_ie->channel_function, 0);
00391 
00392     if (nested_payload_ie.length < info_length) {
00393         return false;
00394     }
00395     nested_payload_ie.length -= info_length;
00396 
00397 
00398     switch (us_ie->channel_function) {
00399         case 0:
00400             data = ws_channel_function_zero_read(data, &us_ie->function.zero);
00401             break;
00402 
00403         case 1:
00404         case 2:
00405             break;
00406 
00407         case 3:
00408 
00409             data = ws_channel_function_three_read(data, &us_ie->function.three);
00410             info_length = us_ie->function.three.channel_hop_count;
00411             if (nested_payload_ie.length < info_length) {
00412                 return false;
00413             }
00414             nested_payload_ie.length -= info_length;
00415             data += info_length;
00416             break;
00417         default:
00418             return false;
00419 
00420     }
00421 
00422     return true;
00423 }
00424 bool ws_wp_nested_bs_read(uint8_t *data, uint16_t length, struct ws_bs_ie *bs_ie)
00425 {
00426     mac_nested_payload_IE_t nested_payload_ie;
00427     nested_payload_ie.id = WP_PAYLOAD_IE_BS_TYPE;
00428     nested_payload_ie.type_long = true;
00429     if (mac_ie_nested_discover(data, length, &nested_payload_ie) < 10) {
00430         return false;
00431     }
00432     data = nested_payload_ie.content_ptr;
00433     bs_ie->broadcast_interval = common_read_32_bit_inverse(data);
00434     bs_ie->broadcast_schedule_identifier = common_read_16_bit_inverse(data + 4);
00435     data += 6;
00436     bs_ie->dwell_interval = *data++;
00437     bs_ie->clock_drift = *data++;
00438     bs_ie->timing_accurancy = *data++;
00439 
00440     bs_ie->channel_plan = (*data & 3);
00441     bs_ie->channel_function = (*data & 0x38) >> 3;
00442     bs_ie->excluded_channel_ctrl = (*data & 0xc0) >> 6;
00443     data++;
00444     nested_payload_ie.length -= 10;
00445     uint16_t info_length = 0;
00446 
00447     info_length = ws_channel_plan_length(bs_ie->channel_plan);
00448     if (nested_payload_ie.length < info_length) {
00449         return false;
00450     }
00451     nested_payload_ie.length -= info_length;
00452     switch (bs_ie->channel_plan) {
00453         case 0:
00454             data = ws_channel_plan_zero_read(data, &bs_ie->plan.zero);
00455             break;
00456 
00457         case 1:
00458             data = ws_channel_plan_one_read(data, &bs_ie->plan.one);
00459             break;
00460         default:
00461             return false;
00462 
00463     }
00464 
00465     info_length = ws_channel_function_length(bs_ie->channel_function, 0);
00466     if (nested_payload_ie.length < info_length) {
00467         return false;
00468     }
00469     nested_payload_ie.length -= info_length;
00470 
00471     switch (bs_ie->channel_function) {
00472         case 0:
00473             data = ws_channel_function_zero_read(data, &bs_ie->function.zero);
00474             break;
00475 
00476         case 1:
00477         case 2:
00478             break;
00479 
00480         case 3:
00481             data = ws_channel_function_three_read(data, &bs_ie->function.three);
00482             info_length = bs_ie->function.three.channel_hop_count;
00483             if (nested_payload_ie.length < info_length) {
00484                 return false;
00485             }
00486             nested_payload_ie.length -= info_length;
00487             data += info_length;
00488             break;
00489         default:
00490             return false;
00491 
00492     }
00493 
00494     return true;
00495 }
00496 
00497 bool ws_wp_nested_pan_read(uint8_t *data, uint16_t length, struct ws_pan_information_s *pan_congiguration)
00498 {
00499     mac_nested_payload_IE_t nested_payload_ie;
00500     nested_payload_ie.id = WP_PAYLOAD_IE_PAN_TYPE;
00501     nested_payload_ie.type_long = false;
00502     if (mac_ie_nested_discover(data, length, &nested_payload_ie) !=  5) {
00503         return false;
00504     }
00505 
00506     pan_congiguration->pan_size = common_read_16_bit_inverse(nested_payload_ie.content_ptr);
00507     pan_congiguration->routing_cost = common_read_16_bit_inverse(nested_payload_ie.content_ptr + 2);
00508     pan_congiguration->use_parent_bs = (nested_payload_ie.content_ptr[4] & 0x01) == 0x01;
00509     pan_congiguration->rpl_routing_method = (nested_payload_ie.content_ptr[4] & 0x02) == 0x02;
00510     pan_congiguration->version = (nested_payload_ie.content_ptr[4] & 0xe0) >> 5;
00511 
00512     return true;
00513 }
00514 
00515 bool ws_wp_nested_pan_version_read(uint8_t *data, uint16_t length, uint16_t *pan_version)
00516 {
00517     mac_nested_payload_IE_t nested_payload_ie;
00518     nested_payload_ie.id = WP_PAYLOAD_IE_PAN_VER_TYPE;
00519     nested_payload_ie.type_long = false;
00520     if (mac_ie_nested_discover(data, length, &nested_payload_ie) !=  2) {
00521         return false;
00522     }
00523     *pan_version = common_read_16_bit_inverse(nested_payload_ie.content_ptr);
00524 
00525     return true;
00526 }
00527 
00528 uint8_t *ws_wp_nested_gtkhash_read(uint8_t *data, uint16_t length)
00529 {
00530     mac_nested_payload_IE_t nested_payload_ie;
00531     nested_payload_ie.id = WP_PAYLOAD_IE_GTKHASH_TYPE;
00532     nested_payload_ie.type_long = false;
00533     if (mac_ie_nested_discover(data, length, &nested_payload_ie) !=  32) {
00534         return NULL;
00535     }
00536 
00537     return nested_payload_ie.content_ptr;
00538 }
00539 
00540 
00541 bool ws_wp_nested_network_name_read(uint8_t *data, uint16_t length, ws_wp_network_name_t *network_name)
00542 {
00543     mac_nested_payload_IE_t nested_payload_ie;
00544     nested_payload_ie.id = WP_PAYLOAD_IE_NETNAME_TYPE;
00545     nested_payload_ie.type_long = false;
00546 
00547     if (0 == mac_ie_nested_discover(data, length, &nested_payload_ie)) {
00548         return false;
00549     } else if (nested_payload_ie.length > 32) {
00550         //Too long name
00551         return false;
00552     }
00553     network_name->network_name = nested_payload_ie.content_ptr;
00554     network_name->network_name_length = nested_payload_ie.length;
00555     return true;
00556 }
00557