host/sm: Add CSIS SIRK encryption

nimble/host/include/host/ble_sm.h

@@ -22,6 +22,8 @@
 
 #include <inttypes.h>
 #include "syscfg/syscfg.h"
+#include <stdbool.h>
+#include "nimble/ble.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -112,6 +114,23 @@ struct ble_sm_io {
 
 int ble_sm_sc_oob_generate_data(struct ble_sm_sc_oob_data *oob_data);
 
+#if MYNEWT_VAL(BLE_SM_CSIS_SIRK)
+/**
+ * Resolves CSIS RSI to check if advertising device is part of the same Coordinated Set,
+ * as the device with specified SIRK
+ *
+ * @param rsi                   RSI value from Advertising Data
+ * @param sirk                  SIRK
+ * @param ltk_peer_addr         If SIRK is in plaintext form this should be set to NULL,
+ *                              otherwise peer address should be passed here to get
+ *                              LTK and decrypt SIRK
+ *
+ * @return                      0 if RSI was resolved succesfully; nonzero on failure.
+ */
+int ble_sm_csis_resolve_rsi(const uint8_t *rsi, const uint8_t *sirk,
+                            const ble_addr_t *ltk_peer_addr);
+#endif
+
 #if NIMBLE_BLE_SM
 int ble_sm_inject_io(uint16_t conn_handle, struct ble_sm_io *pkey);
 #else

nimble/host/src/ble_sm.c

@@ -2910,4 +2910,107 @@ ble_sm_create_chan(uint16_t conn_handle)
     return chan;
 }
 
+#if MYNEWT_VAL(BLE_SM_CSIS_SIRK)
+int
+ble_sm_csis_decrypt_sirk(const uint8_t *ltk, const uint8_t *enc_sirk, uint8_t *out)
+{
+    int rc;
+
+    /* Decrypt SIRK with sdf(K, EncSIRK)  */
+    rc = ble_sm_alg_csis_sdf(ltk, enc_sirk, out);
+
+    return rc;
+}
+
+int
+ble_sm_csis_resolve_rsi(const uint8_t *rsi, const uint8_t *sirk,
+                        const ble_addr_t *ltk_peer_addr)
+{
+    struct ble_store_key_sec key_sec;
+    struct ble_store_value_sec value_sec;
+    uint8_t plaintext_sirk[16] = {0};
+    uint8_t local_hash[3] = {0};
+    uint8_t prand[3] = {0};
+    uint8_t hash[3] = {0};
+    int rc;
+
+    memcpy(hash, rsi, 3);
+    memcpy(prand, rsi + 3, 3);
+
+    if (ltk_peer_addr) {
+        memset(&key_sec, 0, sizeof(key_sec));
+        key_sec.peer_addr = *ltk_peer_addr;
+
+        rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+        if (rc != 0) {
+            return rc;
+        } else if (!value_sec.ltk_present) {
+            return BLE_HS_ENOENT;
+        }
+
+        rc = ble_sm_csis_decrypt_sirk(value_sec.ltk, sirk, plaintext_sirk);
+        if (rc != 0) {
+            return rc;
+        }
+    } else {
+        memcpy(plaintext_sirk, sirk, 16);
+    }
+
+    rc = ble_sm_alg_csis_sih(plaintext_sirk, prand, local_hash);
+    if (rc != 0) {
+        return rc;
+    }
+
+    if (memcmp(local_hash, hash, 3)) {
+        return BLE_HS_EAUTHEN;
+    }
+
+    return 0;
+}
+
+int
+ble_sm_csis_encrypt_sirk(const uint8_t *ltk, const uint8_t *plaintext_sirk, uint8_t *out)
+{
+    int rc;
+
+    /* Encrypt SIRK with sef(K, SIRK) */
+    rc = ble_sm_alg_csis_sef(ltk, plaintext_sirk, out);
+
+    return rc;
+}
+
+int
+ble_sm_csis_generate_rsi(const uint8_t *sirk, uint8_t *out)
+{
+    const uint8_t prand_check_all_set[3] = {0xff, 0xff, 0xef};
+    const uint8_t prand_check_all_reset[3] = {0x0, 0x0, 0x40};
+    uint8_t prand[3] = {0};
+    uint8_t hash[3] = {0};
+    int rc;
+
+    do {
+        rc = ble_hs_hci_rand(prand, 3);
+        if (rc != 0) {
+            return rc;
+        }
+        /* Two MSBs of prand shall be equal to 0 and 1 */
+        prand[2] &= ~0xc0;
+        prand[2] |= 0x40;
+
+        /* prand's random part shall not be all 0s nor all 1s */
+    } while (memcmp(prand, prand_check_all_set, 3) ||
+             memcmp(prand, prand_check_all_reset, 3));
+
+    rc = ble_sm_alg_csis_sih(sirk, prand, hash);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memcpy(out, hash, 3);
+    memcpy(out + 3, prand, 3);
+
+    return 0;
+}
+
+#endif
 #endif

nimble/host/src/ble_sm_alg.c

@@ -418,6 +418,139 @@ ble_sm_alg_g2(const uint8_t *u, const uint8_t *v, const uint8_t *x,
     return 0;
 }
 
+int
+ble_sm_alg_csis_k1(const uint8_t *n, size_t n_len, const uint8_t *salt,
+                   const uint8_t *p, size_t p_len, uint8_t *out)
+{
+    int rc;
+    uint8_t t[16] = {0};
+    uint8_t salt_be[16] = {0};
+    uint8_t n_be[16] = {0};
+
+    /* XXX: Spec does not specify the maximum N and P parameters length.
+     * We assume that 16 bytes is enough and return error if passed len value is greater
+     * than that */
+    if ((n_len > 16) || (p_len > 16)) {
+        return BLE_HS_EINVAL;
+    }
+
+    swap_buf(salt_be, salt, 16);
+    swap_buf(n_be, n, n_len);
+
+    /* T = AES-CMAC_SALT (N) */
+    rc = ble_sm_alg_aes_cmac(salt_be, n_be, n_len, t);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* AES-CMAC_T (P) */
+    rc = ble_sm_alg_aes_cmac(t, p, p_len, out);
+    if (rc != 0) {
+        return rc;
+    }
+
+    swap_in_place(out, 16);
+
+    return 0;
+}
+
+int
+ble_sm_alg_csis_s1(const uint8_t *m, size_t m_len, uint8_t *out)
+{
+    int rc;
+    uint8_t k_zero[16] = {0};
+
+    /* XXX: Spec does not specify the maximum M parameter length.
+     * We assume that 16 bytes is enough and return error if passed len value is greater
+     * than that */
+    if (m_len > 16) {
+        return BLE_HS_EINVAL;
+    }
+
+    /* AES-CMAC_zero (M) */
+    rc = ble_sm_alg_aes_cmac(k_zero, m, m_len, out);
+    if (rc != 0) {
+        return rc;
+    }
+
+    swap_in_place(out, 16);
+
+    return 0;
+}
+
+int
+ble_sm_alg_csis_sef(const uint8_t *k, const uint8_t *plaintext_sirk, uint8_t *out)
+{
+    uint8_t salt[16];
+    int rc;
+    int i;
+
+    /* s1("SIRKenc") */
+    rc = ble_sm_alg_csis_s1((const uint8_t *) "SIRKenc", 7, salt);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* k1(K, s1("SIRKenc"), "csis") */
+    rc = ble_sm_alg_csis_k1(k, 16, salt, (const uint8_t *) "csis", 4, out);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* k1(K, s1("SIRKenc"), "csis") ^ SIRK */
+    for (i = 0; i < 16; i++) {
+        out[i] ^= plaintext_sirk[i];
+    }
+
+    return 0;
+}
+
+int
+ble_sm_alg_csis_sdf(const uint8_t *k, const uint8_t *enc_sirk, uint8_t *out)
+{
+    uint8_t salt[16];
+    int rc;
+    int i;
+
+    /* s1("SIRKenc") */
+    rc = ble_sm_alg_csis_s1((const uint8_t *) "SIRKenc", 7, salt);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* k1(K, s1("SIRKenc"), "csis") */
+    rc = ble_sm_alg_csis_k1(k, 16, salt, (const uint8_t *) "csis", 4, out);
+    if (rc != 0) {
+        return rc;
+    }
+
+    /* k1(K, s1("SIRKenc"), "csis") ^ EncSIRK */
+    for (i = 0; i < 16; i++) {
+        out[i] ^= enc_sirk[i];
+    }
+
+    return 0;
+}
+
+int
+ble_sm_alg_csis_sih(const uint8_t *k, const uint8_t *r, uint8_t *out)
+{
+    uint8_t r1[16];
+    int rc;
+
+    memcpy(r1, r, 3);
+    memset(r1 + 3, 0, 13);
+
+    rc = ble_sm_alg_encrypt(k, r1, r1);
+    if (rc != 0) {
+        return rc;
+    }
+
+    memcpy(out, r1, 3);
+
+    return 0;
+}
+
 int
 ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x, const uint8_t *peer_pub_key_y,
                      const uint8_t *our_priv_key, uint8_t *out_dhkey)

nimble/host/src/ble_sm_priv.h

@@ -314,12 +314,25 @@ int ble_sm_alg_f6(const uint8_t *w, const uint8_t *n1, const uint8_t *n2,
                   const uint8_t *r, const uint8_t *iocap, uint8_t a1t,
                   const uint8_t *a1, uint8_t a2t, const uint8_t *a2,
                   uint8_t *check);
+int ble_sm_alg_csis_k1(const uint8_t *n, size_t n_len, const uint8_t *salt,
+                       const uint8_t *p, size_t p_len, uint8_t *out);
+int ble_sm_alg_csis_s1(const uint8_t *m, size_t m_len, uint8_t *out);
+int ble_sm_alg_csis_sef(const uint8_t *k, const uint8_t *plaintext_sirk,
+                        uint8_t *out);
+int ble_sm_alg_csis_sdf(const uint8_t *k, const uint8_t *enc_sirk,
+                        uint8_t *out);
+int ble_sm_alg_csis_sih(const uint8_t *k, const uint8_t *r, uint8_t *out);
 int ble_sm_alg_gen_dhkey(const uint8_t *peer_pub_key_x,
                          const uint8_t *peer_pub_key_y,
                          const uint8_t *our_priv_key, uint8_t *out_dhkey);
 int ble_sm_alg_gen_key_pair(uint8_t *pub, uint8_t *priv);
 void ble_sm_alg_ecc_init(void);
 
+int ble_sm_csis_generate_rsi(const uint8_t *sirk, uint8_t *out);
+int ble_sm_csis_encrypt_sirk(const uint8_t *ltk, const uint8_t *plaintext_sirk,
+                             uint8_t *out);
+int ble_sm_csis_decrypt_sirk(const uint8_t *ltk, const uint8_t *enc_sirk, uint8_t *out);
+
 void ble_sm_enc_change_rx(const struct ble_hci_ev_enrypt_chg *ev);
 void ble_sm_enc_key_refresh_rx(const struct ble_hci_ev_enc_key_refresh *ev);
 int ble_sm_ltk_req_rx(const struct ble_hci_ev_le_subev_lt_key_req *ev);

nimble/host/syscfg.yml

@@ -188,6 +188,11 @@ syscfg.defs:
             allows to decrypt air traffic easily and thus should be only used
             for debugging.
         value: 0
+    BLE_SM_CSIS_SIRK:
+        description: >
+            Enable LE Audio CSIS SIRK Encryption and Decryption API.
+        value: 0
+        experimental: 1
 
     # GAP options.
     BLE_GAP_MAX_PENDING_CONN_PARAM_UPDATE: