Merge tag 'acpi-5.1-rc4' of git://git.kernel.org/pub/scm/linux/kernel/git/rafael...
[sfrench/cifs-2.6.git] / net / 6lowpan / nhc_udp.c
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_MASK             0xF8
21 #define LOWPAN_NHC_UDP_ID               0xF0
22 #define LOWPAN_NHC_UDP_IDLEN            1
23
24 #define LOWPAN_NHC_UDP_4BIT_PORT        0xF0B0
25 #define LOWPAN_NHC_UDP_4BIT_MASK        0xFFF0
26 #define LOWPAN_NHC_UDP_8BIT_PORT        0xF000
27 #define LOWPAN_NHC_UDP_8BIT_MASK        0xFF00
28
29 /* values for port compression, _with checksum_ ie bit 5 set to 0 */
30
31 /* all inline */
32 #define LOWPAN_NHC_UDP_CS_P_00  0xF0
33 /* source 16bit inline, dest = 0xF0 + 8 bit inline */
34 #define LOWPAN_NHC_UDP_CS_P_01  0xF1
35 /* source = 0xF0 + 8bit inline, dest = 16 bit inline */
36 #define LOWPAN_NHC_UDP_CS_P_10  0xF2
37 /* source & dest = 0xF0B + 4bit inline */
38 #define LOWPAN_NHC_UDP_CS_P_11  0xF3
39 /* checksum elided */
40 #define LOWPAN_NHC_UDP_CS_C     0x04
41
42 static int udp_uncompress(struct sk_buff *skb, size_t needed)
43 {
44         u8 tmp = 0, val = 0;
45         struct udphdr uh;
46         bool fail;
47         int err;
48
49         fail = lowpan_fetch_skb(skb, &tmp, sizeof(tmp));
50
51         pr_debug("UDP header uncompression\n");
52         switch (tmp & LOWPAN_NHC_UDP_CS_P_11) {
53         case LOWPAN_NHC_UDP_CS_P_00:
54                 fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
55                 fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
56                 break;
57         case LOWPAN_NHC_UDP_CS_P_01:
58                 fail |= lowpan_fetch_skb(skb, &uh.source, sizeof(uh.source));
59                 fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
60                 uh.dest = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
61                 break;
62         case LOWPAN_NHC_UDP_CS_P_10:
63                 fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
64                 uh.source = htons(val + LOWPAN_NHC_UDP_8BIT_PORT);
65                 fail |= lowpan_fetch_skb(skb, &uh.dest, sizeof(uh.dest));
66                 break;
67         case LOWPAN_NHC_UDP_CS_P_11:
68                 fail |= lowpan_fetch_skb(skb, &val, sizeof(val));
69                 uh.source = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val >> 4));
70                 uh.dest = htons(LOWPAN_NHC_UDP_4BIT_PORT + (val & 0x0f));
71                 break;
72         default:
73                 BUG();
74         }
75
76         pr_debug("uncompressed UDP ports: src = %d, dst = %d\n",
77                  ntohs(uh.source), ntohs(uh.dest));
78
79         /* checksum */
80         if (tmp & LOWPAN_NHC_UDP_CS_C) {
81                 pr_debug_ratelimited("checksum elided currently not supported\n");
82                 fail = true;
83         } else {
84                 fail |= lowpan_fetch_skb(skb, &uh.check, sizeof(uh.check));
85         }
86
87         if (fail)
88                 return -EINVAL;
89
90         /* UDP length needs to be infered from the lower layers
91          * here, we obtain the hint from the remaining size of the
92          * frame
93          */
94         switch (lowpan_dev(skb->dev)->lltype) {
95         case LOWPAN_LLTYPE_IEEE802154:
96                 if (lowpan_802154_cb(skb)->d_size)
97                         uh.len = htons(lowpan_802154_cb(skb)->d_size -
98                                        sizeof(struct ipv6hdr));
99                 else
100                         uh.len = htons(skb->len + sizeof(struct udphdr));
101                 break;
102         default:
103                 uh.len = htons(skb->len + sizeof(struct udphdr));
104                 break;
105         }
106         pr_debug("uncompressed UDP length: src = %d", ntohs(uh.len));
107
108         /* replace the compressed UDP head by the uncompressed UDP
109          * header
110          */
111         err = skb_cow(skb, needed);
112         if (unlikely(err))
113                 return err;
114
115         skb_push(skb, sizeof(struct udphdr));
116         skb_copy_to_linear_data(skb, &uh, sizeof(struct udphdr));
117
118         return 0;
119 }
120
121 static int udp_compress(struct sk_buff *skb, u8 **hc_ptr)
122 {
123         const struct udphdr *uh = udp_hdr(skb);
124         u8 tmp;
125
126         if (((ntohs(uh->source) & LOWPAN_NHC_UDP_4BIT_MASK) ==
127              LOWPAN_NHC_UDP_4BIT_PORT) &&
128             ((ntohs(uh->dest) & LOWPAN_NHC_UDP_4BIT_MASK) ==
129              LOWPAN_NHC_UDP_4BIT_PORT)) {
130                 pr_debug("UDP header: both ports compression to 4 bits\n");
131                 /* compression value */
132                 tmp = LOWPAN_NHC_UDP_CS_P_11;
133                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
134                 /* source and destination port */
135                 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_4BIT_PORT +
136                       ((ntohs(uh->source) - LOWPAN_NHC_UDP_4BIT_PORT) << 4);
137                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
138         } else if ((ntohs(uh->dest) & LOWPAN_NHC_UDP_8BIT_MASK) ==
139                         LOWPAN_NHC_UDP_8BIT_PORT) {
140                 pr_debug("UDP header: remove 8 bits of dest\n");
141                 /* compression value */
142                 tmp = LOWPAN_NHC_UDP_CS_P_01;
143                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
144                 /* source port */
145                 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
146                 /* destination port */
147                 tmp = ntohs(uh->dest) - LOWPAN_NHC_UDP_8BIT_PORT;
148                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
149         } else if ((ntohs(uh->source) & LOWPAN_NHC_UDP_8BIT_MASK) ==
150                         LOWPAN_NHC_UDP_8BIT_PORT) {
151                 pr_debug("UDP header: remove 8 bits of source\n");
152                 /* compression value */
153                 tmp = LOWPAN_NHC_UDP_CS_P_10;
154                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
155                 /* source port */
156                 tmp = ntohs(uh->source) - LOWPAN_NHC_UDP_8BIT_PORT;
157                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
158                 /* destination port */
159                 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
160         } else {
161                 pr_debug("UDP header: can't compress\n");
162                 /* compression value */
163                 tmp = LOWPAN_NHC_UDP_CS_P_00;
164                 lowpan_push_hc_data(hc_ptr, &tmp, sizeof(tmp));
165                 /* source port */
166                 lowpan_push_hc_data(hc_ptr, &uh->source, sizeof(uh->source));
167                 /* destination port */
168                 lowpan_push_hc_data(hc_ptr, &uh->dest, sizeof(uh->dest));
169         }
170
171         /* checksum is always inline */
172         lowpan_push_hc_data(hc_ptr, &uh->check, sizeof(uh->check));
173
174         return 0;
175 }
176
177 static void udp_nhid_setup(struct lowpan_nhc *nhc)
178 {
179         nhc->id[0] = LOWPAN_NHC_UDP_ID;
180         nhc->idmask[0] = LOWPAN_NHC_UDP_MASK;
181 }
182
183 LOWPAN_NHC(nhc_udp, "RFC6282 UDP", NEXTHDR_UDP, sizeof(struct udphdr),
184            udp_nhid_setup, LOWPAN_NHC_UDP_IDLEN, udp_uncompress, udp_compress);
185
186 module_lowpan_nhc(nhc_udp);
187 MODULE_DESCRIPTION("6LoWPAN next header RFC6282 UDP compression");
188 MODULE_LICENSE("GPL");