net/ieee802154/header_ops.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) 2014 Fraunhofer ITWM
0004  *
0005  * Written by:
0006  * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
0007  */
0008
0009 #include <linux/ieee802154.h>
0010
0011 #include <net/mac802154.h>
0012 #include <net/ieee802154_netdev.h>
0013
0014 static int
0015 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr,
0016              bool omit_pan)
0017 {
0018     int pos = 0;
0019
0020     if (addr->mode == IEEE802154_ADDR_NONE)
0021         return 0;
0022
0023     if (!omit_pan) {
0024         memcpy(buf + pos, &addr->pan_id, 2);
0025         pos += 2;
0026     }
0027
0028     switch (addr->mode) {
0029     case IEEE802154_ADDR_SHORT:
0030         memcpy(buf + pos, &addr->short_addr, 2);
0031         pos += 2;
0032         break;
0033
0034     case IEEE802154_ADDR_LONG:
0035         memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
0036         pos += IEEE802154_ADDR_LEN;
0037         break;
0038
0039     default:
0040         return -EINVAL;
0041     }
0042
0043     return pos;
0044 }
0045
0046 static int
0047 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr)
0048 {
0049     int pos = 5;
0050
0051     memcpy(buf, hdr, 1);
0052     memcpy(buf + 1, &hdr->frame_counter, 4);
0053
0054     switch (hdr->key_id_mode) {
0055     case IEEE802154_SCF_KEY_IMPLICIT:
0056         return pos;
0057
0058     case IEEE802154_SCF_KEY_INDEX:
0059         break;
0060
0061     case IEEE802154_SCF_KEY_SHORT_INDEX:
0062         memcpy(buf + pos, &hdr->short_src, 4);
0063         pos += 4;
0064         break;
0065
0066     case IEEE802154_SCF_KEY_HW_INDEX:
0067         memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
0068         pos += IEEE802154_ADDR_LEN;
0069         break;
0070     }
0071
0072     buf[pos++] = hdr->key_id;
0073
0074     return pos;
0075 }
0076
0077 int
0078 ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr)
0079 {
0080     u8 buf[IEEE802154_MAX_HEADER_LEN];
0081     int pos = 2;
0082     int rc;
0083     struct ieee802154_hdr_fc *fc = &hdr->fc;
0084
0085     buf[pos++] = hdr->seq;
0086
0087     fc->dest_addr_mode = hdr->dest.mode;
0088
0089     rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
0090     if (rc < 0)
0091         return -EINVAL;
0092     pos += rc;
0093
0094     fc->source_addr_mode = hdr->source.mode;
0095
0096     if (hdr->source.pan_id == hdr->dest.pan_id &&
0097         hdr->dest.mode != IEEE802154_ADDR_NONE)
0098         fc->intra_pan = true;
0099
0100     rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
0101     if (rc < 0)
0102         return -EINVAL;
0103     pos += rc;
0104
0105     if (fc->security_enabled) {
0106         fc->version = 1;
0107
0108         rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
0109         if (rc < 0)
0110             return -EINVAL;
0111
0112         pos += rc;
0113     }
0114
0115     memcpy(buf, fc, 2);
0116
0117     memcpy(skb_push(skb, pos), buf, pos);
0118
0119     return pos;
0120 }
0121 EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
0122
0123 static int
0124 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
0125             struct ieee802154_addr *addr)
0126 {
0127     int pos = 0;
0128
0129     addr->mode = mode;
0130
0131     if (mode == IEEE802154_ADDR_NONE)
0132         return 0;
0133
0134     if (!omit_pan) {
0135         memcpy(&addr->pan_id, buf + pos, 2);
0136         pos += 2;
0137     }
0138
0139     if (mode == IEEE802154_ADDR_SHORT) {
0140         memcpy(&addr->short_addr, buf + pos, 2);
0141         return pos + 2;
0142     } else {
0143         memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
0144         return pos + IEEE802154_ADDR_LEN;
0145     }
0146 }
0147
0148 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
0149 {
0150     int pan_len = omit_pan ? 0 : 2;
0151
0152     switch (mode) {
0153     case IEEE802154_ADDR_NONE: return 0;
0154     case IEEE802154_ADDR_SHORT: return 2 + pan_len;
0155     case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
0156     default: return -EINVAL;
0157     }
0158 }
0159
0160 static int
0161 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
0162 {
0163     int pos = 5;
0164
0165     memcpy(hdr, buf, 1);
0166     memcpy(&hdr->frame_counter, buf + 1, 4);
0167
0168     switch (hdr->key_id_mode) {
0169     case IEEE802154_SCF_KEY_IMPLICIT:
0170         return pos;
0171
0172     case IEEE802154_SCF_KEY_INDEX:
0173         break;
0174
0175     case IEEE802154_SCF_KEY_SHORT_INDEX:
0176         memcpy(&hdr->short_src, buf + pos, 4);
0177         pos += 4;
0178         break;
0179
0180     case IEEE802154_SCF_KEY_HW_INDEX:
0181         memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
0182         pos += IEEE802154_ADDR_LEN;
0183         break;
0184     }
0185
0186     hdr->key_id = buf[pos++];
0187
0188     return pos;
0189 }
0190
0191 static int ieee802154_sechdr_lengths[4] = {
0192     [IEEE802154_SCF_KEY_IMPLICIT] = 5,
0193     [IEEE802154_SCF_KEY_INDEX] = 6,
0194     [IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
0195     [IEEE802154_SCF_KEY_HW_INDEX] = 14,
0196 };
0197
0198 static int ieee802154_hdr_sechdr_len(u8 sc)
0199 {
0200     return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
0201 }
0202
0203 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
0204 {
0205     int dlen, slen;
0206
0207     dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
0208     slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
0209                        hdr->fc.intra_pan);
0210
0211     if (slen < 0 || dlen < 0)
0212         return -EINVAL;
0213
0214     return 3 + dlen + slen + hdr->fc.security_enabled;
0215 }
0216
0217 static int
0218 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
0219 {
0220     int pos = 0;
0221
0222     pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
0223                        false, &hdr->dest);
0224     pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
0225                        hdr->fc.intra_pan, &hdr->source);
0226
0227     if (hdr->fc.intra_pan)
0228         hdr->source.pan_id = hdr->dest.pan_id;
0229
0230     return pos;
0231 }
0232
0233 int
0234 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
0235 {
0236     int pos = 3, rc;
0237
0238     if (!pskb_may_pull(skb, 3))
0239         return -EINVAL;
0240
0241     memcpy(hdr, skb->data, 3);
0242
0243     rc = ieee802154_hdr_minlen(hdr);
0244     if (rc < 0 || !pskb_may_pull(skb, rc))
0245         return -EINVAL;
0246
0247     pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
0248
0249     if (hdr->fc.security_enabled) {
0250         int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
0251
0252         if (!pskb_may_pull(skb, want))
0253             return -EINVAL;
0254
0255         pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
0256     }
0257
0258     skb_pull(skb, pos);
0259     return pos;
0260 }
0261 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
0262
0263 int
0264 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
0265 {
0266     const u8 *buf = skb_mac_header(skb);
0267     int pos = 3, rc;
0268
0269     if (buf + 3 > skb_tail_pointer(skb))
0270         return -EINVAL;
0271
0272     memcpy(hdr, buf, 3);
0273
0274     rc = ieee802154_hdr_minlen(hdr);
0275     if (rc < 0 || buf + rc > skb_tail_pointer(skb))
0276         return -EINVAL;
0277
0278     pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
0279     return pos;
0280 }
0281 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
0282
0283 int
0284 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
0285 {
0286     const u8 *buf = skb_mac_header(skb);
0287     int pos;
0288
0289     pos = ieee802154_hdr_peek_addrs(skb, hdr);
0290     if (pos < 0)
0291         return -EINVAL;
0292
0293     if (hdr->fc.security_enabled) {
0294         u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
0295         int want = pos + ieee802154_sechdr_lengths[key_id_mode];
0296
0297         if (buf + want > skb_tail_pointer(skb))
0298             return -EINVAL;
0299
0300         pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
0301     }
0302
0303     return pos;
0304 }
0305 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
0306
0307 int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
0308 {
0309     int hlen = ieee802154_hdr_minlen(hdr);
0310
0311     if (hdr->fc.security_enabled) {
0312         hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
0313         hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
0314     }
0315
0316     return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
0317 }
0318 EXPORT_SYMBOL_GPL(ieee802154_max_payload);