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