Bluetooth: Mesh: Encrypt friend packets on send

Stores friend queue packets unencrypted, removing any out-of-order
issues caused by seqnum allocation. Also moves as much of the metadata
storage as possible into the packet, allowing us to free up some bytes
of net_buf user data for friend packets.

Fixes #18488

Signed-off-by: Trond Einar Snekvik <Trond.Einar.Snekvik@nordicsemi.no>

subsys/bluetooth/mesh/adv.h

@@ -31,16 +31,10 @@ struct bt_mesh_adv {
 	u8_t      type:2,
 		  busy:1;
 	u8_t      xmit;
-
+	/* For transport layer segment sending */
-	union {
+	struct {
-		/* Address, used e.g. for Friend Queue messages */
+		u8_t attempts;
-		u16_t addr;
+	} seg;
-
-		/* For transport layer segment sending */
-		struct {
-			u8_t attempts;
-		} seg;
-	};
 };
 
 typedef struct bt_mesh_adv *(*bt_mesh_adv_alloc_t)(int id);

subsys/bluetooth/mesh/friend.c

@@ -35,9 +35,6 @@
 #define FRIEND_BUF_COUNT    ((CONFIG_BT_MESH_FRIEND_QUEUE_SIZE + 1) * \
 			     CONFIG_BT_MESH_FRIEND_LPN_COUNT)
 
-#define FRIEND_ADV(buf)     CONTAINER_OF(BT_MESH_ADV(buf), \
-					 struct friend_adv, adv)
-
 /* PDUs from Friend to the LPN should only be transmitted once with the
  * smallest possible interval (20ms).
  */
@@ -58,14 +55,11 @@ struct friend_pdu_info {
 NET_BUF_POOL_FIXED_DEFINE(friend_buf_pool, FRIEND_BUF_COUNT,
 			  BT_MESH_ADV_DATA_SIZE, NULL);
 
-static struct friend_adv {
+static struct bt_mesh_adv adv_pool[FRIEND_BUF_COUNT];
-	struct bt_mesh_adv adv;
-	u64_t seq_auth;
-} adv_pool[FRIEND_BUF_COUNT];
 
 static struct bt_mesh_adv *adv_alloc(int id)
 {
-	return &adv_pool[id].adv;
+	return &adv_pool[id];
 }
 
 static bool is_lpn_unicast(struct bt_mesh_friend *frnd, u16_t addr)
@@ -295,13 +289,7 @@ static struct net_buf *create_friend_pdu(struct bt_mesh_friend *frnd,
 					 struct friend_pdu_info *info,
 					 struct net_buf_simple *sdu)
 {
-	struct bt_mesh_subnet *sub;
-	const u8_t *enc, *priv;
 	struct net_buf *buf;
-	u8_t nid;
-
-	sub = bt_mesh_subnet_get(frnd->net_idx);
-	__ASSERT_NO_MSG(sub != NULL);
 
 	buf = bt_mesh_adv_create_from_pool(&friend_buf_pool, adv_alloc,
 					   BT_MESH_ADV_DATA,
@@ -310,22 +298,7 @@ static struct net_buf *create_friend_pdu(struct bt_mesh_friend *frnd,
 		return NULL;
 	}
 
-	BT_MESH_ADV(buf)->addr = info->src;
+	net_buf_add_u8(buf, (info->iv_index & 1) << 7); /* Will be reset in encryption */
-	FRIEND_ADV(buf)->seq_auth = TRANS_SEQ_AUTH_NVAL;
-
-	/* Friend Offer needs master security credentials */
-	if (info->ctl && TRANS_CTL_OP(sdu->data) == TRANS_CTL_OP_FRIEND_OFFER) {
-		enc = sub->keys[sub->kr_flag].enc;
-		priv = sub->keys[sub->kr_flag].privacy;
-		nid = sub->keys[sub->kr_flag].nid;
-	} else {
-		if (friend_cred_get(sub, frnd->lpn, &nid, &enc, &priv)) {
-			BT_ERR("friend_cred_get failed");
-			goto failed;
-		}
-	}
-
-	net_buf_add_u8(buf, (nid | (info->iv_index & 1) << 7));
 
 	if (info->ctl) {
 		net_buf_add_u8(buf, info->ttl | 0x80);
@@ -340,25 +313,55 @@ static struct net_buf *create_friend_pdu(struct bt_mesh_friend *frnd,
 
 	net_buf_add_mem(buf, sdu->data, sdu->len);
 
-	/* We re-encrypt and obfuscate using the received IVI rather than
-	 * the normal TX IVI (which may be different) since the transport
-	 * layer nonce includes the IVI.
-	 */
-	if (bt_mesh_net_encrypt(enc, &buf->b, info->iv_index, false)) {
-		BT_ERR("Re-encrypting failed");
-		goto failed;
-	}
-
-	if (bt_mesh_net_obfuscate(buf->data, info->iv_index, priv)) {
-		BT_ERR("Re-obfuscating failed");
-		goto failed;
-	}
-
 	return buf;
+}
 
-failed:
+static int encrypt_friend_pdu(struct bt_mesh_friend *frnd, struct net_buf *buf, bool master_cred)
-	net_buf_unref(buf);
+{
-	return NULL;
+	struct bt_mesh_subnet *sub = bt_mesh_subnet_get(frnd->net_idx);
+	const u8_t *enc, *priv;
+	u32_t iv_index;
+	u16_t src;
+	u8_t nid;
+
+	if (master_cred) {
+		enc = sub->keys[sub->kr_flag].enc;
+		priv = sub->keys[sub->kr_flag].privacy;
+		nid = sub->keys[sub->kr_flag].nid;
+	} else {
+		if (friend_cred_get(sub, frnd->lpn, &nid, &enc, &priv)) {
+			BT_ERR("friend_cred_get failed");
+			return -ENOENT;
+		}
+	}
+
+	src = sys_get_be16(&buf->data[5]);
+
+	if (bt_mesh_elem_find(src)) {
+		/* Local messages were stored with a blank seqnum */
+		u32_t seq = bt_mesh_next_seq();
+		buf->data[2] = seq >> 16;
+		buf->data[3] = seq >> 8;
+		buf->data[4] = seq;
+		iv_index = BT_MESH_NET_IVI_TX;
+	} else {
+		u8_t ivi = (buf->data[0] >> 7);
+		iv_index = (bt_mesh.iv_index - ((bt_mesh.iv_index & 1) != ivi));
+	}
+
+	buf->data[0] = (nid | (iv_index & 1) << 7);
+
+	if (bt_mesh_net_encrypt(enc, &buf->b, iv_index, false)) {
+		BT_ERR("Encrypting failed");
+		return -EINVAL;
+	}
+
+	if (bt_mesh_net_obfuscate(buf->data, iv_index, priv)) {
+		BT_ERR("Obfuscating failed");
+		return -EINVAL;
+	}
+
+	return 0;
 }
 
 static struct net_buf *encode_friend_ctl(struct bt_mesh_friend *frnd,
@@ -366,7 +369,6 @@ static struct net_buf *encode_friend_ctl(struct bt_mesh_friend *frnd,
 					 struct net_buf_simple *sdu)
 {
 	struct friend_pdu_info info;
-	u32_t seq;
 
 	BT_DBG("LPN 0x%04x", frnd->lpn);
 
@@ -378,10 +380,7 @@ static struct net_buf *encode_friend_ctl(struct bt_mesh_friend *frnd,
 	info.ctl = 1U;
 	info.ttl = 0U;
 
-	seq = bt_mesh_next_seq();
+	memset(info.seq, 0, sizeof(info.seq));
-	info.seq[0] = seq >> 16;
-	info.seq[1] = seq >> 8;
-	info.seq[2] = seq;
 
 	info.iv_index = BT_MESH_NET_IVI_TX;
 
@@ -427,6 +426,10 @@ static void enqueue_sub_cfm(struct bt_mesh_friend *frnd, u8_t xact)
 		return;
 	}
 
+	if (encrypt_friend_pdu(frnd, buf, false)) {
+		return;
+	}
+
 	if (frnd->last) {
 		BT_DBG("Discarding last PDU");
 		net_buf_unref(frnd->last);
@@ -733,6 +736,10 @@ static void enqueue_offer(struct bt_mesh_friend *frnd, s8_t rssi)
 		return;
 	}
 
+	if (encrypt_friend_pdu(frnd, buf, true)) {
+		return;
+	}
+
 	frnd->counter++;
 
 	if (frnd->last) {
@@ -872,8 +879,29 @@ init_friend:
 	return 0;
 }
 
+static bool is_seg(struct bt_mesh_friend_seg *seg, u16_t src, u16_t seq_zero)
+{
+	struct net_buf *buf = (void *)sys_slist_peek_head(&seg->queue);
+	struct net_buf_simple_state state;
+	u16_t buf_seq_zero;
+	u16_t buf_src;
+
+	if (!buf) {
+		return false;
+	}
+
+	net_buf_simple_save(&buf->b, &state);
+	net_buf_skip(buf, 5);   /* skip IVI, NID, CTL, TTL, SEQ */
+	buf_src = net_buf_pull_be16(buf);
+	net_buf_skip(buf, 3);   /* skip DST, OP/AID */
+	buf_seq_zero = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK);
+	net_buf_simple_restore(&buf->b, &state);
+
+	return ((src == buf_src) && (seq_zero == buf_seq_zero));
+}
+
 static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd,
-					  u16_t src, u64_t *seq_auth,
+					  u16_t src, u16_t seq_zero,
 					  u8_t seg_count)
 {
 	struct bt_mesh_friend_seg *unassigned = NULL;
@@ -881,14 +909,12 @@ static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd,
 
 	for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
 		struct bt_mesh_friend_seg *seg = &frnd->seg[i];
-		struct net_buf *buf = (void *)sys_slist_peek_head(&seg->queue);
 
-		if (buf && BT_MESH_ADV(buf)->addr == src &&
+		if (is_seg(seg, src, seq_zero)) {
-		    FRIEND_ADV(buf)->seq_auth == *seq_auth) {
 			return seg;
 		}
 
-		if (!unassigned && !buf) {
+		if (!unassigned && !sys_slist_peek_head(&seg->queue)) {
 			unassigned = seg;
 		}
 	}
@@ -902,10 +928,10 @@ static struct bt_mesh_friend_seg *get_seg(struct bt_mesh_friend *frnd,
 
 static void enqueue_friend_pdu(struct bt_mesh_friend *frnd,
 			       enum bt_mesh_friend_pdu_type type,
-			       u8_t seg_count, struct net_buf *buf)
+			       u16_t src, u8_t seg_count,
+			       struct net_buf *buf)
 {
 	struct bt_mesh_friend_seg *seg;
-	struct friend_adv *adv;
 
 	BT_DBG("type %u", type);
 
@@ -918,11 +944,11 @@ static void enqueue_friend_pdu(struct bt_mesh_friend *frnd,
 		return;
 	}
 
-	adv = FRIEND_ADV(buf);
+	u16_t seq_zero = (((buf->data[10] << 8 | buf->data[11]) >> 2) & TRANS_SEQ_ZERO_MASK);
-	seg = get_seg(frnd, BT_MESH_ADV(buf)->addr, &adv->seq_auth, seg_count);
+
+	seg = get_seg(frnd, src, seq_zero, seg_count);
 	if (!seg) {
-		BT_ERR("No free friend segment RX contexts for 0x%04x",
+		BT_ERR("No free friend segment RX contexts for 0x%04x", src);
-		       BT_MESH_ADV(buf)->addr);
 		net_buf_unref(buf);
 		return;
 	}
@@ -934,16 +960,9 @@ static void enqueue_friend_pdu(struct bt_mesh_friend *frnd,
 			enqueue_update(frnd, 1);
 		}
 
-		/* Only acks should have a valid SeqAuth in the Friend queue
-		 * (otherwise we can't easily detect them there), so clear
-		 * the SeqAuth information from the segments before merging.
-		 */
-		SYS_SLIST_FOR_EACH_CONTAINER(&seg->queue, buf, node) {
-			FRIEND_ADV(buf)->seq_auth = TRANS_SEQ_AUTH_NVAL;
-			frnd->queue_size++;
-		}
-
 		sys_slist_merge_slist(&frnd->queue, &seg->queue);
+
+		frnd->queue_size += seg->seg_count;
 		seg->seg_count = 0U;
 	} else {
 		/* Mark the buffer as having more to come after it */
@@ -1015,11 +1034,16 @@ static void friend_timeout(struct k_work *work)
 
 	frnd->last = (void *)sys_slist_get(&frnd->queue);
 	if (!frnd->last) {
-		BT_WARN("Friendship not established with 0x%04x", frnd->lpn);
+		BT_WARN("Friendship not established with 0x%04x",
+			frnd->lpn);
 		friend_clear(frnd);
 		return;
 	}
 
+	if (encrypt_friend_pdu(frnd, frnd->last, false)) {
+		return;
+	}
+
 	/* Clear the flag we use for segment tracking */
 	frnd->last->flags &= ~NET_BUF_FRAGS;
 	frnd->last->frags = NULL;
@@ -1057,6 +1081,42 @@ int bt_mesh_friend_init(void)
 	return 0;
 }
 
+static bool is_segack(struct net_buf *buf, u64_t *seqauth, u16_t src)
+{
+	struct net_buf_simple_state state;
+	bool found = false;
+
+	if (buf->len != 16) {
+		return false;
+	}
+
+	net_buf_simple_save(&buf->b, &state);
+
+	net_buf_skip(buf, 1); /* skip IVI, NID */
+
+	if (!(net_buf_pull_u8(buf) >> 7)) {
+		goto end;
+	}
+
+	net_buf_pull(buf, 3); /* skip SEQNUM */
+
+	if (src != net_buf_pull_be16(buf)) {
+		goto end;
+	}
+
+	net_buf_skip(buf, 2); /* skip dst */
+
+	if (TRANS_CTL_OP((u8_t *) net_buf_pull_mem(buf, 1)) != TRANS_CTL_OP_ACK) {
+		goto end;
+	}
+
+	found = ((net_buf_pull_be16(buf) >> 2) & TRANS_SEQ_ZERO_MASK) ==
+		(*seqauth & TRANS_SEQ_ZERO_MASK);
+end:
+	net_buf_simple_restore(&buf->b, &state);
+	return found;
+}
+
 static void friend_purge_old_ack(struct bt_mesh_friend *frnd, u64_t *seq_auth,
 				 u16_t src)
 {
@@ -1068,8 +1128,7 @@ static void friend_purge_old_ack(struct bt_mesh_friend *frnd, u64_t *seq_auth,
 	     cur != NULL; prev = cur, cur = sys_slist_peek_next(cur)) {
 		struct net_buf *buf = (void *)cur;
 
-		if (BT_MESH_ADV(buf)->addr == src &&
+		if (is_segack(buf, seq_auth, src)) {
-		    FRIEND_ADV(buf)->seq_auth == *seq_auth) {
 			BT_DBG("Removing old ack from Friend Queue");
 
 			sys_slist_remove(&frnd->queue, prev, cur);
@@ -1121,11 +1180,7 @@ static void friend_lpn_enqueue_rx(struct bt_mesh_friend *frnd,
 		return;
 	}
 
-	if (seq_auth) {
+	enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
-		FRIEND_ADV(buf)->seq_auth = *seq_auth;
-	}
-
-	enqueue_friend_pdu(frnd, type, seg_count, buf);
 
 	BT_DBG("Queued message for LPN 0x%04x, queue_size %u",
 	       frnd->lpn, frnd->queue_size);
@@ -1139,7 +1194,6 @@ static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd,
 {
 	struct friend_pdu_info info;
 	struct net_buf *buf;
-	u32_t seq;
 
 	BT_DBG("LPN 0x%04x", frnd->lpn);
 
@@ -1153,10 +1207,7 @@ static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd,
 	info.ttl = tx->ctx->send_ttl;
 	info.ctl = (tx->ctx->app_idx == BT_MESH_KEY_UNUSED);
 
-	seq = bt_mesh_next_seq();
+	memset(info.seq, 0, sizeof(info.seq)); /* Will be allocated before encryption */
-	info.seq[0] = seq >> 16;
-	info.seq[1] = seq >> 8;
-	info.seq[2] = seq;
 
 	info.iv_index = BT_MESH_NET_IVI_TX;
 
@@ -1166,11 +1217,7 @@ static void friend_lpn_enqueue_tx(struct bt_mesh_friend *frnd,
 		return;
 	}
 
-	if (seq_auth) {
+	enqueue_friend_pdu(frnd, type, info.src, seg_count, buf);
-		FRIEND_ADV(buf)->seq_auth = *seq_auth;
-	}
-
-	enqueue_friend_pdu(frnd, type, seg_count, buf);
 
 	BT_DBG("Queued message for LPN 0x%04x", frnd->lpn);
 }
@@ -1233,17 +1280,11 @@ static bool friend_queue_has_space(struct bt_mesh_friend *frnd, u16_t addr,
 	for (i = 0; i < ARRAY_SIZE(frnd->seg); i++) {
 		struct bt_mesh_friend_seg *seg = &frnd->seg[i];
 
-		if (seq_auth) {
+		if (seq_auth && is_seg(seg, addr, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
-			struct net_buf *buf;
-
 			/* If there's a segment queue for this message then the
 			 * space verification has already happened.
 			 */
-			buf = (void *)sys_slist_peek_head(&seg->queue);
+			return true;
-			if (buf && BT_MESH_ADV(buf)->addr == addr &&
-			    FRIEND_ADV(buf)->seq_auth == *seq_auth) {
-				return true;
-			}
 		}
 
 		total += seg->seg_count;
@@ -1417,18 +1458,8 @@ void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src,
 
 		for (j = 0; j < ARRAY_SIZE(frnd->seg); j++) {
 			struct bt_mesh_friend_seg *seg = &frnd->seg[j];
-			struct net_buf *buf;
 
-			buf = (void *)sys_slist_peek_head(&seg->queue);
+			if (!is_seg(seg, src, *seq_auth & TRANS_SEQ_ZERO_MASK)) {
-			if (!buf) {
-				continue;
-			}
-
-			if (BT_MESH_ADV(buf)->addr != src) {
-				continue;
-			}
-
-			if (FRIEND_ADV(buf)->seq_auth != *seq_auth) {
 				continue;
 			}
 
@@ -1436,6 +1467,7 @@ void bt_mesh_friend_clear_incomplete(struct bt_mesh_subnet *sub, u16_t src,
 
 			purge_buffers(&seg->queue);
 			seg->seg_count = 0U;
+			break;
 		}
 	}
 }