static int lowpan_get_frag_info(struct sk_buff *skb, const u8 frag_type, struct lowpan_frag_info *frag_info) { bool fail; u8 pattern = 0, low = 0; __be16 d_tag = 0; fail = lowpan_fetch_skb(skb, &pattern, 1); fail |= lowpan_fetch_skb(skb, &low, 1); frag_info->d_size = (pattern & 7) << 8 | low; fail |= lowpan_fetch_skb(skb, &d_tag, 2); frag_info->d_tag = ntohs(d_tag); if (frag_type == LOWPAN_DISPATCH_FRAGN) { fail |= lowpan_fetch_skb(skb, &frag_info->d_offset, 1); } else { skb_reset_network_header(skb); frag_info->d_offset = 0; } if (unlikely(fail)) return -EIO; return 0; }
static int lowpan_get_cb(struct sk_buff *skb, u8 frag_type, struct lowpan_802154_cb *cb) { bool fail; u8 high = 0, low = 0; __be16 d_tag = 0; fail = lowpan_fetch_skb(skb, &high, 1); fail |= lowpan_fetch_skb(skb, &low, 1); /* remove the dispatch value and use first three bits as high value * for the datagram size */ cb->d_size = (high & LOWPAN_FRAG_DGRAM_SIZE_HIGH_MASK) << LOWPAN_FRAG_DGRAM_SIZE_HIGH_SHIFT | low; fail |= lowpan_fetch_skb(skb, &d_tag, 2); cb->d_tag = ntohs(d_tag); if (frag_type == LOWPAN_DISPATCH_FRAGN) { fail |= lowpan_fetch_skb(skb, &cb->d_offset, 1); } else { skb_reset_network_header(skb); cb->d_offset = 0; /* check if datagram_size has ipv6hdr on FRAG1 */ fail |= cb->d_size < sizeof(struct ipv6hdr); /* check if we can dereference the dispatch value */ fail |= !skb->len; } if (unlikely(fail)) return -EIO; return 0; }
static int uncompress_udp_header(struct sk_buff *skb, struct udphdr *uh) { bool fail; u8 tmp = 0, val = 0; fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); if ((tmp & LOWPAN_NHC_UDP_MASK) == LOWPAN_NHC_UDP_ID) { pr_debug("UDP header uncompression\n"); switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { case LOWPAN_NHC_UDP_CS_P_00: fail |= lowpan_fetch_skb(skb, &uh->source, sizeof(uh->source)); fail |= lowpan_fetch_skb(skb, &uh->dest, sizeof(uh->dest)); break; case LOWPAN_NHC_UDP_CS_P_01: fail |= lowpan_fetch_skb(skb, &uh->source, sizeof(uh->source)); fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh->dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); break; case LOWPAN_NHC_UDP_CS_P_10: fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh->source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); fail |= lowpan_fetch_skb(skb, &uh->dest, sizeof(uh->dest)); break; case LOWPAN_NHC_UDP_CS_P_11: fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh->source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); uh->dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); break; default: pr_debug("ERROR: unknown UDP format\n"); goto err; } pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", ntohs(uh->source), ntohs(uh->dest)); /* checksum */ if (tmp & LOWPAN_NHC_UDP_CS_C) { pr_debug_ratelimited("checksum elided currently not supported\n"); goto err; } else { fail |= lowpan_fetch_skb(skb, &uh->check, sizeof(uh->check)); } /* UDP length needs to be infered from the lower layers * here, we obtain the hint from the remaining size of the * frame */ uh->len = htons(skb->len + sizeof(struct udphdr)); pr_debug("uncompressed UDP length: src = %d", ntohs(uh->len)); } else {
/* Uncompress function for multicast destination address, * when M bit is set. */ static int lowpan_uncompress_multicast_daddr(struct sk_buff *skb, struct in6_addr *ipaddr, const u8 dam) { bool fail; switch (dam) { case LOWPAN_IPHC_DAM_00: /* 00: 128 bits. The full address * is carried in-line. */ fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); break; case LOWPAN_IPHC_DAM_01: /* 01: 48 bits. The address takes * the form ffXX::00XX:XXXX:XXXX. */ ipaddr->s6_addr[0] = 0xFF; fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[11], 5); break; case LOWPAN_IPHC_DAM_10: /* 10: 32 bits. The address takes * the form ffXX::00XX:XXXX. */ ipaddr->s6_addr[0] = 0xFF; fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[1], 1); fail |= lowpan_fetch_skb(skb, &ipaddr->s6_addr[13], 3); break; case LOWPAN_IPHC_DAM_11: /* 11: 8 bits. The address takes * the form ff02::00XX. */ ipaddr->s6_addr[0] = 0xFF; ipaddr->s6_addr[1] = 0x02; fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[15], 1); break; default: pr_debug("DAM value has a wrong value: 0x%x\n", dam); return -EINVAL; } if (fail) { pr_debug("Failed to fetch skb data\n"); return -EIO; } raw_dump_inline(NULL, "Reconstructed ipv6 multicast addr is", ipaddr->s6_addr, 16); return 0; }
static int udp_uncompress(struct sk_buff *skb, size_t needed) { u8 tmp = 0, val = 0; struct udphdr uh; bool fail; int err; fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); pr_debug("UDP header uncompression\n"); switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { case LOWPAN_NHC_UDP_CS_P_00: fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); break; case LOWPAN_NHC_UDP_CS_P_01: fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); break; case LOWPAN_NHC_UDP_CS_P_10: fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); break; case LOWPAN_NHC_UDP_CS_P_11: fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); break; default: BUG(); } pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", ntohs(uh.source), ntohs(uh.dest)); /* checksum */ if (tmp & LOWPAN_NHC_UDP_CS_C) { pr_debug_ratelimited("checksum elided currently not supported\n"); fail = true; } else { fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check)); } if (fail) return -EINVAL; /* UDP length needs to be infered from the lower layers * here, we obtain the hint from the remaining size of the * frame */ switch (lowpan_priv(skb->dev)->lltype) { case LOWPAN_LLTYPE_IEEE802154: if (lowpan_802154_cb(skb)->d_size) uh.len = htons(lowpan_802154_cb(skb)->d_size - sizeof(struct ipv6hdr)); else uh.len = htons(skb->len + sizeof(struct udphdr)); break; default: uh.len = htons(skb->len + sizeof(struct udphdr)); break; } pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len)); /* replace the compressed UDP head by the uncompressed UDP * header */ err = skb_cow(skb, needed); if (unlikely(err)) return err; skb_push(skb, sizeof(struct udphdr)); skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); return 0; }
/* Uncompress address function for source and * destination address(non-multicast). * * address_mode is sam value or dam value. */ static int uncompress_addr(struct sk_buff *skb, struct in6_addr *ipaddr, const u8 address_mode, const u8 *lladdr, const u8 addr_type, const u8 addr_len) { bool fail; switch (address_mode) { case LOWPAN_IPHC_ADDR_00: /* for global link addresses */ fail = lowpan_fetch_skb(skb, ipaddr->s6_addr, 16); break; case LOWPAN_IPHC_ADDR_01: /* fe:80::XXXX:XXXX:XXXX:XXXX */ ipaddr->s6_addr[0] = 0xFE; ipaddr->s6_addr[1] = 0x80; fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[8], 8); break; case LOWPAN_IPHC_ADDR_02: /* fe:80::ff:fe00:XXXX */ ipaddr->s6_addr[0] = 0xFE; ipaddr->s6_addr[1] = 0x80; ipaddr->s6_addr[11] = 0xFF; ipaddr->s6_addr[12] = 0xFE; fail = lowpan_fetch_skb(skb, &ipaddr->s6_addr[14], 2); break; case LOWPAN_IPHC_ADDR_03: fail = false; switch (addr_type) { case IEEE802154_ADDR_LONG: /* fe:80::XXXX:XXXX:XXXX:XXXX * \_________________/ * hwaddr */ ipaddr->s6_addr[0] = 0xFE; ipaddr->s6_addr[1] = 0x80; memcpy(&ipaddr->s6_addr[8], lladdr, addr_len); /* second bit-flip (Universe/Local) * is done according RFC2464 */ ipaddr->s6_addr[8] ^= 0x02; break; case IEEE802154_ADDR_SHORT: /* fe:80::ff:fe00:XXXX * \__/ * short_addr * * Universe/Local bit is zero. */ ipaddr->s6_addr[0] = 0xFE; ipaddr->s6_addr[1] = 0x80; ipaddr->s6_addr[11] = 0xFF; ipaddr->s6_addr[12] = 0xFE; ipaddr->s6_addr16[7] = htons(*((u16 *)lladdr)); break; default: pr_debug("Invalid addr_type set\n"); return -EINVAL; } break; default: pr_debug("Invalid address mode value: 0x%x\n", address_mode); return -EINVAL; } if (fail) { pr_debug("Failed to fetch skb data\n"); return -EIO; } raw_dump_inline(NULL, "Reconstructed ipv6 addr is", ipaddr->s6_addr, 16); return 0; }