bsd/net/frame802154.c

   1 /*
   2  * Copyright (c) 2017-2020 Apple Inc. All rights reserved.
   3  *
   4  * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
   5  *
   6  * This file contains Original Code and/or Modifications of Original Code
   7  * as defined in and that are subject to the Apple Public Source License
   8  * Version 2.0 (the 'License'). You may not use this file except in
   9  * compliance with the License. The rights granted to you under the License
  10  * may not be used to create, or enable the creation or redistribution of,
  11  * unlawful or unlicensed copies of an Apple operating system, or to
  12  * circumvent, violate, or enable the circumvention or violation of, any
  13  * terms of an Apple operating system software license agreement.
  14  *
  15  * Please obtain a copy of the License at
  16  * http://www.opensource.apple.com/apsl/ and read it before using this file.
  17  *
  18  * The Original Code and all software distributed under the License are
  19  * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
  20  * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
  21  * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
  22  * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
  23  * Please see the License for the specific language governing rights and
  24  * limitations under the License.
  25  *
  26  * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
  27  */
  28 /*
  29  *
  30  *  Copyright (c) 2008, Swedish Institute of Computer Science
  31  *  All rights reserved.
  32  *
  33  *  Additional fixes for AVR contributed by:
  34  *
  35  *      Colin O'Flynn coflynn@newae.com
  36  *      Eric Gnoske egnoske@gmail.com
  37  *      Blake Leverett bleverett@gmail.com
  38  *      Mike Vidales mavida404@gmail.com
  39  *      Kevin Brown kbrown3@uccs.edu
  40  *      Nate Bohlmann nate@elfwerks.com
  41  *
  42  *  Additional fixes for MSP430 contributed by:
  43  *        Joakim Eriksson
  44  *        Niclas Finne
  45  *        Nicolas Tsiftes
  46  *
  47  *   All rights reserved.
  48  *
  49  *   Redistribution and use in source and binary forms, with or without
  50  *   modification, are permitted provided that the following conditions are met:
  51  *
  52  *   * Redistributions of source code must retain the above copyright
  53  *     notice, this list of conditions and the following disclaimer.
  54  *   * Redistributions in binary form must reproduce the above copyright
  55  *     notice, this list of conditions and the following disclaimer in
  56  *     the documentation and/or other materials provided with the
  57  *     distribution.
  58  *   * Neither the name of the copyright holders nor the names of
  59  *     contributors may be used to endorse or promote products derived
  60  *     from this software without specific prior written permission.
  61  *
  62  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
  63  *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  64  *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  65  *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
  66  *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
  67  *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
  68  *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
  69  *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
  70  *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
  71  *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  72  *  POSSIBILITY OF SUCH DAMAGE.
  73  *
  74  */
  75 /*
  76  *  \brief This file is where the main functions that relate to frame
  77  *  manipulation will reside.
  78  */
  79
  80 /**
  81  *  \file
  82  *  \brief 802.15.4 frame creation and parsing functions
  83  *
  84  *  This file converts to and from a structure to a packed 802.15.4
  85  *  frame.
  86  */
  87
  88 /**
  89  *   \addtogroup frame802154
  90  *   @{
  91  */
  92
  93 #include "cc.h"
  94 #include "frame802154.h"
  95 //#include "net/llsec/llsec802154.h"
  96 #include "linkaddr.h"
  97 #include <string.h>
  98
  99 /**
 100  *  \brief Structure that contains the lengths of the various addressing and security fields
 101  *  in the 802.15.4 header.  This structure is used in \ref frame802154_create()
 102  */
 103 typedef struct {
 104         uint8_t dest_pid_len;    /**<  Length (in bytes) of destination PAN ID field */
 105         uint8_t dest_addr_len;   /**<  Length (in bytes) of destination address field */
 106         uint8_t src_pid_len;     /**<  Length (in bytes) of source PAN ID field */
 107         uint8_t src_addr_len;    /**<  Length (in bytes) of source address field */
 108         uint8_t aux_sec_len;     /**<  Length (in bytes) of aux security header field */
 109 } field_length_t;
 110
 111 /*----------------------------------------------------------------------------*/
 112 CC_INLINE static uint8_t
 113 addr_len(uint8_t mode)
 114 {
 115         switch (mode) {
 116         case FRAME802154_SHORTADDRMODE:          /* 16-bit address */
 117                 return 2;
 118         case FRAME802154_LONGADDRMODE:           /* 64-bit address */
 119                 return 8;
 120         default:
 121                 return 0;
 122         }
 123 }
 124 /*----------------------------------------------------------------------------*/
 125 #if LLSEC802154_USES_EXPLICIT_KEYS
 126 static uint8_t
 127 get_key_id_len(uint8_t key_id_mode)
 128 {
 129         switch (key_id_mode) {
 130         case FRAME802154_1_BYTE_KEY_ID_MODE:
 131                 return 1;
 132         case FRAME802154_5_BYTE_KEY_ID_MODE:
 133                 return 5;
 134         case FRAME802154_9_BYTE_KEY_ID_MODE:
 135                 return 9;
 136         default:
 137                 return 0;
 138         }
 139 }
 140 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 141 /*----------------------------------------------------------------------------*/
 142 static void
 143 field_len(frame802154_t *p, field_length_t *flen)
 144 {
 145         /* init flen to zeros */
 146         memset(flen, 0, sizeof(field_length_t));
 147
 148         /* Determine lengths of each field based on fcf and other args */
 149         if (p->fcf.dest_addr_mode & 3) {
 150                 flen->dest_pid_len = 2;
 151         }
 152         if (p->fcf.src_addr_mode & 3) {
 153                 flen->src_pid_len = 2;
 154         }
 155
 156         /* Set PAN ID compression bit if src pan id matches dest pan id. */
 157         if (p->fcf.dest_addr_mode & 3 && p->fcf.src_addr_mode & 3 &&
 158             p->src_pid == p->dest_pid) {
 159                 p->fcf.panid_compression = 1;
 160
 161                 /* compressed header, only do dest pid */
 162                 flen->src_pid_len = 0;
 163         } else {
 164                 p->fcf.panid_compression = 0;
 165         }
 166
 167         /* determine address lengths */
 168         flen->dest_addr_len = addr_len(p->fcf.dest_addr_mode & 3);
 169         flen->src_addr_len = addr_len(p->fcf.src_addr_mode & 3);
 170
 171 #if LLSEC802154_SECURITY_LEVEL
 172         /* Aux security header */
 173         if (p->fcf.security_enabled & 1) {
 174                 flen->aux_sec_len = 5
 175 #if LLSEC802154_USES_EXPLICIT_KEYS
 176                     + get_key_id_len(p->aux_hdr.security_control.key_id_mode);
 177 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 178                 ;
 179         }
 180 #endif /* LLSEC802154_SECURITY_LEVEL */
 181 }
 182 /*----------------------------------------------------------------------------*/
 183 /**
 184  *   \brief Calculates the length of the frame header.  This function is
 185  *   meant to be called by a higher level function, that interfaces to a MAC.
 186  *
 187  *   \param p Pointer to frame802154_t_t struct, which specifies the
 188  *   frame to send.
 189  *
 190  *   \return The length of the frame header.
 191  */
 192 int
 193 frame802154_hdrlen(frame802154_t *p)
 194 {
 195         field_length_t flen;
 196         field_len(p, &flen);
 197         return 3 + flen.dest_pid_len + flen.dest_addr_len +
 198                flen.src_pid_len + flen.src_addr_len + flen.aux_sec_len;
 199 }
 200 /*----------------------------------------------------------------------------*/
 201 /**
 202  *   \brief Creates a frame for transmission over the air.  This function is
 203  *   meant to be called by a higher level function, that interfaces to a MAC.
 204  *
 205  *   \param p Pointer to frame802154_t struct, which specifies the
 206  *   frame to send.
 207  *
 208  *   \param buf Pointer to the buffer to use for the frame.
 209  *
 210  *   \return The length of the frame header
 211  */
 212 int
 213 frame802154_create(frame802154_t *p, uint8_t *buf)
 214 {
 215         int c;
 216         field_length_t flen;
 217         uint8_t pos;
 218 #if LLSEC802154_USES_EXPLICIT_KEYS
 219         uint8_t key_id_mode;
 220 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 221
 222         field_len(p, &flen);
 223
 224         /* OK, now we have field lengths.  Time to actually construct */
 225         /* the outgoing frame, and store it in buf */
 226         buf[0] = (uint8_t)((p->fcf.frame_type & 7) |
 227             ((p->fcf.security_enabled & 1) << 3) |
 228             ((p->fcf.frame_pending & 1) << 4) |
 229             ((p->fcf.ack_required & 1) << 5) |
 230             ((p->fcf.panid_compression & 1) << 6));
 231         buf[1] = (uint8_t)(((p->fcf.dest_addr_mode & 3) << 2) |
 232             ((p->fcf.frame_version & 3) << 4) |
 233             ((p->fcf.src_addr_mode & 3) << 6));
 234
 235         /* sequence number */
 236         buf[2] = p->seq;
 237         pos = 3;
 238
 239         /* Destination PAN ID */
 240         if (flen.dest_pid_len == 2) {
 241                 buf[pos++] = p->dest_pid & 0xff;
 242                 buf[pos++] = (p->dest_pid >> 8) & 0xff;
 243         }
 244
 245         /* Destination address */
 246         for (c = flen.dest_addr_len; c > 0; c--) {
 247                 buf[pos++] = p->dest_addr[c - 1];
 248         }
 249
 250         /* Source PAN ID */
 251         if (flen.src_pid_len == 2) {
 252                 buf[pos++] = p->src_pid & 0xff;
 253                 buf[pos++] = (p->src_pid >> 8) & 0xff;
 254         }
 255
 256         /* Source address */
 257         for (c = flen.src_addr_len; c > 0; c--) {
 258                 buf[pos++] = p->src_addr[c - 1];
 259         }
 260
 261 #if LLSEC802154_SECURITY_LEVEL
 262         /* Aux header */
 263         if (flen.aux_sec_len) {
 264                 buf[pos++] = p->aux_hdr.security_control.security_level
 265 #if LLSEC802154_USES_EXPLICIT_KEYS
 266                     | (p->aux_hdr.security_control.key_id_mode << 3)
 267 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 268                 ;
 269                 memcpy(buf + pos, p->aux_hdr.frame_counter.u8, 4);
 270                 pos += 4;
 271
 272 #if LLSEC802154_USES_EXPLICIT_KEYS
 273                 key_id_mode = p->aux_hdr.security_control.key_id_mode;
 274                 if (key_id_mode) {
 275                         c = (key_id_mode - 1) * 4;
 276                         memcpy(buf + pos, p->aux_hdr.key_source.u8, c);
 277                         pos += c;
 278                         buf[pos++] = p->aux_hdr.key_index;
 279                 }
 280 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 281         }
 282 #endif /* LLSEC802154_SECURITY_LEVEL */
 283
 284         return (int)pos;
 285 }
 286 /*----------------------------------------------------------------------------*/
 287 /**
 288  *   \brief Parses an input frame.  Scans the input frame to find each
 289  *   section, and stores the information of each section in a
 290  *   frame802154_t structure.
 291  *
 292  *   \param data The input data from the radio chip.
 293  *   \param len The size of the input data
 294  *   \param pf The frame802154_t struct to store the parsed frame information.
 295  */
 296 size_t
 297 frame802154_parse(uint8_t *data, size_t len, frame802154_t *pf, uint8_t **payload)
 298 {
 299         uint8_t *p;
 300         frame802154_fcf_t fcf;
 301         size_t c;
 302 #if LLSEC802154_USES_EXPLICIT_KEYS
 303         uint8_t key_id_mode;
 304 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 305
 306         if (len < 3) {
 307                 return 0;
 308         }
 309
 310         p = data;
 311
 312         /* decode the FCF */
 313         fcf.frame_type = p[0] & 7;
 314         fcf.security_enabled = (p[0] >> 3) & 1;
 315         fcf.frame_pending = (p[0] >> 4) & 1;
 316         fcf.ack_required = (p[0] >> 5) & 1;
 317         fcf.panid_compression = (p[0] >> 6) & 1;
 318
 319         fcf.dest_addr_mode = (p[1] >> 2) & 3;
 320         fcf.frame_version = (p[1] >> 4) & 3;
 321         fcf.src_addr_mode = (p[1] >> 6) & 3;
 322
 323         /* copy fcf and seqNum */
 324         memcpy(&pf->fcf, &fcf, sizeof(frame802154_fcf_t));
 325         pf->seq = p[2];
 326         p += 3;                             /* Skip first three bytes */
 327
 328         /* Destination address, if any */
 329         if (fcf.dest_addr_mode) {
 330                 /* Destination PAN */
 331                 pf->dest_pid = (uint16_t)(p[0] + (p[1] << 8));
 332                 p += 2;
 333
 334                 /* Destination address */
 335                 /*     l = addr_len(fcf.dest_addr_mode); */
 336                 /*     for(c = 0; c < l; c++) { */
 337                 /*       pf->dest_addr.u8[c] = p[l - c - 1]; */
 338                 /*     } */
 339                 /*     p += l; */
 340                 if (fcf.dest_addr_mode == FRAME802154_SHORTADDRMODE) {
 341                         linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
 342                         pf->dest_addr[0] = p[1];
 343                         pf->dest_addr[1] = p[0];
 344                         p += 2;
 345                 } else if (fcf.dest_addr_mode == FRAME802154_LONGADDRMODE) {
 346                         for (c = 0; c < 8; c++) {
 347                                 pf->dest_addr[c] = p[7 - c];
 348                         }
 349                         p += 8;
 350                 }
 351         } else {
 352                 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
 353                 pf->dest_pid = 0;
 354         }
 355
 356         /* Source address, if any */
 357         if (fcf.src_addr_mode) {
 358                 /* Source PAN */
 359                 if (!fcf.panid_compression) {
 360                         pf->src_pid = (uint16_t)(p[0] + (p[1] << 8));
 361                         p += 2;
 362                 } else {
 363                         pf->src_pid = pf->dest_pid;
 364                 }
 365
 366                 /* Source address */
 367                 /*     l = addr_len(fcf.src_addr_mode); */
 368                 /*     for(c = 0; c < l; c++) { */
 369                 /*       pf->src_addr.u8[c] = p[l - c - 1]; */
 370                 /*     } */
 371                 /*     p += l; */
 372                 if (fcf.src_addr_mode == FRAME802154_SHORTADDRMODE) {
 373                         linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
 374                         pf->src_addr[0] = p[1];
 375                         pf->src_addr[1] = p[0];
 376                         p += 2;
 377                 } else if (fcf.src_addr_mode == FRAME802154_LONGADDRMODE) {
 378                         for (c = 0; c < 8; c++) {
 379                                 pf->src_addr[c] = p[7 - c];
 380                         }
 381                         p += 8;
 382                 }
 383         } else {
 384                 linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
 385                 pf->src_pid = 0;
 386         }
 387
 388 #if LLSEC802154_SECURITY_LEVEL
 389         if (fcf.security_enabled) {
 390                 pf->aux_hdr.security_control.security_level = p[0] & 7;
 391 #if LLSEC802154_USES_EXPLICIT_KEYS
 392                 pf->aux_hdr.security_control.key_id_mode = (p[0] >> 3) & 3;
 393 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 394                 p += 1;
 395
 396                 memcpy(pf->aux_hdr.frame_counter.u8, p, 4);
 397                 p += 4;
 398
 399 #if LLSEC802154_USES_EXPLICIT_KEYS
 400                 key_id_mode = pf->aux_hdr.security_control.key_id_mode;
 401                 if (key_id_mode) {
 402                         c = (key_id_mode - 1) * 4;
 403                         memcpy(pf->aux_hdr.key_source.u8, p, c);
 404                         p += c;
 405                         pf->aux_hdr.key_index = p[0];
 406                         p += 1;
 407                 }
 408 #endif /* LLSEC802154_USES_EXPLICIT_KEYS */
 409         }
 410 #endif /* LLSEC802154_SECURITY_LEVEL */
 411
 412         /* header length */
 413         c = p - data;
 414         /* payload length */
 415         pf->payload_len = (int)(len - c);
 416         /* payload */
 417         *payload = p;
 418
 419         /* return header length if successful */
 420         return c > len ? 0 : c;
 421 }
 422 /** \}   */