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_hdr_sechdr_len(u8 sc) 199 { 200 switch (IEEE802154_SCF_KEY_ID_MODE(sc)) { 201 case IEEE802154_SCF_KEY_IMPLICIT: return 5; 202 case IEEE802154_SCF_KEY_INDEX: return 6; 203 case IEEE802154_SCF_KEY_SHORT_INDEX: return 10; 204 case IEEE802154_SCF_KEY_HW_INDEX: return 14; 205 default: return -EINVAL; 206 } 207 } 208 209 static int ieee802154_hdr_minlen(const struct ieee802154_hdr *hdr) 210 { 211 int dlen, slen; 212 213 dlen = ieee802154_hdr_addr_len(hdr->fc.dest_addr_mode, false); 214 slen = ieee802154_hdr_addr_len(hdr->fc.source_addr_mode, 215 hdr->fc.intra_pan); 216 217 if (slen < 0 || dlen < 0) 218 return -EINVAL; 219 220 return 3 + dlen + slen + hdr->fc.security_enabled; 221 } 222 223 static int 224 ieee802154_hdr_get_addrs(const u8 *buf, struct ieee802154_hdr *hdr) 225 { 226 int pos = 0; 227 228 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.dest_addr_mode, 229 false, &hdr->dest); 230 pos += ieee802154_hdr_get_addr(buf + pos, hdr->fc.source_addr_mode, 231 hdr->fc.intra_pan, &hdr->source); 232 233 if (hdr->fc.intra_pan) 234 hdr->source.pan_id = hdr->dest.pan_id; 235 236 return pos; 237 } 238 239 int 240 ieee802154_hdr_pull(struct sk_buff *skb, struct ieee802154_hdr *hdr) 241 { 242 int pos = 3, rc; 243 244 if (!pskb_may_pull(skb, 3)) 245 return -EINVAL; 246 247 memcpy(hdr, skb->data, 3); 248 249 rc = ieee802154_hdr_minlen(hdr); 250 if (rc < 0 || !pskb_may_pull(skb, rc)) 251 return -EINVAL; 252 253 pos += ieee802154_hdr_get_addrs(skb->data + pos, hdr); 254 255 if (hdr->fc.security_enabled) { 256 int want = pos + ieee802154_hdr_sechdr_len(skb->data[pos]); 257 258 if (!pskb_may_pull(skb, want)) 259 return -EINVAL; 260 261 pos += ieee802154_hdr_get_sechdr(skb->data + pos, &hdr->sec); 262 } 263 264 skb_pull(skb, pos); 265 return pos; 266 } 267 EXPORT_SYMBOL_GPL(ieee802154_hdr_pull); 268 269 int 270 ieee802154_hdr_peek_addrs(const struct sk_buff *skb, struct ieee802154_hdr *hdr) 271 { 272 const u8 *buf = skb_mac_header(skb); 273 int pos = 3, rc; 274 275 if (buf + 3 > skb_tail_pointer(skb)) 276 return -EINVAL; 277 278 memcpy(hdr, buf, 3); 279 280 rc = ieee802154_hdr_minlen(hdr); 281 if (rc < 0 || buf + rc > skb_tail_pointer(skb)) 282 return -EINVAL; 283 284 pos += ieee802154_hdr_get_addrs(buf + pos, hdr); 285 return pos; 286 } 287 EXPORT_SYMBOL_GPL(ieee802154_hdr_peek_addrs); 288