You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@mynewt.apache.org by we...@apache.org on 2016/04/16 00:55:00 UTC
[1/2] incubator-mynewt-core git commit: MYNEWT-99: Implement LL
encryption procedure.
Repository: incubator-mynewt-core
Updated Branches:
refs/heads/develop 09e50c6a0 -> 33afb22a2
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/drivers/nrf51/src/ble_phy.c
----------------------------------------------------------------------
diff --git a/net/nimble/drivers/nrf51/src/ble_phy.c b/net/nimble/drivers/nrf51/src/ble_phy.c
index 6c74fd4..4de8e83 100644
--- a/net/nimble/drivers/nrf51/src/ble_phy.c
+++ b/net/nimble/drivers/nrf51/src/ble_phy.c
@@ -32,7 +32,7 @@
#define NRF_RADIO_IRQ_MASK_ALL (0x34FF)
/*
- * We configure the nrf52 with a 1 byte S0 field, 8 bit length field, and
+ * We configure the nrf with a 1 byte S0 field, 8 bit length field, and
* zero bit S1 field. The preamble is 8 bits long.
*/
#define NRF_LFLEN_BITS (8)
@@ -103,8 +103,8 @@ STATS_NAME_END(ble_phy_stats)
* NOTE:
* Tested the following to see what would happen:
* -> NVIC has radio irq enabled (interrupt # 1, mask 0x2).
- * -> Set up nrf52 to receive. Clear ADDRESS event register.
- * -> Enable ADDRESS interrupt on nrf52 by writing to INTENSET.
+ * -> Set up nrf to receive. Clear ADDRESS event register.
+ * -> Enable ADDRESS interrupt on nrf5 by writing to INTENSET.
* -> Enable RX.
* -> Disable interrupts globally using OS_ENTER_CRITICAL().
* -> Wait until a packet is received and the ADDRESS event occurs.
@@ -159,7 +159,7 @@ ble_phy_rxpdu_get(void)
}
static void
-nrf52_wait_disabled(void)
+nrf_wait_disabled(void)
{
uint32_t state;
@@ -438,7 +438,7 @@ int
ble_phy_rx(void)
{
/* Check radio state */
- nrf52_wait_disabled();
+ nrf_wait_disabled();
if (NRF_RADIO->STATE != RADIO_STATE_STATE_Disabled) {
ble_phy_disable();
STATS_INC(ble_phy_stats, radio_state_errs);
@@ -506,7 +506,7 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t beg_trans, uint8_t end_trans)
assert(txpdu != NULL);
/* If radio is not disabled, */
- nrf52_wait_disabled();
+ nrf_wait_disabled();
if (beg_trans == BLE_PHY_TRANSITION_RX_TX) {
if ((NRF_RADIO->SHORTS & RADIO_SHORTS_DISABLED_TXEN_Msk) == 0) {
@@ -661,7 +661,7 @@ ble_phy_setchan(uint8_t chan, uint32_t access_addr, uint32_t crcinit)
return BLE_PHY_ERR_INV_PARAM;
}
- /* Get correct nrf52 frequency */
+ /* Get correct frequency */
if (chan < BLE_PHY_NUM_DATA_CHANS) {
if (chan < 11) {
/* Data channel 0 starts at 2404. 0 - 10 are contiguous */
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/drivers/nrf52/src/ble_hw.c
----------------------------------------------------------------------
diff --git a/net/nimble/drivers/nrf52/src/ble_hw.c b/net/nimble/drivers/nrf52/src/ble_hw.c
index 36d5ac7..aacd983 100644
--- a/net/nimble/drivers/nrf52/src/ble_hw.c
+++ b/net/nimble/drivers/nrf52/src/ble_hw.c
@@ -6,7 +6,7 @@
* to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance
* with the License. You may obtain a copy of the License at
- *
+ *
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing,
@@ -37,8 +37,8 @@ ble_rng_isr_cb_t g_ble_rng_isr_cb;
/**
* Clear the whitelist
- *
- * @return int
+ *
+ * @return int
*/
void
ble_hw_whitelist_clear(void)
@@ -48,11 +48,11 @@ ble_hw_whitelist_clear(void)
}
/**
- * Add a device to the hw whitelist
- *
- * @param addr
- * @param addr_type
- *
+ * Add a device to the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
* @return int 0: success, BLE error code otherwise
*/
int
@@ -80,11 +80,11 @@ ble_hw_whitelist_add(uint8_t *addr, uint8_t addr_type)
}
/**
- * Remove a device from the hw whitelist
- *
- * @param addr
- * @param addr_type
- *
+ * Remove a device from the hw whitelist
+ *
+ * @param addr
+ * @param addr_type
+ *
*/
void
ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
@@ -126,8 +126,8 @@ ble_hw_whitelist_rmv(uint8_t *addr, uint8_t addr_type)
}
/**
- * Returns the size of the whitelist in HW
- *
+ * Returns the size of the whitelist in HW
+ *
* @return int Number of devices allowed in whitelist
*/
uint8_t
@@ -137,7 +137,7 @@ ble_hw_whitelist_size(void)
}
/**
- * Enable the whitelisted devices
+ * Enable the whitelisted devices
*/
void
ble_hw_whitelist_enable(void)
@@ -147,7 +147,7 @@ ble_hw_whitelist_enable(void)
}
/**
- * Disables the whitelisted devices
+ * Disables the whitelisted devices
*/
void
ble_hw_whitelist_disable(void)
@@ -157,10 +157,10 @@ ble_hw_whitelist_disable(void)
}
/**
- * Boolean function which returns true ('1') if there is a match on the
- * whitelist.
- *
- * @return int
+ * Boolean function which returns true ('1') if there is a match on the
+ * whitelist.
+ *
+ * @return int
*/
int
ble_hw_whitelist_match(void)
@@ -173,9 +173,12 @@ int
ble_hw_encrypt_block(struct ble_encryption_block *ecb)
{
int rc;
+ uint32_t end;
+ uint32_t err;
/* Stop ECB */
NRF_ECB->TASKS_STOPECB = 1;
+ /* XXX: does task stop clear these counters? Anyway to do this quicker? */
NRF_ECB->EVENTS_ENDECB = 0;
NRF_ECB->EVENTS_ERRORECB = 0;
NRF_ECB->ECBDATAPTR = (uint32_t)ecb;
@@ -184,14 +187,14 @@ ble_hw_encrypt_block(struct ble_encryption_block *ecb)
NRF_ECB->TASKS_STARTECB = 1;
/* Wait till error or done */
+ rc = 0;
while (1) {
- if (NRF_ECB->EVENTS_ENDECB != 0) {
- rc = 0;
- break;
- }
-
- if (NRF_ECB->EVENTS_ERRORECB != 0) {
- rc = -1;
+ end = NRF_ECB->EVENTS_ENDECB;
+ err = NRF_ECB->EVENTS_ERRORECB;
+ if (end || err) {
+ if (err) {
+ rc = -1;
+ }
break;
}
}
@@ -225,11 +228,11 @@ ble_rng_isr(void)
/**
* Initialize the random number generator
- *
- * @param cb
- * @param bias
- *
- * @return int
+ *
+ * @param cb
+ * @param bias
+ *
+ * @return int
*/
int
ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
@@ -254,8 +257,8 @@ ble_hw_rng_init(ble_rng_isr_cb_t cb, int bias)
/**
* Start the random number generator
- *
- * @return int
+ *
+ * @return int
*/
int
ble_hw_rng_start(void)
@@ -278,8 +281,8 @@ ble_hw_rng_start(void)
/**
* Stop the random generator
- *
- * @return int
+ *
+ * @return int
*/
int
ble_hw_rng_stop(void)
@@ -298,8 +301,8 @@ ble_hw_rng_stop(void)
/**
* Read the random number generator.
- *
- * @return uint8_t
+ *
+ * @return uint8_t
*/
uint8_t
ble_hw_rng_read(void)
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/drivers/nrf52/src/ble_phy.c
----------------------------------------------------------------------
diff --git a/net/nimble/drivers/nrf52/src/ble_phy.c b/net/nimble/drivers/nrf52/src/ble_phy.c
index 92bff8e..2c55d48 100644
--- a/net/nimble/drivers/nrf52/src/ble_phy.c
+++ b/net/nimble/drivers/nrf52/src/ble_phy.c
@@ -18,6 +18,7 @@
*/
#include <stdint.h>
+#include <string.h>
#include <assert.h>
#include "os/os.h"
#include "bsp/cmsis_nvic.h"
@@ -26,6 +27,11 @@
#include "controller/ble_ll.h"
#include "mcu/nrf52_bitfields.h"
+/*
+ * XXX: need to make the copy from mbuf into the PHY data structures 32-bit
+ * copies or we are screwed.
+ */
+
/* XXX: 4) Make sure RF is higher priority interrupt than schedule */
/* To disable all radio interrupts */
@@ -55,6 +61,7 @@ struct ble_phy_obj
uint8_t phy_state;
uint8_t phy_transition;
uint8_t phy_rx_started;
+ uint8_t phy_encrypted;
uint32_t phy_access_address;
struct os_mbuf *rxpdu;
void *txend_arg;
@@ -64,6 +71,9 @@ struct ble_phy_obj g_ble_phy_data;
/* Global transmit/receive buffer */
static uint32_t g_ble_phy_txrx_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+static uint32_t g_ble_phy_enc_buf[(BLE_PHY_MAX_PDU_LEN + 3) / 4];
+#endif
/* Statistics */
STATS_SECT_START(ble_phy_stats)
@@ -128,6 +138,21 @@ STATS_NAME_END(ble_phy_stats)
* bit in the NVIC just to be sure when we disable the PHY.
*/
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+/* NRF requires 43 bytes of scratch for encryption */
+/* XXX: align this? */
+uint32_t g_nrf_encrypt_scratchpad[100];
+struct nrf_ccm_data
+{
+ uint8_t key[16];
+ uint64_t pkt_counter;
+ uint8_t dir_bit;
+ uint8_t iv[8];
+} __attribute__((packed));
+
+struct nrf_ccm_data g_nrf_ccm_data;
+#endif
+
/**
* ble phy rxpdu get
*
@@ -185,6 +210,7 @@ ble_phy_isr(void)
uint32_t irq_en;
uint32_t state;
uint32_t wfr_time;
+ uint8_t *dptr;
struct os_mbuf *rxpdu;
struct ble_mbuf_hdr *ble_hdr;
@@ -197,7 +223,7 @@ ble_phy_isr(void)
assert(g_ble_phy_data.phy_state == BLE_PHY_STATE_TX);
ble_ll_log(BLE_LL_LOG_ID_PHY_TXEND, (g_ble_phy_txrx_buf[0] >> 8) & 0xFF,
- 0, NRF_TIMER0->CC[2]);
+ g_ble_phy_data.phy_encrypted, NRF_TIMER0->CC[2]);
/* Clear events and clear interrupt on disabled event */
NRF_RADIO->EVENTS_DISABLED = 0;
@@ -205,6 +231,19 @@ ble_phy_isr(void)
NRF_RADIO->EVENTS_END = 0;
state = NRF_RADIO->SHORTS;
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ /*
+ * XXX: not sure what to do. We had a HW error during transmission.
+ * For now I just count a stat but continue on like all is good.
+ */
+ if (g_ble_phy_data.phy_encrypted) {
+ if (NRF_CCM->EVENTS_ERROR) {
+ STATS_INC(ble_phy_stats, tx_hw_err);
+ NRF_CCM->EVENTS_ERROR = 0;
+ }
+ }
+#endif
+
transition = g_ble_phy_data.phy_transition;
if (transition == BLE_PHY_TRANSITION_TX_RX) {
/* Clear the rx started flag */
@@ -212,7 +251,23 @@ ble_phy_isr(void)
/* Packet pointer needs to be reset. */
if (g_ble_phy_data.rxpdu != NULL) {
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (g_ble_phy_data.phy_encrypted) {
+ NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+ NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+ NRF_CCM->OUTPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+ NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+ NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | CCM_MODE_MODE_Decryption;
+ NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+ NRF_CCM->SHORTS = 0;
+ NRF_CCM->EVENTS_ENDCRYPT = 0;
+ NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk;
+ } else {
+ NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+ }
+#else
NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+#endif
/* I want to know when 1st byte received (after address) */
NRF_RADIO->BCC = 8; /* in bits */
@@ -322,6 +377,7 @@ ble_phy_isr(void)
assert(NRF_RADIO->EVENTS_RSSIEND != 0);
ble_hdr->rxinfo.rssi = -1 * NRF_RADIO->RSSISAMPLE;
ble_hdr->end_cputime = NRF_TIMER0->CC[2];
+ dptr = g_ble_phy_data.rxpdu->om_data;
/* Count PHY crc errors and valid packets */
crcok = (uint8_t)NRF_RADIO->CRCSTATUS;
@@ -330,11 +386,50 @@ ble_phy_isr(void)
} else {
STATS_INC(ble_phy_stats, rx_valid);
ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_CRC_OK;
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (g_ble_phy_data.phy_encrypted) {
+ /* Only set MIC failure flag if frame is not zero length */
+ if ((dptr[1] != 0) && (NRF_CCM->MICSTATUS == 0)) {
+ ble_hdr->rxinfo.flags |= BLE_MBUF_HDR_F_MIC_FAILURE;
+ }
+
+ /*
+ * XXX: not sure how to deal with this. This should not
+ * be a MIC failure but we should not hand it up. I guess
+ * this is just some form of rx error and that is how we
+ * handle it? For now, just set CRC error flags
+ */
+ if (NRF_CCM->EVENTS_ERROR) {
+ STATS_INC(ble_phy_stats, rx_hw_err);
+ ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+ }
+
+ /*
+ * XXX: This is a total hack work-around for now but I dont
+ * know what else to do. If ENDCRYPT is not set and we are
+ * encrypted we need to not trust this frame and drop it.
+ */
+ if (NRF_CCM->EVENTS_ENDCRYPT == 0) {
+ STATS_INC(ble_phy_stats, rx_hw_err);
+ ble_hdr->rxinfo.flags &= ~BLE_MBUF_HDR_F_CRC_OK;
+ }
+ }
+#endif
}
/* Call Link Layer receive payload function */
rxpdu = g_ble_phy_data.rxpdu;
g_ble_phy_data.rxpdu = NULL;
+
+ /*
+ * XXX: This is a horrible ugly hack to deal with the RAM S1 byte
+ * that is not sent over the air but is present here. Simply move the
+ * data pointer to deal with it. Fix this later. Do this in the nrf52
+ */
+ dptr[2] = dptr[1];
+ dptr[1] = dptr[0];
+ rxpdu->om_data += 1;
+
rc = ble_ll_rx_end(rxpdu, ble_hdr);
if (rc < 0) {
/* Disable the PHY. */
@@ -389,8 +484,10 @@ ble_phy_init(void)
/* Set configuration registers */
NRF_RADIO->MODE = RADIO_MODE_MODE_Ble_1Mbit;
NRF_RADIO->PCNF0 = (NRF_LFLEN_BITS << RADIO_PCNF0_LFLEN_Pos) |
+ RADIO_PCNF0_S1INCL_Msk |
(NRF_S0_LEN << RADIO_PCNF0_S0LEN_Pos) |
(RADIO_PCNF0_PLEN_8bit << RADIO_PCNF0_PLEN_Pos);
+ /* XXX: should maxlen be 251 for encryption? */
NRF_RADIO->PCNF1 = NRF_MAXLEN |
(RADIO_PCNF1_ENDIAN_Little << RADIO_PCNF1_ENDIAN_Pos) |
(NRF_BALEN << RADIO_PCNF1_BALEN_Pos) |
@@ -415,6 +512,14 @@ ble_phy_init(void)
*/
NRF_PPI->CHENSET = PPI_CHEN_CH27_Msk;
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ NRF_CCM->INTENCLR = 0xffffffff;
+ NRF_CCM->SHORTS = CCM_SHORTS_ENDKSGEN_CRYPT_Msk;
+ NRF_CCM->ENABLE = CCM_ENABLE_ENABLE_Enabled;
+ NRF_CCM->EVENTS_ERROR = 0;
+ memset(g_nrf_encrypt_scratchpad, 0, sizeof(g_nrf_encrypt_scratchpad));
+#endif
+
/* Set isr in vector table and enable interrupt */
NVIC_SetPriority(RADIO_IRQn, 0);
NVIC_SetVector(RADIO_IRQn, (uint32_t)ble_phy_isr);
@@ -452,7 +557,15 @@ ble_phy_rx(void)
}
/* Set packet pointer */
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (g_ble_phy_data.phy_encrypted) {
+ NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+ } else {
+ NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+ }
+#else
NRF_RADIO->PACKETPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+#endif
/* Make sure all interrupts are disabled */
NRF_RADIO->INTENCLR = NRF_RADIO_IRQ_MASK_ALL;
@@ -465,6 +578,22 @@ ble_phy_rx(void)
NRF_RADIO->EVENTS_RSSIEND = 0;
NRF_RADIO->EVENTS_DEVMATCH = 0;
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (g_ble_phy_data.phy_encrypted) {
+ NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+ NRF_CCM->OUTPTR = (uint32_t)g_ble_phy_data.rxpdu->om_data;
+ NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+ NRF_CCM->EVENTS_ERROR = 0;
+ NRF_CCM->EVENTS_ENDCRYPT = 0;
+ NRF_CCM->MODE = CCM_MODE_LENGTH_Msk | CCM_MODE_MODE_Decryption;
+ /* XXX: can I just set this once? (i.e per connection)? In other
+ words, only do this during encrypt enable? */
+ NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+ NRF_CCM->SHORTS = 0;
+ NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk;
+ }
+#endif
+
/* I want to know when 1st byte received (after address) */
NRF_RADIO->BCC = 8; /* in bits */
NRF_RADIO->SHORTS = RADIO_SHORTS_END_DISABLE_Msk |
@@ -486,6 +615,46 @@ ble_phy_rx(void)
return 0;
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+/**
+ * Called to enable encryption at the PHY. Note that this state will persist
+ * in the PHY; in other words, if you call this function you have to call
+ * disable so that future PHY transmits/receives will not be encrypted.
+ *
+ * @param pkt_counter
+ * @param iv
+ * @param key
+ * @param is_master
+ */
+void
+ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+ uint8_t is_master)
+{
+ memcpy(g_nrf_ccm_data.key, key, 16);
+ g_nrf_ccm_data.pkt_counter = pkt_counter;
+ memcpy(g_nrf_ccm_data.iv, iv, 8);
+ g_nrf_ccm_data.dir_bit = is_master;
+ g_ble_phy_data.phy_encrypted = 1;
+}
+
+
+void
+ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir)
+{
+ g_nrf_ccm_data.pkt_counter = pkt_counter;
+ g_nrf_ccm_data.dir_bit = dir;
+}
+
+void
+ble_phy_encrypt_disable(void)
+{
+ NRF_PPI->CHENCLR = (PPI_CHEN_CH24_Msk | PPI_CHEN_CH25_Msk);
+ NRF_CCM->TASKS_STOP = 1;
+ NRF_CCM->EVENTS_ERROR = 0;
+ g_ble_phy_data.phy_encrypted = 0;
+}
+#endif
+
void
ble_phy_set_txend_cb(ble_phy_tx_end_func txend_cb, void *arg)
{
@@ -526,15 +695,48 @@ ble_phy_tx(struct os_mbuf *txpdu, uint8_t beg_trans, uint8_t end_trans)
return BLE_PHY_ERR_RADIO_STATE;
}
- /* Write LL header first */
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (g_ble_phy_data.phy_encrypted) {
+ /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
+ ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+ dptr = (uint8_t *)&g_ble_phy_enc_buf[0];
+ dptr[0] = ble_hdr->txinfo.hdr_byte;
+ dptr[1] = ble_hdr->txinfo.pyld_len;
+ dptr[2] = 0;
+ dptr += 3;
+
+ NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+ NRF_CCM->SHORTS = 1;
+ NRF_CCM->INPTR = (uint32_t)&g_ble_phy_enc_buf[0];
+ NRF_CCM->OUTPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+ NRF_CCM->SCRATCHPTR = (uint32_t)&g_nrf_encrypt_scratchpad[0];
+ NRF_CCM->EVENTS_ERROR = 0;
+ NRF_CCM->MODE = CCM_MODE_LENGTH_Msk;
+ NRF_CCM->CNFPTR = (uint32_t)&g_nrf_ccm_data;
+ NRF_PPI->CHENCLR = PPI_CHEN_CH25_Msk;
+ NRF_PPI->CHENSET = PPI_CHEN_CH24_Msk;
+ } else {
+ /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
+ ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+ dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
+ dptr[0] = ble_hdr->txinfo.hdr_byte;
+ dptr[1] = ble_hdr->txinfo.pyld_len;
+ dptr[2] = 0;
+ dptr += 3;
+
+ NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+ }
+#else
+ /* RAM representation has S0, LENGTH and S1 fields. (3 bytes) */
ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
dptr = (uint8_t *)&g_ble_phy_txrx_buf[0];
dptr[0] = ble_hdr->txinfo.hdr_byte;
dptr[1] = ble_hdr->txinfo.pyld_len;
- dptr += 2;
+ dptr[2] = 0;
+ dptr += 3;
- /* Set radio transmit data pointer */
NRF_RADIO->PACKETPTR = (uint32_t)&g_ble_phy_txrx_buf[0];
+#endif
/* Clear the ready, end and disabled events */
NRF_RADIO->EVENTS_READY = 0;
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/host/src/ble_l2cap_sm.c
----------------------------------------------------------------------
diff --git a/net/nimble/host/src/ble_l2cap_sm.c b/net/nimble/host/src/ble_l2cap_sm.c
index 8ff79e0..74ddb91 100644
--- a/net/nimble/host/src/ble_l2cap_sm.c
+++ b/net/nimble/host/src/ble_l2cap_sm.c
@@ -6,7 +6,7 @@
* to you under the Apache License, Version 2.0 (the
* "License"); you may not use this file except in compliance
* with the License. You may obtain a copy of the License at
- *
+ *
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing,
@@ -609,13 +609,13 @@ ble_l2cap_sm_confirm_prepare_args(struct ble_l2cap_sm_proc *proc,
conn = ble_hs_conn_find(proc->fsm_proc.conn_handle);
if (conn != NULL) {
if (proc->flags & BLE_L2CAP_SM_PROC_F_INITIATOR) {
- *iat = BLE_ADDR_TYPE_PUBLIC; /* XXX: Support random addresses. */
+ *iat = BLE_ADDR_TYPE_RANDOM; /* XXX: Support random addresses. */
memcpy(ia, ble_hs_our_dev.public_addr, 6);
*rat = conn->bhc_addr_type;
memcpy(ra, conn->bhc_addr, 6);
} else {
- *rat = BLE_ADDR_TYPE_PUBLIC; /* XXX: Support random addresses. */
+ *rat = BLE_ADDR_TYPE_RANDOM; /* XXX: Support random addresses. */
memcpy(ra, ble_hs_our_dev.public_addr, 6);
*iat = conn->bhc_addr_type;
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/include/nimble/ble.h
----------------------------------------------------------------------
diff --git a/net/nimble/include/nimble/ble.h b/net/nimble/include/nimble/ble.h
index 38343dc..894dff4 100644
--- a/net/nimble/include/nimble/ble.h
+++ b/net/nimble/include/nimble/ble.h
@@ -68,7 +68,7 @@ struct ble_mbuf_hdr_rxinfo
/* Flag definitions for rxinfo */
#define BLE_MBUF_HDR_F_CRC_OK (0x80)
#define BLE_MBUF_HDR_F_DEVMATCH (0x40)
-#define BLE_MBUF_HDR_F_CONN_REQ_TXD (0x20)
+#define BLE_MBUF_HDR_F_MIC_FAILURE (0x20)
#define BLE_MBUF_HDR_F_SCAN_RSP_TXD (0x10)
#define BLE_MBUF_HDR_F_SCAN_RSP_CHK (0x08)
#define BLE_MBUF_HDR_F_RXSTATE_MASK (0x07)
@@ -97,21 +97,24 @@ struct ble_mbuf_hdr
*/
#define BLE_MBUF_PAYLOAD_SIZE (260)
-#define BLE_MBUF_HDR_CRC_OK(hdr) \
+#define BLE_MBUF_HDR_CRC_OK(hdr) \
((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_CRC_OK)
-#define BLE_MBUF_HDR_RX_STATE(hdr) \
+#define BLE_MBUF_HDR_MIC_FAILURE(hdr) \
+ ((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_MIC_FAILURE)
+
+#define BLE_MBUF_HDR_RX_STATE(hdr) \
((hdr)->rxinfo.flags & BLE_MBUF_HDR_F_RXSTATE_MASK)
-#define BLE_MBUF_HDR_PTR(om) \
+#define BLE_MBUF_HDR_PTR(om) \
(struct ble_mbuf_hdr *)((uint8_t *)om + sizeof(struct os_mbuf) + \
sizeof(struct os_mbuf_pkthdr))
/* BLE mbuf overhead per packet header mbuf */
-#define BLE_MBUF_PKTHDR_OVERHEAD \
+#define BLE_MBUF_PKTHDR_OVERHEAD \
(sizeof(struct os_mbuf_pkthdr) + sizeof(struct ble_mbuf_hdr))
-#define BLE_MBUF_MEMBLOCK_OVERHEAD \
+#define BLE_MBUF_MEMBLOCK_OVERHEAD \
(sizeof(struct os_mbuf) + BLE_MBUF_PKTHDR_OVERHEAD)
#define BLE_DEV_ADDR_LEN (6)
[2/2] incubator-mynewt-core git commit: MYNEWT-99: Implement LL
encryption procedure.
Posted by we...@apache.org.
MYNEWT-99: Implement LL encryption procedure.
This is the first cut at LL encryption for the controller. The
feature is currently turned off in the code but I wanted to
commit it so others could take a look. There are a number of
unresolved issues:
1) This does not yet implement the encryption pause procedure.
2) This has yet to be ported to the nrf51.
3) We see occasional HW errors with decryption. We are ignoring
these currently but they need to be explained. We are working
with nordic on this.
Project: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/repo
Commit: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/commit/33afb22a
Tree: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/tree/33afb22a
Diff: http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/diff/33afb22a
Branch: refs/heads/develop
Commit: 33afb22a2ce574c9a50c7afef675846c80ed1b6c
Parents: 09e50c6
Author: William San Filippo <wi...@runtime.io>
Authored: Fri Apr 15 15:52:37 2016 -0700
Committer: William San Filippo <wi...@runtime.io>
Committed: Fri Apr 15 15:54:51 2016 -0700
----------------------------------------------------------------------
apps/bletest/src/main.c | 76 ++-
.../src/arch/cortex_m4/gcc_startup_nrf52.s | 12 +-
hw/bsp/nrf51dk/nrf51dk.ld | 7 +-
.../src/arch/cortex_m0/gcc_startup_nrf51.s | 22 +-
.../src/arch/cortex_m4/gcc_startup_nrf52.s | 12 +-
.../controller/include/controller/ble_ll.h | 12 +
.../controller/include/controller/ble_ll_conn.h | 89 ++-
.../controller/include/controller/ble_ll_ctrl.h | 17 +-
.../controller/include/controller/ble_phy.h | 5 +
net/nimble/controller/src/ble_ll.c | 34 +-
net/nimble/controller/src/ble_ll_adv.c | 4 +
net/nimble/controller/src/ble_ll_conn.c | 428 ++++++++++----
net/nimble/controller/src/ble_ll_conn_hci.c | 106 +++-
net/nimble/controller/src/ble_ll_conn_priv.h | 2 +
net/nimble/controller/src/ble_ll_ctrl.c | 580 +++++++++++++++++--
net/nimble/controller/src/ble_ll_hci.c | 10 +
net/nimble/controller/src/ble_ll_hci_ev.c | 63 ++
net/nimble/controller/src/ble_ll_scan.c | 4 +
net/nimble/controller/src/ble_ll_supp_cmd.c | 4 +
net/nimble/drivers/nrf51/src/ble_phy.c | 14 +-
net/nimble/drivers/nrf52/src/ble_hw.c | 81 +--
net/nimble/drivers/nrf52/src/ble_phy.c | 210 ++++++-
net/nimble/host/src/ble_l2cap_sm.c | 6 +-
net/nimble/include/nimble/ble.h | 15 +-
24 files changed, 1522 insertions(+), 291 deletions(-)
----------------------------------------------------------------------
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/apps/bletest/src/main.c
----------------------------------------------------------------------
diff --git a/apps/bletest/src/main.c b/apps/bletest/src/main.c
index 13083ab..254abcb 100755
--- a/apps/bletest/src/main.c
+++ b/apps/bletest/src/main.c
@@ -77,7 +77,7 @@ uint8_t g_host_adv_data[BLE_HCI_MAX_ADV_DATA_LEN];
uint8_t g_host_adv_len;
/* Create a mbuf pool of BLE mbufs */
-#define MBUF_NUM_MBUFS (32)
+#define MBUF_NUM_MBUFS (16)
#define MBUF_BUF_SIZE OS_ALIGN(BLE_MBUF_PAYLOAD_SIZE, 4)
#define MBUF_MEMBLOCK_SIZE (MBUF_BUF_SIZE + BLE_MBUF_MEMBLOCK_OVERHEAD)
#define MBUF_MEMPOOL_SIZE OS_MEMPOOL_SIZE(MBUF_NUM_MBUFS, MBUF_MEMBLOCK_SIZE)
@@ -145,6 +145,7 @@ uint32_t g_bletest_next_led_time;
uint16_t g_bletest_handle;
uint16_t g_bletest_completed_pkts;
uint16_t g_bletest_outstanding_pkts;
+uint16_t g_bletest_ltk_reply_handle;
/* --- For LE encryption testing --- */
/* Key: 0x4C68384139F574D836BCF34E9DFB01BF */
@@ -168,7 +169,20 @@ const uint8_t g_ble_ll_encrypt_test_encrypted_data[16] =
0x05, 0x8e, 0x3b, 0x8e, 0x27, 0xc2, 0xc6, 0x66
};
-uint8_t g_ble_ll_encrypted_data[16];
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+/* LTK 0x4C68384139F574D836BCF34E9DFB01BF */
+const uint8_t g_bletest_LTK[16] =
+{
+ 0x4C,0x68,0x38,0x41,0x39,0xF5,0x74,0xD8,
+ 0x36,0xBC,0xF3,0x4E,0x9D,0xFB,0x01,0xBF
+};
+uint16_t g_bletest_EDIV = 0x2474;
+uint64_t g_bletest_RAND = 0xABCDEF1234567890;
+uint64_t g_bletest_SKDm = 0xACBDCEDFE0F10213;
+uint64_t g_bletest_SKDs = 0x0213243546576879;
+uint32_t g_bletest_IVm = 0xBADCAB24;
+uint32_t g_bletest_IVs = 0xDEAFBABE;
+#endif
#if (BLETEST_THROUGHPUT_TEST == 1)
void
@@ -233,6 +247,32 @@ bletest_send_conn_update(uint16_t handle)
host_hci_outstanding_opcode = 0;
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+void
+bletest_ltk_req_reply(uint16_t handle)
+{
+ g_bletest_ltk_reply_handle = handle;
+}
+
+void
+bletest_send_ltk_req_neg_reply(uint16_t handle)
+{
+ host_hci_cmd_le_lt_key_req_neg_reply(handle);
+ host_hci_outstanding_opcode = 0;
+}
+
+void
+bletest_send_ltk_req_reply(uint16_t handle)
+{
+ struct hci_lt_key_req_reply hkr;
+
+ hkr.conn_handle = handle;
+ swap_buf(hkr.long_term_key, (uint8_t *)g_bletest_LTK, 16);
+ host_hci_cmd_le_lt_key_req_reply(&hkr);
+ host_hci_outstanding_opcode = 0;
+}
+#endif
+
/**
* Sets the advertising data to be sent in advertising pdu's which contain
* advertising data.
@@ -508,10 +548,25 @@ bletest_execute_initiator(void)
new_chan_map[0] = 0;
new_chan_map[1] = 0x3;
new_chan_map[2] = 0;
- new_chan_map[3] = 0;
+ new_chan_map[3] = 0x1F;
new_chan_map[4] = 0;
host_hci_cmd_le_set_host_chan_class(new_chan_map);
host_hci_outstanding_opcode = 0;
+ } else if (g_bletest_state == 4) {
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ struct hci_start_encrypt hsle;
+ for (i = 0; i < g_bletest_current_conns; ++i) {
+ if (ble_ll_conn_find_active_conn(i + 1)) {
+ hsle.connection_handle = i + 1;
+ hsle.encrypted_diversifier = g_bletest_EDIV;
+ hsle.random_number = g_bletest_RAND;
+ swap_buf(hsle.long_term_key, (uint8_t *)g_bletest_LTK,
+ 16);
+ host_hci_cmd_le_start_encrypt(&hsle);
+ host_hci_outstanding_opcode = 0;
+ }
+ }
+#endif
} else {
for (i = 0; i < g_bletest_current_conns; ++i) {
if (ble_ll_conn_find_active_conn(i + 1)) {
@@ -520,8 +575,11 @@ bletest_execute_initiator(void)
}
}
}
- ++g_bletest_state;
- g_next_os_time = os_time_get() + OS_TICKS_PER_SEC * 5;
+ if (g_bletest_state < 5) {
+ ++g_bletest_state;
+ } else {
+ }
+ g_next_os_time = os_time_get() + OS_TICKS_PER_SEC * 3;
}
}
}
@@ -623,6 +681,14 @@ bletest_execute_advertiser(void)
#if (BLETEST_CONCURRENT_CONN_TEST == 1)
/* See if it is time to hand a data packet to the connection */
if ((int32_t)(os_time_get() - g_next_os_time) >= 0) {
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ /* Do we need to send a LTK reply? */
+ if (g_bletest_ltk_reply_handle) {
+ //bletest_send_ltk_req_neg_reply(g_bletest_ltk_reply_handle);
+ bletest_send_ltk_req_reply(g_bletest_ltk_reply_handle);
+ g_bletest_ltk_reply_handle = 0;
+ }
+#endif
if (g_bletest_current_conns) {
for (i = 0; i < g_bletest_current_conns; ++i) {
if ((g_last_handle_used == 0) ||
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/hw/bsp/bmd300eval/src/arch/cortex_m4/gcc_startup_nrf52.s
----------------------------------------------------------------------
diff --git a/hw/bsp/bmd300eval/src/arch/cortex_m4/gcc_startup_nrf52.s b/hw/bsp/bmd300eval/src/arch/cortex_m4/gcc_startup_nrf52.s
index 9e71579..8bdf2f6 100755
--- a/hw/bsp/bmd300eval/src/arch/cortex_m4/gcc_startup_nrf52.s
+++ b/hw/bsp/bmd300eval/src/arch/cortex_m4/gcc_startup_nrf52.s
@@ -1,4 +1,4 @@
-/*
+/*
Copyright (c) 2015, Nordic Semiconductor ASA
All rights reserved.
@@ -28,8 +28,8 @@ 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.
*/
-/*
-NOTE: Template files (including this one) are application specific and therefore
+/*
+NOTE: Template files (including this one) are application specific and therefore
expected to be copied into the application project folder prior to its use!
*/
@@ -38,7 +38,7 @@ expected to be copied into the application project folder prior to its use!
.section .stack
.align 3
- .equ Stack_Size, 384
+ .equ Stack_Size, 432
.globl __StackTop
.globl __StackLimit
__StackLimit:
@@ -63,7 +63,7 @@ __HeapBase:
.size __HeapBase, . - __HeapBase
__HeapLimit:
.size __HeapLimit, . - __HeapLimit
-
+
.section .isr_vector
.align 2
.globl __isr_vector
@@ -140,7 +140,7 @@ Reset_Handler:
/* Loop to copy data from read only memory to RAM. The ranges
- * of copy from/to are specified by following symbols evaluated in
+ * of copy from/to are specified by following symbols evaluated in
* linker script.
* __etext: End of code section, i.e., begin of data sections to copy from.
* __data_start__/__data_end__: RAM address range that data should be
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/hw/bsp/nrf51dk/nrf51dk.ld
----------------------------------------------------------------------
diff --git a/hw/bsp/nrf51dk/nrf51dk.ld b/hw/bsp/nrf51dk/nrf51dk.ld
index ca85250..b5547fc 100755
--- a/hw/bsp/nrf51dk/nrf51dk.ld
+++ b/hw/bsp/nrf51dk/nrf51dk.ld
@@ -19,7 +19,7 @@ OUTPUT_FORMAT ("elf32-littlearm", "elf32-bigarm", "elf32-littlearm")
MEMORY
{
- FLASH (rx) : ORIGIN = 0x00008000, LENGTH = 0x1b800
+ FLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0x40000
RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 0x8000
}
@@ -54,11 +54,6 @@ ENTRY(Reset_Handler)
SECTIONS
{
- .imghdr (NOLOAD):
- {
- . = . + 0x20;
- } > FLASH
-
.text :
{
__isr_vector_start = .;
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/hw/bsp/nrf51dk/src/arch/cortex_m0/gcc_startup_nrf51.s
----------------------------------------------------------------------
diff --git a/hw/bsp/nrf51dk/src/arch/cortex_m0/gcc_startup_nrf51.s b/hw/bsp/nrf51dk/src/arch/cortex_m0/gcc_startup_nrf51.s
index 1dea388..431849d 100755
--- a/hw/bsp/nrf51dk/src/arch/cortex_m0/gcc_startup_nrf51.s
+++ b/hw/bsp/nrf51dk/src/arch/cortex_m0/gcc_startup_nrf51.s
@@ -1,4 +1,4 @@
-/*
+/*
Copyright (c) 2015, Nordic Semiconductor ASA
All rights reserved.
@@ -28,8 +28,8 @@ 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.
*/
-/*
-NOTE: Template files (including this one) are application specific and therefore
+/*
+NOTE: Template files (including this one) are application specific and therefore
expected to be copied into the application project folder prior to its use!
*/
@@ -38,7 +38,7 @@ expected to be copied into the application project folder prior to its use!
.section .stack
.align 3
- .equ Stack_Size, 384
+ .equ Stack_Size, 432
.globl __StackTop
.globl __StackLimit
__StackLimit:
@@ -63,7 +63,7 @@ __HeapBase:
.size __HeapBase, . - __HeapBase
__HeapLimit:
.size __HeapLimit, . - __HeapLimit
-
+
.section .isr_vector
.align 2
.globl __isr_vector
@@ -125,7 +125,7 @@ __isr_vector:
.equ NRF_POWER_RAMON_ADDRESS, 0x40000524
.equ NRF_POWER_RAMONB_ADDRESS, 0x40000554
- .equ NRF_POWER_RAMONx_RAMxON_ONMODE_Msk, 0x3
+ .equ NRF_POWER_RAMONx_RAMxON_ONMODE_Msk, 0x3
.text
.thumb
@@ -138,7 +138,7 @@ Reset_Handler:
/* Make sure ALL RAM banks are powered on */
MOVS R1, #NRF_POWER_RAMONx_RAMxON_ONMODE_Msk
-
+
LDR R0, =NRF_POWER_RAMON_ADDRESS
LDR R2, [R0]
ORRS R2, R1
@@ -150,7 +150,7 @@ Reset_Handler:
STR R2, [R0]
/* Loop to copy data from read only memory to RAM. The ranges
- * of copy from/to are specified by following symbols evaluated in
+ * of copy from/to are specified by following symbols evaluated in
* linker script.
* __etext: End of code section, i.e., begin of data sections to copy from.
* __data_start__/__data_end__: RAM address range that data should be
@@ -169,7 +169,7 @@ Reset_Handler:
str r0, [r2,r3]
bgt .LC1
.LC0:
-
+
LDR R0, =SystemInit
BLX R0
LDR R0, =_start
@@ -214,7 +214,7 @@ SysTick_Handler:
/* Default handler. This uses the vector in the relocated vector table */
.globl Default_Handler
.type Default_Handler, %function
-Default_Handler:
+Default_Handler:
LDR R2, =__vector_tbl_reloc__
MRS R0, PSR
MOVS R1, #0x3F
@@ -224,7 +224,7 @@ Default_Handler:
BX R0
.size Default_Handler, . - Default_Handler
-/*
+/*
* All of the following IRQ Handlers will point to the default handler unless
* they are defined elsewhere.
*/
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/hw/bsp/nrf52pdk/src/arch/cortex_m4/gcc_startup_nrf52.s
----------------------------------------------------------------------
diff --git a/hw/bsp/nrf52pdk/src/arch/cortex_m4/gcc_startup_nrf52.s b/hw/bsp/nrf52pdk/src/arch/cortex_m4/gcc_startup_nrf52.s
index 9e71579..8bdf2f6 100755
--- a/hw/bsp/nrf52pdk/src/arch/cortex_m4/gcc_startup_nrf52.s
+++ b/hw/bsp/nrf52pdk/src/arch/cortex_m4/gcc_startup_nrf52.s
@@ -1,4 +1,4 @@
-/*
+/*
Copyright (c) 2015, Nordic Semiconductor ASA
All rights reserved.
@@ -28,8 +28,8 @@ 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.
*/
-/*
-NOTE: Template files (including this one) are application specific and therefore
+/*
+NOTE: Template files (including this one) are application specific and therefore
expected to be copied into the application project folder prior to its use!
*/
@@ -38,7 +38,7 @@ expected to be copied into the application project folder prior to its use!
.section .stack
.align 3
- .equ Stack_Size, 384
+ .equ Stack_Size, 432
.globl __StackTop
.globl __StackLimit
__StackLimit:
@@ -63,7 +63,7 @@ __HeapBase:
.size __HeapBase, . - __HeapBase
__HeapLimit:
.size __HeapLimit, . - __HeapLimit
-
+
.section .isr_vector
.align 2
.globl __isr_vector
@@ -140,7 +140,7 @@ Reset_Handler:
/* Loop to copy data from read only memory to RAM. The ranges
- * of copy from/to are specified by following symbols evaluated in
+ * of copy from/to are specified by following symbols evaluated in
* linker script.
* __etext: End of code section, i.e., begin of data sections to copy from.
* __data_start__/__data_end__: RAM address range that data should be
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/include/controller/ble_ll.h
----------------------------------------------------------------------
diff --git a/net/nimble/controller/include/controller/ble_ll.h b/net/nimble/controller/include/controller/ble_ll.h
index 71a2316..af16f9a 100644
--- a/net/nimble/controller/include/controller/ble_ll.h
+++ b/net/nimble/controller/include/controller/ble_ll.h
@@ -366,6 +366,7 @@ int ble_ll_rand_start(void);
* log
*/
#undef BLE_LL_LOG
+#include "console/console.h"
#define BLE_LL_LOG_ID_PHY_SETCHAN (1)
#define BLE_LL_LOG_ID_RX_START (2)
@@ -378,6 +379,7 @@ int ble_ll_rand_start(void);
#define BLE_LL_LOG_ID_CONN_RX (16)
#define BLE_LL_LOG_ID_CONN_TX_RETRY (17)
#define BLE_LL_LOG_ID_CONN_RX_ACK (18)
+#define BLE_LL_LOG_ID_LL_CTRL_RX (19)
#define BLE_LL_LOG_ID_CONN_EV_END (20)
#define BLE_LL_LOG_ID_CONN_END (30)
#define BLE_LL_LOG_ID_ADV_TXBEG (50)
@@ -389,5 +391,15 @@ void ble_ll_log(uint8_t id, uint8_t arg8, uint16_t arg16, uint32_t arg32);
#define ble_ll_log(m,n,o,p)
#endif
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+/* LTK 0x4C68384139F574D836BCF34E9DFB01BF */
+extern const uint8_t g_bletest_LTK[];
+extern uint16_t g_bletest_EDIV;
+extern uint64_t g_bletest_RAND;
+extern uint64_t g_bletest_SKDm;
+extern uint64_t g_bletest_SKDs;
+extern uint32_t g_bletest_IVm;
+extern uint32_t g_bletest_IVs;
+#endif
#endif /* H_LL_ */
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/include/controller/ble_ll_conn.h
----------------------------------------------------------------------
diff --git a/net/nimble/controller/include/controller/ble_ll_conn.h b/net/nimble/controller/include/controller/ble_ll_conn.h
index 2f2204a..f9937aa 100644
--- a/net/nimble/controller/include/controller/ble_ll_conn.h
+++ b/net/nimble/controller/include/controller/ble_ll_conn.h
@@ -53,35 +53,63 @@
/* Definition for RSSI when the RSSI is unknown */
#define BLE_LL_CONN_UNKNOWN_RSSI (127)
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
/*
- * Length of empty pdu mbuf. Each connection state machine contains an
- * empty pdu since we dont want to allocate a full mbuf for an empty pdu
- * and we always want to have one available. The empty pdu length is of
- * type uint32_t so we have 4 byte alignment.
+ * Encryption states for a connection
+ *
+ * NOTE: the states are ordered so that we can check to see if the state
+ * is greater than ENCRYPTED. If so, it means that the start or pause
+ * encryption procedure is running and we should not send data pdu's.
+ */
+enum conn_enc_state {
+ CONN_ENC_S_UNENCRYPTED = 1,
+ CONN_ENC_S_ENCRYPTED,
+ CONN_ENC_S_ENC_RSP_WAIT,
+ CONN_ENC_S_START_ENC_REQ_WAIT,
+ CONN_ENC_S_START_ENC_RSP_WAIT,
+ CONN_ENC_S_LTK_REQ_WAIT,
+ CONN_ENC_S_LTK_NEG_REPLY
+};
+
+/*
+ * Note that the LTK is the key, the SDK is the plain text, and the
+ * session key is the cipher text portion of the encryption block.
*/
-#define BLE_LL_EMPTY_PDU_MBUF_SIZE (BLE_MBUF_MEMBLOCK_OVERHEAD / 4)
+struct ble_ll_conn_enc_data
+{
+ uint8_t enc_state;
+ uint8_t tx_encrypted;
+ uint16_t enc_div;
+ uint16_t tx_pkt_cntr;
+ uint16_t rx_pkt_cntr;
+ uint64_t host_rand_num;
+ uint8_t iv[8];
+ struct ble_encryption_block enc_block;
+};
+#endif
/* Connection state machine flags. */
union ble_ll_conn_sm_flags {
struct {
- uint16_t pdu_txd:1;
- uint16_t pkt_rxd:1;
- uint16_t terminate_ind_txd:1;
- uint16_t terminate_ind_rxd:1;
- uint16_t allow_slave_latency:1;
- uint16_t slave_set_last_anchor:1;
- uint16_t awaiting_host_reply:1;
- uint16_t send_conn_upd_event:1;
- uint16_t conn_update_sched:1;
- uint16_t host_expects_upd_event:1;
- uint16_t version_ind_sent:1;
- uint16_t rxd_version_ind:1;
- uint16_t chanmap_update_scheduled:1;
- uint16_t conn_empty_pdu_txd:1;
- uint16_t last_txd_md:1;
- uint16_t conn_req_txd:1;
+ uint32_t pkt_rxd:1;
+ uint32_t terminate_ind_txd:1;
+ uint32_t terminate_ind_rxd:1;
+ uint32_t allow_slave_latency:1;
+ uint32_t slave_set_last_anchor:1;
+ uint32_t awaiting_host_reply:1;
+ uint32_t send_conn_upd_event:1;
+ uint32_t conn_update_sched:1;
+ uint32_t host_expects_upd_event:1;
+ uint32_t version_ind_sent:1;
+ uint32_t rxd_version_ind:1;
+ uint32_t chanmap_update_scheduled:1;
+ uint32_t conn_empty_pdu_txd:1;
+ uint32_t last_txd_md:1;
+ uint32_t conn_req_txd:1;
+ uint32_t send_ltk_req:1;
+ uint32_t encrypted:1;
} cfbit;
- uint16_t conn_flags;
+ uint32_t conn_flags;
} __attribute__((packed));
/* Connection state machine */
@@ -197,7 +225,17 @@ struct ble_ll_conn_sm
/* For scheduling connections */
struct ble_ll_sched_item conn_sch;
- /* XXX: ifdef this by feature? */
+ /*
+ * XXX: a note on all these structures for control procedures. First off,
+ * all of these need to be ifdef'd to save memory. Another thing to
+ * consider is this: since most control procedures can only run when no
+ * others are running, can I use just one structure (a union)? Should I
+ * allocate these from a pool? Not sure what to do. For now, I just use
+ * a large chunk of memory per connection.
+ */
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ struct ble_ll_conn_enc_data enc_data;
+#endif
/*
* For connection update procedure. XXX: can make this a pointer and
* malloc it if we want to save space.
@@ -216,6 +254,11 @@ struct ble_ll_conn_sm
#define CONN_F_EMPTY_PDU_TXD(csm) ((csm)->csmflags.cfbit.conn_empty_pdu_txd)
#define CONN_F_LAST_TXD_MD(csm) ((csm)->csmflags.cfbit.last_txd_md)
#define CONN_F_CONN_REQ_TXD(csm) ((csm)->csmflags.cfbit.conn_req_txd)
+#define CONN_F_ENCRYPTED(csm) ((csm)->csmflags.cfbit.encrypted)
+
+/* Role */
+#define CONN_IS_MASTER(csm) (csm->conn_role == BLE_LL_CONN_ROLE_MASTER)
+#define CONN_IS_SLAVE(csm) (csm->conn_role == BLE_LL_CONN_ROLE_SLAVE)
/*
* Given a handle, returns an active connection state machine (or NULL if the
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/include/controller/ble_ll_ctrl.h
----------------------------------------------------------------------
diff --git a/net/nimble/controller/include/controller/ble_ll_ctrl.h b/net/nimble/controller/include/controller/ble_ll_ctrl.h
index e1a5881..4a9b1df 100644
--- a/net/nimble/controller/include/controller/ble_ll_ctrl.h
+++ b/net/nimble/controller/include/controller/ble_ll_ctrl.h
@@ -127,9 +127,6 @@ struct ble_ll_enc_rsp
#define BLE_LL_CTRL_ENC_RSP_LEN (12)
-/* LL control start enc req and start enc rsp have no data */
-#define BLE_LL_CTRL_START_ENC_LEN (0)
-
/*
* LL control unknown response
* -> 1 byte which contains the unknown or un-supported opcode.
@@ -231,12 +228,16 @@ int ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om);
void ble_ll_ctrl_chk_proc_start(struct ble_ll_conn_sm *connsm);
void ble_ll_ctrl_terminate_start(struct ble_ll_conn_sm *connsm);
int ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode);
-int ble_ll_ctrl_is_reject_ind_ext(uint8_t hdr, uint8_t opcode);
uint8_t ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm,
uint8_t *rsp,
struct ble_ll_conn_params *req);
-int ble_ll_ctrl_reject_ind_ext_send(struct ble_ll_conn_sm *connsm,
- uint8_t rej_opcode, uint8_t err);
+int ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm,
+ uint8_t rej_opcode, uint8_t err);
+int ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm, uint8_t opcode);
+int ble_ll_ctrl_enc_allowed_pdu(struct os_mbuf_pkthdr *pkthdr);
+int ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm);
+int ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu);
+int ble_ll_ctrl_is_start_enc_req(struct os_mbuf *txpdu);
void ble_ll_hci_ev_datalen_chg(struct ble_ll_conn_sm *connsm);
void ble_ll_hci_ev_rem_conn_parm_req(struct ble_ll_conn_sm *connsm,
@@ -245,5 +246,9 @@ void ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status);
void ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm,
uint8_t status);
void ble_ll_hci_ev_rd_rem_ver(struct ble_ll_conn_sm *connsm, uint8_t status);
+void ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status);
+int ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm);
+
+void ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm);
#endif /* H_BLE_LL_CTRL_ */
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/include/controller/ble_phy.h
----------------------------------------------------------------------
diff --git a/net/nimble/controller/include/controller/ble_phy.h b/net/nimble/controller/include/controller/ble_phy.h
index 06f22d2..88373c2 100644
--- a/net/nimble/controller/include/controller/ble_phy.h
+++ b/net/nimble/controller/include/controller/ble_phy.h
@@ -123,4 +123,9 @@ int ble_phy_rx_started(void);
/* Gets the current access address */
uint32_t ble_phy_access_addr_get(void);
+void ble_phy_encrypt_enable(uint64_t pkt_counter, uint8_t *iv, uint8_t *key,
+ uint8_t is_master);
+void ble_phy_encrypt_disable(void);
+void ble_phy_encrypt_set_pkt_cntr(uint64_t pkt_counter, int dir);
+
#endif /* H_BLE_PHY_ */
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll.c b/net/nimble/controller/src/ble_ll.c
index 87993b4..0ffcfe9 100644
--- a/net/nimble/controller/src/ble_ll.c
+++ b/net/nimble/controller/src/ble_ll.c
@@ -736,8 +736,8 @@ ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
crcok = BLE_MBUF_HDR_CRC_OK(ble_hdr);
ble_ll_log(BLE_LL_LOG_ID_RX_END,
- chan,
- ((uint16_t)crcok << 8) | rxbuf[1],
+ rxbuf[0],
+ ((uint16_t)ble_hdr->rxinfo.flags << 8) | rxbuf[1],
(BLE_MBUF_HDR_PTR(rxpdu))->end_cputime);
/* Check channel type */
@@ -821,11 +821,9 @@ ble_ll_rx_end(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *ble_hdr)
case BLE_LL_STATE_INITIATING:
rc = ble_ll_init_rx_isr_end(rxpdu, crcok);
break;
- /* Invalid states */
- case BLE_LL_STATE_CONNECTION:
default:
rc = -1;
- assert(0);
+ STATS_INC(ble_ll_stats, bad_ll_state);
break;
}
@@ -1154,3 +1152,29 @@ ble_ll_init(uint8_t ll_task_prio, uint8_t num_acl_pkts, uint16_t acl_pkt_size)
return rc;
}
+#ifdef BLE_LL_LOG
+void
+ble_ll_log_dump_index(int i)
+{
+ struct ble_ll_log *log;
+
+ log = &g_ble_ll_log[i];
+
+ console_printf("cputime=%lu id=%u a8=%u a16=%u a32=%lu\n",
+ log->cputime, log->log_id, log->log_a8,
+ log->log_a16, log->log_a32);
+}
+
+void
+ble_ll_log_dump(void)
+{
+ int i;
+
+ for (i = g_ble_ll_log_index; i < BLE_LL_LOG_LEN; ++i) {
+ ble_ll_log_dump_index(i);
+ }
+ for (i = 0; i < g_ble_ll_log_index; ++i) {
+ ble_ll_log_dump_index(i);
+ }
+}
+#endif
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_adv.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_adv.c b/net/nimble/controller/src/ble_ll_adv.c
index a74d07e..d6a208a 100644
--- a/net/nimble/controller/src/ble_ll_adv.c
+++ b/net/nimble/controller/src/ble_ll_adv.c
@@ -353,6 +353,10 @@ ble_ll_adv_tx_start_cb(struct ble_ll_sched_item *sch)
rc = ble_phy_setchan(advsm->adv_chan, 0, 0);
assert(rc == 0);
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ ble_phy_encrypt_disable();
+#endif
+
/* Set phy mode based on type of advertisement */
if (advsm->adv_type == BLE_HCI_ADV_TYPE_ADV_NONCONN_IND) {
end_trans = BLE_PHY_TRANSITION_NONE;
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_conn.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_conn.c b/net/nimble/controller/src/ble_ll_conn.c
index 4386353..da9acb4 100644
--- a/net/nimble/controller/src/ble_ll_conn.c
+++ b/net/nimble/controller/src/ble_ll_conn.c
@@ -142,7 +142,7 @@ struct ble_ll_conn_sm *g_ble_ll_conn_create_sm;
struct ble_ll_conn_sm *g_ble_ll_conn_cur_sm;
/* Connection state machine array */
-bssnz_t struct ble_ll_conn_sm g_ble_ll_conn_sm[NIMBLE_OPT_MAX_CONNECTIONS];
+struct ble_ll_conn_sm g_ble_ll_conn_sm[NIMBLE_OPT_MAX_CONNECTIONS];
/* List of active connections */
struct ble_ll_conn_active_list g_ble_ll_conn_active_list;
@@ -155,7 +155,6 @@ STATS_SECT_START(ble_ll_conn_stats)
STATS_SECT_ENTRY(conn_ev_late)
STATS_SECT_ENTRY(wfr_expirations)
STATS_SECT_ENTRY(handle_not_found)
- STATS_SECT_ENTRY(no_tx_pdu)
STATS_SECT_ENTRY(no_conn_sm)
STATS_SECT_ENTRY(no_free_conn_sm)
STATS_SECT_ENTRY(rx_data_pdu_no_conn)
@@ -176,6 +175,7 @@ STATS_SECT_START(ble_ll_conn_stats)
STATS_SECT_ENTRY(tx_l2cap_pdus)
STATS_SECT_ENTRY(tx_l2cap_bytes)
STATS_SECT_ENTRY(tx_empty_pdus)
+ STATS_SECT_ENTRY(mic_failures)
STATS_SECT_END
STATS_SECT_DECL(ble_ll_conn_stats) ble_ll_conn_stats;
@@ -184,7 +184,6 @@ STATS_NAME_START(ble_ll_conn_stats)
STATS_NAME(ble_ll_conn_stats, conn_ev_late)
STATS_NAME(ble_ll_conn_stats, wfr_expirations)
STATS_NAME(ble_ll_conn_stats, handle_not_found)
- STATS_NAME(ble_ll_conn_stats, no_tx_pdu)
STATS_NAME(ble_ll_conn_stats, no_conn_sm)
STATS_NAME(ble_ll_conn_stats, no_free_conn_sm)
STATS_NAME(ble_ll_conn_stats, rx_data_pdu_no_conn)
@@ -205,9 +204,28 @@ STATS_NAME_START(ble_ll_conn_stats)
STATS_NAME(ble_ll_conn_stats, tx_l2cap_pdus)
STATS_NAME(ble_ll_conn_stats, tx_l2cap_bytes)
STATS_NAME(ble_ll_conn_stats, tx_empty_pdus)
+ STATS_NAME(ble_ll_conn_stats, mic_failures)
STATS_NAME_END(ble_ll_conn_stats)
-/* */
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+/**
+ * Called to determine if the received PDU is an empty PDU or not.
+ */
+static int
+ble_ll_conn_is_empty_pdu(struct os_mbuf *rxpdu)
+{
+ int rc;
+ uint8_t llid;
+
+ llid = rxpdu->om_data[0] & BLE_LL_DATA_HDR_LLID_MASK;
+ if ((llid == BLE_LL_LLID_DATA_FRAG) && (rxpdu->om_data[1] == 0)) {
+ rc = 1;
+ } else {
+ rc = 0;
+ }
+ return rc;
+}
+#endif
/**
* Called to return the currently running connection state machine end time.
@@ -250,27 +268,6 @@ ble_ll_conn_get_ce_end_time(void)
}
/**
- * Checks if pdu is a L2CAP pdu, meaning it is not a control pdu nor an empty
- * pdu. Called only for transmit PDU's.
- *
- * @return int
- */
-static int
-ble_ll_conn_is_l2cap_pdu(struct ble_mbuf_hdr *ble_hdr)
-{
- int rc;
- uint8_t llid;
-
- llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
- if ((llid != BLE_LL_LLID_CTRL) && (ble_hdr->txinfo.pyld_len != 0)) {
- rc = 1;
- } else {
- rc = 0;
- }
- return rc;
-}
-
-/**
* Called when the current connection state machine is no longer being used.
* This function will:
* -> Disable the PHY, which will prevent any transmit/receive interrupts.
@@ -597,6 +594,42 @@ ble_ll_conn_wait_txend(void *arg)
ble_ll_event_send(&connsm->conn_ev_end);
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+static void
+ble_ll_conn_start_rx_encrypt(void *arg)
+{
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = (struct ble_ll_conn_sm *)arg;
+ CONN_F_ENCRYPTED(connsm) = 1;
+ ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr,
+ connsm->enc_data.iv,
+ connsm->enc_data.enc_block.cipher_text,
+ !CONN_IS_MASTER(connsm));
+}
+
+static void
+ble_ll_conn_txend_encrypt(void *arg)
+{
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = (struct ble_ll_conn_sm *)arg;
+ CONN_F_ENCRYPTED(connsm) = 1;
+ ble_ll_conn_current_sm_over();
+ ble_ll_event_send(&connsm->conn_ev_end);
+}
+
+static void
+ble_ll_conn_continue_rx_encrypt(void *arg)
+{
+ struct ble_ll_conn_sm *connsm;
+
+ connsm = (struct ble_ll_conn_sm *)arg;
+ ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.rx_pkt_cntr,
+ !CONN_IS_MASTER(connsm));
+}
+#endif
+
/**
* Returns the cputime of the next scheduled item on the scheduler list or
* when the current connection will start its next interval (whichever is
@@ -631,6 +664,52 @@ ble_ll_conn_get_next_sched_time(struct ble_ll_conn_sm *connsm)
}
/**
+ * Called to check if certain connection state machine flags have been
+ * set.
+ *
+ * @param connsm
+ */
+static void
+ble_ll_conn_chk_csm_flags(struct ble_ll_conn_sm *connsm)
+{
+ uint8_t update_status;
+
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (connsm->csmflags.cfbit.send_ltk_req) {
+ /*
+ * Send Long term key request event to host. If masked, we need to
+ * send a REJECT_IND.
+ */
+ if (ble_ll_hci_ev_ltk_req(connsm)) {
+ ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ,
+ BLE_ERR_PINKEY_MISSING);
+ }
+ connsm->csmflags.cfbit.send_ltk_req = 0;
+ }
+#endif
+
+ /*
+ * There are two cases where this flag gets set:
+ * 1) A connection update procedure was started and the event counter
+ * has passed the instant.
+ * 2) We successfully sent the reject reason.
+ */
+ if (connsm->csmflags.cfbit.host_expects_upd_event) {
+ update_status = BLE_ERR_SUCCESS;
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) {
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE);
+ } else {
+ if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
+ update_status = connsm->reject_reason;
+ }
+ }
+ ble_ll_hci_ev_conn_update(connsm, update_status);
+ connsm->csmflags.cfbit.host_expects_upd_event = 0;
+ }
+}
+
+/**
* Called when we want to send a data channel pdu inside a connection event.
*
* Context: interrupt
@@ -648,7 +727,7 @@ ble_ll_conn_tx_data_pdu(struct ble_ll_conn_sm *connsm, int beg_transition)
uint8_t end_transition;
uint8_t cur_txlen;
uint8_t next_txlen;
- uint8_t rem_bytes;
+ uint8_t cur_offset;
uint16_t pktlen;
uint32_t next_event_time;
uint32_t ticks;
@@ -679,25 +758,50 @@ ble_ll_conn_tx_data_pdu(struct ble_ll_conn_sm *connsm, int beg_transition)
* If we dont have a pdu we have previously transmitted, take it off
* the connection transmit queue
*/
+ cur_offset = 0;
if (!connsm->cur_tx_pdu && !CONN_F_EMPTY_PDU_TXD(connsm)) {
- /* Take packet off queue*/
+ /* Convert packet header to mbuf */
+ m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
nextpkthdr = STAILQ_NEXT(pkthdr, omp_next);
- STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next);
- m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ /*
+ * If we are encrypting, we are only allowed to send certain
+ * kinds of LL control PDU's. If none is enqueued, send empty pdu!
+ */
+ if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+ if (!ble_ll_ctrl_enc_allowed_pdu(pkthdr)) {
+ CONN_F_EMPTY_PDU_TXD(connsm) = 1;
+ goto conn_tx_pdu;
+ }
+
+ /*
+ * We will allow a next packet if it itself is allowed or we are
+ * a slave and we are sending the START_ENC_RSP. The master has
+ * to wait to receive the START_ENC_RSP from the slave before
+ * packets can be let go.
+ */
+ if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu(nextpkthdr)
+ && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) ||
+ !ble_ll_ctrl_is_start_enc_rsp(m))) {
+ nextpkthdr = NULL;
+ }
+ }
+#endif
+ /* Take packet off queue*/
+ STAILQ_REMOVE_HEAD(&connsm->conn_txq, omp_next);
ble_hdr = BLE_MBUF_HDR_PTR(m);
/* Determine packet length we will transmit */
cur_txlen = connsm->eff_max_tx_octets;
pktlen = pkthdr->omp_len;
- rem_bytes = pktlen - ble_hdr->txinfo.offset;
- if (cur_txlen > rem_bytes) {
- cur_txlen = rem_bytes;
+ if (cur_txlen > pktlen) {
+ cur_txlen = pktlen;
}
+ ble_hdr->txinfo.pyld_len = cur_txlen;
/* NOTE: header was set when first enqueued */
hdr_byte = ble_hdr->txinfo.hdr_byte;
- ble_hdr->txinfo.pyld_len = cur_txlen;
connsm->cur_tx_pdu = m;
} else {
nextpkthdr = pkthdr;
@@ -706,13 +810,33 @@ ble_ll_conn_tx_data_pdu(struct ble_ll_conn_sm *connsm, int beg_transition)
ble_hdr = BLE_MBUF_HDR_PTR(m);
pktlen = OS_MBUF_PKTLEN(m);
cur_txlen = ble_hdr->txinfo.pyld_len;
- if (ble_hdr->txinfo.offset == 0) {
+ cur_offset = ble_hdr->txinfo.offset;
+ if (cur_offset == 0) {
hdr_byte = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
}
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+ /* We will allow a next packet if it itself is allowed */
+ pkthdr = OS_MBUF_PKTHDR(connsm->cur_tx_pdu);
+ if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu(nextpkthdr)
+ && ((connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) ||
+ !ble_ll_ctrl_is_start_enc_rsp(connsm->cur_tx_pdu))) {
+ nextpkthdr = NULL;
+ }
+ }
+#endif
} else {
- /* NOTE: header byte gets set later */
+ /* Empty PDU here. NOTE: header byte gets set later */
pktlen = 0;
cur_txlen = 0;
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+ /* We will allow a next packet if it itself is allowed */
+ if (nextpkthdr && !ble_ll_ctrl_enc_allowed_pdu(nextpkthdr)) {
+ nextpkthdr = NULL;
+ }
+ }
+#endif
}
}
@@ -720,7 +844,7 @@ ble_ll_conn_tx_data_pdu(struct ble_ll_conn_sm *connsm, int beg_transition)
* Set the more data data flag if we have more data to send and we
* have not been asked to terminate
*/
- if ((nextpkthdr || ((ble_hdr->txinfo.offset + cur_txlen) < pktlen)) &&
+ if ((nextpkthdr || ((cur_offset + cur_txlen) < pktlen)) &&
!connsm->csmflags.cfbit.terminate_ind_rxd) {
/* Get next event time */
next_event_time = ble_ll_conn_get_next_sched_time(connsm);
@@ -737,8 +861,8 @@ ble_ll_conn_tx_data_pdu(struct ble_ll_conn_sm *connsm, int beg_transition)
* -> wait IFS, receive maximum size frame.
* -> wait IFS, send next frame.
*/
- if ((ble_hdr->txinfo.offset + cur_txlen) < pktlen) {
- next_txlen = pktlen - (ble_hdr->txinfo.offset + cur_txlen);
+ if ((cur_offset + cur_txlen) < pktlen) {
+ next_txlen = pktlen - (cur_offset + cur_txlen);
} else {
if (nextpkthdr->omp_len > connsm->eff_max_tx_octets) {
next_txlen = connsm->eff_max_tx_octets;
@@ -828,6 +952,37 @@ conn_tx_pdu:
txend_func = NULL;
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (ble_ll_ctrl_is_start_enc_rsp(m)) {
+ CONN_F_ENCRYPTED(connsm) = 1;
+ connsm->enc_data.tx_encrypted = 1;
+ ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr,
+ connsm->enc_data.iv,
+ connsm->enc_data.enc_block.cipher_text,
+ CONN_IS_MASTER(connsm));
+ } else if (ble_ll_ctrl_is_start_enc_req(m)) {
+ CONN_F_ENCRYPTED(connsm) = 0;
+ connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT;
+ connsm->enc_data.tx_encrypted = 0;
+ ble_phy_encrypt_disable();
+ if (txend_func == NULL) {
+ txend_func = ble_ll_conn_start_rx_encrypt;
+ } else {
+ txend_func = ble_ll_conn_txend_encrypt;
+ }
+ } else {
+ /* If encrypted set packet counter */
+ if (CONN_F_ENCRYPTED(connsm)) {
+ connsm->enc_data.tx_encrypted = 1;
+ ble_phy_encrypt_set_pkt_cntr(connsm->enc_data.tx_pkt_cntr,
+ CONN_IS_MASTER(connsm));
+ if (txend_func == NULL) {
+ txend_func = ble_ll_conn_continue_rx_encrypt;
+ }
+ }
+ }
+#endif
+
/* Set transmit end callback */
ble_phy_set_txend_cb(txend_func, connsm);
rc = ble_phy_tx(m, beg_transition, end_transition);
@@ -839,9 +994,6 @@ conn_tx_pdu:
((uint16_t)ble_hdr->txinfo.offset << 8) | cur_txlen,
(uint32_t)m);
- /* Set flag denoting we transmitted a pdu */
- connsm->csmflags.cfbit.pdu_txd = 1;
-
/* Set last transmitted MD bit */
CONN_F_LAST_TXD_MD(connsm) = md;
@@ -861,7 +1013,7 @@ conn_tx_pdu:
}
/**
- * Schedule callback for start of connection event
+ * Schedule callback for start of connection event.
*
* Context: Interrupt
*
@@ -895,11 +1047,20 @@ ble_ll_conn_event_start_cb(struct ble_ll_sched_item *sch)
connsm->conn_handle, connsm->ce_end_time);
/* Set channel */
- rc = ble_phy_setchan(connsm->data_chan_index, connsm->access_addr,
- connsm->crcinit);
- assert(rc == 0);
+ ble_phy_setchan(connsm->data_chan_index, connsm->access_addr,
+ connsm->crcinit);
if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (CONN_F_ENCRYPTED(connsm)) {
+ ble_phy_encrypt_enable(connsm->enc_data.tx_pkt_cntr,
+ connsm->enc_data.iv,
+ connsm->enc_data.enc_block.cipher_text,
+ 1);
+ } else {
+ ble_phy_encrypt_disable();
+ }
+#endif
rc = ble_ll_conn_tx_data_pdu(connsm, BLE_PHY_TRANSITION_NONE);
if (!rc) {
rc = BLE_LL_SCHED_STATE_RUNNING;
@@ -908,6 +1069,16 @@ ble_ll_conn_event_start_cb(struct ble_ll_sched_item *sch)
rc = BLE_LL_SCHED_STATE_DONE;
}
} else {
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (CONN_F_ENCRYPTED(connsm)) {
+ ble_phy_encrypt_enable(connsm->enc_data.rx_pkt_cntr,
+ connsm->enc_data.iv,
+ connsm->enc_data.enc_block.cipher_text,
+ 1);
+ } else {
+ ble_phy_encrypt_disable();
+ }
+#endif
rc = ble_phy_rx();
if (rc) {
/* End the connection event as we have no more buffers */
@@ -1171,6 +1342,12 @@ ble_ll_conn_sm_new(struct ble_ll_conn_sm *connsm)
connsm->eff_max_tx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
connsm->eff_max_rx_octets = BLE_LL_CONN_SUPP_BYTES_MIN;
+ /* Reset encryption data */
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ memset(&connsm->enc_data, 0, sizeof(struct ble_ll_conn_enc_data));
+ connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+#endif
+
/* Add to list of active connections */
SLIST_INSERT_HEAD(&g_ble_ll_conn_active_list, connsm, act_sle);
}
@@ -1311,7 +1488,6 @@ ble_ll_conn_end(struct ble_ll_conn_sm *connsm, uint8_t ble_err)
static int
ble_ll_conn_next_event(struct ble_ll_conn_sm *connsm)
{
- uint8_t update_status;
uint16_t latency;
uint32_t itvl;
uint32_t tmo;
@@ -1319,34 +1495,8 @@ ble_ll_conn_next_event(struct ble_ll_conn_sm *connsm)
uint32_t max_ww;
struct ble_ll_conn_upd_req *upd;
- /*
- * XXX: we could send the connection update event a bit earlier. The
- * way this is coded is that we will generally send it when the
- * connection event corresponding to the instant ends. We can send
- * it when it begins if we want.
- */
-
/* XXX: deal with connection request procedure here as well */
-
- /*
- * There are two cases where this flag gets set:
- * 1) A connection update procedure was started and the event counter
- * has passed the instant.
- * 2) We successfully sent the reject reason.
- */
- if (connsm->csmflags.cfbit.host_expects_upd_event) {
- update_status = BLE_ERR_SUCCESS;
- if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE)) {
- ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_UPDATE);
- } else {
- if (IS_PENDING_CTRL_PROC(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ)) {
- ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
- update_status = connsm->reject_reason;
- }
- }
- ble_ll_hci_ev_conn_update(connsm, update_status);
- connsm->csmflags.cfbit.host_expects_upd_event = 0;
- }
+ ble_ll_conn_chk_csm_flags(connsm);
/* Set event counter to the next connection event that we will tx/rx in */
itvl = connsm->conn_itvl * BLE_LL_CONN_ITVL_USECS;
@@ -2008,6 +2158,10 @@ ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
tmo = connsm->supervision_tmo * BLE_HCI_CONN_SPVN_TMO_UNITS * 1000;
cputime_timer_relative(&connsm->conn_spvn_timer, tmo);
+ /* Check state machine */
+ ble_ll_conn_chk_csm_flags(connsm);
+
+ /* Validate rx data pdu */
rxbuf = rxpdu->om_data;
hdr_byte = rxbuf[0];
acl_len = rxbuf[1];
@@ -2047,6 +2201,19 @@ ble_ll_conn_rx_data_pdu(struct os_mbuf *rxpdu, struct ble_mbuf_hdr *hdr)
goto conn_rx_data_pdu_end;
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ /*
+ * XXX: should we check to see if we are in a state where we
+ * might expect to get an encrypted PDU?
+ */
+ if (BLE_MBUF_HDR_MIC_FAILURE(hdr)) {
+ STATS_INC(ble_ll_conn_stats, mic_failures);
+ /* Control procedure has timed out. Kill the connection */
+ ble_ll_conn_timeout(connsm, BLE_ERR_CONN_TERM_MIC);
+ goto conn_rx_data_pdu_end;
+ }
+#endif
+
if (acl_hdr == BLE_LL_LLID_CTRL) {
/* Process control frame */
STATS_INC(ble_ll_conn_stats, rx_ctrl_pdus);
@@ -2161,10 +2328,21 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu, uint32_t aa)
/* Store received header byte in state machine */
hdr_byte = rxpdu->om_data[0];
- connsm->last_rxd_hdr_byte = hdr_byte;
- ble_ll_log(BLE_LL_LOG_ID_CONN_RX, hdr_byte, connsm->tx_seqnum,
- connsm->next_exp_seqnum);
+ /* Check for valid LLID before proceeding. */
+ if ((hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == 0) {
+ /*
+ * XXX: for now, just exit since we dont trust the length
+ * and may erroneously adjust anchor. Once we fix the anchor
+ * point issue we need to decide what to do on bad llid. Note
+ * that an error stat gets counted at the LL
+ */
+ reply = 0;
+ goto conn_exit;
+ }
+
+ /* Set last received header byte */
+ connsm->last_rxd_hdr_byte = hdr_byte;
/*
* If SN bit from header does not match NESN in connection, this is
@@ -2174,13 +2352,29 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu, uint32_t aa)
conn_nesn = connsm->next_exp_seqnum;
if ((hdr_sn && conn_nesn) || (!hdr_sn && !conn_nesn)) {
connsm->next_exp_seqnum ^= 1;
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (CONN_F_ENCRYPTED(connsm) && !ble_ll_conn_is_empty_pdu(rxpdu)) {
+ ++connsm->enc_data.rx_pkt_cntr;
+ }
+#endif
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ ble_ll_log(BLE_LL_LOG_ID_CONN_RX,
+ hdr_byte,
+ (uint16_t)connsm->tx_seqnum << 8 | conn_nesn,
+ connsm->enc_data.rx_pkt_cntr);
+#else
+ ble_ll_log(BLE_LL_LOG_ID_CONN_RX,
+ hdr_byte,
+ (uint16_t)connsm->tx_seqnum << 8 | conn_nesn, 0);
+#endif
+
/*
* Check NESN bit from header. If same as tx seq num, the transmission
* is acknowledged. Otherwise we need to resend this PDU.
*/
- if (connsm->csmflags.cfbit.pdu_txd) {
+ if (CONN_F_EMPTY_PDU_TXD(connsm) || connsm->cur_tx_pdu) {
hdr_nesn = hdr_byte & BLE_LL_DATA_HDR_NESN_MASK;
conn_sn = connsm->tx_seqnum;
if ((hdr_nesn && conn_sn) || (!hdr_nesn && !conn_sn)) {
@@ -2203,27 +2397,22 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu, uint32_t aa)
*/
txpdu = connsm->cur_tx_pdu;
if (txpdu) {
- /* Did we transmit a TERMINATE_IND? If so, we are done */
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ if (connsm->enc_data.tx_encrypted) {
+ ++connsm->enc_data.tx_pkt_cntr;
+ }
+#endif
txhdr = BLE_MBUF_HDR_PTR(txpdu);
- if (ble_ll_ctrl_is_terminate_ind(txhdr->txinfo.hdr_byte,
- txpdu->om_data[0])) {
- connsm->csmflags.cfbit.terminate_ind_txd = 1;
- os_mbuf_free_chain(txpdu);
+ if ((txhdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK)
+ == BLE_LL_LLID_CTRL) {
connsm->cur_tx_pdu = NULL;
- rc = -1;
- goto conn_rx_pdu_end;
- }
-
- /*
- * Did we transmit a REJECT_IND_EXT? If so we need
- * to make sure we send the connection update event
- */
- if (ble_ll_ctrl_is_reject_ind_ext(txhdr->txinfo.hdr_byte,
- txpdu->om_data[0])) {
- if (connsm->cur_ctrl_proc ==
- BLE_LL_CTRL_PROC_CONN_PARAM_REQ) {
- connsm->reject_reason = txpdu->om_data[2];
- connsm->csmflags.cfbit.host_expects_upd_event = 1;
+ /* Note: the mbuf is freed by this call */
+ rc = ble_ll_ctrl_tx_done(txpdu, connsm);
+ if (rc) {
+ /* Means we transmitted a TERMINATE_IND */
+ goto conn_rx_pdu_end;
+ } else {
+ goto chk_rx_terminate_ind;
}
}
@@ -2231,11 +2420,10 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu, uint32_t aa)
txhdr->txinfo.offset += txhdr->txinfo.pyld_len;
if (txhdr->txinfo.offset >= OS_MBUF_PKTLEN(txpdu)) {
/* If l2cap pdu, increment # of completed packets */
- if (ble_ll_conn_is_l2cap_pdu(txhdr)) {
+ if (txhdr->txinfo.pyld_len != 0) {
#if (BLETEST_THROUGHPUT_TEST == 1)
bletest_completed_pkt(connsm->conn_handle);
#endif
-
++connsm->completed_pkts;
}
os_mbuf_free_chain(txpdu);
@@ -2248,9 +2436,6 @@ ble_ll_conn_rx_isr_end(struct os_mbuf *rxpdu, uint32_t aa)
txhdr->txinfo.pyld_len = rem_bytes;
}
}
- } else {
- /* No packet on queue? This is an error! */
- STATS_INC(ble_ll_conn_stats, no_tx_pdu);
}
}
}
@@ -2295,9 +2480,13 @@ conn_rx_pdu_end:
/* Set anchor point (and last) if 1st received frame in connection event */
if (connsm->csmflags.cfbit.slave_set_last_anchor) {
connsm->csmflags.cfbit.slave_set_last_anchor = 0;
- connsm->last_anchor_point = rxhdr->end_cputime -
- cputime_usecs_to_ticks(BLE_TX_DUR_USECS_M(rxpdu->om_data[1]));
- connsm->anchor_point = connsm->last_anchor_point;
+ /* XXX: For now, we just wont adjust the anchor point on crc error
+ until I fix this issue */
+ if (BLE_MBUF_HDR_CRC_OK(rxhdr)) {
+ connsm->last_anchor_point = rxhdr->end_cputime -
+ cputime_usecs_to_ticks(BLE_TX_DUR_USECS_M(rxpdu->om_data[1]));
+ connsm->anchor_point = connsm->last_anchor_point;
+ }
}
/* Send link layer a connection end event if over */
@@ -2329,6 +2518,7 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
os_sr_t sr;
struct os_mbuf_pkthdr *pkthdr;
struct ble_mbuf_hdr *ble_hdr;
+ int lifo;
/* Initialize the mbuf */
ble_ll_mbuf_init(om, length, hdr_byte);
@@ -2342,10 +2532,38 @@ ble_ll_conn_enqueue_pkt(struct ble_ll_conn_sm *connsm, struct os_mbuf *om,
ble_hdr->txinfo.pyld_len = connsm->eff_max_tx_octets;
}
+ lifo = 0;
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+ /*
+ * If this is one of the following types we need to insert it at
+ * head of queue.
+ */
+ ble_hdr = BLE_MBUF_HDR_PTR(om);
+ if ((ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
+ switch (om->om_data[0]) {
+ case BLE_LL_CTRL_TERMINATE_IND:
+ case BLE_LL_CTRL_REJECT_IND:
+ case BLE_LL_CTRL_REJECT_IND_EXT:
+ case BLE_LL_CTRL_START_ENC_REQ:
+ case BLE_LL_CTRL_START_ENC_RSP:
+ lifo = 1;
+ break;
+ default:
+ break;
+ }
+ }
+ }
+#endif
+
/* Add to transmit queue for the connection */
pkthdr = OS_MBUF_PKTHDR(om);
OS_ENTER_CRITICAL(sr);
- STAILQ_INSERT_TAIL(&connsm->conn_txq, pkthdr, omp_next);
+ if (lifo) {
+ STAILQ_INSERT_HEAD(&connsm->conn_txq, pkthdr, omp_next);
+ } else {
+ STAILQ_INSERT_TAIL(&connsm->conn_txq, pkthdr, omp_next);
+ }
OS_EXIT_CRITICAL(sr);
}
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_conn_hci.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_conn_hci.c b/net/nimble/controller/src/ble_ll_conn_hci.c
index 5e78e7f..2802866 100644
--- a/net/nimble/controller/src/ble_ll_conn_hci.c
+++ b/net/nimble/controller/src/ble_ll_conn_hci.c
@@ -485,8 +485,14 @@ ble_ll_conn_hci_update(uint8_t *cmdbuf)
struct ble_ll_conn_sm *connsm;
struct hci_conn_update *hcu;
- /* XXX: must deal with slave not supporting this feature and using
- conn update */
+ /*
+ * XXX: must deal with slave not supporting this feature and using
+ * conn update! Right now, we only check if WE support the connection
+ * parameters request procedure. We dont check if the remote does.
+ * We should also be able to deal with sending the parameter request,
+ * getting an UNKOWN_RSP ctrl pdu and resorting to use normal
+ * connection update procedure.
+ */
/* If no connection handle exit with error */
handle = le16toh(cmdbuf);
@@ -524,9 +530,8 @@ ble_ll_conn_hci_update(uint8_t *cmdbuf)
connsm->csmflags.cfbit.awaiting_host_reply = 0;
/* XXX: If this fails no reject ind will be sent! */
- ble_ll_ctrl_reject_ind_ext_send(connsm,
- connsm->host_reply_opcode,
- BLE_ERR_LMP_COLLISION);
+ ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode,
+ BLE_ERR_LMP_COLLISION);
}
}
@@ -618,9 +623,8 @@ ble_ll_conn_hci_param_reply(uint8_t *cmdbuf, int positive_reply)
}
} else {
/* XXX: check return code and deal */
- ble_ll_ctrl_reject_ind_ext_send(connsm,
- connsm->host_reply_opcode,
- ble_err);
+ ble_ll_ctrl_reject_ind_send(connsm, connsm->host_reply_opcode,
+ ble_err);
}
connsm->csmflags.cfbit.awaiting_host_reply = 0;
@@ -911,3 +915,89 @@ ble_ll_conn_hci_set_data_len(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t *rsplen)
}
#endif
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+/**
+ * LE start encrypt command
+ *
+ * @param cmdbuf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_start_encrypt(uint8_t *cmdbuf)
+{
+ int rc;
+ uint16_t handle;
+ struct ble_ll_conn_sm *connsm;
+
+ handle = le16toh(cmdbuf);
+ connsm = ble_ll_conn_find_active_conn(handle);
+ if (!connsm) {
+ rc = BLE_ERR_UNK_CONN_ID;
+ } else if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+ rc = BLE_ERR_UNSPECIFIED;
+ } else {
+ /* XXX: implement pause procedure */
+ /* Start the control procedure */
+ connsm->enc_data.host_rand_num = le64toh(cmdbuf + 2);
+ connsm->enc_data.enc_div = le16toh(cmdbuf + 10);
+ swap_buf(connsm->enc_data.enc_block.key, cmdbuf + 12, 16);
+ ble_ll_ctrl_proc_start(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+ rc = BLE_ERR_SUCCESS;
+ }
+
+ return rc;
+}
+
+/**
+ * Called to process the LE long term key reply.
+ *
+ * Context: Link Layer Task.
+ *
+ * @param cmdbuf
+ * @param rspbuf
+ * @param ocf
+ *
+ * @return int
+ */
+int
+ble_ll_conn_hci_le_ltk_reply(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t ocf)
+{
+ int rc;
+ uint16_t handle;
+ struct ble_ll_conn_sm *connsm;
+
+ /* Find connection handle */
+ handle = le16toh(cmdbuf);
+ connsm = ble_ll_conn_find_active_conn(handle);
+ if (!connsm) {
+ rc = BLE_ERR_UNK_CONN_ID;
+ goto ltk_key_cmd_complete;
+ }
+
+ /* Should never get this if we are a master! */
+ if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+ rc = BLE_ERR_UNSPECIFIED;
+ goto ltk_key_cmd_complete;
+ }
+
+ /* The connection should be awaiting a reply. If not, just discard */
+ if (connsm->enc_data.enc_state == CONN_ENC_S_LTK_REQ_WAIT) {
+ if (ocf == BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY) {
+ swap_buf(connsm->enc_data.enc_block.key, cmdbuf + 2, 16);
+ ble_ll_calc_session_key(connsm);
+ ble_ll_ctrl_start_enc_send(connsm, BLE_LL_CTRL_START_ENC_REQ);
+ } else {
+ /* We received a negative reply! Send REJECT_IND */
+ ble_ll_ctrl_reject_ind_send(connsm, BLE_LL_CTRL_ENC_REQ,
+ BLE_ERR_PINKEY_MISSING);
+ connsm->enc_data.enc_state = CONN_ENC_S_LTK_NEG_REPLY;
+ }
+ }
+ rc = BLE_ERR_SUCCESS;
+
+ltk_key_cmd_complete:
+ htole16(rspbuf, handle);
+ return rc;
+}
+#endif
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_conn_priv.h
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_conn_priv.h b/net/nimble/controller/src/ble_ll_conn_priv.h
index 38a39a9..4cfd041 100644
--- a/net/nimble/controller/src/ble_ll_conn_priv.h
+++ b/net/nimble/controller/src/ble_ll_conn_priv.h
@@ -133,4 +133,6 @@ int ble_ll_conn_hci_rd_chan_map(uint8_t *cmdbuf, uint8_t *rspbuf,
uint8_t *rsplen);
int ble_ll_conn_hci_set_data_len(uint8_t *cmdbuf, uint8_t *rspbuf,
uint8_t *rsplen);
+int ble_ll_conn_hci_le_start_encrypt(uint8_t *cmdbuf);
+int ble_ll_conn_hci_le_ltk_reply(uint8_t *cmdbuf, uint8_t *rspbuf, uint8_t ocf);
#endif /* H_BLE_LL_CONN_PRIV_ */
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_ctrl.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_ctrl.c b/net/nimble/controller/src/ble_ll_ctrl.c
index 3f2dba5..04ddd5b 100644
--- a/net/nimble/controller/src/ble_ll_ctrl.c
+++ b/net/nimble/controller/src/ble_ll_ctrl.c
@@ -26,6 +26,16 @@
#include "controller/ble_ll_hci.h"
#include "controller/ble_ll_ctrl.h"
#include "ble_ll_conn_priv.h"
+#include "controller/ble_hw.h"
+
+/* To use spec sample data for testing */
+#undef BLE_LL_ENCRYPT_USE_TEST_DATA
+
+/* For console debug to show session key calculation */
+#undef BLE_LL_ENCRYPT_DEBUG
+#ifdef BLE_LL_ENCRYPT_DEBUG
+#include "console/console.h"
+#endif
/*
* XXX: TODO
@@ -320,6 +330,332 @@ ble_ll_ctrl_datalen_upd_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
htole16(dptr + 7, connsm->max_tx_time);
}
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+void
+ble_ll_calc_session_key(struct ble_ll_conn_sm *connsm)
+{
+#ifdef BLE_LL_ENCRYPT_DEBUG
+ int cnt;
+#endif
+
+ /* XXX: possibly have some way out of this if this locks up */
+ while (1) {
+ if (!ble_hw_encrypt_block(&connsm->enc_data.enc_block)) {
+ break;
+ }
+ }
+
+#ifdef BLE_LL_ENCRYPT_DEBUG
+ console_printf("Calculating Session Key for handle=%u",
+ connsm->conn_handle);
+
+ console_printf("\nLTK:");
+ for (cnt = 0; cnt < 16; ++cnt) {
+ console_printf("%02x", connsm->enc_data.enc_block.key[cnt]);
+ }
+ console_printf("\nSKD:");
+ for (cnt = 0; cnt < 16; ++cnt) {
+ console_printf("%02x", connsm->enc_data.enc_block.plain_text[cnt]);
+ }
+ console_printf("\nSession Key:");
+ for (cnt = 0; cnt < 16; ++cnt) {
+ console_printf("%02x", connsm->enc_data.enc_block.cipher_text[cnt]);
+ }
+ console_printf("\nIV:");
+ for (cnt = 0; cnt < 8; ++ cnt) {
+ console_printf("%02x", connsm->enc_data.iv[cnt]);
+ }
+ console_printf("\n");
+#endif
+}
+
+int
+ble_ll_ctrl_enc_allowed_pdu(struct os_mbuf_pkthdr *pkthdr)
+{
+ int allowed;
+ uint8_t opcode;
+ uint8_t llid;
+ struct os_mbuf *m;
+ struct ble_mbuf_hdr *ble_hdr;
+
+ allowed = 0;
+ m = OS_MBUF_PKTHDR_TO_MBUF(pkthdr);
+ ble_hdr = BLE_MBUF_HDR_PTR(m);
+
+ llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+ if (llid == BLE_LL_LLID_CTRL) {
+ opcode = m->om_data[0];
+ switch (opcode) {
+ case BLE_LL_CTRL_REJECT_IND:
+ case BLE_LL_CTRL_REJECT_IND_EXT:
+ case BLE_LL_CTRL_START_ENC_RSP:
+ case BLE_LL_CTRL_START_ENC_REQ:
+ case BLE_LL_CTRL_ENC_REQ:
+ case BLE_LL_CTRL_ENC_RSP:
+ case BLE_LL_CTRL_TERMINATE_IND:
+ allowed = 1;
+ break;
+ default:
+ break;
+ }
+ }
+
+ return allowed;
+}
+
+int
+ble_ll_ctrl_is_start_enc_rsp(struct os_mbuf *txpdu)
+{
+ int is_start_enc_rsp;
+ uint8_t opcode;
+ uint8_t llid;
+ struct ble_mbuf_hdr *ble_hdr;
+
+ is_start_enc_rsp = 0;
+ ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+
+ llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+ if (llid == BLE_LL_LLID_CTRL) {
+ opcode = txpdu->om_data[0];
+ if (opcode == BLE_LL_CTRL_START_ENC_RSP) {
+ is_start_enc_rsp = 1;
+ }
+ }
+
+ return is_start_enc_rsp;
+}
+
+int
+ble_ll_ctrl_is_start_enc_req(struct os_mbuf *txpdu)
+{
+ int is_start_enc_req;
+ uint8_t opcode;
+ uint8_t llid;
+ struct ble_mbuf_hdr *ble_hdr;
+
+ is_start_enc_req = 0;
+ ble_hdr = BLE_MBUF_HDR_PTR(txpdu);
+
+ llid = ble_hdr->txinfo.hdr_byte & BLE_LL_DATA_HDR_LLID_MASK;
+ if (llid == BLE_LL_LLID_CTRL) {
+ opcode = txpdu->om_data[0];
+ if (opcode == BLE_LL_CTRL_START_ENC_REQ) {
+ is_start_enc_req = 1;
+ }
+ }
+
+ return is_start_enc_req;
+}
+
+/**
+ * Called to create and send a LL_START_ENC_REQ or LL_START_ENC_RSP
+ *
+ * @param connsm
+ * @param rej_opcode
+ * @param err
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_start_enc_send(struct ble_ll_conn_sm *connsm, uint8_t opcode)
+{
+ int rc;
+ struct os_mbuf *om;
+
+ om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN,
+ sizeof(struct ble_mbuf_hdr));
+ if (om) {
+ om->om_data[0] = opcode;
+ ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, 1);
+ rc = 0;
+ } else {
+ rc = -1;
+ }
+ return rc;
+}
+
+/**
+ * Create a link layer control "encrypt request" PDU.
+ *
+ * The LL_ENC_REQ PDU format is:
+ * Rand (8)
+ * EDIV (2)
+ * SKDm (8)
+ * IVm (4)
+ *
+ * The random number and encrypted diversifier come from the host command.
+ * Controller generates master portion of SDK and IV.
+ *
+ * NOTE: this function does not set the LL data pdu header nor does it
+ * set the opcode in the buffer.
+ *
+ * @param connsm
+ * @param dptr: Pointer to where control pdu payload starts
+ */
+static void
+ble_ll_ctrl_enc_req_make(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ htole64(dptr, connsm->enc_data.host_rand_num);
+ htole16(dptr + 8, connsm->enc_data.enc_div);
+
+#ifdef BLE_LL_ENCRYPT_USE_TEST_DATA
+ /* IV stored LSB to MSB, IVm is LSB, IVs is MSB */
+ htole64(dptr + 10, g_bletest_SKDm);
+ swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+ htole32(dptr + 18, g_bletest_IVm);
+ memcpy(connsm->enc_data.iv, dptr + 18, 4);
+ return;
+#endif
+
+ ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text + 8, 8);
+ swap_buf(dptr + 10, connsm->enc_data.enc_block.plain_text + 8, 8);
+ ble_ll_rand_data_get(connsm->enc_data.iv, 4);
+ memcpy(dptr + 18, connsm->enc_data.iv, 4);
+}
+
+/**
+ * Called when LL_ENC_RSP is received by the master.
+ *
+ * Context: Link Layer Task.
+ *
+ * Format of the LL_ENC_RSP is:
+ * SKDs (8)
+ * IVs (4)
+ *
+ * The master now has the long term key (from the start encrypt command)
+ * and the SKD (stored in the plain text encryption block). From this the
+ * sessionKey is generated.
+ *
+ * @param connsm
+ * @param dptr
+ */
+static void
+ble_ll_ctrl_rx_enc_rsp(struct ble_ll_conn_sm *connsm, uint8_t *dptr)
+{
+ /* Calculate session key now that we have received the ENC_RSP */
+ if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_ENCRYPT) {
+ swap_buf(connsm->enc_data.enc_block.plain_text, dptr, 8);
+ memcpy(connsm->enc_data.iv + 4, dptr + 8, 4);
+ ble_ll_calc_session_key(connsm);
+ connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_REQ_WAIT;
+ }
+}
+
+/**
+ * Called when we have received a LL control encryption request PDU. This
+ * should only be received by a slave.
+ *
+ * The LL_ENC_REQ PDU format is:
+ * Rand (8)
+ * EDIV (2)
+ * SKDm (8)
+ * IVm (4)
+ *
+ * This function returns the response opcode. Typically this will be ENC_RSP
+ * but it could be a reject ind. Note that the caller of this function
+ * will send the REJECT_IND_EXT if supported by remote.
+ *
+ * NOTE: if this is received by a master we will silently discard the PDU
+ * (denoted by return BLE_ERR_MAX).
+ *
+ * @param connsm
+ * @param dptr Pointer to start of encrypt request data.
+ * @param rspbuf
+ */
+static uint8_t
+ble_ll_ctrl_rx_enc_req(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t *rspdata)
+{
+ if (connsm->conn_role != BLE_LL_CONN_ROLE_SLAVE) {
+ return BLE_ERR_MAX;
+ }
+
+ /* Extract information from request */
+ connsm->enc_data.host_rand_num = le64toh(dptr);
+ connsm->enc_data.enc_div = le16toh(dptr + 8);
+
+#if BLE_LL_ENCRYPT_USE_TEST_DATA
+ swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+ memcpy(connsm->enc_data.iv, dptr + 18, 4);
+
+ htole64(rspdata, g_bletest_SKDs);
+ swap_buf(connsm->enc_data.enc_block.plain_text, rspdata, 8);
+ htole32(rspdata + 8, g_bletest_IVs);
+ memcpy(connsm->enc_data.iv + 4, rspdata + 8, 4);
+ return BLE_LL_CTRL_ENC_RSP;
+#endif
+
+ swap_buf(connsm->enc_data.enc_block.plain_text + 8, dptr + 10, 8);
+ memcpy(connsm->enc_data.iv, dptr + 18, 4);
+
+ /* Create the ENC_RSP. Concatenate our SKD and IV */
+ ble_ll_rand_data_get(connsm->enc_data.enc_block.plain_text, 8);
+ swap_buf(rspdata, connsm->enc_data.enc_block.plain_text, 8);
+ ble_ll_rand_data_get(connsm->enc_data.iv + 4, 4);
+ memcpy(rspdata + 8, connsm->enc_data.iv + 4, 4);
+
+ return BLE_LL_CTRL_ENC_RSP;
+}
+
+static uint8_t
+ble_ll_ctrl_rx_start_enc_req(struct ble_ll_conn_sm *connsm)
+{
+ int rc;
+
+ /* Only master should receive start enc request */
+ rc = BLE_ERR_MAX;
+ if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+ /* We only want to send a START_ENC_RSP if we havent yet */
+ if (connsm->enc_data.enc_state == CONN_ENC_S_START_ENC_REQ_WAIT) {
+ connsm->enc_data.enc_state = CONN_ENC_S_START_ENC_RSP_WAIT;
+ rc = BLE_LL_CTRL_START_ENC_RSP;
+ }
+ }
+ return rc;
+}
+
+/**
+ * Called when we have received a LL_CTRL_START_ENC_RSP.
+ *
+ * Context: Link-layer task
+ *
+ * @param connsm
+ *
+ * @return uint8_t
+ */
+static uint8_t
+ble_ll_ctrl_rx_start_enc_rsp(struct ble_ll_conn_sm *connsm)
+{
+ int rc;
+
+ /* Not in proper state. Discard */
+ if (connsm->enc_data.enc_state != CONN_ENC_S_START_ENC_RSP_WAIT) {
+ return BLE_ERR_MAX;
+ }
+
+ /* If master, we are done. Stop control procedure and sent event to host */
+ if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
+ /* We are encrypted */
+ connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED;
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+ rc = BLE_ERR_MAX;
+ } else {
+ /* Procedure has completed but slave needs to send START_ENC_RSP */
+ rc = BLE_LL_CTRL_START_ENC_RSP;
+ }
+
+ /*
+ * XXX: for now, a Slave sends this event when it receivest the
+ * START_ENC_RSP from the master. It might be technically incorrect
+ * to send it before we transmit our own START_ENC_RSP.
+ */
+ ble_ll_hci_ev_encrypt_chg(connsm, BLE_ERR_SUCCESS);
+
+ return rc;
+}
+
+#endif
+
/**
* Called to make a connection parameter request or response control pdu.
*
@@ -519,6 +855,48 @@ ble_ll_ctrl_conn_param_reply(struct ble_ll_conn_sm *connsm, uint8_t *rsp,
}
/**
+ * Called when we have received a LL_REJECT_IND or LL_REJECT_IND_EXT link
+ * layer control Dat Channel pdu.
+ *
+ * @param connsm
+ * @param dptr
+ * @param opcode
+ */
+static void
+ble_ll_ctrl_rx_reject_ind(struct ble_ll_conn_sm *connsm, uint8_t *dptr,
+ uint8_t opcode)
+{
+ uint8_t ble_error;
+
+ /* Get error out of received PDU */
+ if (opcode == BLE_LL_CTRL_REJECT_IND) {
+ ble_error = dptr[0];
+ } else {
+ ble_error = dptr[1];
+ }
+
+ /* XXX: should I check to make sure the rejected opcode is sane
+ if we receive ind ext? */
+ switch (connsm->cur_ctrl_proc) {
+ case BLE_LL_CTRL_PROC_CONN_PARAM_REQ:
+ if (opcode == BLE_LL_CTRL_REJECT_IND_EXT) {
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
+ ble_ll_hci_ev_conn_update(connsm, ble_error);
+ }
+ break;
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ case BLE_LL_CTRL_PROC_ENCRYPT:
+ ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_ENCRYPT);
+ ble_ll_hci_ev_encrypt_chg(connsm, ble_error);
+ connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+ break;
+#endif
+ default:
+ break;
+ }
+}
+
+/**
* Called when we receive a connection update event
*
* @param connsm
@@ -815,22 +1193,25 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
uint8_t len;
uint8_t opcode;
uint8_t *dptr;
+ uint8_t *ctrdata;
struct os_mbuf *om;
/* Get an mbuf for the control pdu */
om = os_msys_get_pkthdr(BLE_LL_CTRL_MAX_PDU_LEN, sizeof(struct ble_mbuf_hdr));
if (om) {
+ /* The control data starts after the opcode (1 byte) */
dptr = om->om_data;
+ ctrdata = dptr + 1;
switch (ctrl_proc) {
case BLE_LL_CTRL_PROC_CONN_UPDATE:
opcode = BLE_LL_CTRL_CONN_UPDATE_REQ;
- ble_ll_ctrl_conn_upd_make(connsm, dptr + 1, NULL);
+ ble_ll_ctrl_conn_upd_make(connsm, ctrdata, NULL);
break;
case BLE_LL_CTRL_PROC_CHAN_MAP_UPD:
opcode = BLE_LL_CTRL_CHANNEL_MAP_REQ;
- ble_ll_ctrl_chanmap_req_make(connsm, dptr + 1);
+ ble_ll_ctrl_chanmap_req_make(connsm, ctrdata);
break;
case BLE_LL_CTRL_PROC_FEATURE_XCHG:
if (connsm->conn_role == BLE_LL_CONN_ROLE_MASTER) {
@@ -838,24 +1219,31 @@ ble_ll_ctrl_proc_init(struct ble_ll_conn_sm *connsm, int ctrl_proc)
} else {
opcode = BLE_LL_CTRL_SLAVE_FEATURE_REQ;
}
- dptr[1] = ble_ll_read_supp_features();
+ ctrdata[0] = ble_ll_read_supp_features();
break;
case BLE_LL_CTRL_PROC_VERSION_XCHG:
opcode = BLE_LL_CTRL_VERSION_IND;
- ble_ll_ctrl_version_ind_make(connsm, dptr + 1);
+ ble_ll_ctrl_version_ind_make(connsm, ctrdata);
break;
case BLE_LL_CTRL_PROC_TERMINATE:
opcode = BLE_LL_CTRL_TERMINATE_IND;
- dptr[1] = connsm->disconnect_reason;
+ ctrdata[0] = connsm->disconnect_reason;
break;
case BLE_LL_CTRL_PROC_CONN_PARAM_REQ:
opcode = BLE_LL_CTRL_CONN_PARM_REQ;
- ble_ll_ctrl_conn_param_pdu_make(connsm, dptr + 1, NULL);
+ ble_ll_ctrl_conn_param_pdu_make(connsm, ctrdata, NULL);
break;
case BLE_LL_CTRL_PROC_DATA_LEN_UPD:
opcode = BLE_LL_CTRL_LENGTH_REQ;
ble_ll_ctrl_datalen_upd_make(connsm, dptr);
break;
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ /* XXX: deal with already encrypted connection.*/
+ case BLE_LL_CTRL_PROC_ENCRYPT:
+ opcode = BLE_LL_CTRL_ENC_REQ;
+ ble_ll_ctrl_enc_req_make(connsm, ctrdata);
+ break;
+#endif
default:
assert(0);
break;
@@ -895,29 +1283,6 @@ ble_ll_ctrl_is_terminate_ind(uint8_t hdr, uint8_t opcode)
}
/**
- * Called to determine if the pdu is a TERMINATE_IND
- *
- * @param hdr
- * @param opcode
- *
- * @return int
- */
-int
-ble_ll_ctrl_is_reject_ind_ext(uint8_t hdr, uint8_t opcode)
-{
- int rc;
-
- rc = 0;
- if ((hdr & BLE_LL_DATA_HDR_LLID_MASK) == BLE_LL_LLID_CTRL) {
- if (opcode == BLE_LL_CTRL_REJECT_IND_EXT) {
- rc = 1;
- }
- }
- return rc;
-}
-
-
-/**
* Stops the LL control procedure indicated by 'ctrl_proc'.
*
* Context: Link Layer task
@@ -1079,18 +1444,31 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
uint8_t rsp_opcode;
uint8_t *dptr;
uint8_t *rspbuf;
+ uint8_t *rspdata;
/* XXX: where do we validate length received and packet header length?
* do this in LL task when received. Someplace!!! What I mean
* is we should validate the over the air length with the mbuf length.
Should the PHY do that???? */
- /* Get length and opcode from PDU.*/
+ /*
+ * dptr points to om_data pointer. The first byte of om_data is the
+ * first byte of the Data Channel PDU header. Get length from header and
+ * opcode from LL control PDU.
+ */
dptr = om->om_data;
- rspbuf = dptr;
len = dptr[1];
opcode = dptr[2];
+ /*
+ * rspbuf points to first byte of response. The response buffer does not
+ * contain the Data Channel PDU. Thus, the first byte of rspbuf is the
+ * LL control PDU payload (the opcode of the control PDU). rspdata
+ * points to CtrData in the control PDU.
+ */
+ rspbuf = dptr;
+ rspdata = rspbuf + 1;
+
/* Move data pointer to start of control data (2 byte PDU hdr + opcode) */
dptr += (BLE_LL_PDU_HDR_LEN + 1);
@@ -1107,6 +1485,8 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
goto rx_malformed_ctrl;
}
+ ble_ll_log(BLE_LL_LOG_ID_LL_CTRL_RX, opcode, len, 0);
+
/* Check if the feature is supported. */
switch (opcode) {
case BLE_LL_CTRL_LENGTH_REQ:
@@ -1132,8 +1512,20 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
if (feature) {
features = ble_ll_read_supp_features();
if ((features & feature) == 0) {
- /* Construct unknown rsp pdu */
- rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+ if (opcode == BLE_LL_CTRL_ENC_REQ) {
+ if (connsm->common_features & BLE_LL_FEAT_EXTENDED_REJ) {
+ rsp_opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+ rspbuf[1] = opcode;
+ rspbuf[2] = BLE_ERR_UNSUPP_REM_FEATURE;
+
+ } else {
+ rsp_opcode = BLE_LL_CTRL_REJECT_IND;
+ rspbuf[1] = BLE_ERR_UNSUPP_REM_FEATURE;
+ }
+ } else {
+ /* Construct unknown rsp pdu */
+ rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
+ }
goto ll_ctrl_send_rsp;
}
}
@@ -1194,19 +1586,32 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
}
break;
case BLE_LL_CTRL_VERSION_IND:
- rsp_opcode = ble_ll_ctrl_rx_version_ind(connsm, dptr, rspbuf + 1);
+ rsp_opcode = ble_ll_ctrl_rx_version_ind(connsm, dptr, rspdata);
break;
case BLE_LL_CTRL_SLAVE_FEATURE_REQ:
rsp_opcode = ble_ll_ctrl_rx_feature_req(connsm, dptr, rspbuf, opcode);
break;
-#if defined(BLE_LL_CFG_FEAT_LE_ENCYPTION)
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
case BLE_LL_CTRL_ENC_REQ:
- rsp_opcode = ble_ll_ctrl_rx_enc_req(connsm, dptr, rspbuf);
+ rsp_opcode = ble_ll_ctrl_rx_enc_req(connsm, dptr, rspdata);
+ break;
+ case BLE_LL_CTRL_ENC_RSP:
+ ble_ll_ctrl_rx_enc_rsp(connsm, dptr);
+ break;
case BLE_LL_CTRL_START_ENC_REQ:
+ rsp_opcode = ble_ll_ctrl_rx_start_enc_req(connsm);
+ break;
+
+ case BLE_LL_CTRL_START_ENC_RSP:
+ rsp_opcode = ble_ll_ctrl_rx_start_enc_rsp(connsm);
+ break;
+
case BLE_LL_CTRL_PAUSE_ENC_REQ:
+ /* XXX: implement */
break;
#endif
case BLE_LL_CTRL_PING_REQ:
+ /* XXX: implement */
rsp_opcode = BLE_LL_CTRL_UNKNOWN_RSP;
break;
case BLE_LL_CTRL_CONN_PARM_REQ:
@@ -1215,14 +1620,10 @@ ble_ll_ctrl_rx_pdu(struct ble_ll_conn_sm *connsm, struct os_mbuf *om)
case BLE_LL_CTRL_CONN_PARM_RSP:
rsp_opcode = ble_ll_ctrl_rx_conn_param_rsp(connsm, dptr, rspbuf);
break;
+ /* Fall-through intentional... */
+ case BLE_LL_CTRL_REJECT_IND:
case BLE_LL_CTRL_REJECT_IND_EXT:
- /* XXX: not sure what other control procedures to check out, but
- add them when needed */
- /* XXX: should I check to make sure that the rejected opcode is sane? */
- if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) {
- ble_ll_ctrl_proc_stop(connsm, BLE_LL_CTRL_PROC_CONN_PARAM_REQ);
- ble_ll_hci_ev_conn_update(connsm, dptr[1]);
- }
+ ble_ll_ctrl_rx_reject_ind(connsm, dptr, opcode);
break;
default:
/* Nothing to do here */
@@ -1234,6 +1635,10 @@ ll_ctrl_send_rsp:
if (rsp_opcode == 255) {
os_mbuf_free_chain(om);
} else {
+ /*
+ * Write the response opcode into the buffer. If this is an unknown
+ * response, put opcode of unknown pdu into buffer.
+ */
rspbuf[0] = rsp_opcode;
if (rsp_opcode == BLE_LL_CTRL_UNKNOWN_RSP) {
rspbuf[1] = opcode;
@@ -1249,8 +1654,7 @@ rx_malformed_ctrl:
}
/**
- * Called to creeate and send a REJECT_IND_EXT control PDU
- *
+ * Called to create and send a REJECT_IND_EXT control PDU or a REJECT_IND
*
* @param connsm
* @param rej_opcode
@@ -1259,11 +1663,12 @@ rx_malformed_ctrl:
* @return int
*/
int
-ble_ll_ctrl_reject_ind_ext_send(struct ble_ll_conn_sm *connsm,
- uint8_t rej_opcode, uint8_t err)
+ble_ll_ctrl_reject_ind_send(struct ble_ll_conn_sm *connsm, uint8_t rej_opcode,
+ uint8_t err)
{
int rc;
uint8_t len;
+ uint8_t opcode;
uint8_t *rspbuf;
struct os_mbuf *om;
@@ -1271,10 +1676,21 @@ ble_ll_ctrl_reject_ind_ext_send(struct ble_ll_conn_sm *connsm,
sizeof(struct ble_mbuf_hdr));
if (om) {
rspbuf = om->om_data;
- rspbuf[0] = BLE_LL_CTRL_REJECT_IND_EXT;
- rspbuf[1] = rej_opcode;
- rspbuf[2] = err;
- len = BLE_LL_CTRL_REJECT_IND_EXT_LEN + 1;
+ opcode = BLE_LL_CTRL_REJECT_IND_EXT;
+ if (rej_opcode == BLE_LL_CTRL_ENC_REQ) {
+ if ((connsm->common_features & BLE_LL_FEAT_EXTENDED_REJ) == 0) {
+ opcode = BLE_LL_CTRL_REJECT_IND;
+ }
+ }
+ rspbuf[0] = opcode;
+ if (opcode == BLE_LL_CTRL_REJECT_IND) {
+ rspbuf[1] = err;
+ len = BLE_LL_CTRL_REJ_IND_LEN + 1;
+ } else {
+ rspbuf[1] = rej_opcode;
+ rspbuf[2] = err;
+ len = BLE_LL_CTRL_REJECT_IND_EXT_LEN + 1;
+ }
ble_ll_conn_enqueue_pkt(connsm, om, BLE_LL_LLID_CTRL, len);
rc = 0;
} else {
@@ -1282,3 +1698,65 @@ ble_ll_ctrl_reject_ind_ext_send(struct ble_ll_conn_sm *connsm,
}
return rc;
}
+
+/**
+ * Called when a Link Layer Control pdu has been transmitted successfully.
+ * This is called when we have a received a PDU during the ISR.
+ *
+ * Context: ISR
+ *
+ * @param txpdu
+ *
+ * @return int
+ */
+int
+ble_ll_ctrl_tx_done(struct os_mbuf *txpdu, struct ble_ll_conn_sm *connsm)
+{
+ int rc;
+ uint8_t opcode;
+
+ rc = 0;
+ opcode = txpdu->om_data[0];
+ switch (opcode) {
+ case BLE_LL_CTRL_TERMINATE_IND:
+ connsm->csmflags.cfbit.terminate_ind_txd = 1;
+ rc = -1;
+ break;
+ case BLE_LL_CTRL_REJECT_IND_EXT:
+ if (connsm->cur_ctrl_proc == BLE_LL_CTRL_PROC_CONN_PARAM_REQ) {
+ connsm->reject_reason = txpdu->om_data[2];
+ connsm->csmflags.cfbit.host_expects_upd_event = 1;
+ }
+#if defined (BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ if (connsm->enc_data.enc_state > CONN_ENC_S_ENCRYPTED) {
+ connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+ }
+#endif
+ break;
+ case BLE_LL_CTRL_REJECT_IND:
+#if defined (BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ connsm->enc_data.enc_state = CONN_ENC_S_UNENCRYPTED;
+#endif
+ break;
+#if defined (BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ case BLE_LL_CTRL_ENC_REQ:
+ connsm->enc_data.enc_state = CONN_ENC_S_ENC_RSP_WAIT;
+ break;
+ case BLE_LL_CTRL_ENC_RSP:
+ connsm->enc_data.enc_state = CONN_ENC_S_LTK_REQ_WAIT;
+ connsm->csmflags.cfbit.send_ltk_req = 1;
+ break;
+ case BLE_LL_CTRL_START_ENC_RSP:
+ /* We are encrypted */
+ if (connsm->conn_role == BLE_LL_CONN_ROLE_SLAVE) {
+ connsm->enc_data.enc_state = CONN_ENC_S_ENCRYPTED;
+ }
+ break;
+#endif
+ default:
+ break;
+ }
+
+ os_mbuf_free_chain(txpdu);
+ return rc;
+}
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_hci.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_hci.c b/net/nimble/controller/src/ble_ll_hci.c
index 1c94a1e..14a6b52 100644
--- a/net/nimble/controller/src/ble_ll_hci.c
+++ b/net/nimble/controller/src/ble_ll_hci.c
@@ -599,6 +599,16 @@ ble_ll_hci_le_cmd_proc(uint8_t *cmdbuf, uint16_t ocf, uint8_t *rsplen)
case BLE_HCI_OCF_LE_RAND:
rc = ble_ll_hci_le_rand(rspbuf, rsplen);
break;
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+ case BLE_HCI_OCF_LE_START_ENCRYPT:
+ rc = ble_ll_conn_hci_le_start_encrypt(cmdbuf);
+ break;
+ case BLE_HCI_OCF_LE_LT_KEY_REQ_REPLY:
+ case BLE_HCI_OCF_LE_LT_KEY_REQ_NEG_REPLY:
+ rc = ble_ll_conn_hci_le_ltk_reply(cmdbuf, rspbuf, ocf);
+ *rsplen = sizeof(uint16_t);
+ break;
+#endif
case BLE_HCI_OCF_LE_RD_SUPP_STATES :
rc = ble_ll_hci_le_read_supp_states(rspbuf, rsplen);
break;
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_hci_ev.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_hci_ev.c b/net/nimble/controller/src/ble_ll_hci_ev.c
index 4bad8a1..d35d59b 100644
--- a/net/nimble/controller/src/ble_ll_hci_ev.c
+++ b/net/nimble/controller/src/ble_ll_hci_ev.c
@@ -26,6 +26,10 @@
#include "controller/ble_ll_ctrl.h"
#include "ble_ll_conn_priv.h"
+#if (BLETEST_CONCURRENT_CONN_TEST == 1)
+extern void bletest_ltk_req_reply(uint16_t handle);
+#endif
+
/**
* Send a data length change event for a connection to the host.
*
@@ -106,6 +110,65 @@ ble_ll_hci_ev_conn_update(struct ble_ll_conn_sm *connsm, uint8_t status)
}
}
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+void
+ble_ll_hci_ev_encrypt_chg(struct ble_ll_conn_sm *connsm, uint8_t status)
+{
+ uint8_t *evbuf;
+
+ if (ble_ll_hci_is_event_enabled(BLE_HCI_EVCODE_ENCRYPT_CHG)) {
+ evbuf = os_memblock_get(&g_hci_cmd_pool);
+ if (evbuf) {
+ evbuf[0] = BLE_HCI_EVCODE_ENCRYPT_CHG;
+ evbuf[1] = BLE_HCI_EVENT_ENCRYPT_CHG_LEN;
+ evbuf[2] = status;
+ htole16(evbuf + 3, connsm->conn_handle);
+ if (status == BLE_ERR_SUCCESS) {
+ evbuf[5] = 0x01;
+ } else {
+ evbuf[5] = 0;
+ }
+ ble_ll_hci_event_send(evbuf);
+ }
+ }
+}
+
+/**
+ * Send a long term key request event for a connection to the host.
+ *
+ * @param connsm Pointer to connection state machine
+ */
+int
+ble_ll_hci_ev_ltk_req(struct ble_ll_conn_sm *connsm)
+{
+ int rc;
+ uint8_t *evbuf;
+
+ if (ble_ll_hci_is_le_event_enabled(BLE_HCI_LE_SUBEV_LT_KEY_REQ)) {
+ evbuf = os_memblock_get(&g_hci_cmd_pool);
+ if (evbuf) {
+ evbuf[0] = BLE_HCI_EVCODE_LE_META;
+ evbuf[1] = BLE_HCI_LE_LT_KEY_REQ_LEN;
+ evbuf[2] = BLE_HCI_LE_SUBEV_LT_KEY_REQ;
+ htole16(evbuf + 3, connsm->conn_handle);
+ htole64(evbuf + 5, connsm->enc_data.host_rand_num);
+ htole16(evbuf + 13, connsm->enc_data.enc_div);
+ ble_ll_hci_event_send(evbuf);
+ }
+ rc = 0;
+ } else {
+ rc = -1;
+ }
+
+#if (BLETEST_CONCURRENT_CONN_TEST == 1)
+ if (rc == 0) {
+ bletest_ltk_req_reply(connsm->conn_handle);
+ }
+#endif
+ return rc;
+}
+#endif
+
void
ble_ll_hci_ev_rd_rem_used_feat(struct ble_ll_conn_sm *connsm, uint8_t status)
{
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_scan.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_scan.c b/net/nimble/controller/src/ble_ll_scan.c
index 27805ba..dbcc63a 100644
--- a/net/nimble/controller/src/ble_ll_scan.c
+++ b/net/nimble/controller/src/ble_ll_scan.c
@@ -505,6 +505,10 @@ ble_ll_scan_start(struct ble_ll_scan_sm *scansm, uint8_t chan)
*/
ble_phy_set_txend_cb(NULL, NULL);
+#ifdef BLE_LL_CFG_FEAT_LE_ENCRYPTION
+ ble_phy_encrypt_disable();
+#endif
+
/* Start receiving */
rc = ble_phy_rx();
if (!rc) {
http://git-wip-us.apache.org/repos/asf/incubator-mynewt-core/blob/33afb22a/net/nimble/controller/src/ble_ll_supp_cmd.c
----------------------------------------------------------------------
diff --git a/net/nimble/controller/src/ble_ll_supp_cmd.c b/net/nimble/controller/src/ble_ll_supp_cmd.c
index 68a9fd5..d921ccf 100644
--- a/net/nimble/controller/src/ble_ll_supp_cmd.c
+++ b/net/nimble/controller/src/ble_ll_supp_cmd.c
@@ -126,7 +126,11 @@
)
/* Octet 28 */
+#if defined(BLE_LL_CFG_FEAT_LE_ENCRYPTION)
+#define BLE_SUPP_CMD_LE_START_ENCRYPT (1 << 0)
+#else
#define BLE_SUPP_CMD_LE_START_ENCRYPT (0 << 0)
+#endif
#define BLE_SUPP_CMD_LE_LTK_REQ_REPLY (0 << 1)
#define BLE_SUPP_CMD_LE_LTK_REQ_NEG_REPLY (0 << 2)
#define BLE_SUPP_CMD_LE_READ_SUPP_STATES (1 << 3)