net/ieee802154/header_ops.c

   1 // SPDX-License-Identifier: GPL-2.0-only
   2 /*
   3  * Copyright (C) 2014 Fraunhofer ITWM
   4  *
   5  * Written by:
   6  * Phoebe Buckheister <phoebe.buckheister@itwm.fraunhofer.de>
   7  */
   8
   9 #include <linux/ieee802154.h>
  10
  11 #include <net/mac802154.h>
  12 #include <net/ieee802154_netdev.h>
  13
  14 static int
  15 ieee802154_hdr_push_addr(u8 *buf, const struct ieee802154_addr *addr,
  16                          bool omit_pan)
  17 {
  18         int pos = 0;
  19
  20         if (addr->mode == IEEE802154_ADDR_NONE)
  21                 return 0;
  22
  23         if (!omit_pan) {
  24                 memcpy(buf + pos, &addr->pan_id, 2);
  25                 pos += 2;
  26         }
  27
  28         switch (addr->mode) {
  29         case IEEE802154_ADDR_SHORT:
  30                 memcpy(buf + pos, &addr->short_addr, 2);
  31                 pos += 2;
  32                 break;
  33
  34         case IEEE802154_ADDR_LONG:
  35                 memcpy(buf + pos, &addr->extended_addr, IEEE802154_ADDR_LEN);
  36                 pos += IEEE802154_ADDR_LEN;
  37                 break;
  38
  39         default:
  40                 return -EINVAL;
  41         }
  42
  43         return pos;
  44 }
  45
  46 static int
  47 ieee802154_hdr_push_sechdr(u8 *buf, const struct ieee802154_sechdr *hdr)
  48 {
  49         int pos = 5;
  50
  51         memcpy(buf, hdr, 1);
  52         memcpy(buf + 1, &hdr->frame_counter, 4);
  53
  54         switch (hdr->key_id_mode) {
  55         case IEEE802154_SCF_KEY_IMPLICIT:
  56                 return pos;
  57
  58         case IEEE802154_SCF_KEY_INDEX:
  59                 break;
  60
  61         case IEEE802154_SCF_KEY_SHORT_INDEX:
  62                 memcpy(buf + pos, &hdr->short_src, 4);
  63                 pos += 4;
  64                 break;
  65
  66         case IEEE802154_SCF_KEY_HW_INDEX:
  67                 memcpy(buf + pos, &hdr->extended_src, IEEE802154_ADDR_LEN);
  68                 pos += IEEE802154_ADDR_LEN;
  69                 break;
  70         }
  71
  72         buf[pos++] = hdr->key_id;
  73
  74         return pos;
  75 }
  76
  77 int
  78 ieee802154_hdr_push(struct sk_buff *skb, struct ieee802154_hdr *hdr)
  79 {
  80         u8 buf[IEEE802154_MAX_HEADER_LEN];
  81         int pos = 2;
  82         int rc;
  83         struct ieee802154_hdr_fc *fc = &hdr->fc;
  84
  85         buf[pos++] = hdr->seq;
  86
  87         fc->dest_addr_mode = hdr->dest.mode;
  88
  89         rc = ieee802154_hdr_push_addr(buf + pos, &hdr->dest, false);
  90         if (rc < 0)
  91                 return -EINVAL;
  92         pos += rc;
  93
  94         fc->source_addr_mode = hdr->source.mode;
  95
  96         if (hdr->source.pan_id == hdr->dest.pan_id &&
  97             hdr->dest.mode != IEEE802154_ADDR_NONE)
  98                 fc->intra_pan = true;
  99
 100         rc = ieee802154_hdr_push_addr(buf + pos, &hdr->source, fc->intra_pan);
 101         if (rc < 0)
 102                 return -EINVAL;
 103         pos += rc;
 104
 105         if (fc->security_enabled) {
 106                 fc->version = 1;
 107
 108                 rc = ieee802154_hdr_push_sechdr(buf + pos, &hdr->sec);
 109                 if (rc < 0)
 110                         return -EINVAL;
 111
 112                 pos += rc;
 113         }
 114
 115         memcpy(buf, fc, 2);
 116
 117         memcpy(skb_push(skb, pos), buf, pos);
 118
 119         return pos;
 120 }
 121 EXPORT_SYMBOL_GPL(ieee802154_hdr_push);
 122
 123 int ieee802154_beacon_push(struct sk_buff *skb,
 124                            struct ieee802154_beacon_frame *beacon)
 125 {
 126         struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl;
 127         struct ieee802154_hdr *mhr = &beacon->mhr;
 128         int ret;
 129
 130         skb_reserve(skb, sizeof(*mhr));
 131         ret = ieee802154_hdr_push(skb, mhr);
 132         if (ret < 0)
 133                 return ret;
 134
 135         skb_reset_mac_header(skb);
 136         skb->mac_len = ret;
 137
 138         skb_put_data(skb, mac_pl, sizeof(*mac_pl));
 139
 140         if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count)
 141                 return -EOPNOTSUPP;
 142
 143         return 0;
 144 }
 145 EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
 146
 147 static int
 148 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
 149                         struct ieee802154_addr *addr)
 150 {
 151         int pos = 0;
 152
 153         addr->mode = mode;
 154
 155         if (mode == IEEE802154_ADDR_NONE)
 156                 return 0;
 157
 158         if (!omit_pan) {
 159                 memcpy(&addr->pan_id, buf + pos, 2);
 160                 pos += 2;
 161         }
 162
 163         if (mode == IEEE802154_ADDR_SHORT) {
 164                 memcpy(&addr->short_addr, buf + pos, 2);
 165                 return pos + 2;
 166         } else {
 167                 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
 168                 return pos + IEEE802154_ADDR_LEN;
 169         }
 170 }
 171
 172 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
 173 {
 174         int pan_len = omit_pan ? 0 : 2;
 175
 176         switch (mode) {
 177         case IEEE802154_ADDR_NONE: return 0;
 178         case IEEE802154_ADDR_SHORT: return 2 + pan_len;
 179         case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
 180         default: return -EINVAL;
 181         }
 182 }
 183
 184 static int
 185 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
 186 {
 187         int pos = 5;
 188
 189         memcpy(hdr, buf, 1);
 190         memcpy(&hdr->frame_counter, buf + 1, 4);
 191
 192         switch (hdr->key_id_mode) {
 193         case IEEE802154_SCF_KEY_IMPLICIT:
 194                 return pos;
 195
 196         case IEEE802154_SCF_KEY_INDEX:
 197                 break;
 198
 199         case IEEE802154_SCF_KEY_SHORT_INDEX:
 200                 memcpy(&hdr->short_src, buf + pos, 4);
 201                 pos += 4;
 202                 break;
 203
 204         case IEEE802154_SCF_KEY_HW_INDEX:
 205                 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
 206                 pos += IEEE802154_ADDR_LEN;
 207                 break;
 208         }
 209
 210         hdr->key_id = buf[pos++];
 211
 212         return pos;
 213 }
 214
 215 static int ieee802154_sechdr_lengths[4] = {
 216         [IEEE802154_SCF_KEY_IMPLICIT] = 5,
 217         [IEEE802154_SCF_KEY_INDEX] = 6,
 218         [IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
 219         [IEEE802154_SCF_KEY_HW_INDEX] = 14,
 220 };
 221
 222 static int ieee802154_hdr_sechdr_len(u8 sc)
 223 {
 224         return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
 225 }
 226
 227 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
 228 {
 229         int dlen, slen;
 230
 231         dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
 232         slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
 233                                        hdr->fc.intra_pan);
 234
 235         if (slen < 0 || dlen < 0)
 236                 return -EINVAL;
 237
 238         return 3 + dlen + slen + hdr->fc.security_enabled;
 239 }
 240
 241 static int
 242 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
 243 {
 244         int pos = 0;
 245
 246         pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
 247                                        false, &hdr->dest);
 248         pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
 249                                        hdr->fc.intra_pan, &hdr->source);
 250
 251         if (hdr->fc.intra_pan)
 252                 hdr->source.pan_id = hdr->dest.pan_id;
 253
 254         return pos;
 255 }
 256
 257 int
 258 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
 259 {
 260         int pos = 3, rc;
 261
 262         if (!pskb_may_pull(skb, 3))
 263                 return -EINVAL;
 264
 265         memcpy(hdr, skb->data, 3);
 266
 267         rc = ieee802154_hdr_minlen(hdr);
 268         if (rc < 0 || !pskb_may_pull(skb, rc))
 269                 return -EINVAL;
 270
 271         pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
 272
 273         if (hdr->fc.security_enabled) {
 274                 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
 275
 276                 if (!pskb_may_pull(skb, want))
 277                         return -EINVAL;
 278
 279                 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
 280         }
 281
 282         skb_pull(skb, pos);
 283         return pos;
 284 }
 285 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
 286
 287 int
 288 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
 289 {
 290         const u8 *buf = skb_mac_header(skb);
 291         int pos = 3, rc;
 292
 293         if (buf + 3 > skb_tail_pointer(skb))
 294                 return -EINVAL;
 295
 296         memcpy(hdr, buf, 3);
 297
 298         rc = ieee802154_hdr_minlen(hdr);
 299         if (rc < 0 || buf + rc > skb_tail_pointer(skb))
 300                 return -EINVAL;
 301
 302         pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
 303         return pos;
 304 }
 305 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
 306
 307 int
 308 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
 309 {
 310         const u8 *buf = skb_mac_header(skb);
 311         int pos;
 312
 313         pos = ieee802154_hdr_peek_addrs(skb, hdr);
 314         if (pos < 0)
 315                 return -EINVAL;
 316
 317         if (hdr->fc.security_enabled) {
 318                 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
 319                 int want = pos + ieee802154_sechdr_lengths[key_id_mode];
 320
 321                 if (buf + want > skb_tail_pointer(skb))
 322                         return -EINVAL;
 323
 324                 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
 325         }
 326
 327         return pos;
 328 }
 329 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
 330
 331 int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
 332 {
 333         int hlen = ieee802154_hdr_minlen(hdr);
 334
 335         if (hdr->fc.security_enabled) {
 336                 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
 337                 hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
 338         }
 339
 340         return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
 341 }
 342 EXPORT_SYMBOL_GPL(ieee802154_max_payload);