michaelh/zephyr
1
0
Fork 0
Code Releases Activity
zephyr/subsys/net/l2/ieee802154/ieee802154_frame.c
Miquel Raynal b86b7d3a41 net: l2: ieee802154: Fix address length macros 
When validating a MAC command, there is sometimes a misunderstanding of
what "src" and "dst" length mean.

There are actually two fields in the MHR:
- One giving the type of address, if it is short or extended, it is the
  value provided by the macros IEEE802154_ADDR_MODE_{SHORT,EXTENDED} and
  their respective decimal values are 2 and 3.
- One giving the size of the address field, this is
  IEEE802154_{SHORT,EXT}_ADDR_LENGTH and their value is actually 2 and 8
  (bytes).

The function validate_mac_command() provides inputs to
validate_mac_command_cfi_to_mhr() which expects the former information.

Signed-off-by: Miquel Raynal <miquel.raynal@bootlin.com>
2023-01-02 10:54:46 +01:00

1000 lines
26 KiB
C
Raw Blame History

/*
* Copyright (c) 2016 Intel Corporation.
*
* SPDX-License-Identifier: Apache-2.0
*/
/**
* @file
* @brief IEEE 802.15.4 MAC frame related functions implementation
*/
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(net_ieee802154_frame, CONFIG_NET_L2_IEEE802154_LOG_LEVEL);
#include "ieee802154_frame.h"
#include "ieee802154_security.h"
#include <zephyr/net/net_core.h>
#include <zephyr/net/net_if.h>
#include <ipv6.h>
#include <nbr.h>
#define dbg_print_fs(fs) \
NET_DBG("fs(1): %u/%u/%u/%u/%u/%u", fs->fc.frame_type, fs->fc.security_enabled, \
fs->fc.frame_pending, fs->fc.ar, fs->fc.pan_id_comp, fs->fc.reserved); \
NET_DBG("fs(2): %u/%u/%u/%u/%u - %u", fs->fc.seq_num_suppr, fs->fc.ie_list, \
fs->fc.dst_addr_mode, fs->fc.frame_version, fs->fc.src_addr_mode, fs->sequence)
#define BUF_TIMEOUT K_MSEC(50)
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
const uint8_t level_2_tag_size[4] = {0, IEEE8021254_AUTH_TAG_LENGTH_32,
IEEE8021254_AUTH_TAG_LENGTH_64,
IEEE8021254_AUTH_TAG_LENGTH_128};
#endif
struct ieee802154_fcf_seq *ieee802154_validate_fc_seq(uint8_t *buf, uint8_t **p_buf,
uint8_t *length)
{
struct ieee802154_fcf_seq *fs = (struct ieee802154_fcf_seq *)buf;
dbg_print_fs(fs);
/** Basic FC checks */
if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_RESERVED ||
fs->fc.frame_version >= IEEE802154_VERSION_RESERVED) {
return NULL;
}
if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MULTIPURPOSE) {
if (fs->fc.frame_version != 0) {
return NULL;
}
} else {
/** Only for versions 2003/2006 */
if (fs->fc.frame_version < IEEE802154_VERSION_802154 &&
(fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_RESERVED ||
fs->fc.frame_type >= IEEE802154_FRAME_TYPE_RESERVED)) {
return NULL;
}
}
if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_BEACON &&
(fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE ||
fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE || fs->fc.pan_id_comp)) {
/** See section 7.2.2.1.1 */
return NULL;
} else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_DATA &&
fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE &&
fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
/** See section 7.2.2.2.1 */
return NULL;
} else if (fs->fc.frame_type == IEEE802154_FRAME_TYPE_MAC_COMMAND && fs->fc.frame_pending) {
/** See section 7.3 */
return NULL;
}
#ifndef CONFIG_NET_L2_IEEE802154_SECURITY
if (fs->fc.security_enabled) {
return NULL;
}
#endif
if (p_buf) {
*length -= IEEE802154_FCF_SEQ_LENGTH;
*p_buf = buf + IEEE802154_FCF_SEQ_LENGTH;
}
return fs;
}
static inline bool validate_addr(uint8_t *buf, uint8_t **p_buf, uint8_t *length,
enum ieee802154_addressing_mode mode, bool pan_id_compression,
struct ieee802154_address_field **addr)
{
uint8_t len = 0;
*p_buf = buf;
NET_DBG("Buf %p - mode %d - pan id comp %d", buf, mode, pan_id_compression);
if (mode == IEEE802154_ADDR_MODE_NONE) {
*addr = NULL;
return true;
}
if (!pan_id_compression) {
len = IEEE802154_PAN_ID_LENGTH;
}
if (mode == IEEE802154_ADDR_MODE_SHORT) {
len += IEEE802154_SHORT_ADDR_LENGTH;
} else {
/* IEEE802154_ADDR_MODE_EXTENDED */
len += IEEE802154_EXT_ADDR_LENGTH;
}
if (len > *length) {
return false;
}
*p_buf += len;
*length -= len;
*addr = (struct ieee802154_address_field *)buf;
return true;
}
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
struct ieee802154_aux_security_hdr *
ieee802154_validate_aux_security_hdr(uint8_t *buf, uint8_t **p_buf, uint8_t *length)
{
struct ieee802154_aux_security_hdr *ash = (struct ieee802154_aux_security_hdr *)buf;
uint8_t len = IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
/* At least the asf is sized of: control field + frame counter */
if (*length < len) {
return NULL;
}
/* Only implicit key mode is supported for now */
if (ash->control.key_id_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
return NULL;
}
/* Explicit key must have a key index != 0x00, see section 7.6.2.4.2 */
switch (ash->control.key_id_mode) {
case IEEE802154_KEY_ID_MODE_IMPLICIT:
break;
case IEEE802154_KEY_ID_MODE_INDEX:
len += IEEE8021254_KEY_ID_FIELD_INDEX_LENGTH;
if (*length < len) {
return NULL;
}
if (!ash->kif.mode_1.key_index) {
return NULL;
}
break;
case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
len += IEEE8021254_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
if (*length < len) {
return NULL;
}
if (!ash->kif.mode_2.key_index) {
return NULL;
}
break;
case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
len += IEEE8021254_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
if (*length < len) {
return NULL;
}
if (!ash->kif.mode_3.key_index) {
return NULL;
}
break;
}
*p_buf = buf + len;
*length -= len;
return ash;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
static inline bool validate_beacon(struct ieee802154_mpdu *mpdu, uint8_t *buf, uint8_t *length)
{
struct ieee802154_beacon *b = (struct ieee802154_beacon *)buf;
struct ieee802154_pas_spec *pas;
uint8_t len = IEEE802154_BEACON_SF_SIZE + IEEE802154_BEACON_GTS_SPEC_SIZE;
if (*length < len) {
return false;
}
if (b->gts.desc_count) {
len += IEEE802154_BEACON_GTS_DIR_SIZE +
b->gts.desc_count * IEEE802154_BEACON_GTS_SIZE;
}
if (*length < len) {
return false;
}
pas = (struct ieee802154_pas_spec *)buf + len;
len += IEEE802154_BEACON_PAS_SPEC_SIZE;
if (*length < len) {
return false;
}
if (pas->nb_sap || pas->nb_eap) {
len += (pas->nb_sap * IEEE802154_SHORT_ADDR_LENGTH) +
(pas->nb_eap * IEEE802154_EXT_ADDR_LENGTH);
}
if (*length < len) {
return false;
}
*length -= len;
mpdu->beacon = b;
return true;
}
static inline bool validate_mac_command_cfi_to_mhr(struct ieee802154_mhr *mhr, uint8_t ar,
uint8_t comp, uint8_t src,
bool src_pan_brdcst_chk, uint8_t dst,
bool dst_brdcst_chk)
{
if (mhr->fs->fc.ar != ar || mhr->fs->fc.pan_id_comp != comp) {
return false;
}
if ((mhr->fs->fc.src_addr_mode != src) || (mhr->fs->fc.dst_addr_mode != dst)) {
return false;
}
/* This should be set only when comp == 0 */
if (src_pan_brdcst_chk) {
if (mhr->src_addr->plain.pan_id != IEEE802154_BROADCAST_PAN_ID) {
return false;
}
}
/* This should be set only when comp == 0 */
if (dst_brdcst_chk) {
/* broadcast address is symmetric so no need to swap byte order */
if (mhr->dst_addr->plain.addr.short_addr != IEEE802154_BROADCAST_ADDRESS) {
return false;
}
}
return true;
}
static inline bool validate_mac_command(struct ieee802154_mpdu *mpdu, uint8_t *buf, uint8_t *length)
{
struct ieee802154_command *c = (struct ieee802154_command *)buf;
uint8_t len = IEEE802154_CMD_CFI_LENGTH;
bool src_pan_brdcst_chk = false;
bool dst_brdcst_chk = false;
uint8_t comp = 0U;
uint8_t ar = 0U;
uint8_t src, dst;
if (*length < len) {
return false;
}
switch (c->cfi) {
case IEEE802154_CFI_UNKNOWN:
return false;
case IEEE802154_CFI_ASSOCIATION_REQUEST:
len += IEEE802154_CMD_ASSOC_REQ_LENGTH;
src = IEEE802154_ADDR_MODE_EXTENDED;
src_pan_brdcst_chk = true;
dst = IEEE802154_ADDR_MODE_SHORT | IEEE802154_ADDR_MODE_EXTENDED;
break;
case IEEE802154_CFI_ASSOCIATION_RESPONSE:
len += IEEE802154_CMD_ASSOC_RES_LENGTH;
__fallthrough;
case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
if (c->cfi == IEEE802154_CFI_DISASSOCIATION_NOTIFICATION) {
len += IEEE802154_CMD_DISASSOC_NOTE_LENGTH;
}
__fallthrough;
case IEEE802154_CFI_PAN_ID_CONLICT_NOTIFICATION:
ar = 1U;
comp = 1U;
src = IEEE802154_ADDR_MODE_EXTENDED;
dst = IEEE802154_ADDR_MODE_EXTENDED;
break;
case IEEE802154_CFI_DATA_REQUEST:
ar = 1U;
src = IEEE802154_ADDR_MODE_SHORT | IEEE802154_ADDR_MODE_EXTENDED;
if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_NONE) {
dst = IEEE802154_ADDR_MODE_NONE;
} else {
comp = 1U;
dst = IEEE802154_ADDR_MODE_SHORT | IEEE802154_ADDR_MODE_EXTENDED;
}
break;
case IEEE802154_CFI_ORPHAN_NOTIFICATION:
comp = 1U;
src = IEEE802154_ADDR_MODE_EXTENDED;
dst = IEEE802154_ADDR_MODE_SHORT;
break;
case IEEE802154_CFI_BEACON_REQUEST:
src = IEEE802154_ADDR_MODE_NONE;
dst = IEEE802154_ADDR_MODE_SHORT;
dst_brdcst_chk = true;
break;
case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
len += IEEE802154_CMD_COORD_REALIGN_LENGTH;
src = IEEE802154_ADDR_MODE_EXTENDED;
if (mpdu->mhr.fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
dst = IEEE802154_ADDR_MODE_SHORT;
dst_brdcst_chk = true;
} else {
dst = IEEE802154_ADDR_MODE_EXTENDED;
}
break;
case IEEE802154_CFI_GTS_REQUEST:
len += IEEE802154_GTS_REQUEST_LENGTH;
ar = 1U;
src = IEEE802154_ADDR_MODE_SHORT;
dst = IEEE802154_ADDR_MODE_NONE;
break;
default:
return false;
}
if (*length < len) {
return false;
}
if (!validate_mac_command_cfi_to_mhr(&mpdu->mhr, ar, comp, src, src_pan_brdcst_chk, dst,
dst_brdcst_chk)) {
return false;
}
*length -= len;
mpdu->command = c;
return true;
}
static inline bool validate_payload_and_mfr(struct ieee802154_mpdu *mpdu, uint8_t *buf,
uint8_t *p_buf, uint8_t *length)
{
uint8_t type = mpdu->mhr.fs->fc.frame_type;
NET_DBG("Header size: %u, payload size %u", (uint32_t)(p_buf - buf), *length);
if (type == IEEE802154_FRAME_TYPE_BEACON) {
if (!validate_beacon(mpdu, p_buf, length)) {
return false;
}
} else if (type == IEEE802154_FRAME_TYPE_DATA) {
/** A data frame embeds a payload */
if (*length == 0U) {
return false;
}
mpdu->payload = (void *)p_buf;
} else if (type == IEEE802154_FRAME_TYPE_ACK) {
/** An ACK frame has no payload */
if (*length) {
return false;
}
mpdu->payload = NULL;
} else {
if (!validate_mac_command(mpdu, p_buf, length)) {
return false;
}
}
if (*length) {
mpdu->mfr = (struct ieee802154_mfr *)(p_buf + *length);
} else {
mpdu->mfr = NULL;
}
return true;
}
bool ieee802154_validate_frame(uint8_t *buf, uint8_t length, struct ieee802154_mpdu *mpdu)
{
uint8_t *p_buf = NULL;
if (length > IEEE802154_MTU || length < IEEE802154_MIN_LENGTH) {
NET_DBG("Wrong packet length: %d", length);
return false;
}
mpdu->mhr.fs = ieee802154_validate_fc_seq(buf, &p_buf, &length);
if (!mpdu->mhr.fs) {
return false;
}
/* TODO: Support later version's frame types */
if (mpdu->mhr.fs->fc.frame_type > IEEE802154_FRAME_TYPE_MAC_COMMAND) {
return false;
}
if (!validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.dst_addr_mode, false,
&mpdu->mhr.dst_addr) ||
!validate_addr(p_buf, &p_buf, &length, mpdu->mhr.fs->fc.src_addr_mode,
(mpdu->mhr.fs->fc.pan_id_comp), &mpdu->mhr.src_addr)) {
return false;
}
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
if (mpdu->mhr.fs->fc.security_enabled) {
mpdu->mhr.aux_sec = ieee802154_validate_aux_security_hdr(p_buf, &p_buf, &length);
if (!mpdu->mhr.aux_sec) {
return false;
}
}
#endif
return validate_payload_and_mfr(mpdu, buf, p_buf, &length);
}
uint8_t ieee802154_compute_header_and_authtag_size(struct net_if *iface, struct net_linkaddr *dst,
struct net_linkaddr *src)
{
bool broadcast = !dst->addr;
uint8_t hdr_len = sizeof(struct ieee802154_fcf_seq);
/* PAN ID */
hdr_len += IEEE802154_PAN_ID_LENGTH;
/* Destination Address - see get_dst_addr_mode() */
hdr_len += broadcast ? IEEE802154_SHORT_ADDR_LENGTH : dst->len;
/* Source Address - see data_addr_to_fs_settings() */
hdr_len += src->addr ? src->len : dst->len;
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
if (broadcast) {
NET_DBG("Broadcast packets are not being encrypted.");
goto done;
}
struct ieee802154_context *ctx = (struct ieee802154_context *)net_if_l2_data(iface);
k_sem_take(&ctx->ctx_lock, K_FOREVER);
struct ieee802154_security_ctx *sec_ctx =
&ctx->sec_ctx;
if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
goto release;
}
/* Compute aux-sec hdr size and add it to hdr_len */
hdr_len += IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
switch (sec_ctx->key_mode) {
case IEEE802154_KEY_ID_MODE_IMPLICIT:
/* The only mode supported for now,
* generate_aux_securiy_hdr() will fail on other modes
*/
break;
case IEEE802154_KEY_ID_MODE_INDEX:
hdr_len += IEEE8021254_KEY_ID_FIELD_INDEX_LENGTH;
break;
case IEEE802154_KEY_ID_MODE_SRC_4_INDEX:
hdr_len += IEEE8021254_KEY_ID_FIELD_SRC_4_INDEX_LENGTH;
break;
case IEEE802154_KEY_ID_MODE_SRC_8_INDEX:
hdr_len += IEEE8021254_KEY_ID_FIELD_SRC_8_INDEX_LENGTH;
}
/* This is a _HACK_: As net_buf does not allow to reserve tailroom
* - here for authentication tag (see section 7.6.3.4.3) - it "reserves"
* it in headroom so the payload won't occupy all the left space
* and then when it will come to finalize the data frame it will
* reduce the reserved space by the tag size, move the payload
* foward accordingly, and only then: run the encryption/authentication
* which will fill the tag space in the end.
*/
if (sec_ctx->level < IEEE802154_SECURITY_LEVEL_ENC) {
hdr_len += level_2_tag_size[sec_ctx->level];
} else {
hdr_len += level_2_tag_size[sec_ctx->level - 4U];
}
release:
k_sem_give(&ctx->ctx_lock);
done:
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
NET_DBG("Computed size of %u", hdr_len);
return hdr_len;
}
static inline struct ieee802154_fcf_seq *generate_fcf_grounds(uint8_t **p_buf, bool ack)
{
struct ieee802154_fcf_seq *fs;
fs = (struct ieee802154_fcf_seq *)*p_buf;
fs->fc.security_enabled = 0U;
fs->fc.frame_pending = 0U;
fs->fc.ar = ack;
fs->fc.pan_id_comp = 0U;
fs->fc.reserved = 0U;
/* We support version 2006 only for now */
fs->fc.seq_num_suppr = 0U;
fs->fc.ie_list = 0U;
fs->fc.frame_version = IEEE802154_VERSION_802154_2006;
*p_buf += sizeof(struct ieee802154_fcf_seq);
return fs;
}
static inline enum ieee802154_addressing_mode get_dst_addr_mode(struct net_linkaddr *dst,
bool *broadcast)
{
if (!dst->addr) {
NET_DBG("Broadcast destination");
*broadcast = true;
return IEEE802154_ADDR_MODE_SHORT;
}
if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
uint16_t short_addr = ntohs(*(uint16_t *)(dst->addr));
*broadcast = (short_addr == IEEE802154_BROADCAST_ADDRESS);
return IEEE802154_ADDR_MODE_SHORT;
} else {
*broadcast = false;
}
if (dst->len == IEEE802154_EXT_ADDR_LENGTH) {
return IEEE802154_ADDR_MODE_EXTENDED;
}
return IEEE802154_ADDR_MODE_NONE;
}
static inline bool data_addr_to_fs_settings(struct net_linkaddr *dst, struct ieee802154_fcf_seq *fs,
struct ieee802154_frame_params *params)
{
bool broadcast;
fs->fc.dst_addr_mode = get_dst_addr_mode(dst, &broadcast);
if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
fs->fc.pan_id_comp = 1U;
if (broadcast) {
params->dst.short_addr = IEEE802154_BROADCAST_ADDRESS;
params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
fs->fc.ar = 0U;
} else if (dst->len == IEEE802154_SHORT_ADDR_LENGTH) {
params->dst.short_addr = ntohs(*(uint16_t *)(dst->addr));
params->dst.len = IEEE802154_SHORT_ADDR_LENGTH;
} else {
__ASSERT_NO_MSG(dst->len == IEEE802154_EXT_ADDR_LENGTH);
params->dst.ext_addr = dst->addr;
params->dst.len = IEEE802154_EXT_ADDR_LENGTH;
}
}
if (params->short_addr) {
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
} else {
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
}
return broadcast;
}
static uint8_t *generate_addressing_fields(struct ieee802154_context *ctx,
struct ieee802154_fcf_seq *fs,
struct ieee802154_frame_params *params, uint8_t *p_buf)
{
struct ieee802154_address_field *af;
/* destination address */
if (fs->fc.dst_addr_mode != IEEE802154_ADDR_MODE_NONE) {
af = (struct ieee802154_address_field *)p_buf;
af->plain.pan_id = params->dst.pan_id;
p_buf += IEEE802154_PAN_ID_LENGTH;
if (fs->fc.dst_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
af->plain.addr.short_addr = sys_cpu_to_le16(params->dst.short_addr);
p_buf += IEEE802154_SHORT_ADDR_LENGTH;
} else {
sys_memcpy_swap(af->plain.addr.ext_addr, params->dst.ext_addr,
IEEE802154_EXT_ADDR_LENGTH);
p_buf += IEEE802154_EXT_ADDR_LENGTH;
}
}
/* source address */
if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_NONE) {
return p_buf;
}
af = (struct ieee802154_address_field *)p_buf;
struct ieee802154_address *src_addr;
if (fs->fc.pan_id_comp) {
src_addr = &af->comp.addr;
} else {
af->plain.pan_id = params->pan_id;
src_addr = &af->plain.addr;
p_buf += IEEE802154_PAN_ID_LENGTH;
}
if (fs->fc.src_addr_mode == IEEE802154_ADDR_MODE_SHORT) {
src_addr->short_addr = sys_cpu_to_le16(params->short_addr);
p_buf += IEEE802154_SHORT_ADDR_LENGTH;
} else {
memcpy(src_addr->ext_addr, ctx->ext_addr, IEEE802154_EXT_ADDR_LENGTH);
p_buf += IEEE802154_EXT_ADDR_LENGTH;
}
return p_buf;
}
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
static uint8_t *generate_aux_security_hdr(struct ieee802154_security_ctx *sec_ctx, uint8_t *p_buf)
{
struct ieee802154_aux_security_hdr *aux_sec;
if (sec_ctx->level == IEEE802154_SECURITY_LEVEL_NONE) {
return p_buf;
}
if (sec_ctx->key_mode != IEEE802154_KEY_ID_MODE_IMPLICIT) {
/* TODO: it supports implicit mode only, for now */
return NULL;
}
aux_sec = (struct ieee802154_aux_security_hdr *)p_buf;
aux_sec->control.security_level = sec_ctx->level;
aux_sec->control.key_id_mode = sec_ctx->key_mode;
aux_sec->control.reserved = 0U;
aux_sec->frame_counter = sys_cpu_to_le32(sec_ctx->frame_counter);
return p_buf + IEEE802154_SECURITY_CF_LENGTH + IEEE802154_SECURITY_FRAME_COUNTER_LENGTH;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
bool ieee802154_create_data_frame(struct ieee802154_context *ctx, struct net_linkaddr *dst,
struct net_linkaddr *src, struct net_buf *buf, uint8_t hdr_len)
{
struct ieee802154_frame_params params = {0};
struct ieee802154_fcf_seq *fs;
uint8_t *p_buf = buf->data;
uint8_t *buf_start = p_buf;
bool ret = false;
bool broadcast;
k_sem_take(&ctx->ctx_lock, K_FOREVER);
fs = generate_fcf_grounds(&p_buf, ctx->ack_requested);
fs->fc.frame_type = IEEE802154_FRAME_TYPE_DATA;
fs->sequence = ctx->sequence++;
params.dst.pan_id = ctx->pan_id;
params.pan_id = ctx->pan_id;
if (src->addr && src->len == IEEE802154_SHORT_ADDR_LENGTH) {
params.short_addr = ntohs(*(uint16_t *)(src->addr));
if (ctx->short_addr != params.short_addr) {
goto out;
}
} else {
if (src->len != IEEE802154_EXT_ADDR_LENGTH) {
goto out;
}
uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];
sys_memcpy_swap(ext_addr_le, src->addr, IEEE802154_EXT_ADDR_LENGTH);
if (memcmp(ctx->ext_addr, ext_addr_le, src->len)) {
goto out;
}
}
broadcast = data_addr_to_fs_settings(dst, fs, &params);
p_buf = generate_addressing_fields(ctx, fs, &params, p_buf);
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
if (broadcast) {
/* TODO: This may not always be correct. */
NET_DBG("No security hdr needed: broadcasting");
goto no_security_hdr;
}
if (ctx->sec_ctx.level == IEEE802154_SECURITY_LEVEL_NONE) {
NET_WARN("IEEE 802.15.4 security is enabled but has not been configured.");
goto no_security_hdr;
}
fs->fc.security_enabled = 1U;
p_buf = generate_aux_security_hdr(&ctx->sec_ctx, p_buf);
if (!p_buf) {
NET_ERR("Unsupported key mode.");
goto out;
}
uint8_t payload_len = buf->len - hdr_len;
uint8_t level = ctx->sec_ctx.level;
if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
level -= 4U;
}
uint8_t tag_size = level_2_tag_size[level];
if (tag_size > 0) {
/* If tagged, let's create tailroom for the tag by moving the payload left,
*see comment in ieee802154_compute_header_and_authtag_size().
*/
memmove(p_buf, buf_start + hdr_len, payload_len);
hdr_len -= tag_size;
}
/* Let's encrypt/auth only in the end, if needed */
if (!ieee802154_encrypt_auth(broadcast ? NULL : &ctx->sec_ctx, buf_start, hdr_len,
payload_len, tag_size, ctx->ext_addr)) {
goto out;
};
no_security_hdr:
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
if ((p_buf - buf_start) != hdr_len) {
/* hdr_len was too small? We probably overwrote payload bytes */
NET_ERR("Could not generate data frame %zu vs %u", (p_buf - buf_start), hdr_len);
goto out;
}
dbg_print_fs(fs);
ret = true;
out:
k_sem_give(&ctx->ctx_lock);
return ret;
}
#ifdef CONFIG_NET_L2_IEEE802154_RFD
static inline bool cfi_to_fs_settings(enum ieee802154_cfi cfi, struct ieee802154_fcf_seq *fs,
struct ieee802154_frame_params *params)
{
switch (cfi) {
case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
fs->fc.ar = 1U;
fs->fc.pan_id_comp = 1U;
__fallthrough;
case IEEE802154_CFI_ASSOCIATION_REQUEST:
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
if (params->dst.len == IEEE802154_SHORT_ADDR_LENGTH) {
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
} else {
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
}
break;
case IEEE802154_CFI_ASSOCIATION_RESPONSE:
case IEEE802154_CFI_PAN_ID_CONLICT_NOTIFICATION:
fs->fc.ar = 1U;
fs->fc.pan_id_comp = 1U;
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
break;
case IEEE802154_CFI_DATA_REQUEST:
fs->fc.ar = 1U;
/* TODO: src/dst addr mode: see section 7.3.4 */
break;
case IEEE802154_CFI_ORPHAN_NOTIFICATION:
fs->fc.pan_id_comp = 1U;
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
break;
case IEEE802154_CFI_BEACON_REQUEST:
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_NONE;
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_SHORT;
break;
case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_EXTENDED;
/* TODO: ar and dst addr mode: see section 7.3.8 */
break;
case IEEE802154_CFI_GTS_REQUEST:
fs->fc.ar = 1U;
fs->fc.src_addr_mode = IEEE802154_ADDR_MODE_SHORT;
fs->fc.dst_addr_mode = IEEE802154_ADDR_MODE_NONE;
break;
default:
return false;
}
return true;
}
static inline uint8_t mac_command_length(enum ieee802154_cfi cfi)
{
uint8_t length = 1U; /* cfi is at least present */
switch (cfi) {
case IEEE802154_CFI_ASSOCIATION_REQUEST:
case IEEE802154_CFI_DISASSOCIATION_NOTIFICATION:
case IEEE802154_CFI_GTS_REQUEST:
length += 1U;
break;
case IEEE802154_CFI_ASSOCIATION_RESPONSE:
length += 3U;
break;
case IEEE802154_CFI_COORDINATOR_REALIGNEMENT:
length += 8U;
break;
default:
break;
}
return length;
}
struct net_pkt *ieee802154_create_mac_cmd_frame(struct net_if *iface, enum ieee802154_cfi type,
struct ieee802154_frame_params *params)
{
struct ieee802154_context *ctx = net_if_l2_data(iface);
struct ieee802154_fcf_seq *fs;
struct net_pkt *pkt = NULL;
uint8_t *p_buf, *p_start;
k_sem_take(&ctx->ctx_lock, K_FOREVER);
/* It would be costly to compute the size when actual frame are never
* bigger than 125 bytes, so let's allocate that size as buffer.
*/
pkt = net_pkt_alloc_with_buffer(iface, IEEE802154_MTU, AF_UNSPEC, 0, BUF_TIMEOUT);
if (!pkt) {
goto out;
}
p_buf = net_pkt_data(pkt);
p_start = p_buf;
fs = generate_fcf_grounds(
&p_buf, type == IEEE802154_CFI_BEACON_REQUEST ? false : ctx->ack_requested);
fs->fc.frame_type = IEEE802154_FRAME_TYPE_MAC_COMMAND;
fs->sequence = ctx->sequence;
if (!cfi_to_fs_settings(type, fs, params)) {
goto error;
}
p_buf = generate_addressing_fields(ctx, fs, params, p_buf);
net_buf_add(pkt->buffer, p_buf - p_start);
/* Let's insert the cfi */
((struct ieee802154_command *)p_buf)->cfi = type;
dbg_print_fs(fs);
goto out;
error:
net_pkt_unref(pkt);
pkt = NULL;
out:
k_sem_give(&ctx->ctx_lock);
return pkt;
}
void ieee802154_mac_cmd_finalize(struct net_pkt *pkt, enum ieee802154_cfi type)
{
net_buf_add(pkt->buffer, mac_command_length(type));
}
#endif /* CONFIG_NET_L2_IEEE802154_RFD */
#ifdef CONFIG_NET_L2_IEEE802154_ACK_REPLY
bool ieee802154_create_ack_frame(struct net_if *iface, struct net_pkt *pkt, uint8_t seq)
{
uint8_t *p_buf = net_pkt_data(pkt);
struct ieee802154_fcf_seq *fs;
if (!p_buf) {
return false;
}
fs = generate_fcf_grounds(&p_buf, false);
fs->fc.dst_addr_mode = 0U;
fs->fc.src_addr_mode = 0U;
fs->fc.frame_type = IEEE802154_FRAME_TYPE_ACK;
fs->sequence = seq;
net_buf_add(pkt->buffer, IEEE802154_ACK_PKT_LENGTH);
return true;
}
#endif /* CONFIG_NET_L2_IEEE802154_ACK_REPLY */
#ifdef CONFIG_NET_L2_IEEE802154_SECURITY
bool ieee802154_decipher_data_frame(struct net_if *iface, struct net_pkt *pkt,
struct ieee802154_mpdu *mpdu)
{
struct ieee802154_context *ctx = net_if_l2_data(iface);
bool ret = false;
k_sem_take(&ctx->ctx_lock, K_FOREVER);
uint8_t level = ctx->sec_ctx.level;
if (!mpdu->mhr.fs->fc.security_enabled) {
ret = true;
goto out;
}
/* Section 7.2.3 (i) talks about "security level policy" conformance
* but such policy does not seem to be detailed. So let's assume both
* ends should have same security level.
*/
if (mpdu->mhr.aux_sec->control.security_level != level) {
goto out;
}
if (level >= IEEE802154_SECURITY_LEVEL_ENC) {
level -= 4U;
}
uint8_t tag_size = level_2_tag_size[level];
uint8_t hdr_len = (uint8_t *)mpdu->payload - net_pkt_data(pkt);
uint8_t payload_len = net_pkt_get_len(pkt) - hdr_len - tag_size;
uint8_t ext_addr_le[IEEE802154_EXT_ADDR_LENGTH];
/* TODO: Handle src short address.
* This will require to look up in nbr cache with short addr
* in order to get the extended address related to it.
*/
if (net_pkt_lladdr_src(pkt)->len != IEEE802154_EXT_ADDR_LENGTH) {
NET_ERR("Decrypting packages with short source addresses is not supported.");
goto out;
}
sys_memcpy_swap(ext_addr_le, net_pkt_lladdr_src(pkt)->addr, net_pkt_lladdr_src(pkt)->len);
if (!ieee802154_decrypt_auth(&ctx->sec_ctx, net_pkt_data(pkt), hdr_len, payload_len,
tag_size, ext_addr_le,
sys_le32_to_cpu(mpdu->mhr.aux_sec->frame_counter))) {
NET_ERR("Could not decipher the frame");
goto out;
}
/* We remove tag size from buf's length, it is now useless. */
pkt->buffer->len -= tag_size;
ret = true;
out:
k_sem_give(&ctx->ctx_lock);
return ret;
}
#endif /* CONFIG_NET_L2_IEEE802154_SECURITY */
View git blame Copy permalink