net/ieee802154/header_ops.c

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