Alexander Aring | cc6ed26 | 2015-01-09 16:42:58 +0100 | [diff] [blame^] | 1 | /* |
| 2 | * 6LoWPAN IPv6 UDP compression according to RFC6282 |
| 3 | * |
| 4 | * |
| 5 | * Authors: |
| 6 | * Alexander Aring <aar@pengutronix.de> |
| 7 | * |
| 8 | * Orignal written by: |
| 9 | * Alexander Smirnov <alex.bluesman.smirnov@gmail.com> |
| 10 | * Jon Smirl <jonsmirl@gmail.com> |
| 11 | * |
| 12 | * This program is free software; you can redistribute it and/or |
| 13 | * modify it under the terms of the GNU General Public License |
| 14 | * as published by the Free Software Foundation; either version |
| 15 | * 2 of the License, or (at your option) any later version. |
| 16 | */ |
| 17 | |
| 18 | #include "nhc.h" |
| 19 | |
| 20 | #define LOWPAN_NHC_UDP_IDLEN 1 |
| 21 | |
| 22 | static int udp_uncompress(struct sk_buff *skb, size_t needed) |
| 23 | { |
| 24 | u8 tmp = 0, val = 0; |
| 25 | struct udphdr uh; |
| 26 | bool fail; |
| 27 | int err; |
| 28 | |
| 29 | fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp)); |
| 30 | |
| 31 | pr_debug("UDP header uncompression\n"); |
| 32 | switch (tmp & LOWPAN_NHC_UDP_CS_P_11) { |
| 33 | case LOWPAN_NHC_UDP_CS_P_00: |
| 34 | fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); |
| 35 | fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); |
| 36 | break; |
| 37 | case LOWPAN_NHC_UDP_CS_P_01: |
| 38 | fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source)); |
| 39 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); |
| 40 | uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); |
| 41 | break; |
| 42 | case LOWPAN_NHC_UDP_CS_P_10: |
| 43 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); |
| 44 | uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT); |
| 45 | fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest)); |
| 46 | break; |
| 47 | case LOWPAN_NHC_UDP_CS_P_11: |
| 48 | fail |= lowpan_fetch_skb(skb, &val, sizeof(val)); |
| 49 | uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4)); |
| 50 | uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f)); |
| 51 | break; |
| 52 | default: |
| 53 | BUG(); |
| 54 | } |
| 55 | |
| 56 | pr_debug("uncompressed UDP ports: src = %d, dst = %d\n", |
| 57 | ntohs(uh.source), ntohs(uh.dest)); |
| 58 | |
| 59 | /* checksum */ |
| 60 | if (tmp & LOWPAN_NHC_UDP_CS_C) { |
| 61 | pr_debug_ratelimited("checksum elided currently not supported\n"); |
| 62 | fail = true; |
| 63 | } else { |
| 64 | fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check)); |
| 65 | } |
| 66 | |
| 67 | if (fail) |
| 68 | return -EINVAL; |
| 69 | |
| 70 | /* UDP length needs to be infered from the lower layers |
| 71 | * here, we obtain the hint from the remaining size of the |
| 72 | * frame |
| 73 | */ |
| 74 | uh.len = htons(skb->len + sizeof(struct udphdr)); |
| 75 | pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len)); |
| 76 | |
| 77 | /* replace the compressed UDP head by the uncompressed UDP |
| 78 | * header |
| 79 | */ |
| 80 | err = skb_cow(skb, needed); |
| 81 | if (unlikely(err)) |
| 82 | return err; |
| 83 | |
| 84 | skb_push(skb, sizeof(struct udphdr)); |
| 85 | skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr)); |
| 86 | |
| 87 | return 0; |
| 88 | } |
| 89 | |
| 90 | static int udp_compress(struct sk_buff *skb, u8 **hc_ptr) |
| 91 | { |
| 92 | const struct udphdr *uh = udp_hdr(skb); |
| 93 | u8 tmp; |
| 94 | |
| 95 | if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) == |
| 96 | LOWPAN_NHC_UDP_4BIT_PORT) && |
| 97 | ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) == |
| 98 | LOWPAN_NHC_UDP_4BIT_PORT)) { |
| 99 | pr_debug("UDP header: both ports compression to 4 bits\n"); |
| 100 | /* compression value */ |
| 101 | tmp = LOWPAN_NHC_UDP_CS_P_11; |
| 102 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 103 | /* source and destination port */ |
| 104 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT + |
| 105 | ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4); |
| 106 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 107 | } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) == |
| 108 | LOWPAN_NHC_UDP_8BIT_PORT) { |
| 109 | pr_debug("UDP header: remove 8 bits of dest\n"); |
| 110 | /* compression value */ |
| 111 | tmp = LOWPAN_NHC_UDP_CS_P_01; |
| 112 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 113 | /* source port */ |
| 114 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); |
| 115 | /* destination port */ |
| 116 | tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT; |
| 117 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 118 | } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) == |
| 119 | LOWPAN_NHC_UDP_8BIT_PORT) { |
| 120 | pr_debug("UDP header: remove 8 bits of source\n"); |
| 121 | /* compression value */ |
| 122 | tmp = LOWPAN_NHC_UDP_CS_P_10; |
| 123 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 124 | /* source port */ |
| 125 | tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT; |
| 126 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 127 | /* destination port */ |
| 128 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); |
| 129 | } else { |
| 130 | pr_debug("UDP header: can't compress\n"); |
| 131 | /* compression value */ |
| 132 | tmp = LOWPAN_NHC_UDP_CS_P_00; |
| 133 | lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp)); |
| 134 | /* source port */ |
| 135 | lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source)); |
| 136 | /* destination port */ |
| 137 | lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest)); |
| 138 | } |
| 139 | |
| 140 | /* checksum is always inline */ |
| 141 | lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check)); |
| 142 | |
| 143 | return 0; |
| 144 | } |
| 145 | |
| 146 | static void udp_nhid_setup(struct lowpan_nhc *nhc) |
| 147 | { |
| 148 | nhc->id[0] = LOWPAN_NHC_UDP_ID; |
| 149 | nhc->idmask[0] = LOWPAN_NHC_UDP_MASK; |
| 150 | } |
| 151 | |
| 152 | LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr), |
| 153 | udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress); |
| 154 | |
| 155 | module_lowpan_nhc(nhc_udp); |
| 156 | MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression"); |
| 157 | MODULE_LICENSE("GPL"); |