host/sm: Add CSIS SIRK encryption

This adds functions and algorithms to encrypt and decrypt
SIRK from Coordinated Set Identification Service.

This also adds API to resolve RSI. Application can use it
to find devices that are part of Coordinated Set.

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: