BLE Host - rx slave security request.

6 files changed, 203 insertions, 1 deletions

diff --git a/net/nimble/host/src/ble_l2cap_sm.c b/net/nimble/host/src/ble_l2cap_sm.c
index 97035199..05adfca9 100644
--- a/net/nimble/host/src/ble_l2cap_sm.c
+++ b/net/nimble/host/src/ble_l2cap_sm.c
@@ -114,6 +114,7 @@ static ble_l2cap_sm_rx_fn ble_l2cap_sm_rx_pair_confirm;
 static ble_l2cap_sm_rx_fn ble_l2cap_sm_rx_pair_random;
 static ble_l2cap_sm_rx_fn ble_l2cap_sm_rx_pair_fail;
 static ble_l2cap_sm_rx_fn ble_l2cap_sm_rx_key_exchange;
+static ble_l2cap_sm_rx_fn ble_l2cap_sm_rx_sec_req;
 
 static ble_l2cap_sm_rx_fn * const ble_l2cap_sm_dispatch[] = {
    [BLE_L2CAP_SM_OP_PAIR_REQ] = ble_l2cap_sm_rx_pair_req,
@@ -126,7 +127,7 @@ static ble_l2cap_sm_rx_fn * const ble_l2cap_sm_dispatch[] = {
    [BLE_L2CAP_SM_OP_IDENTITY_INFO] = ble_l2cap_sm_rx_key_exchange,
    [BLE_L2CAP_SM_OP_IDENTITY_ADDR_INFO] = ble_l2cap_sm_rx_key_exchange,
    [BLE_L2CAP_SM_OP_SIGN_INFO] = ble_l2cap_sm_rx_key_exchange,
-   [BLE_L2CAP_SM_OP_SEC_REQ] = ble_l2cap_sm_rx_noop,
+   [BLE_L2CAP_SM_OP_SEC_REQ] = ble_l2cap_sm_rx_sec_req,
    [BLE_L2CAP_SM_OP_PAIR_PUBLIC_KEY] = ble_l2cap_sm_rx_noop,
    [BLE_L2CAP_SM_OP_PAIR_DHKEY_CHECK] = ble_l2cap_sm_rx_noop,
    [BLE_L2CAP_SM_OP_PAIR_KEYPRESS_NOTIFY] = ble_l2cap_sm_rx_noop,
@@ -1757,6 +1758,59 @@ ble_l2cap_sm_rx_encryption_change(struct hci_encrypt_change *evt)
 }
 
 static int
+ble_l2cap_sm_rx_sec_req(uint16_t conn_handle, uint8_t op, struct os_mbuf **om)
+{
+    struct ble_l2cap_sm_sec_req cmd;
+    struct ble_l2cap_sm_proc *proc;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    rc = ble_hs_misc_pullup_base(om, BLE_L2CAP_SM_SEC_REQ_SZ);
+    if (rc != 0) {
+        return rc;
+    }
+
+    ble_l2cap_sm_sec_req_parse((*om)->om_data, (*om)->om_len, &cmd);
+
+    BLE_HS_LOG(DEBUG, "rxed sm sec req; authreq=%d\n", cmd.authreq);
+
+    ble_hs_lock();
+
+    /* Only handle the security request if a procedure isn't already in
+     * progress for this connection.
+     */
+    proc = ble_l2cap_sm_proc_find(conn_handle, BLE_L2CAP_SM_PROC_STATE_NONE,
+                                  -1, NULL);
+    if (proc != NULL) {
+        rc = BLE_HS_EALREADY;
+    } else {
+        conn = ble_hs_conn_find(conn_handle);
+        if (conn == NULL) {
+            rc = BLE_HS_ENOTCONN;
+        } else if (!(conn->bhc_flags & BLE_HS_CONN_F_MASTER)) {
+            rc = BLE_HS_SM_US_ERR(BLE_L2CAP_SM_ERR_CMD_NOT_SUPP);
+            ble_l2cap_sm_pair_fail_tx(conn_handle,
+                                      BLE_L2CAP_SM_ERR_CMD_NOT_SUPP);
+        } else {
+            rc = 0;
+        }
+    }
+
+    ble_hs_unlock();
+
+    if (rc == 0) {
+        /* XXX: Ask app / someone if there is a persisted LTK such that:
+         *     o It corresponds to this peer.
+         *     o It meets the specified authreq criteria.
+         * For now, assume we don't have an appropriate LTK; initiate pairing.
+         */
+        rc = ble_l2cap_sm_pair_initiate(conn_handle);
+    }
+
+    return rc;
+}
+
+static int
 ble_l2cap_sm_rx(uint16_t conn_handle, struct os_mbuf **om)
 {
     ble_l2cap_sm_rx_fn *rx_cb;
diff --git a/net/nimble/host/src/ble_l2cap_sm_cmd.c b/net/nimble/host/src/ble_l2cap_sm_cmd.c
index 62149e6e..14c50934 100644
--- a/net/nimble/host/src/ble_l2cap_sm_cmd.c
+++ b/net/nimble/host/src/ble_l2cap_sm_cmd.c
@@ -471,4 +471,50 @@ done:
     return rc;
 }
 
+void
+ble_l2cap_sm_sec_req_parse(void *payload, int len,
+                           struct ble_l2cap_sm_sec_req *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SM_SEC_REQ_SZ);
+
+    u8ptr = payload;
+    cmd->authreq = *u8ptr;
+}
+
+void
+ble_l2cap_sm_sec_req_write(void *payload, int len,
+                           struct ble_l2cap_sm_sec_req *cmd)
+{
+    uint8_t *u8ptr;
+
+    BLE_HS_DBG_ASSERT(len >= BLE_L2CAP_SM_HDR_SZ + BLE_L2CAP_SM_SEC_REQ_SZ);
+
+    u8ptr = payload;
+
+    u8ptr[0] = BLE_L2CAP_SM_OP_SEC_REQ;
+    u8ptr[1] = cmd->authreq;
+}
+
+int
+ble_l2cap_sm_sec_req_tx(uint16_t conn_handle, struct ble_l2cap_sm_sec_req *cmd)
+{
+    struct os_mbuf *txom;
+    int rc;
+
+    rc = ble_l2cap_sm_init_req(BLE_L2CAP_SM_SEC_REQ_SZ, &txom);
+    if (rc != 0) {
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    ble_l2cap_sm_sec_req_write(txom->om_data, txom->om_len, cmd);
+    rc = ble_l2cap_sm_tx(conn_handle, txom);
+    txom = NULL;
+
+done:
+    os_mbuf_free_chain(txom);
+    return rc;
+}
 #endif
diff --git a/net/nimble/host/src/ble_l2cap_sm_priv.h b/net/nimble/host/src/ble_l2cap_sm_priv.h
index 5838ba5b..99070dc7 100644
--- a/net/nimble/host/src/ble_l2cap_sm_priv.h
+++ b/net/nimble/host/src/ble_l2cap_sm_priv.h
@@ -141,6 +141,17 @@ struct ble_l2cap_sm_signing_info {
     uint8_t sig_key_le[16];
 };
 
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | (Code=0x0B)                        | 1                 |
+ * | authreq                            | 1                 |
+ */
+#define BLE_L2CAP_SM_SEC_REQ_SZ         1
+struct ble_l2cap_sm_sec_req {
+    uint8_t authreq;
+};
+
 
 #if NIMBLE_OPT_SM
 
@@ -206,6 +217,12 @@ void ble_l2cap_sm_signing_info_parse(void *payload, int len,
                                      struct ble_l2cap_sm_signing_info *cmd);
 int ble_l2cap_sm_signing_info_tx(uint16_t conn_handle,
                                  struct ble_l2cap_sm_signing_info *cmd);
+void ble_l2cap_sm_sec_req_parse(void *payload, int len,
+                                struct ble_l2cap_sm_sec_req *cmd);
+void ble_l2cap_sm_sec_req_write(void *payload, int len,
+                                struct ble_l2cap_sm_sec_req *cmd);
+int ble_l2cap_sm_sec_req_tx(uint16_t conn_handle,
+                            struct ble_l2cap_sm_sec_req *cmd);
 
 void ble_l2cap_sm_rx_encryption_change(struct hci_encrypt_change *evt);
 int ble_l2cap_sm_rx_lt_key_req(struct hci_le_lt_key_req *evt);
diff --git a/net/nimble/host/src/test/ble_hs_test_util.c b/net/nimble/host/src/test/ble_hs_test_util.c
index d0b8c0b5..bfb46966 100644
--- a/net/nimble/host/src/test/ble_hs_test_util.c
+++ b/net/nimble/host/src/test/ble_hs_test_util.c
@@ -117,6 +117,14 @@ ble_hs_test_util_prev_tx_queue_sz(void)
     return cnt;
 }
 
+void
+ble_hs_test_util_prev_tx_queue_clear(void)
+{
+    while (!STAILQ_EMPTY(&ble_hs_test_util_prev_tx_queue)) {
+        ble_hs_test_util_prev_tx_dequeue();
+    }
+}
+
 void *
 ble_hs_test_util_get_first_hci_tx(void)
 {
diff --git a/net/nimble/host/src/test/ble_hs_test_util.h b/net/nimble/host/src/test/ble_hs_test_util.h
index baffffc0..cd754526 100644
--- a/net/nimble/host/src/test/ble_hs_test_util.h
+++ b/net/nimble/host/src/test/ble_hs_test_util.h
@@ -38,6 +38,7 @@ void ble_hs_test_util_prev_tx_enqueue(struct os_mbuf *om);
 struct os_mbuf *ble_hs_test_util_prev_tx_dequeue(void);
 struct os_mbuf *ble_hs_test_util_prev_tx_dequeue_pullup(void);
 int ble_hs_test_util_prev_tx_queue_sz(void);
+void ble_hs_test_util_prev_tx_queue_clear(void);
 
 void ble_hs_test_util_set_ack_params(uint16_t opcode, uint8_t status,
                                      void *params, uint8_t params_len);
diff --git a/net/nimble/host/src/test/ble_l2cap_sm_test.c b/net/nimble/host/src/test/ble_l2cap_sm_test.c
index 43d93501..5e3dce92 100644
--- a/net/nimble/host/src/test/ble_l2cap_sm_test.c
+++ b/net/nimble/host/src/test/ble_l2cap_sm_test.c
@@ -248,6 +248,36 @@ ble_l2cap_sm_test_util_rx_random(uint16_t conn_handle,
     TEST_ASSERT_FATAL(rc == exp_status);
 }
 
+static void
+ble_l2cap_sm_test_util_rx_sec_req(uint16_t conn_handle,
+                                  struct ble_l2cap_sm_sec_req *cmd,
+                                  int exp_status)
+{
+    struct hci_data_hdr hci_hdr;
+    struct os_mbuf *om;
+    void *v;
+    int payload_len;
+    int rc;
+
+    hci_hdr = BLE_L2CAP_SM_TEST_UTIL_HCI_HDR(
+        2, BLE_HCI_PB_FIRST_FLUSH,
+        BLE_L2CAP_HDR_SZ + BLE_L2CAP_SM_HDR_SZ + BLE_L2CAP_SM_SEC_REQ_SZ);
+
+    om = ble_hs_misc_pkthdr();
+    TEST_ASSERT_FATAL(om != NULL);
+
+    payload_len = BLE_L2CAP_SM_HDR_SZ + BLE_L2CAP_SM_SEC_REQ_SZ;
+
+    v = os_mbuf_extend(om, payload_len);
+    TEST_ASSERT_FATAL(v != NULL);
+
+    ble_l2cap_sm_sec_req_write(v, payload_len, cmd);
+
+    rc = ble_hs_test_util_l2cap_rx_first_frag(conn_handle, BLE_L2CAP_CID_SM,
+                                              &hci_hdr, om);
+    TEST_ASSERT_FATAL(rc == exp_status);
+}
+
 static struct os_mbuf *
 ble_l2cap_sm_test_util_verify_tx_hdr(uint8_t sm_op, uint16_t payload_len)
 {
@@ -1638,6 +1668,51 @@ TEST_CASE(ble_l2cap_sm_test_case_conn_broken)
     TEST_ASSERT(!ble_l2cap_sm_test_sec_state.authenticated);
 }
 
+TEST_CASE(ble_l2cap_sm_test_case_peer_sec_req_inval)
+{
+    struct ble_l2cap_sm_pair_fail fail;
+    struct ble_l2cap_sm_sec_req sec_req;
+    struct ble_hs_conn *conn;
+    int rc;
+
+    ble_l2cap_sm_test_util_init();
+
+    ble_hs_test_util_create_conn(2, ((uint8_t[6]){1,2,3,5,6,7}),
+                                 ble_l2cap_sm_test_util_conn_cb,
+                                 NULL);
+
+    /* This test inspects and modifies the connection object without locking
+     * the host mutex.  It is not OK for real code to do this, but this test
+     * can assume the connection list is unchanging.
+     */
+    ble_hs_lock();
+    conn = ble_hs_conn_find(2);
+    TEST_ASSERT_FATAL(conn != NULL);
+    ble_hs_unlock();
+
+    /*** We are the slave; reject the security request. */
+    conn->bhc_flags &= ~BLE_HS_CONN_F_MASTER;
+
+    sec_req.authreq = 0;
+    ble_l2cap_sm_test_util_rx_sec_req(
+        2, &sec_req, BLE_HS_SM_US_ERR(BLE_L2CAP_SM_ERR_CMD_NOT_SUPP));
+
+    ble_hs_test_util_tx_all();
+
+    fail.reason = BLE_L2CAP_SM_ERR_CMD_NOT_SUPP;
+    ble_l2cap_sm_test_util_verify_tx_pair_fail(&fail);
+
+    /*** Pairing already in progress; ignore security request. */
+    rc = ble_l2cap_sm_pair_initiate(2);
+    TEST_ASSERT_FATAL(rc == 0);
+    ble_hs_test_util_tx_all();
+    ble_hs_test_util_prev_tx_queue_clear();
+
+    ble_l2cap_sm_test_util_rx_sec_req(2, &sec_req, BLE_HS_EALREADY);
+    ble_hs_test_util_tx_all();
+    TEST_ASSERT(ble_hs_test_util_prev_tx_queue_sz() == 0);
+}
+
 TEST_SUITE(ble_l2cap_sm_test_suite)
 {
     ble_l2cap_sm_test_case_peer_fail_inval();
@@ -1649,6 +1724,7 @@ TEST_SUITE(ble_l2cap_sm_test_suite)
     ble_l2cap_sm_test_case_peer_bonding_good();
     ble_l2cap_sm_test_case_peer_bonding_bad();
     ble_l2cap_sm_test_case_conn_broken();
+    ble_l2cap_sm_test_case_peer_sec_req_inval();
 }
 #endif