From aad8908d9c062f187bc3e83cee8f6a7780e3b6be Mon Sep 17 00:00:00 2001
From: lly <lly@espressif.com>
Date: Tue, 24 Mar 2020 18:37:50 +0800
Subject: [PATCH] ble_mesh: Enable Segmented Control Messages [Zephyr]

The function bt_mesh_ctl_send() used to support maximum length of
11 bytes. The segmentation complies with the BLE Mesh Standard.
The ack is disabled in case of non unicast address.
---
 .../bt/esp_ble_mesh/mesh_core/transport.c     | 111 ++++++++++++++++--
 1 file changed, 104 insertions(+), 7 deletions(-)

diff --git a/components/bt/esp_ble_mesh/mesh_core/transport.c b/components/bt/esp_ble_mesh/mesh_core/transport.c
index bb8a1cc548..c888d30e9e 100644
--- a/components/bt/esp_ble_mesh/mesh_core/transport.c
+++ b/components/bt/esp_ble_mesh/mesh_core/transport.c
@@ -1039,16 +1039,12 @@ static inline s32_t ack_timeout(struct seg_rx *rx)
     return MAX(to, K_MSEC(400));
 }
 
-int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
-                     size_t data_len, u64_t *seq_auth,
-                     const struct bt_mesh_send_cb *cb, void *cb_data)
+static int ctl_send_unseg(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
+                          size_t data_len, u64_t *seq_auth,
+                          const struct bt_mesh_send_cb *cb, void *cb_data)
 {
     struct net_buf *buf = NULL;
 
-    BT_DBG("src 0x%04x dst 0x%04x ttl 0x%02x ctl 0x%02x", tx->src,
-           tx->ctx->addr, tx->ctx->send_ttl, ctl_op);
-    BT_DBG("len %u: %s", data_len, bt_hex(data, data_len));
-
     buf = bt_mesh_adv_create(BLE_MESH_ADV_DATA, tx->xmit, BUF_TIMEOUT);
     if (!buf) {
         BT_ERR("%s, Out of transport buffers", __func__);
@@ -1076,6 +1072,107 @@ int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
     return bt_mesh_net_send(tx, buf, cb, cb_data);
 }
 
+static int ctl_send_seg(struct bt_mesh_net_tx *tx, u8_t ctl_op,
+                        void *data, size_t data_len, u64_t *seq_auth,
+                        const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+    struct seg_tx *tx_seg = NULL;
+    u16_t unsent = data_len;
+    u16_t seq_zero = 0;
+    u8_t seg_o = 0;
+    int i;
+
+    for (tx_seg = NULL, i = 0; i < ARRAY_SIZE(seg_tx); i++) {
+        if (!seg_tx[i].nack_count) {
+            tx_seg = &seg_tx[i];
+            break;
+        }
+    }
+
+    if (!tx_seg) {
+        BT_ERR("%s, No multi-segment message contexts available", __func__);
+        return -EBUSY;
+    }
+
+    tx_seg->dst = tx->ctx->addr;
+    tx_seg->seg_n = (data_len - 1) / 8;
+    tx_seg->nack_count = tx_seg->seg_n + 1;
+    tx_seg->seq_auth = SEQ_AUTH(BLE_MESH_NET_IVI_TX, bt_mesh.seq);
+    tx_seg->sub = tx->sub;
+    tx_seg->new_key = tx->sub->kr_flag;
+    tx_seg->cb = cb;
+    tx_seg->cb_data = cb_data;
+
+    if (tx->ctx->send_ttl == BLE_MESH_TTL_DEFAULT) {
+        tx_seg->ttl = bt_mesh_default_ttl_get();
+    } else {
+        tx_seg->ttl = tx->ctx->send_ttl;
+    }
+
+    seq_zero = tx_seg->seq_auth & TRANS_SEQ_ZERO_MASK;
+
+    BT_DBG("SeqZero 0x%04x", seq_zero);
+
+    for (seg_o = 0; seg_o <= tx_seg->seg_n; seg_o++) {
+        struct net_buf *seg = NULL;
+        u16_t len = 0;
+        int err = 0;
+
+        seg = bt_mesh_adv_create(BLE_MESH_ADV_DATA, tx->xmit,
+                                 BUF_TIMEOUT);
+        if (!seg) {
+            BT_ERR("%s, Out of segment buffers", __func__);
+            seg_tx_reset(tx_seg);
+            return -ENOBUFS;
+        }
+
+        BLE_MESH_ADV(seg)->seg.attempts = SEG_RETRANSMIT_ATTEMPTS;
+
+        net_buf_reserve(seg, BLE_MESH_NET_HDR_LEN);
+
+        net_buf_add_u8(seg, TRANS_CTL_HDR(ctl_op, 1));
+        net_buf_add_u8(seg, (tx->aszmic << 7) | seq_zero >> 6);
+        net_buf_add_u8(seg, (((seq_zero & 0x3f) << 2) | (seg_o >> 3)));
+        net_buf_add_u8(seg, ((seg_o & 0x07) << 5) | tx_seg->seg_n);
+
+        len = MIN(unsent, 8);
+        net_buf_add_mem(seg, (u8_t *)data + (data_len - unsent), len);
+        unsent -= len;
+
+        tx_seg->seg[seg_o] = net_buf_ref(seg);
+
+        BT_DBG("Sending %u/%u", seg_o, tx_seg->seg_n);
+
+        err = bt_mesh_net_send(tx, seg,
+                               seg_o ? &seg_sent_cb : &first_sent_cb,
+                               tx_seg);
+        if (err) {
+            BT_ERR("%s, Sending segment failed", __func__);
+            seg_tx_reset(tx_seg);
+            return err;
+        }
+    }
+
+    return 0;
+}
+
+int bt_mesh_ctl_send(struct bt_mesh_net_tx *tx, u8_t ctl_op, void *data,
+                     size_t data_len, u64_t *seq_auth,
+                     const struct bt_mesh_send_cb *cb, void *cb_data)
+{
+    BT_DBG("src 0x%04x dst 0x%04x ttl 0x%02x ctl 0x%02x", tx->src,
+            tx->ctx->addr, tx->ctx->send_ttl, ctl_op);
+    BT_DBG("len %zu: %s", data_len, bt_hex(data, data_len));
+
+    if (data_len <= 11) {
+        return ctl_send_unseg(tx, ctl_op, data, data_len, seq_auth,
+                              cb, cb_data);
+    } else {
+        return ctl_send_seg(tx, ctl_op, data, data_len, seq_auth,
+                            cb, cb_data);
+    }
+}
+
 static int send_ack(struct bt_mesh_subnet *sub, u16_t src, u16_t dst,
                     u8_t ttl, u64_t *seq_auth, u32_t block, u8_t obo)
 {