void *zb_buf_cut_left(zb_buf_t *zbbuf, zb_uint8_t size)
{
ZB_ASSERT(ZB_BUF_LEN(zbbuf) >= (size)); /* ++VS */
(zbbuf)->u.hdr.len -= (size);
(zbbuf)->u.hdr.data_offset += (size);
return (void *)ZB_BUF_BEGIN(zbbuf);
}
void zb_mac_store_pan_desc(zb_buf_t *beacon_buf)
{
zb_uint8_t *mac_hdr = ZB_MAC_GET_FCF_PTR(ZB_BUF_BEGIN(beacon_buf));
zb_mac_mhr_t mhr;
zb_uint8_t mhr_len;
zb_pan_descriptor_t pan_desc;
zb_pan_descriptor_t *pan_desc_buf;
zb_buf_t *desc_list_buf;
zb_uint8_t desc_count;
TRACE_MSG(TRACE_NWK1, ">>store_pan_desc %p", (FMT__P, beacon_buf));
mhr_len = zb_parse_mhr(&mhr, mac_hdr);
TRACE_MSG(TRACE_NWK3, "add pan desc", (FMT__0));
pan_desc.coord_addr_mode = ZB_FCF_GET_SRC_ADDRESSING_MODE(&mhr.frame_control);
pan_desc.coord_pan_id = mhr.src_pan_id;
ZB_MEMCPY(&pan_desc.coord_address, &mhr.src_addr, sizeof(union zb_addr_u));
pan_desc.logical_channel = ZB_MAC_GET_CURRENT_LOGICAL_CHANNEL();
ZB_GET_SUPERFRAME(mac_hdr, mhr_len, &pan_desc.super_frame_spec);
pan_desc.gts_permit = 0; /* use ZB_MAC_GET_GTS_FIELDS() to get exact gts value.
Zigbee uses beaconless mode, so gts is not used always */
pan_desc.link_quality = ZB_MAC_GET_LQI(beacon_buf);
if (MAC_CTX().rt_ctx.active_scan.pan_desc_buf_param == ZB_UNDEFINED_BUFFER)
{
desc_list_buf = beacon_buf;
ZB_BUF_REUSE(desc_list_buf);
MAC_CTX().rt_ctx.active_scan.pan_desc_buf_param = ZB_REF_FROM_BUF(beacon_buf);
}
else
{
desc_list_buf = ZB_BUF_FROM_REF(MAC_CTX().rt_ctx.active_scan.pan_desc_buf_param);
}
/* do not calculate pan descriptors number - it can be calculated using buffer length */
/* in this check take into account size of scan confirm structure - descriptors will follow it */
desc_count = ZB_BUF_LEN(desc_list_buf) / sizeof(zb_pan_descriptor_t);
if ( (ZB_BUF_GET_FREE_SIZE(desc_list_buf) >= (sizeof(zb_pan_descriptor_t) + sizeof(zb_mac_scan_confirm_t))) &&
desc_count < ZB_ACTIVE_SCAN_MAX_PAN_DESC_COUNT)
{
ZB_BUF_ALLOC_RIGHT(desc_list_buf, sizeof(zb_pan_descriptor_t), pan_desc_buf);
ZB_MEMCPY(pan_desc_buf, &pan_desc, sizeof(zb_pan_descriptor_t));
}
else
{
TRACE_MSG(TRACE_NWK3, "stop scan, no free space", (FMT__0));
MAC_CTX().rt_ctx.active_scan.stop_scan = 1;
}
TRACE_MSG(TRACE_NWK1, "<<store_pan_desc", (FMT__0));
}
void zb_aps_send_command(zb_uint8_t param, zb_uint16_t dest_addr, zb_uint8_t command, zb_bool_t secure)
{
zb_buf_t *buf = (zb_buf_t *)ZB_BUF_FROM_REF(param);
zb_ushort_t need_ack = !ZB_NWK_IS_ADDRESS_BROADCAST(dest_addr);
#ifdef ZB_DISABLE_APS_ACK_REQ
/* Specially for test with Daintree: do not ask ACK for key transport:
Daintree wants to encrypt ACK by its predefined key before it receive key from us */
if (command == APS_CMD_TRANSPORT_KEY)
{
need_ack = 0;
}
#endif
TRACE_MSG(TRACE_SECUR3, ">>zb_aps_send_command param %hd cmd %hd secur %hd to %d need_ack %hd", (FMT__H_H_H_D_H, param, command, secure, dest_addr, need_ack));
if (need_ack)
{
zb_uint8_t i;
for (i = 0 ; i < ZB_N_APS_RETRANS_ENTRIES ; ++i)
{
if (ZG->aps.retrans.hash[i].state == ZB_APS_RETRANS_ENT_FREE)
{
ZG->aps.retrans.hash[i].addr = dest_addr;
ZG->aps.retrans.hash[i].aps_counter = ZB_AIB_APS_COUNTER();
ZG->aps.retrans.hash[i].buf = param;
ZG->aps.retrans.hash[i].nwk_insecure = !secure;
ZG->aps.retrans.hash[i].aps_retries = ZB_N_APS_MAX_FRAME_ENTRIES;
ZG->aps.retrans.hash[i].state = ZB_APS_RETRANS_ENT_SENT_MAC_NOT_CONFIRMED_ALRM_RUNNING;
TRACE_MSG(TRACE_APS2, "Store buf %hd len %hd in retrans hash %d", (FMT__H_H, param, ZB_BUF_LEN(buf), i));
DUMP_TRAF("sending aps cmd", ZB_BUF_BEGIN(buf), ZB_BUF_LEN(buf));
break;
}
}
if (i == ZB_N_APS_RETRANS_ENTRIES)
{
TRACE_MSG(TRACE_APS2, "ACK table is FULL", (FMT__0));
}
else
{
ZB_SCHEDULE_ALARM(zb_aps_ack_timer_cb, i, ZB_N_APS_ACK_WAIT_DURATION);
}
}
/* Fill APS command header - see 2.2.5.2.2 APS Command Frame Format.
At the same time alloc and fill aux security header
*/
{
zb_aps_command_header_t *hdr;
#ifdef ZB_SECURITY
#ifdef APS_FRAME_SECURITY
buf->u.hdr.encrypt_type = ZB_SECUR_NO_ENCR;
if (secure)
{
/* Allocate here space for APS command header, aux header and command id
* (it is in payload). */
secure = zb_aps_command_add_secur(buf, command);
hdr = (zb_aps_command_header_t *)ZB_BUF_BEGIN(buf);
}
else
#endif
#endif
{
/* no security - just aps command header */
ZB_BUF_ALLOC_LEFT(buf, sizeof (*hdr), hdr);
hdr->fc = 0;
hdr->aps_command_id = command;
}
ZB_APS_FC_SET_COMMAND(hdr->fc, need_ack);
hdr->aps_counter = ZB_AIB_APS_COUNTER();
}
ZB_AIB_APS_COUNTER_INC();
fill_nldereq(param, dest_addr, secure);
TRACE_MSG(TRACE_SECUR3, "send APS cmd %hd secur %hd to %d", (FMT__H_H_D, command, secure, dest_addr));
ZB_SCHEDULE_CALLBACK(zb_nlde_data_request, param);
}
zb_ret_t zb_aps_unsecure_frame(zb_buf_t *buf)
{
zb_ret_t ret = RET_OK;
zb_uint8_t *aps_hdr;
zb_aps_nwk_aux_frame_hdr_t *aux;
zb_ushort_t a_size;
zb_uint8_t *key;
zb_uint8_t *payload;
zb_address_ieee_ref_t addr_ref;
{
zb_apsde_data_indication_t * ind = ZB_GET_BUF_PARAM(buf, zb_apsde_data_indication_t);
ret = zb_address_by_short(ind->src_addr, ZB_FALSE, ZB_FALSE, &addr_ref);
if (ret != RET_OK)
{
TRACE_MSG(TRACE_SECUR3, "can't get addr %d", (FMT__D, ind->src_addr));
}
}
if (ret == RET_OK)
{
aps_hdr = ZB_BUF_BEGIN(buf);
aux = (zb_aps_nwk_aux_frame_hdr_t *)(aps_hdr + ZB_APS_HDR_SIZE(*aps_hdr));
a_size = ((zb_uint8_t *)aux - aps_hdr);
if (ZB_SECUR_AUX_HDR_GET_KEY_TYPE(aux->secur_control) == ZB_SECUR_DATA_KEY)
{
aux->secur_control = ZB_APS_DATA_STD_SECUR_CONTROL;
payload = (zb_uint8_t *)aux + sizeof(zb_aps_data_aux_frame_hdr_t);
/* TODO: implement Data key */
/* obtain secure material. */
/* check frame counters */
key = 0;
a_size += sizeof(zb_aps_data_aux_frame_hdr_t);
}
else
{
/* NWK key */
zb_neighbor_tbl_ent_t *nbe;
/* Update security level which was zeroed before send */
aux->secur_control = ZB_APS_NWK_STD_SECUR_CONTROL;
payload = (zb_uint8_t *)aux + sizeof(zb_aps_nwk_aux_frame_hdr_t);
key = secur_nwk_key_by_seq(aux->key_seq_number);
a_size += sizeof(zb_aps_nwk_aux_frame_hdr_t);
/* Get neighbor table entry by source address. */
ret = zb_nwk_neighbor_get(addr_ref, ZB_TRUE, &nbe);
if (ret == RET_OK)
{
if (key && nbe->key_seq_number != aux->key_seq_number)
{
/* Peer now use another nwk key */
nbe->incoming_frame_counter = 0;
nbe->key_seq_number = aux->key_seq_number;
TRACE_MSG(TRACE_SECUR3, "peer switched key", (FMT__0));
}
{
/* Check NWK FrameCounter */
zb_uint32_t frame_counter;
ZB_LETOH32(&frame_counter, &aux->frame_counter);
if (nbe->incoming_frame_counter > frame_counter
|| nbe->incoming_frame_counter == (zb_uint32_t)~0)
{
ret = RET_ERROR;
TRACE_MSG(TRACE_SECUR3, "frm cnt %ld->%ld shift back", (FMT__L_L,
nbe->incoming_frame_counter, frame_counter));
}
else
{
nbe->incoming_frame_counter = frame_counter;
}
}
}
else
{
TRACE_MSG(TRACE_SECUR3, "can't get neighbor", (FMT__0));
}
}
}
if (ret == RET_OK && !key)
{
/* set 'frame security failed' */
ret = RET_ERROR;
TRACE_MSG(TRACE_SECUR3, "no key by seq %hd", (FMT__H, aux->key_seq_number));
}
if (ret == RET_OK)
{
/* decrypt */
zb_secur_ccm_nonce_t nonce;
zb_address_ieee_by_ref(nonce.source_address, addr_ref);
nonce.frame_counter = aux->frame_counter;
nonce.secur_control = aux->secur_control;
ret = zb_ccm_decrypt_n_auth_stdsecur(key,
(zb_uint8_t *)&nonce,
buf, a_size,
(ZB_BUF_BEGIN(buf) + ZB_BUF_LEN(buf)) - (aps_hdr + a_size));
if (ret == RET_OK)
{
/* Remove MIC */
TRACE_MSG(TRACE_SECUR3, "unsecured frm %p[%hd] ok", (FMT__P_H, buf, ZB_BUF_LEN(buf)));
}
else
{
TRACE_MSG(TRACE_SECUR3, "unsecure failed", (FMT__0));
}
}
return ret;
}
void zb_aps_secure_frame(zb_buf_t *src, zb_uint_t mac_hdr_size, zb_buf_t *dst)
{
zb_uint8_t *aps_hdr;
zb_uint8_t *payload;
zb_secur_ccm_nonce_t nonce;
zb_uint8_t *dp;
zb_uint8_t *key;
zb_ushort_t hdrs_size;
zb_aps_nwk_aux_frame_hdr_t *aux;
{
zb_nwk_hdr_t *nwk_hdr = (zb_nwk_hdr_t *)(ZB_BUF_BEGIN(src) + mac_hdr_size);
aps_hdr = (zb_uint8_t *)nwk_hdr + ZB_NWK_HDR_SIZE(nwk_hdr->frame_control);
}
aux = (zb_aps_nwk_aux_frame_hdr_t *)(aps_hdr + ZB_APS_HDR_SIZE(*aps_hdr));
if (ZB_SECUR_AUX_HDR_GET_KEY_TYPE(aux->secur_control) == ZB_SECUR_DATA_KEY)
{
payload = (zb_uint8_t *)aux + sizeof(zb_aps_data_aux_frame_hdr_t);
/* get src and dst address from APS header, get data key */
key = 0; /* aps data key */
}
else
{
/* nwk key */
payload = (zb_uint8_t *)aux + sizeof(zb_aps_nwk_aux_frame_hdr_t);
key = ZG->nwk.nib.secur_material_set[ZG->nwk.nib.active_secur_material_i].key;
}
/* fill nonce - see 4.5.2.2 */
nonce.frame_counter = aux->frame_counter;
nonce.secur_control = aux->secur_control;
ZB_IEEE_ADDR_COPY(nonce.source_address, ZB_PIB_EXTENDED_ADDRESS());
hdrs_size = payload - ZB_BUF_BEGIN(src);
/* Secure */
(void)zb_ccm_encrypt_n_auth(key,
(zb_uint8_t *)&nonce,
(zb_uint8_t *)aps_hdr, (payload - aps_hdr),
(zb_uint8_t *)payload, (ZB_BUF_LEN(src) - hdrs_size),
dst);
ZB_BUF_ALLOC_LEFT(dst, (aps_hdr - ZB_BUF_BEGIN(src)), dp);
/* copy headers */
ZB_MEMCPY(dp, ZB_BUF_BEGIN(src), (aps_hdr - ZB_BUF_BEGIN(src)));
/* clear security level - see 4.4.1.1/11 */
aux = (zb_aps_nwk_aux_frame_hdr_t *)(ZB_BUF_BEGIN(dst) + ((zb_uint8_t*)aux - ZB_BUF_BEGIN(src)));
if (ZB_SECUR_AUX_HDR_GET_KEY_TYPE(aux->secur_control) == ZB_SECUR_DATA_KEY)
{
aux->secur_control = ZB_APS_DATA_STD_SECUR_CONTROL_ZEROED_LEVEL;
}
else
{
aux->secur_control = ZB_APS_NWK_STD_SECUR_CONTROL_ZEROED_LEVEL;
}
TRACE_MSG(TRACE_SECUR3, "secured aps frm %p[%hd] -> %p hdrs_size %hd fcnt %lx", (FMT__P_H_P_H_L,
src, ZB_BUF_LEN(src), dst, hdrs_size, aux->frame_counter));
}
/**
Get buffer tail of size 'size'
Macro usually used to place external information (some parameters) to the
buffer
@param zbbuf - buffer
@param size - requested size
@return pointer to the buffer tail
*/
zb_void_t *zb_get_buf_tail(zb_buf_t *zbbuf, zb_uint8_t size)
{
ZB_ASSERT((zbbuf) && ZB_BUF_LEN(zbbuf) + (zbbuf)->u.hdr.data_offset + (size) <= ZB_IO_BUF_SIZE);
return (void *)((zbbuf)->buf + (ZB_IO_BUF_SIZE - (size)));
}
void zb_buf_cut_right(zb_buf_t *zbbuf, zb_uint8_t size)
{
ZB_ASSERT(ZB_BUF_LEN(zbbuf) >= (size));
(zbbuf)->u.hdr.len -= (size);
}
