[apple/xnu.git] / bsd / net / frame802154.c

/*
 * Copyright (c) 2017 Apple Inc. All rights reserved.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_START@
 *
 * This file contains Original Code and/or Modifications of Original Code
 * as defined in and that are subject to the Apple Public Source License
 * Version 2.0 (the 'License'). You may not use this file except in
 * compliance with the License. The rights granted to you under the License
 * may not be used to create, or enable the creation or redistribution of,
 * unlawful or unlicensed copies of an Apple operating system, or to
 * circumvent, violate, or enable the circumvention or violation of, any
 * terms of an Apple operating system software license agreement.
 *
 * Please obtain a copy of the License at
 * http://www.opensource.apple.com/apsl/ and read it before using this file.
 *
 * The Original Code and all software distributed under the License are
 * distributed on an 'AS IS' basis, WITHOUT WARRANTY OF ANY KIND, EITHER
 * EXPRESS OR IMPLIED, AND APPLE HEREBY DISCLAIMS ALL SUCH WARRANTIES,
 * INCLUDING WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE, QUIET ENJOYMENT OR NON-INFRINGEMENT.
 * Please see the License for the specific language governing rights and
 * limitations under the License.
 *
 * @APPLE_OSREFERENCE_LICENSE_HEADER_END@
 */
/*
 *
 *  Copyright (c) 2008, Swedish Institute of Computer Science
 *  All rights reserved.
 *
 *  Additional fixes for AVR contributed by:
 *
 *      Colin O'Flynn coflynn@newae.com
 *      Eric Gnoske egnoske@gmail.com
 *      Blake Leverett bleverett@gmail.com
 *      Mike Vidales mavida404@gmail.com
 *      Kevin Brown kbrown3@uccs.edu
 *      Nate Bohlmann nate@elfwerks.com
 *
 *  Additional fixes for MSP430 contributed by:
 *        Joakim Eriksson
 *        Niclas Finne
 *        Nicolas Tsiftes
 *
 *   All rights reserved.
 *
 *   Redistribution and use in source and binary forms, with or without
 *   modification, are permitted provided that the following conditions are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in
 *     the documentation and/or other materials provided with the
 *     distribution.
 *   * Neither the name of the copyright holders nor the names of
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 *  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 *  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 *  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 *  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 *  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 *  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 *  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *  \brief This file is where the main functions that relate to frame
 *  manipulation will reside.
 */

/**
 *  \file
 *  \brief 802.15.4 frame creation and parsing functions
 *
 *  This file converts to and from a structure to a packed 802.15.4
 *  frame.
 */

/**
 *   \addtogroup frame802154
 *   @{
 */

#include "cc.h"
#include "frame802154.h"
//#include "net/llsec/llsec802154.h"
#include "linkaddr.h"
#include <string.h>

/**
 *  \brief Structure that contains the lengths of the various addressing and security fields
 *  in the 802.15.4 header.  This structure is used in \ref frame802154_create()
 */
typedef struct {
	uint8_t dest_pid_len;    /**<  Length (in bytes) of destination PAN ID field */
	uint8_t dest_addr_len;   /**<  Length (in bytes) of destination address field */
	uint8_t src_pid_len;     /**<  Length (in bytes) of source PAN ID field */
	uint8_t src_addr_len;    /**<  Length (in bytes) of source address field */
	uint8_t aux_sec_len;     /**<  Length (in bytes) of aux security header field */
} field_length_t;

/*----------------------------------------------------------------------------*/
CC_INLINE static uint8_t
addr_len(uint8_t mode)
{
	switch (mode) {
	case FRAME802154_SHORTADDRMODE:          /* 16-bit address */
		return 2;
	case FRAME802154_LONGADDRMODE:           /* 64-bit address */
		return 8;
	default:
		return 0;
	}
}
/*----------------------------------------------------------------------------*/
#if LLSEC802154_USES_EXPLICIT_KEYS
static uint8_t
get_key_id_len(uint8_t key_id_mode)
{
	switch (key_id_mode) {
	case FRAME802154_1_BYTE_KEY_ID_MODE:
		return 1;
	case FRAME802154_5_BYTE_KEY_ID_MODE:
		return 5;
	case FRAME802154_9_BYTE_KEY_ID_MODE:
		return 9;
	default:
		return 0;
	}
}
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
/*----------------------------------------------------------------------------*/
static void
field_len(frame802154_t *p, field_length_t *flen)
{
	/* init flen to zeros */
	memset(flen, 0, sizeof(field_length_t));

	/* Determine lengths of each field based on fcf and other args */
	if (p->fcf.dest_addr_mode & 3) {
		flen->dest_pid_len = 2;
	}
	if (p->fcf.src_addr_mode & 3) {
		flen->src_pid_len = 2;
	}

	/* Set PAN ID compression bit if src pan id matches dest pan id. */
	if (p->fcf.dest_addr_mode & 3 && p->fcf.src_addr_mode & 3 &&
	    p->src_pid == p->dest_pid) {
		p->fcf.panid_compression = 1;

		/* compressed header, only do dest pid */
		flen->src_pid_len = 0;
	} else {
		p->fcf.panid_compression = 0;
	}

	/* determine address lengths */
	flen->dest_addr_len = addr_len(p->fcf.dest_addr_mode & 3);
	flen->src_addr_len = addr_len(p->fcf.src_addr_mode & 3);

#if LLSEC802154_SECURITY_LEVEL
	/* Aux security header */
	if (p->fcf.security_enabled & 1) {
		flen->aux_sec_len = 5
#if LLSEC802154_USES_EXPLICIT_KEYS
		    + get_key_id_len(p->aux_hdr.security_control.key_id_mode);
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
		;
	}
#endif /* LLSEC802154_SECURITY_LEVEL */
}
/*----------------------------------------------------------------------------*/
/**
 *   \brief Calculates the length of the frame header.  This function is
 *   meant to be called by a higher level function, that interfaces to a MAC.
 *
 *   \param p Pointer to frame802154_t_t struct, which specifies the
 *   frame to send.
 *
 *   \return The length of the frame header.
 */
int
frame802154_hdrlen(frame802154_t *p)
{
	field_length_t flen;
	field_len(p, &flen);
	return 3 + flen.dest_pid_len + flen.dest_addr_len +
	       flen.src_pid_len + flen.src_addr_len + flen.aux_sec_len;
}
/*----------------------------------------------------------------------------*/
/**
 *   \brief Creates a frame for transmission over the air.  This function is
 *   meant to be called by a higher level function, that interfaces to a MAC.
 *
 *   \param p Pointer to frame802154_t struct, which specifies the
 *   frame to send.
 *
 *   \param buf Pointer to the buffer to use for the frame.
 *
 *   \return The length of the frame header
 */
int
frame802154_create(frame802154_t *p, uint8_t *buf)
{
	int c;
	field_length_t flen;
	uint8_t pos;
#if LLSEC802154_USES_EXPLICIT_KEYS
	uint8_t key_id_mode;
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */

	field_len(p, &flen);

	/* OK, now we have field lengths.  Time to actually construct */
	/* the outgoing frame, and store it in buf */
	buf[0] = (p->fcf.frame_type & 7) |
	    ((p->fcf.security_enabled & 1) << 3) |
	    ((p->fcf.frame_pending & 1) << 4) |
	    ((p->fcf.ack_required & 1) << 5) |
	    ((p->fcf.panid_compression & 1) << 6);
	buf[1] = ((p->fcf.dest_addr_mode & 3) << 2) |
	    ((p->fcf.frame_version & 3) << 4) |
	    ((p->fcf.src_addr_mode & 3) << 6);

	/* sequence number */
	buf[2] = p->seq;
	pos = 3;

	/* Destination PAN ID */
	if (flen.dest_pid_len == 2) {
		buf[pos++] = p->dest_pid & 0xff;
		buf[pos++] = (p->dest_pid >> 8) & 0xff;
	}

	/* Destination address */
	for (c = flen.dest_addr_len; c > 0; c--) {
		buf[pos++] = p->dest_addr[c - 1];
	}

	/* Source PAN ID */
	if (flen.src_pid_len == 2) {
		buf[pos++] = p->src_pid & 0xff;
		buf[pos++] = (p->src_pid >> 8) & 0xff;
	}

	/* Source address */
	for (c = flen.src_addr_len; c > 0; c--) {
		buf[pos++] = p->src_addr[c - 1];
	}

#if LLSEC802154_SECURITY_LEVEL
	/* Aux header */
	if (flen.aux_sec_len) {
		buf[pos++] = p->aux_hdr.security_control.security_level
#if LLSEC802154_USES_EXPLICIT_KEYS
		    | (p->aux_hdr.security_control.key_id_mode << 3)
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
		;
		memcpy(buf + pos, p->aux_hdr.frame_counter.u8, 4);
		pos += 4;

#if LLSEC802154_USES_EXPLICIT_KEYS
		key_id_mode = p->aux_hdr.security_control.key_id_mode;
		if (key_id_mode) {
			c = (key_id_mode - 1) * 4;
			memcpy(buf + pos, p->aux_hdr.key_source.u8, c);
			pos += c;
			buf[pos++] = p->aux_hdr.key_index;
		}
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
	}
#endif /* LLSEC802154_SECURITY_LEVEL */

	return (int)pos;
}
/*----------------------------------------------------------------------------*/
/**
 *   \brief Parses an input frame.  Scans the input frame to find each
 *   section, and stores the information of each section in a
 *   frame802154_t structure.
 *
 *   \param data The input data from the radio chip.
 *   \param len The size of the input data
 *   \param pf The frame802154_t struct to store the parsed frame information.
 */
int
frame802154_parse(uint8_t *data, int len, frame802154_t *pf, uint8_t **payload)
{
	uint8_t *p;
	frame802154_fcf_t fcf;
	int c;
#if LLSEC802154_USES_EXPLICIT_KEYS
	uint8_t key_id_mode;
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */

	if (len < 3) {
		return 0;
	}

	p = data;

	/* decode the FCF */
	fcf.frame_type = p[0] & 7;
	fcf.security_enabled = (p[0] >> 3) & 1;
	fcf.frame_pending = (p[0] >> 4) & 1;
	fcf.ack_required = (p[0] >> 5) & 1;
	fcf.panid_compression = (p[0] >> 6) & 1;

	fcf.dest_addr_mode = (p[1] >> 2) & 3;
	fcf.frame_version = (p[1] >> 4) & 3;
	fcf.src_addr_mode = (p[1] >> 6) & 3;

	/* copy fcf and seqNum */
	memcpy(&pf->fcf, &fcf, sizeof(frame802154_fcf_t));
	pf->seq = p[2];
	p += 3;                             /* Skip first three bytes */

	/* Destination address, if any */
	if (fcf.dest_addr_mode) {
		/* Destination PAN */
		pf->dest_pid = p[0] + (p[1] << 8);
		p += 2;

		/* Destination address */
		/*     l = addr_len(fcf.dest_addr_mode); */
		/*     for(c = 0; c < l; c++) { */
		/*       pf->dest_addr.u8[c] = p[l - c - 1]; */
		/*     } */
		/*     p += l; */
		if (fcf.dest_addr_mode == FRAME802154_SHORTADDRMODE) {
			linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
			pf->dest_addr[0] = p[1];
			pf->dest_addr[1] = p[0];
			p += 2;
		} else if (fcf.dest_addr_mode == FRAME802154_LONGADDRMODE) {
			for (c = 0; c < 8; c++) {
				pf->dest_addr[c] = p[7 - c];
			}
			p += 8;
		}
	} else {
		linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->dest_addr), &linkaddr_null);
		pf->dest_pid = 0;
	}

	/* Source address, if any */
	if (fcf.src_addr_mode) {
		/* Source PAN */
		if (!fcf.panid_compression) {
			pf->src_pid = p[0] + (p[1] << 8);
			p += 2;
		} else {
			pf->src_pid = pf->dest_pid;
		}

		/* Source address */
		/*     l = addr_len(fcf.src_addr_mode); */
		/*     for(c = 0; c < l; c++) { */
		/*       pf->src_addr.u8[c] = p[l - c - 1]; */
		/*     } */
		/*     p += l; */
		if (fcf.src_addr_mode == FRAME802154_SHORTADDRMODE) {
			linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
			pf->src_addr[0] = p[1];
			pf->src_addr[1] = p[0];
			p += 2;
		} else if (fcf.src_addr_mode == FRAME802154_LONGADDRMODE) {
			for (c = 0; c < 8; c++) {
				pf->src_addr[c] = p[7 - c];
			}
			p += 8;
		}
	} else {
		linkaddr_copy((linkaddr_t *)(uintptr_t)&(pf->src_addr), &linkaddr_null);
		pf->src_pid = 0;
	}

#if LLSEC802154_SECURITY_LEVEL
	if (fcf.security_enabled) {
		pf->aux_hdr.security_control.security_level = p[0] & 7;
#if LLSEC802154_USES_EXPLICIT_KEYS
		pf->aux_hdr.security_control.key_id_mode = (p[0] >> 3) & 3;
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
		p += 1;

		memcpy(pf->aux_hdr.frame_counter.u8, p, 4);
		p += 4;

#if LLSEC802154_USES_EXPLICIT_KEYS
		key_id_mode = pf->aux_hdr.security_control.key_id_mode;
		if (key_id_mode) {
			c = (key_id_mode - 1) * 4;
			memcpy(pf->aux_hdr.key_source.u8, p, c);
			p += c;
			pf->aux_hdr.key_index = p[0];
			p += 1;
		}
#endif /* LLSEC802154_USES_EXPLICIT_KEYS */
	}
#endif /* LLSEC802154_SECURITY_LEVEL */

	/* header length */
	c = p - data;
	/* payload length */
	pf->payload_len = (len - c);
	/* payload */
	*payload = p;

	/* return header length if successful */
	return c > len ? 0 : c;
}
/** \}   */
Commit	Line	Data
cb323159 A	1	/*
	2	* Copyright (c) 2017 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] = (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] = ((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	int
297	frame802154_parse(uint8_t data, int len, frame802154_t pf, uint8_t **payload)
298	{
299	uint8_t *p;
300	frame802154_fcf_t fcf;
301	int 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 = 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 = 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 = (len - c);
416	/* payload */
417	*payload = p;
418
419	/* return header length if successful */
420	return c > len ? 0 : c;
421	}
422	/** \} */