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_mac_cmd_push(struct sk_buff *skb, void *f,
 124                             const void *pl, unsigned int pl_len)
 125 {
 126         struct ieee802154_mac_cmd_frame *frame = f;
 127         struct ieee802154_mac_cmd_pl *mac_pl = &frame->mac_pl;
 128         struct ieee802154_hdr *mhr = &frame->mhr;
 129         int ret;
 130
 131         skb_reserve(skb, sizeof(*mhr));
 132         ret = ieee802154_hdr_push(skb, mhr);
 133         if (ret < 0)
 134                 return ret;
 135
 136         skb_reset_mac_header(skb);
 137         skb->mac_len = ret;
 138
 139         skb_put_data(skb, mac_pl, sizeof(*mac_pl));
 140         skb_put_data(skb, pl, pl_len);
 141
 142         return 0;
 143 }
 144 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_push);
 145
 146 int ieee802154_beacon_push(struct sk_buff *skb,
 147                            struct ieee802154_beacon_frame *beacon)
 148 {
 149         struct ieee802154_beacon_hdr *mac_pl = &beacon->mac_pl;
 150         struct ieee802154_hdr *mhr = &beacon->mhr;
 151         int ret;
 152
 153         skb_reserve(skb, sizeof(*mhr));
 154         ret = ieee802154_hdr_push(skb, mhr);
 155         if (ret < 0)
 156                 return ret;
 157
 158         skb_reset_mac_header(skb);
 159         skb->mac_len = ret;
 160
 161         skb_put_data(skb, mac_pl, sizeof(*mac_pl));
 162
 163         if (mac_pl->pend_short_addr_count || mac_pl->pend_ext_addr_count)
 164                 return -EOPNOTSUPP;
 165
 166         return 0;
 167 }
 168 EXPORT_SYMBOL_GPL(ieee802154_beacon_push);
 169
 170 static int
 171 ieee802154_hdr_get_addr(const u8 *buf, int mode, bool omit_pan,
 172                         struct ieee802154_addr *addr)
 173 {
 174         int pos = 0;
 175
 176         addr->mode = mode;
 177
 178         if (mode == IEEE802154_ADDR_NONE)
 179                 return 0;
 180
 181         if (!omit_pan) {
 182                 memcpy(&addr->pan_id, buf + pos, 2);
 183                 pos += 2;
 184         }
 185
 186         if (mode == IEEE802154_ADDR_SHORT) {
 187                 memcpy(&addr->short_addr, buf + pos, 2);
 188                 return pos + 2;
 189         } else {
 190                 memcpy(&addr->extended_addr, buf + pos, IEEE802154_ADDR_LEN);
 191                 return pos + IEEE802154_ADDR_LEN;
 192         }
 193 }
 194
 195 static int ieee802154_hdr_addr_len(int mode, bool omit_pan)
 196 {
 197         int pan_len = omit_pan ? 0 : 2;
 198
 199         switch (mode) {
 200         case IEEE802154_ADDR_NONE: return 0;
 201         case IEEE802154_ADDR_SHORT: return 2 + pan_len;
 202         case IEEE802154_ADDR_LONG: return IEEE802154_ADDR_LEN + pan_len;
 203         default: return -EINVAL;
 204         }
 205 }
 206
 207 static int
 208 ieee802154_hdr_get_sechdr(const u8 *buf, struct ieee802154_sechdr *hdr)
 209 {
 210         int pos = 5;
 211
 212         memcpy(hdr, buf, 1);
 213         memcpy(&hdr->frame_counter, buf + 1, 4);
 214
 215         switch (hdr->key_id_mode) {
 216         case IEEE802154_SCF_KEY_IMPLICIT:
 217                 return pos;
 218
 219         case IEEE802154_SCF_KEY_INDEX:
 220                 break;
 221
 222         case IEEE802154_SCF_KEY_SHORT_INDEX:
 223                 memcpy(&hdr->short_src, buf + pos, 4);
 224                 pos += 4;
 225                 break;
 226
 227         case IEEE802154_SCF_KEY_HW_INDEX:
 228                 memcpy(&hdr->extended_src, buf + pos, IEEE802154_ADDR_LEN);
 229                 pos += IEEE802154_ADDR_LEN;
 230                 break;
 231         }
 232
 233         hdr->key_id = buf[pos++];
 234
 235         return pos;
 236 }
 237
 238 static int ieee802154_sechdr_lengths[4] = {
 239         [IEEE802154_SCF_KEY_IMPLICIT] = 5,
 240         [IEEE802154_SCF_KEY_INDEX] = 6,
 241         [IEEE802154_SCF_KEY_SHORT_INDEX] = 10,
 242         [IEEE802154_SCF_KEY_HW_INDEX] = 14,
 243 };
 244
 245 static int ieee802154_hdr_sechdr_len(u8 sc)
 246 {
 247         return ieee802154_sechdr_lengths[IEEE802154_SCF_KEY_ID_MODE(sc)];
 248 }
 249
 250 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr)
 251 {
 252         int dlen, slen;
 253
 254         dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false);
 255         slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode,
 256                                        hdr->fc.intra_pan);
 257
 258         if (slen < 0 || dlen < 0)
 259                 return -EINVAL;
 260
 261         return 3 + dlen + slen + hdr->fc.security_enabled;
 262 }
 263
 264 static int
 265 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr)
 266 {
 267         int pos = 0;
 268
 269         pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode,
 270                                        false, &hdr->dest);
 271         pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode,
 272                                        hdr->fc.intra_pan, &hdr->source);
 273
 274         if (hdr->fc.intra_pan)
 275                 hdr->source.pan_id = hdr->dest.pan_id;
 276
 277         return pos;
 278 }
 279
 280 int
 281 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr)
 282 {
 283         int pos = 3, rc;
 284
 285         if (!pskb_may_pull(skb, 3))
 286                 return -EINVAL;
 287
 288         memcpy(hdr, skb->data, 3);
 289
 290         rc = ieee802154_hdr_minlen(hdr);
 291         if (rc < 0 || !pskb_may_pull(skb, rc))
 292                 return -EINVAL;
 293
 294         pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr);
 295
 296         if (hdr->fc.security_enabled) {
 297                 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]);
 298
 299                 if (!pskb_may_pull(skb, want))
 300                         return -EINVAL;
 301
 302                 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec);
 303         }
 304
 305         skb_pull(skb, pos);
 306         return pos;
 307 }
 308 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull);
 309
 310 int ieee802154_mac_cmd_pl_pull(struct sk_buff *skb,
 311                                struct ieee802154_mac_cmd_pl *mac_pl)
 312 {
 313         if (!pskb_may_pull(skb, sizeof(*mac_pl)))
 314                 return -EINVAL;
 315
 316         memcpy(mac_pl, skb->data, sizeof(*mac_pl));
 317         skb_pull(skb, sizeof(*mac_pl));
 318
 319         return 0;
 320 }
 321 EXPORT_SYMBOL_GPL(ieee802154_mac_cmd_pl_pull);
 322
 323 int
 324 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
 325 {
 326         const u8 *buf = skb_mac_header(skb);
 327         int pos = 3, rc;
 328
 329         if (buf + 3 > skb_tail_pointer(skb))
 330                 return -EINVAL;
 331
 332         memcpy(hdr, buf, 3);
 333
 334         rc = ieee802154_hdr_minlen(hdr);
 335         if (rc < 0 || buf + rc > skb_tail_pointer(skb))
 336                 return -EINVAL;
 337
 338         pos += ieee802154_hdr_get_addrs(buf + pos, hdr);
 339         return pos;
 340 }
 341 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs);
 342
 343 int
 344 ieee802154_hdr_peek(const struct sk_buff *skb, struct ieee802154_hdr *hdr)
 345 {
 346         const u8 *buf = skb_mac_header(skb);
 347         int pos;
 348
 349         pos = ieee802154_hdr_peek_addrs(skb, hdr);
 350         if (pos < 0)
 351                 return -EINVAL;
 352
 353         if (hdr->fc.security_enabled) {
 354                 u8 key_id_mode = IEEE802154_SCF_KEY_ID_MODE(*(buf + pos));
 355                 int want = pos + ieee802154_sechdr_lengths[key_id_mode];
 356
 357                 if (buf + want > skb_tail_pointer(skb))
 358                         return -EINVAL;
 359
 360                 pos += ieee802154_hdr_get_sechdr(buf + pos, &hdr->sec);
 361         }
 362
 363         return pos;
 364 }
 365 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek);
 366
 367 int ieee802154_max_payload(const struct ieee802154_hdr *hdr)
 368 {
 369         int hlen = ieee802154_hdr_minlen(hdr);
 370
 371         if (hdr->fc.security_enabled) {
 372                 hlen += ieee802154_sechdr_lengths[hdr->sec.key_id_mode] - 1;
 373                 hlen += ieee802154_sechdr_authtag_len(&hdr->sec);
 374         }
 375
 376         return IEEE802154_MTU - hlen - IEEE802154_MFR_SIZE;
 377 }
 378 EXPORT_SYMBOL_GPL(ieee802154_max_payload);