nimble/gatt: Add support for newt ATT opcodes

This is initial implementation of handling new GATT opcodes.
Gatt Client supports now:
ATT multi Variable read req
receiving ATT multi notifications

Gatt server supports now:
ATT multi Variable read rsp

nimble/host/include/host/ble_att.h

@@ -195,6 +195,14 @@ struct os_mbuf;
 /** Indicate Response. */
 #define BLE_ATT_OP_INDICATE_RSP             0x1e
 
+/** Read Multiple Variable Lenght Request */
+#define BLE_ATT_OP_READ_MULT_VAR_REQ        0x20
+
+/** Read Multiple Variable Lenght Response */
+#define BLE_ATT_OP_READ_MULT_VAR_RSP        0x21
+
+/** Notify Multiple Request */
+#define BLE_ATT_OP_NOTIFY_MULTI_REQ         0x23
 /** Write Command. */
 #define BLE_ATT_OP_WRITE_CMD                0x52
 

nimble/host/include/host/ble_gatt.h

@@ -127,6 +127,17 @@ typedef int ble_gatt_attr_fn(uint16_t conn_handle,
                              struct ble_gatt_attr *attr,
                              void *arg);
 
+/**
+ * The host will free the attribute mbuf automatically after the callback is
+ * executed.  The application can take ownership of the mbuf and prevent it
+ * from being freed by assigning NULL to attr->om.
+ */
+typedef int ble_gatt_attr_mult_fn(uint16_t conn_handle,
+                                  const struct ble_gatt_error *error,
+                                  struct ble_gatt_attr *attrs,
+                                  uint8_t num_attrs,
+                                  void *arg);
+
 /**
  * The host will free the attribute mbufs automatically after the callback is
  * executed.  The application can take ownership of the mbufs and prevent them
@@ -343,6 +354,9 @@ int ble_gattc_read_mult(uint16_t conn_handle, const uint16_t *handles,
                         uint8_t num_handles, ble_gatt_attr_fn *cb,
                         void *cb_arg);
 
+int ble_gattc_read_mult_var(uint16_t conn_handle, const uint16_t *handles,
+                            uint8_t num_handles, ble_gatt_attr_mult_fn *cb,
+                            void *cb_arg);
 /**
  * Initiates GATT procedure: Write Without Response.  This function consumes
  * the supplied mbuf regardless of the outcome.

nimble/host/services/gatt/src/ble_svc_gatt.c

@@ -179,4 +179,8 @@ ble_svc_gatt_init(void)
     if (MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0) {
         ble_svc_gatt_cl_sup_feat |= (1 << BLE_SVC_GATT_CLI_SUP_FEAT_EATT_BIT);
     }
+
+    if (MYNEWT_VAL(BLE_ATT_SVR_NOTIFY_MULTI) > 0) {
+        ble_svc_gatt_cl_sup_feat |= (1 << BLE_SVC_GATT_CLI_SUP_FEAT_MULT_NTF_BIT);
+    }
 }

nimble/host/src/ble_att.c

@@ -69,6 +69,9 @@ static const struct ble_att_rx_dispatch_entry ble_att_rx_dispatch[] = {
     { BLE_ATT_OP_NOTIFY_REQ,           ble_att_svr_rx_notify },
     { BLE_ATT_OP_INDICATE_REQ,         ble_att_svr_rx_indicate },
     { BLE_ATT_OP_INDICATE_RSP,         ble_att_clt_rx_indicate },
+    { BLE_ATT_OP_READ_MULT_VAR_REQ,    ble_att_svr_rx_read_mult_var },
+    { 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 },
 };
 

nimble/host/src/ble_att_clt.c

@@ -540,7 +540,7 @@ ble_att_clt_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
  *****************************************************************************/
 int
 ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, const uint16_t *handles,
-                         int num_handles)
+                         int num_handles, bool variable)
 {
 #if !NIMBLE_BLE_ATT_CLT_READ_MULT
     return BLE_HS_ENOTSUP;
@@ -549,12 +549,15 @@ ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid, const uint16_t *han
     struct ble_att_read_mult_req *req;
     struct os_mbuf *txom;
     int i;
+    uint8_t op;
 
     if (num_handles < 1) {
         return BLE_HS_EINVAL;
     }
 
-    req = ble_att_cmd_get(BLE_ATT_OP_READ_MULT_REQ,
+    op = variable ? BLE_ATT_OP_READ_MULT_VAR_REQ : BLE_ATT_OP_READ_MULT_REQ;
+
+    req = ble_att_cmd_get(op,
                           sizeof(req->handles[0]) * num_handles,
                           &txom);
     if (req == NULL) {
@@ -576,7 +579,19 @@ ble_att_clt_rx_read_mult(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
 #endif
 
     /* Pass the Attribute Value field to GATT. */
-    ble_gattc_rx_read_mult_rsp(conn_handle, cid, 0, rxom);
+    ble_gattc_rx_read_mult_rsp(conn_handle, cid, 0, rxom, false);
+    return 0;
+}
+
+int
+ble_att_clt_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
+{
+#if !NIMBLE_BLE_ATT_CLT_READ_MULT_VAR
+    return BLE_HS_ENOTSUP;
+#endif
+
+    /* Pass the Attribute Value field to GATT. */
+    ble_gattc_rx_read_mult_rsp(conn_handle, cid, 0, rxom, true);
     return 0;
 }
 

nimble/host/src/ble_att_cmd_priv.h

@@ -309,6 +309,20 @@ struct ble_att_exec_write_req {
  */
 #define BLE_ATT_EXEC_WRITE_RSP_SZ       1
 
+/**
+ * | Parameter                                     | Size (octets)     |
+ * +-----------------------------------------------+-------------------+
+ * | Attribute Opcode                              | 1                 |
+ * | Attribute Handle Length Value Tuple List      | 8 to (ATT_MTU-1)  |
+ */
+#define BLE_ATT_NOTIFY_MULTI_REQ_BASE_SZ      9
+
+struct ble_att_tuple_list {
+    uint16_t handle;
+    uint16_t value_len;
+    uint8_t data[0];
+} __attribute__((packed));
+
 /**
  * | Parameter                          | Size (octets)     |
  * +------------------------------------+-------------------+

nimble/host/src/ble_att_priv.h

@@ -198,6 +198,8 @@ int ble_att_svr_rx_read_blob(uint16_t conn_handle, uint16_t cid,
                              struct os_mbuf **rxom);
 int ble_att_svr_rx_read_mult(uint16_t conn_handle, uint16_t cid,
                              struct os_mbuf **rxom);
+int ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid,
+                                 struct os_mbuf **rxom);
 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);
@@ -207,6 +209,8 @@ int ble_att_svr_rx_exec_write(uint16_t conn_handle, uint16_t cid,
                               struct os_mbuf **rxom);
 int ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid,
                           struct os_mbuf **rxom);
+int ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid,
+                                struct os_mbuf **rxom);
 int ble_att_svr_rx_indicate(uint16_t conn_handle, uint16_t cid,
                             struct os_mbuf **rxom);
 void ble_att_svr_prep_clear(struct ble_att_prep_entry_list *prep_list);
@@ -261,8 +265,9 @@ int ble_att_clt_tx_read_blob(uint16_t conn_handle, uint16_t cid, uint16_t handle
                              uint16_t offset);
 int ble_att_clt_rx_read_blob(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
 int ble_att_clt_tx_read_mult(uint16_t conn_handle, uint16_t cid,
-                             const uint16_t *handles, int num_handles);
+                             const uint16_t *handles, int num_handles, bool variable);
 int ble_att_clt_rx_read_mult(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
+int ble_att_clt_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);
 int ble_att_clt_tx_read_type(uint16_t conn_handle, uint16_t cid, uint16_t start_handle,
                              uint16_t end_handle, const ble_uuid_t *uuid);
 int ble_att_clt_rx_read_type(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom);

nimble/host/src/ble_att_svr.c

@@ -1629,6 +1629,113 @@ ble_att_svr_rx_read_mult(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rx
                               att_err, err_handle);
 }
 
+static int
+ble_att_svr_build_read_mult_rsp_var(uint16_t conn_handle, uint16_t cid,
+                                    struct os_mbuf **rxom,
+                                    struct os_mbuf **out_txom,
+                                    uint8_t *att_err,
+                                    uint16_t *err_handle)
+{
+    struct os_mbuf *txom;
+    uint16_t handle;
+    uint16_t mtu;
+    uint16_t tuple_len;
+    struct os_mbuf *tmp = NULL;
+    int rc;
+
+    mtu = ble_att_mtu_by_cid(conn_handle, cid);
+
+    rc = ble_att_svr_pkt(rxom, &txom, att_err);
+    if (rc != 0) {
+        *err_handle = 0;
+        goto done;
+    }
+
+    if (ble_att_cmd_prepare(BLE_ATT_OP_READ_MULT_VAR_RSP, 0, txom) == NULL) {
+        *att_err = BLE_ATT_ERR_INSUFFICIENT_RES;
+        *err_handle = 0;
+        rc = BLE_HS_ENOMEM;
+        goto done;
+    }
+
+    tmp = os_msys_get_pkthdr(2, 0);
+
+    /* Iterate through requested handles, reading the corresponding attribute
+     * for each.  Stop when there are no more handles to process, or the
+     * response is full.
+     */
+    while (OS_MBUF_PKTLEN(*rxom) >= 2 && OS_MBUF_PKTLEN(txom) < mtu) {
+        /* Ensure the full 16-bit handle is contiguous at the start of the
+         * mbuf.
+         */
+        rc = ble_att_svr_pullup_req_base(rxom, 2, att_err);
+        if (rc != 0) {
+            *err_handle = 0;
+            goto done;
+        }
+
+        /* Extract the 16-bit handle and strip it from the front of the
+         * mbuf.
+         */
+        handle = get_le16((*rxom)->om_data);
+        os_mbuf_adj(*rxom, 2);
+
+        rc = ble_att_svr_read_handle(conn_handle, handle, 0, tmp, att_err);
+        if (rc != 0) {
+            *err_handle = handle;
+            goto done;
+        }
+        tuple_len = OS_MBUF_PKTLEN(tmp);
+        rc = os_mbuf_append(txom, &tuple_len, sizeof(tuple_len));
+        if (rc != 0) {
+            *err_handle = handle;
+            goto done;
+        }
+        if (tuple_len != 0) {
+            rc = os_mbuf_appendfrom(txom, tmp, 0, tuple_len);
+            if (rc != 0) {
+                *err_handle = handle;
+                goto done;
+            }
+            os_mbuf_adj(tmp, tuple_len);
+        }
+    }
+    rc = 0;
+
+done:
+
+    if (tmp) {
+        os_mbuf_free_chain(tmp);
+    }
+    *out_txom = txom;
+    return rc;
+}
+
+int
+ble_att_svr_rx_read_mult_var(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
+{
+#if (!MYNEWT_VAL(BLE_ATT_SVR_READ_MULT) || (!MYNEWT_VAL(BLE_VERSION) < 52))
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct os_mbuf *txom;
+    uint16_t err_handle;
+    uint8_t att_err;
+    int rc;
+
+    /* Initialize some values in case of early error. */
+    txom = NULL;
+    err_handle = 0;
+    att_err = 0;
+
+    rc = ble_att_svr_build_read_mult_rsp_var(conn_handle, cid, rxom, &txom, &att_err,
+                                         &err_handle);
+
+    return ble_att_svr_tx_rsp(conn_handle, cid, rc, txom,
+                              BLE_ATT_OP_READ_MULT_VAR_REQ,
+                              att_err, err_handle);
+}
+
 static int
 ble_att_svr_is_valid_read_group_type(const ble_uuid_t *uuid)
 {
@@ -2502,6 +2609,63 @@ ble_att_svr_rx_notify(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
     return 0;
 }
 
+int
+ble_att_svr_rx_notify_multi(uint16_t conn_handle, uint16_t cid, struct os_mbuf **rxom)
+{
+#if !MYNEWT_VAL(BLE_ATT_SVR_NOTIFY_MULTI)
+    return BLE_HS_ENOTSUP;
+#endif
+
+    struct ble_att_tuple_list *req;
+    uint16_t handle;
+    int rc;
+    uint16_t pkt_len;
+    struct os_mbuf *tmp;
+    uint16_t attr_len;
+
+    pkt_len = OS_MBUF_PKTLEN(*rxom);
+    while (pkt_len > 0) {
+        rc = ble_att_svr_pullup_req_base(rxom, sizeof(struct ble_att_tuple_list), NULL);
+        if (rc != 0) {
+            return BLE_HS_ENOMEM;
+        }
+
+        req = (struct ble_att_tuple_list *)(*rxom)->om_data;
+
+        handle = le16toh(req->handle);
+        attr_len = le16toh(req->value_len);
+
+        os_mbuf_adj(*rxom, 4);
+
+        if (attr_len > BLE_ATT_ATTR_MAX_LEN) {
+            /*TODO Figure out what to do here */
+            break;
+        }
+
+        tmp = os_msys_get_pkthdr(attr_len, 0);
+        if (!tmp) {
+            /*TODO Figure out what to do here */
+            break;
+        }
+
+        rc = os_mbuf_appendfrom(tmp, *rxom, 0, attr_len);
+        if (rc) {
+            /*TODO Figure out what to do here */
+            break;
+        }
+
+        ble_gap_notify_rx_event(conn_handle, handle, tmp, 0);
+
+        os_mbuf_adj(*rxom, attr_len);
+        pkt_len = OS_MBUF_PKTLEN(*rxom);
+    }
+
+    os_mbuf_free_chain(*rxom);
+    *rxom = NULL;
+
+    return 0;
+}
+
 /**
  * @return                      0 on success; nonzero on failure.
  */

nimble/host/src/ble_gatt_priv.h

@@ -113,7 +113,7 @@ void ble_gattc_rx_read_rsp(uint16_t conn_handle, uint16_t cid, int status,
 void ble_gattc_rx_read_blob_rsp(uint16_t conn_handle, uint16_t cid, int status,
                                 struct os_mbuf **rxom);
 void ble_gattc_rx_read_mult_rsp(uint16_t conn_handle, uint16_t cid, int status,
-                                struct os_mbuf **rxom);
+                                struct os_mbuf **rxom, bool variable);
 void ble_gattc_rx_read_group_type_adata(uint16_t conn_handle, uint16_t cid,
                                         struct ble_att_read_group_type_adata *adata);
 void ble_gattc_rx_read_group_type_complete(uint16_t conn_handle, uint16_t cid, int rc);