Signed Write support

nimble/host/include/host/ble_att.h

@@ -209,6 +209,9 @@ struct os_mbuf;
 /** Write Command. */
 #define BLE_ATT_OP_WRITE_CMD                0x52
 
+/** Signed Write Command. */
+#define BLE_ATT_OP_SIGNED_WRITE_CMD         0xD2
+
 /** @} */
 
 /** Maximum length of an Attribute Protocol (ATT) attribute. */

nimble/host/include/host/ble_gatt.h

@@ -534,6 +534,21 @@ int ble_gattc_write_no_rsp(uint16_t conn_handle, uint16_t attr_handle,
 int ble_gattc_write_no_rsp_flat(uint16_t conn_handle, uint16_t attr_handle,
                                 const void *data, uint16_t data_len);
 
+/**
+ * Initiates GATT procedure: Signed Write. This function consumes the
+ * supplied mbuf regardless of the outcome.
+ *
+ * @param conn_handle       The connection over which to execute the
+ *                              procedure.
+ * @param attr_handle       The handle of the characteristic value to write
+ *                              to.
+ * @param txom              The value to write to the characteristic.
+ *
+ * @return                  0 on success; nonzero on failure.
+ */
+int ble_gattc_signed_write(uint16_t conn_handle, uint16_t attr_handle,
+                           struct os_mbuf * txom);
+
 /**
  * Initiates GATT procedure: Write Characteristic Value.  This function
  * consumes the supplied mbuf regardless of the outcome.

nimble/host/include/host/ble_store.h

@@ -110,6 +110,8 @@ struct ble_store_value_sec {
     uint8_t csrk[16];
     /** Flag indicating if Connection Signature Resolving Key is present. */
     uint8_t csrk_present:1;
+    /** Sign Counter */
+    uint32_t sign_counter;
 
     /** Flag indicating whether the connection is authenticated. */
     unsigned authenticated:1;

nimble/host/src/ble_att.c

@@ -73,6 +73,7 @@ static const struct ble_att_rx_dispatch_entry ble_att_rx_dispatch[] = {
     { BLE_ATT_OP_READ_MULT_VAR_RSP,    ble_att_clt_rx_read_mult_var },
     { BLE_ATT_OP_NOTIFY_MULTI_REQ,     ble_att_svr_rx_notify_multi},
     { BLE_ATT_OP_WRITE_CMD,            ble_att_svr_rx_write_no_rsp },
+    { BLE_ATT_OP_SIGNED_WRITE_CMD,     ble_att_svr_rx_signed_write },
 };
 
 #define BLE_ATT_RX_DISPATCH_SZ \

nimble/host/src/ble_att_clt.c

@@ -26,6 +26,10 @@
 #include "host/ble_uuid.h"
 #include "ble_hs_priv.h"
 
+#ifndef MIN
+#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#endif
+
 #if NIMBLE_BLE_CONNECT
 /*****************************************************************************
  * $error response                                                           *
@@ -771,6 +775,90 @@ ble_att_clt_rx_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     return 0;
 }
 
+int
+ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t handle, uint8_t *csrk,
+                                uint32_t counter, struct os_mbuf *txom)
+{
+#if !NIMBLE_BLE_ATT_CLT_SIGNED_WRITE
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_signed_write_cmd *cmd;
+    struct os_mbuf *txom2;
+    uint8_t cmac[16];
+    uint8_t *message = NULL;
+    int rc;
+    int i;
+
+    BLE_HS_LOG(DEBUG, "ble_att_clt_tx_signed_write_cmd(): ");
+    for (i = 0; i < OS_MBUF_PKTLEN(txom); i++) {
+        BLE_HS_LOG(DEBUG, "0x%02x", (OS_MBUF_DATA(txom, uint8_t *))[i]);
+    }
+
+    cmd = ble_att_cmd_get(BLE_ATT_OP_SIGNED_WRITE_CMD,
+                          sizeof(*cmd), &txom2);
+    if (cmd == NULL) {
+        rc = BLE_HS_ENOMEM;
+        goto err;
+    }
+    cmd->handle = htole16(handle);
+
+    /* Message to be signed is message||sign_counter,
+     * where || represents concatenation
+     */
+    message = malloc(OS_MBUF_PKTLEN(txom) + sizeof(counter));
+    rc = os_mbuf_copydata(txom, 0, OS_MBUF_PKTLEN(txom), message);
+    if (rc != 0) {
+        goto err;
+    }
+    memcpy(&message[OS_MBUF_PKTLEN(txom)], &counter, sizeof(counter));
+
+    /* ble_sm_alg_aes_cmac takes data in little-endian format,
+     * so converting it to LE.
+     */
+    swap_in_place(message, OS_MBUF_PKTLEN(txom) + sizeof(counter));
+
+    /* Getting the CMAC (Cipher-based Message Authentication Code)
+     * for the message using our CSRK for this connection.
+     */
+    memset(cmac, 0, sizeof cmac);
+    rc = ble_sm_alg_aes_cmac(csrk, message,
+                             OS_MBUF_PKTLEN(txom) + sizeof(counter), cmac);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* After using the csrk to sign data,
+     * the sign counter needs to be updated.
+     */
+    rc = ble_sm_incr_our_sign_counter(conn_handle);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* Converting cmac to little-endian */
+    swap_in_place(cmac, sizeof(cmac));
+
+    /* Creating final signed message */
+    rc = os_mbuf_append(txom, (void *)&counter, sizeof(counter));
+    if (rc != 0) {
+        goto err;
+    }
+    rc = os_mbuf_copyinto(txom, OS_MBUF_PKTLEN(txom),
+                          cmac + (sizeof(cmac)/2), sizeof(cmac)/2);
+    if (rc != 0) {
+        goto err;
+    }
+
+    if(message != NULL) free(message);
+    os_mbuf_concat(txom2, txom);
+    return ble_att_tx(conn_handle, txom2);
+err:
+    if(message != NULL) free(message);
+    os_mbuf_free_chain(txom2);
+    return rc;
+}
+
 /*****************************************************************************
  * $prepare write request                                                    *
  *****************************************************************************/

nimble/host/src/ble_att_cmd_priv.h

@@ -367,6 +367,20 @@ struct ble_att_write_cmd {
     uint8_t value[0];
 } __attribute__((packed));
 
+/**
+ * | Parameter                          | Size (octets)     |
+ * +------------------------------------+-------------------+
+ * | Attribute Opcode                   | 1                 |
+ * | Attribute Handle                   | 2                 |
+ * | Attribute Value                    | 0 to (ATT_MTU-15) |
+ * | Authentication Signature           | 12                |
+ */
+#define BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ 15
+#define BLE_ATT_SIGNED_WRITE_DATA_OFFSET 3
+struct ble_att_signed_write_cmd {
+    uint16_t handle;
+} __attribute__((packed));
+
 void ble_att_error_rsp_parse(const void *payload, int len,
                              struct ble_att_error_rsp *rsp);
 void ble_att_error_rsp_write(void *payload, int len,

nimble/host/src/ble_att_priv.h

@@ -203,6 +203,7 @@ int ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid,
 int ble_att_svr_rx_write(uint16_t conn_handle, uint16_t cid,
                          struct os_mbuf **rxom);
 int ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
+int ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
 int ble_att_svr_rx_prep_write(uint16_t conn_handle, uint16_t cid,
                               struct os_mbuf **rxom);
 int ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid,
@@ -295,6 +296,9 @@ int ble_att_clt_rx_prep_write(uint16_t conn_handle, uint16_t cid,
                               struct os_mbuf **rxom);
 int ble_att_clt_tx_exec_write(uint16_t conn_handle, uint16_t cid,
                               uint8_t flags);
+int ble_att_clt_tx_signed_write_cmd(uint16_t conn_handle, uint16_t handle,
+                                    uint8_t * csrk, uint32_t counter,
+                                    struct os_mbuf * txom);
 int ble_att_clt_rx_exec_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
 int ble_att_clt_rx_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
 int ble_att_clt_tx_notify(uint16_t conn_handle, uint16_t handle,

nimble/host/src/ble_att_svr.c

@@ -2153,6 +2153,111 @@ ble_att_svr_rx_write_no_rsp(uint16_t conn_handle, uint16_t cid, struct os_mbuf *
     return ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
 }
 
+int
+ble_att_svr_rx_signed_write(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_SIGNED_WRITE)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_signed_write_cmd *req;
+    struct ble_store_value_sec value_sec;
+    struct ble_store_key_sec key_sec;
+    struct ble_gap_conn_desc desc;
+    uint8_t att_err;
+    uint16_t handle;
+    uint8_t sign[12];
+    uint8_t cmac[16];
+    uint8_t *message = NULL;
+    int rc;
+
+    rc = ble_gap_conn_find(conn_handle, &desc);
+    if (rc != 0) {
+        goto err;
+    }
+
+    memset(&key_sec, 0, sizeof key_sec);
+    key_sec.peer_addr = desc.peer_id_addr;
+
+    /* Getting the CSRK for authentication */
+    rc = ble_store_read_peer_sec(&key_sec, &value_sec);
+    if (rc != 0) {
+        goto err;
+    }
+    if (value_sec.csrk_present != 1) {
+        rc = BLE_HS_EAUTHEN;
+        goto err;
+    }
+
+    rc = ble_att_svr_pullup_req_base(rxom, sizeof(*req), &att_err);
+    if (rc != 0) {
+        return rc;
+    }
+
+    req = (struct ble_att_signed_write_cmd *)(*rxom)->om_data;
+
+    handle = le16toh(req->handle);
+
+    /* Strip the request base from the front of the mbuf. */
+    os_mbuf_adj(*rxom, sizeof(*req));
+
+    os_mbuf_copydata(*rxom,
+                    OS_MBUF_PKTLEN(*rxom) - (BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET),
+                    BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET,
+                    sign);
+
+    /* Strip the signature from the end of the mbuf. */
+    os_mbuf_adj(*rxom, -(BLE_ATT_SIGNED_WRITE_CMD_BASE_SZ - BLE_ATT_SIGNED_WRITE_DATA_OFFSET));
+
+    /* Authentication procedure */
+    message = malloc(OS_MBUF_PKTLEN(*rxom) + sizeof(value_sec.sign_counter));
+    os_mbuf_copydata(*rxom, 0, OS_MBUF_PKTLEN(*rxom), message);
+    memcpy(&message[OS_MBUF_PKTLEN(*rxom)], &value_sec.sign_counter, sizeof(value_sec.sign_counter));
+
+    /* Converting message into little endian format */
+    swap_in_place(message, OS_MBUF_PKTLEN(*rxom) + sizeof(value_sec.sign_counter));
+
+    /* Using AES-CMAC to get the CMAC from the message and CSRK of this device */
+    memset(cmac, 0, sizeof cmac);
+    rc = ble_sm_alg_aes_cmac(value_sec.csrk, message, OS_MBUF_PKTLEN(*rxom) + sizeof(value_sec.sign_counter), cmac);
+    if (rc != 0) {
+        goto err;
+    }
+
+    /* Converting cmac to little endian */
+    swap_in_place(cmac, sizeof cmac);
+
+    /* Comparing sign counter */
+    if(memcmp(sign, &value_sec.sign_counter, sizeof(value_sec.sign_counter)) != 0) {
+        rc = BLE_HS_EAUTHEN;
+        goto err;
+    }
+
+    /* Comparing signature */
+    if(memcmp(&sign[sizeof(value_sec.sign_counter)], &cmac[sizeof(cmac) / 2], sizeof(cmac) / 2) != 0) {
+        rc = BLE_HS_EAUTHEN;
+        goto err;
+    }
+
+    /* Signature matches, increment sign counter and pass the data to the upper layer */
+    rc = ble_sm_incr_peer_sign_counter(conn_handle);
+    if (rc != 0) {
+        goto err;
+    }
+
+    rc = ble_att_svr_write_handle(conn_handle, handle, 0, rxom, &att_err);
+    if (rc != 0) {
+        goto err;
+    }
+
+    if(message != NULL) free(message);
+    return 0;
+err:
+    if(message != NULL) free(message);
+    ble_gap_terminate(conn_handle, BLE_ERR_AUTH_FAIL);
+    return rc;
+}
+
 int
 ble_att_svr_write_local(uint16_t attr_handle, struct os_mbuf *om)
 {
@@ -2858,7 +2963,7 @@ ble_att_svr_reset(void)
     }
 
     ble_att_svr_id = 0;
-    
+
     /* Note: prep entries do not get freed here because it is assumed there are
      * no established connections.
      */

nimble/host/src/ble_gatt_priv.h

@@ -56,6 +56,8 @@ STATS_SECT_START(ble_gattc_stats)
     STATS_SECT_ENTRY(read_long_fail)
     STATS_SECT_ENTRY(read_mult)
     STATS_SECT_ENTRY(read_mult_fail)
+    STATS_SECT_ENTRY(signed_write)
+    STATS_SECT_ENTRY(signed_write_fail)
     STATS_SECT_ENTRY(write_no_rsp)
     STATS_SECT_ENTRY(write_no_rsp_fail)
     STATS_SECT_ENTRY(write)