Embed:
(wiki syntax)
Show/hide line numbers
iphc_decompress.c
00001 /* 00002 * Copyright (c) 2014-2017, 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 #include "nsconfig.h" 00018 #include "ns_trace.h" 00019 #include "common_functions.h" 00020 #include "nsdynmemLIB.h" 00021 #include <string.h> 00022 #include "NWK_INTERFACE/Include/protocol.h" 00023 #include "6LoWPAN/IPHC_Decode/cipv6.h" 00024 #include "Common_Protocols/ipv6_constants.h" 00025 #include "6LoWPAN/IPHC_Decode/iphc_decompress.h" 00026 00027 #define TRACE_GROUP "iphc" 00028 00029 /* Analyse a 6LoWPAN datagram for fragmentation, checking header length */ 00030 /* Critical fact is that fragments are described with their size+offsets in */ 00031 /* terms of the UNCOMPRESSED IP datagram, so when presented with a 6LoWPAN datagram, */ 00032 /* we need to work backwards. Another constraint is that compressed headers */ 00033 /* must all lie within the first fragment - doesn't concern us here. */ 00034 /* Input: a potentially oversized 6LoWPAN-format datagram without fragmentation */ 00035 /* Return: size of compressed IPHC headers */ 00036 /* Output: uncompressed_size = uncompressed size of IPHC headers */ 00037 uint16_t iphc_header_scan(buffer_t *buf, uint16_t *uncompressed_size) 00038 { 00039 const uint8_t *ptr = buffer_data_pointer(buf); 00040 const uint8_t *end = buffer_data_end(buf); 00041 uint16_t uncomp_len = 0; 00042 const uint8_t *ip_hc; 00043 00044 *uncompressed_size = 0; 00045 00046 /* Fragmentation needs us to handle uncompressed case */ 00047 if (ptr < end && ptr[0] == LOWPAN_DISPATCH_IPV6) { 00048 return 1; 00049 } 00050 00051 /* Handle compressed IP header (LOWPAN_IPHC) - LOWPAN_HC1 etc not handled */ 00052 IPv6START: 00053 ip_hc = ptr; 00054 ptr += 2; 00055 uncomp_len += 40; 00056 00057 if (ptr > end) { 00058 goto truncated; 00059 } 00060 00061 if ((ip_hc[0] & LOWPAN_DISPATCH_IPHC_MASK) != LOWPAN_DISPATCH_IPHC) { 00062 tr_warn("Unexpected 6LoWPAN ID"); 00063 return 0; 00064 } 00065 00066 if (ip_hc[1] & HC_CIDE_COMP) { 00067 ptr++; 00068 } 00069 00070 switch (ip_hc[0] & HC_TF_MASK) { 00071 case HC_TF_ECN_DSCP_FLOW_LABEL: 00072 ptr += 4; 00073 break; 00074 case HC_TF_ECN_FLOW_LABEL: 00075 ptr += 3; 00076 break; 00077 case HC_TF_ECN_DSCP: 00078 ptr += 1; 00079 break; 00080 default: 00081 break; 00082 } 00083 00084 if (!(ip_hc[0] & HC_NEXT_HEADER_MASK)) { 00085 ptr++; 00086 } 00087 00088 if ((ip_hc[0] & HC_HOP_LIMIT_MASK) == HC_HOP_LIMIT_CARRIED_IN_LINE) { 00089 ptr++; 00090 } 00091 00092 switch (ip_hc[1] & HC_SRC_ADR_MODE_MASK) { 00093 case HC_SRC_ADR_128_BIT: 00094 if (!(ip_hc[1] & HC_SRCADR_COMP)) { 00095 ptr += 16; 00096 } 00097 break; 00098 case HC_SRC_ADR_64_BIT: 00099 ptr += 8; 00100 break; 00101 case HC_SRC_ADR_16_BIT: 00102 ptr += 2; 00103 break; 00104 case HC_SRC_ADR_FROM_MAC: 00105 break; 00106 } 00107 00108 if (ip_hc[1] & HC_MULTICAST_COMP) { 00109 switch (ip_hc[1] & (HC_DSTADR_COMP | HC_DST_ADR_MODE_MASK)) { 00110 case HC_128BIT_MULTICAST: 00111 ptr += 16; 00112 break; 00113 case HC_48BIT_MULTICAST: 00114 case HC_48BIT_CONTEXT_MULTICAST: 00115 ptr += 6; 00116 break; 00117 case HC_32BIT_MULTICAST: 00118 ptr += 4; 00119 break; 00120 case HC_8BIT_MULTICAST: 00121 ptr += 1; 00122 break; 00123 default: 00124 tr_warn("Unknown multicast compression"); 00125 return 0; 00126 } 00127 } else { 00128 switch (ip_hc[1] & HC_DST_ADR_MODE_MASK) { 00129 case HC_DST_ADR_128_BIT: 00130 ptr += 16; 00131 break; 00132 case HC_DST_ADR_64_BIT: 00133 ptr += 8; 00134 break; 00135 case HC_DST_ADR_16_BIT: 00136 ptr += 2; 00137 break; 00138 case HC_DST_ADR_FROM_MAC: 00139 break; 00140 } 00141 } 00142 00143 /* Handle following headers (LOWPAN_NHC) */ 00144 bool nhc_next_header = ip_hc[0] & HC_NEXT_HEADER_MASK; 00145 while (nhc_next_header) { 00146 uint8_t nhc_id; 00147 if (ptr >= end) { 00148 goto truncated; 00149 } 00150 nhc_id = *ptr++; 00151 //tr_debug("IPNH: %02x", next_header); 00152 if ((nhc_id & NHC_EXT_HEADER_MASK) == NHC_EXT_HEADER) { 00153 uint8_t len = 0; 00154 switch (nhc_id & NHC_EXT_ID_MASK) { 00155 case NHC_EXT_IPV6: 00156 goto IPv6START; 00157 case NHC_EXT_HOP_BY_HOP: 00158 case NHC_EXT_ROUTING: 00159 case NHC_EXT_FRAG: 00160 case NHC_EXT_DEST_OPT: 00161 case NHC_EXT_MOBILITY: 00162 nhc_next_header = nhc_id & NHC_EXT_NH; 00163 if (!nhc_next_header) { 00164 ptr++; 00165 } 00166 if (ptr >= end) { 00167 goto truncated; 00168 } 00169 if ((nhc_id & NHC_EXT_ID_MASK) == NHC_EXT_FRAG) { 00170 /* See notes in main decompress routine */ 00171 ptr += 7; 00172 uncomp_len += 8; 00173 } else { 00174 len = *ptr++; 00175 /* len is length of following data in bytes */ 00176 ptr += len; 00177 /* Uncompressed headers must be multiple of 8 octets, so 00178 * uncompressed length is 2+len ("NH" + "len" + data), 00179 * rounded up to a multiple of 8. 00180 */ 00181 uncomp_len += ((2 + len) + 7) & ~ 7; 00182 } 00183 break; 00184 default: 00185 goto bad_nhc_id; 00186 } 00187 } else if ((nhc_id & NHC_UDP_MASK) == NHC_UDP) { 00188 uncomp_len += 8; 00189 nhc_next_header = false; 00190 if (!(nhc_id & NHC_UDP_CKSUM_COMPRESS)) { 00191 ptr += 2; 00192 } 00193 switch (nhc_id & NHC_UDP_PORT_COMPRESS_MASK) { 00194 case NHC_UDP_PORT_COMPRESS_DST: 00195 case NHC_UDP_PORT_COMPRESS_SRC: 00196 ptr += 3; 00197 break; 00198 case NHC_UDP_PORT_COMPRESS_NONE: 00199 ptr += 4; 00200 break; 00201 case NHC_UDP_PORT_COMPRESS_BOTH: 00202 default: 00203 ptr += 1; 00204 break; 00205 } 00206 } else { 00207 bad_nhc_id: 00208 tr_warn("Unknown NHC ID: %02x", nhc_id); 00209 nhc_next_header = false; 00210 } 00211 } 00212 00213 if (ptr > end) { 00214 truncated: 00215 tr_warn("Truncated packet"); 00216 return 0; 00217 } 00218 00219 *uncompressed_size = uncomp_len; 00220 return ptr - buffer_data_pointer(buf); 00221 } 00222 00223 static bool decompress_mc_addr(const lowpan_context_list_t *context_list, uint8_t *addr, const uint8_t **in_ptr, const uint8_t *outer_iid, uint8_t context, uint8_t mode) 00224 { 00225 const uint8_t *in = *in_ptr; 00226 (void) outer_iid; 00227 switch (mode) { 00228 case HC_128BIT_MULTICAST: 00229 memcpy(addr, in, 16); 00230 *in_ptr = in + 16; 00231 return true; 00232 case HC_48BIT_MULTICAST: 00233 addr[0] = 0xff; 00234 addr[1] = *in++; 00235 memset(&addr[2], 0, 9); 00236 memcpy(&addr[11], in, 5); 00237 *in_ptr = in + 5; 00238 return true; 00239 case HC_32BIT_MULTICAST: 00240 addr[0] = 0xff; 00241 addr[1] = *in++; 00242 memset(&addr[2], 0, 11); 00243 memcpy(&addr[13], in, 3); 00244 *in_ptr = in + 3; 00245 return true; 00246 case HC_8BIT_MULTICAST: 00247 memcpy(addr, ADDR_LINK_LOCAL_ALL_NODES, 15); 00248 addr[15] = *in++; 00249 *in_ptr = in; 00250 return true; 00251 case HC_48BIT_CONTEXT_MULTICAST: { 00252 lowpan_context_t *ctx = lowpan_contex_get_by_id(context_list, context); 00253 if (!ctx) { 00254 return false; 00255 } 00256 addr[0] = 0xff; 00257 addr[1] = *in++; 00258 addr[2] = *in++; 00259 addr[3] = ctx->length; 00260 memcpy(&addr[4], ctx->prefix, 8); 00261 memcpy(&addr[12], in, 4); 00262 *in_ptr = in + 4; 00263 return true; 00264 } 00265 default: 00266 return false; 00267 } 00268 } 00269 00270 static bool decompress_addr(const lowpan_context_list_t *context_list, uint8_t *addr, const uint8_t **in_ptr, bool is_dst, const uint8_t *outer_iid, uint8_t context, uint8_t mode) 00271 { 00272 if (!is_dst) { 00273 /* Get SRC bits and move into DST position, without multicast bit */ 00274 mode = (mode >> 4) & (HC_DSTADR_COMP | HC_DST_ADR_MODE_MASK); 00275 context >>= 4; 00276 } else { 00277 mode &= (HC_MULTICAST_COMP | HC_DSTADR_COMP | HC_DST_ADR_MODE_MASK); 00278 context &= 0xf; 00279 } 00280 00281 if (mode & HC_MULTICAST_COMP) { 00282 return decompress_mc_addr(context_list, addr, in_ptr, outer_iid, context, mode & ~ HC_MULTICAST_COMP); 00283 } 00284 00285 switch (mode & HC_DST_ADR_MODE_MASK) { 00286 case HC_DST_ADR_128_BIT: 00287 if (mode & HC_DSTADR_COMP) { 00288 /* Special case - different for src and dst */ 00289 if (is_dst) { 00290 return false; 00291 } else { 00292 memset(addr, 0, 16); // unspecified (::) 00293 return true; 00294 } 00295 } 00296 memcpy(addr, *in_ptr, 16); 00297 *in_ptr += 16; 00298 return true; 00299 case HC_DST_ADR_64_BIT: 00300 memcpy(addr + 8, *in_ptr, 8); 00301 *in_ptr += 8; 00302 break; 00303 case HC_DST_ADR_16_BIT: 00304 memcpy(addr + 8, ADDR_SHORT_ADR_SUFFIC, 6); 00305 addr[14] = (*in_ptr)[0]; 00306 addr[15] = (*in_ptr)[1]; 00307 *in_ptr += 2; 00308 break; 00309 case HC_DST_ADR_FROM_MAC: 00310 memcpy(addr + 8, outer_iid, 8); 00311 break; 00312 } 00313 00314 if (mode & HC_DSTADR_COMP) { 00315 lowpan_context_t *ctx = lowpan_contex_get_by_id(context_list, context); 00316 if (!ctx) { 00317 return false; 00318 } 00319 /* Copy a minimum of 64 bits to get required zero fill up to IID - 00320 * we rely on the context storage core having zero-padding in 00321 * the prefix field for short contexts. 00322 */ 00323 bitcopy(addr, ctx->prefix, ctx->length < 64 ? 64 : ctx->length); 00324 return true; 00325 } else { 00326 memcpy(addr, ADDR_LINK_LOCAL_PREFIX, 8); 00327 return true; 00328 } 00329 } 00330 00331 typedef struct iphc_decompress_state { 00332 const lowpan_context_list_t * const context_list; 00333 const uint8_t *in; 00334 const uint8_t *const end; 00335 uint8_t *out; 00336 uint8_t *nh_ptr; 00337 const uint8_t *outer_src_iid; 00338 const uint8_t *outer_dst_iid; 00339 } iphc_decompress_state_t; 00340 00341 static bool decompress_ipv6(iphc_decompress_state_t *restrict ds) 00342 { 00343 const uint8_t *iphc = ds->in; 00344 ds->in += 2; 00345 00346 uint8_t cid; 00347 00348 if (iphc[1] & HC_CIDE_COMP) { 00349 cid = *ds->in++; 00350 } else { 00351 cid = 0; 00352 } 00353 00354 /* First, Traffic Class and Flow Label */ 00355 uint8_t tc = 0; 00356 uint8_t tf = iphc[0] & HC_TF_MASK; 00357 00358 /* Extract ECN */ 00359 if (tf != HC_TF_ELIDED) { 00360 tc = *ds->in >> 6; 00361 } 00362 00363 /* Extract DSCP */ 00364 if (tf == HC_TF_ECN_DSCP || tf == HC_TF_ECN_DSCP_FLOW_LABEL) { 00365 tc |= *ds->in++ << 2; 00366 } 00367 00368 *ds->out++ = 0x60 | (tc >> 4); 00369 00370 if (tf == HC_TF_ECN_FLOW_LABEL || tf == HC_TF_ECN_DSCP_FLOW_LABEL) { 00371 *ds->out++ = (tc << 4) | (*ds->in++ & 0x0f); 00372 *ds->out++ = *ds->in++; 00373 *ds->out++ = *ds->in++; 00374 } else { 00375 *ds->out++ = tc << 4; 00376 *ds->out++ = 0; 00377 *ds->out++ = 0; 00378 } 00379 00380 /* Compute payload length */ 00381 ds->out = common_write_16_bit(ds->end - (ds->out + 36), ds->out); 00382 00383 /* Next Header */ 00384 if (iphc[0] & HC_NEXT_HEADER_MASK) { 00385 /* Reserve space for Next Header - will be filled later */ 00386 ds->nh_ptr = ds->out; 00387 *ds->out++ = IPV6_NH_NONE; 00388 } else { 00389 ds->nh_ptr = NULL; 00390 *ds->out++ = *ds->in++; 00391 } 00392 00393 /* Hop Limit */ 00394 switch (iphc[0] & HC_HOP_LIMIT_MASK) { 00395 case HC_HOP_LIMIT_1: 00396 *ds->out++ = 1; 00397 break; 00398 case HC_HOP_LIMIT_64: 00399 *ds->out++ = 64; 00400 break; 00401 case HC_HOP_LIMIT_255: 00402 *ds->out++ = 255; 00403 break; 00404 case HC_HOP_LIMIT_CARRIED_IN_LINE: 00405 default: 00406 *ds->out++ = *ds->in++; 00407 break; 00408 } 00409 00410 if (!decompress_addr(ds->context_list, ds->out, &ds->in, false, ds->outer_src_iid, cid, iphc[1])) { 00411 tr_warn("SRC Address decompress fail"); 00412 return false; 00413 } 00414 ds->outer_src_iid = ds->out + 8; 00415 ds->out += 16; 00416 if (!decompress_addr(ds->context_list, ds->out, &ds->in, true, ds->outer_dst_iid, cid, iphc[1])) { 00417 tr_warn("DST Address decompress fail"); 00418 return false; 00419 } 00420 ds->outer_dst_iid = ds->out + 8; 00421 ds->out += 16; 00422 00423 return true; 00424 } 00425 00426 static bool decompress_exthdr(iphc_decompress_state_t *ds) 00427 { 00428 uint8_t nh; 00429 00430 switch (*ds->in & NHC_EXT_ID_MASK) { 00431 case NHC_EXT_HOP_BY_HOP: 00432 nh = IPV6_NH_HOP_BY_HOP; 00433 break; 00434 case NHC_EXT_ROUTING: 00435 nh = IPV6_NH_ROUTING; 00436 break; 00437 case NHC_EXT_FRAG: 00438 nh = IPV6_NH_FRAGMENT; 00439 break; 00440 case NHC_EXT_DEST_OPT: 00441 nh = IPV6_NH_DEST_OPT; 00442 break; 00443 case NHC_EXT_MOBILITY: 00444 nh = IPV6_NH_MOBILITY; 00445 break; 00446 case NHC_EXT_IPV6: 00447 *ds->nh_ptr = IPV6_NH_IPV6; 00448 ds->in++; 00449 return decompress_ipv6(ds); 00450 default: 00451 return false; 00452 } 00453 00454 *ds->nh_ptr = nh; 00455 00456 if (*ds->in++ & NHC_EXT_NH) { 00457 /* Reserve space for Next Header - will be filled later */ 00458 ds->nh_ptr = ds->out; 00459 *ds->out++ = IPV6_NH_NONE; 00460 } else { 00461 ds->nh_ptr = NULL; 00462 *ds->out++ = *ds->in++; 00463 } 00464 00465 uint8_t clen; 00466 if (nh == IPV6_NH_FRAGMENT) { 00467 /* Fragmentation header is awkward, and RFC 6282 isn't terribly clear */ 00468 /* Second byte is reserved. It isn't a length field. */ 00469 *ds->out++ = *ds->in++; 00470 clen = 6; 00471 } else { 00472 clen = *ds->in++; /* Compressed data len */ 00473 *ds->out++ = (clen + 2 - 1) >> 3; /* Uncompressed header length byte (8-octet units, excluding first) */ 00474 } 00475 00476 /* Copy main option data */ 00477 memcpy(ds->out, ds->in, clen); 00478 ds->out += clen; 00479 ds->in += clen; 00480 00481 /* If not aligned, add a PAD1 or PADN */ 00482 if ((clen + 2) & 7) { 00483 uint8_t pad = 8 - ((clen + 2) & 7); 00484 if (pad == 1) { 00485 *ds->out++ = IPV6_OPTION_PAD1; 00486 } else { 00487 *ds->out++ = IPV6_OPTION_PADN; 00488 *ds->out++ = (pad -= 2); 00489 while (pad) { 00490 *ds->out++ = 0, pad--; 00491 } 00492 } 00493 } 00494 00495 return true; 00496 } 00497 00498 static bool decompress_udp(iphc_decompress_state_t *ds) 00499 { 00500 uint8_t nhc = *ds->in++; 00501 00502 /* Ports */ 00503 if ((nhc & NHC_UDP_PORT_COMPRESS_MASK) == NHC_UDP_PORT_COMPRESS_BOTH) { 00504 *ds->out++ = 0xf0; 00505 *ds->out++ = 0xb0 | (*ds->in >> 4); 00506 *ds->out++ = 0xf0; 00507 *ds->out++ = 0xb0 | (*ds->in++ & 0x0f); 00508 } else { 00509 *ds->out++ = (nhc & NHC_UDP_PORT_COMPRESS_MASK) == NHC_UDP_PORT_COMPRESS_SRC ? 0xf0 : *ds->in++; 00510 *ds->out++ = *ds->in++; 00511 *ds->out++ = (nhc & NHC_UDP_PORT_COMPRESS_MASK) == NHC_UDP_PORT_COMPRESS_DST ? 0xf0 : *ds->in++; 00512 *ds->out++ = *ds->in++; 00513 } 00514 00515 /* Length */ 00516 ds->out = common_write_16_bit(ds->end - (ds->out - 4), ds->out); 00517 00518 /* Don't currently allow checksum compression */ 00519 if (nhc & NHC_UDP_CKSUM_COMPRESS) { 00520 return false; 00521 } 00522 *ds->out++ = *ds->in++; 00523 *ds->out++ = *ds->in++; 00524 00525 *ds->nh_ptr = IPV6_NH_UDP; 00526 ds->nh_ptr = NULL; 00527 00528 return true; 00529 } 00530 00531 /* Input: A 6LoWPAN frame, starting with an IPHC header, with outer layer 802.15.4 MAC addresses in src+dst */ 00532 /* Output: An IPv6 frame */ 00533 buffer_t *iphc_decompress(const lowpan_context_list_t *context_list, buffer_t *buf) 00534 { 00535 uint8_t src_iid[8], dst_iid[8]; 00536 uint8_t *iphc = NULL; 00537 00538 /* Pre-scan to get compressed and uncompressed header size */ 00539 uint16_t ip_size; 00540 uint16_t hc_size = iphc_header_scan(buf, &ip_size); 00541 if (hc_size == 0) { 00542 tr_warn("IPHC size 0"); 00543 goto decomp_error; 00544 } 00545 00546 /* Copy compressed header into temporary buffer */ 00547 iphc = ns_dyn_mem_temporary_alloc(hc_size); 00548 if (!iphc) { 00549 tr_warn("IPHC header alloc fail %d",hc_size); 00550 goto decomp_error; 00551 } 00552 memcpy(iphc, buffer_data_pointer(buf), hc_size); 00553 00554 /* Reserve buffer room for the uncompressed header */ 00555 buffer_data_strip_header(buf, hc_size); 00556 buf = buffer_headroom(buf, ip_size); 00557 if (!buf) { 00558 tr_warn("IPHC headroom get fail %d",ip_size); 00559 goto decomp_error; 00560 } 00561 buffer_data_reserve_header(buf, ip_size); 00562 00563 if (!addr_iid_from_outer(src_iid, &buf->src_sa ) || !addr_iid_from_outer(dst_iid, &buf->dst_sa )) { 00564 tr_warn("Bad outer addr"); 00565 goto decomp_error; 00566 } 00567 00568 { 00569 iphc_decompress_state_t ds = { 00570 .context_list = context_list, 00571 .in = iphc, 00572 .nh_ptr = NULL, 00573 .out = buffer_data_pointer(buf), 00574 .end = buffer_data_end(buf), 00575 .outer_src_iid = src_iid, 00576 .outer_dst_iid = dst_iid, 00577 }; 00578 00579 /* Always start with the IP header */ 00580 if (!decompress_ipv6(&ds)) { 00581 tr_warn("IPV6 decompres fail"); 00582 goto decomp_error; 00583 } 00584 00585 /* After the first IP header, we switch on the NHC byte */ 00586 /* Know we're finished when there's no NH byte waiting to be filled */ 00587 while (ds.nh_ptr) { 00588 bool ok = false; 00589 if ((ds.in[0] & NHC_UDP_MASK) == NHC_UDP) { 00590 ok = decompress_udp(&ds); 00591 } else if ((ds.in[0] & NHC_EXT_HEADER_MASK) == NHC_EXT_HEADER) { 00592 ok = decompress_exthdr(&ds); 00593 } 00594 if (!ok) { 00595 tr_warn("Unknow NH"); 00596 goto decomp_error; 00597 } 00598 } 00599 00600 if (ds.out != buffer_data_pointer(buf) + ip_size) { 00601 tr_err("IPHC decompression bug"); 00602 goto decomp_error; 00603 } 00604 } 00605 00606 ns_dyn_mem_free(iphc); 00607 return buf; 00608 00609 decomp_error: 00610 tr_warn("IPHC decompression error"); 00611 ns_dyn_mem_free(iphc); 00612 return buffer_free(buf); 00613 }
Generated on Tue Jul 12 2022 12:21:57 by
1.7.2