[PATCH BlueZ v2] mesh: Fix Relaying for multiple nodes

* [PATCH BlueZ v2] mesh: Fix Relaying for multiple nodes
@ 2019-01-18 23:13 Brian Gix
  2019-01-22 20:50 ` Gix, Brian
  0 siblings, 1 reply; 2+ messages in thread
From: Brian Gix @ 2019-01-18 23:13 UTC (permalink / raw)
  To: linux-bluetooth; +Cc: inga.stotland, brian.gix

Relay is now a cooperation between all the nodes on the daemon.
If any one or more nodes have relay enabled and a received packet
is addressed to a Group, or a non-present Unicast address, then the
packet's TTL is decremented and it is resent.
---
 mesh/crypto.c    |   8 +--
 mesh/mesh-defs.h |   9 ++-
 mesh/net.c       | 186 ++++++++++++++++++++++++++++++++-----------------------
 3 files changed, 118 insertions(+), 85 deletions(-)

diff --git a/mesh/crypto.c b/mesh/crypto.c
index e68cc2d5f..451e2da71 100644
--- a/mesh/crypto.c
+++ b/mesh/crypto.c
@@ -1169,13 +1169,13 @@ bool mesh_crypto_packet_parse(const uint8_t *packet, uint8_t packet_len,
 				*payload = packet + 9;
 
 			if (payload_len)
-				*payload_len = packet_len - 9 - 8;
+				*payload_len = packet_len - 9;
 		} else {
 			if (payload)
 				*payload = packet + 10;
 
 			if (payload_len)
-				*payload_len = packet_len - 10 - 8;
+				*payload_len = packet_len - 10;
 		}
 	} else {
 		if (cookie)
@@ -1202,13 +1202,13 @@ bool mesh_crypto_packet_parse(const uint8_t *packet, uint8_t packet_len,
 				*payload = packet + 13;
 
 			if (payload_len)
-				*payload_len = packet_len - 13 - 4;
+				*payload_len = packet_len - 13;
 		} else {
 			if (payload)
 				*payload = packet + 10;
 
 			if (payload_len)
-				*payload_len = packet_len - 10 - 4;
+				*payload_len = packet_len - 10;
 		}
 	}
 
diff --git a/mesh/mesh-defs.h b/mesh/mesh-defs.h
index d40fc43e8..c30041e4a 100644
--- a/mesh/mesh-defs.h
+++ b/mesh/mesh-defs.h
@@ -62,7 +62,9 @@
 #define VIRTUAL_ADDRESS_LOW	0x8000
 #define VIRTUAL_ADDRESS_HIGH	0xbfff
 #define GROUP_ADDRESS_LOW	0xc000
-#define GROUP_ADDRESS_HIGH	0xff00
+#define GROUP_ADDRESS_HIGH	0xfeff
+#define FIXED_GROUP_LOW		0xff00
+#define FIXED_GROUP_HIGH	0xffff
 
 #define NODE_IDENTITY_STOPPED		0x00
 #define NODE_IDENTITY_RUNNING		0x01
@@ -79,6 +81,7 @@
 					((x) < VIRTUAL_ADDRESS_LOW))
 #define IS_VIRTUAL(x)		(((x) >= VIRTUAL_ADDRESS_LOW) && \
 					((x) <= VIRTUAL_ADDRESS_HIGH))
-#define IS_GROUP(x)		(((x) >= GROUP_ADDRESS_LOW) && \
-					((x) <= GROUP_ADDRESS_HIGH))
+#define IS_GROUP(x)		((((x) >= GROUP_ADDRESS_LOW) && \
+					((x) < FIXED_GROUP_HIGH)) || \
+					((x) == ALL_NODES_ADDRESS))
 #define IS_ALL_NODES(x)	((x) == ALL_NODES_ADDRESS)
diff --git a/mesh/net.c b/mesh/net.c
index 6ef056b56..9e509a8ea 100644
--- a/mesh/net.c
+++ b/mesh/net.c
@@ -71,6 +71,13 @@
 
 #define SAR_KEY(src, seq0)	((((uint32_t)(seq0)) << 16) | (src))
 
+enum _relay_advice {
+	RELAY_NONE,		/* Relay not enabled in node */
+	RELAY_ALLOWED,		/* Relay enabled, msg not to node's unicast */
+	RELAY_DISALLOWED,	/* Msg was unicast handled by this node */
+	RELAY_ALWAYS		/* Relay enabled, msg to a group */
+};
+
 enum _iv_upd_state {
 	/* Allows acceptance of any iv_index secure net beacon */
 	IV_UPD_INIT,
@@ -160,7 +167,6 @@ struct mesh_net {
 	struct l_queue *frnd_msgs;
 	struct l_queue *friends;
 	struct l_queue *destinations;
-	struct l_queue *fast_cache;
 
 	uint8_t prov_priv_key[32];
 
@@ -248,6 +254,8 @@ struct net_decode {
 	bool proxy;
 };
 
+#define FAST_CACHE_SIZE 8
+static struct l_queue *fast_cache;
 static struct l_queue *nets;
 
 static inline struct mesh_subnet *get_primary_subnet(struct mesh_net *net)
@@ -673,7 +681,6 @@ struct mesh_net *mesh_net_new(struct mesh_node *node)
 	net->tx_interval = DEFAULT_TRANSMIT_INTERVAL;
 
 	net->subnets = l_queue_new();
-	net->fast_cache = l_queue_new();
 	net->msg_cache = l_queue_new();
 	net->sar_in = l_queue_new();
 	net->sar_out = l_queue_new();
@@ -687,6 +694,9 @@ struct mesh_net *mesh_net_new(struct mesh_node *node)
 	if (!nets)
 		nets = l_queue_new();
 
+	if (!fast_cache)
+		fast_cache = l_queue_new();
+
 	return net;
 }
 
@@ -696,7 +706,6 @@ void mesh_net_free(struct mesh_net *net)
 		return;
 
 	l_queue_destroy(net->subnets, subnet_free);
-	l_queue_destroy(net->fast_cache, mesh_msg_free);
 	l_queue_destroy(net->msg_cache, mesh_msg_free);
 	l_queue_destroy(net->sar_in, mesh_sar_free);
 	l_queue_destroy(net->sar_out, mesh_sar_free);
@@ -1021,7 +1030,6 @@ int mesh_net_add_key(struct mesh_net *net, bool update, uint16_t idx,
 void mesh_net_flush_msg_queues(struct mesh_net *net)
 {
 	l_queue_clear(net->msg_cache, mesh_msg_free);
-	l_queue_clear(net->fast_cache, mesh_msg_free);
 }
 
 uint32_t mesh_net_get_iv_index(struct mesh_net *net)
@@ -2210,23 +2218,23 @@ static bool find_fast_hash(const void *a, const void *b)
 	return *entry == *test;
 }
 
-static void *check_fast_cache(struct mesh_net *net, uint64_t hash)
+static bool check_fast_cache(uint64_t hash)
 {
-	void *found = l_queue_find(net->fast_cache, find_fast_hash, &hash);
+	void *found = l_queue_find(fast_cache, find_fast_hash, &hash);
 	uint64_t *new_hash;
 
 	if (found)
-		return NULL;
+		return false;
 
-	if (l_queue_length(net->fast_cache) >= 8)
-		new_hash = l_queue_pop_head(net->fast_cache);
+	if (l_queue_length(fast_cache) >= FAST_CACHE_SIZE)
+		new_hash = l_queue_pop_head(fast_cache);
 	else
 		new_hash = l_malloc(sizeof(hash));
 
 	*new_hash = hash;
-	l_queue_push_tail(net->fast_cache, new_hash);
+	l_queue_push_tail(fast_cache, new_hash);
 
-	return new_hash;
+	return true;
 }
 
 static bool match_by_dst(const void *a, const void *b)
@@ -2237,8 +2245,9 @@ static bool match_by_dst(const void *a, const void *b)
 	return dest->dst == dst;
 }
 
-static void send_relay_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
+static void send_relay_pkt(struct mesh_net *net, uint8_t *data, uint8_t size)
 {
+	uint8_t packet[30];
 	struct mesh_io *io = net->io;
 	struct mesh_io_send_info info = {
 		.type = MESH_IO_TIMING_TYPE_GENERAL,
@@ -2249,8 +2258,9 @@ static void send_relay_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
 	};
 
 	packet[0] = MESH_AD_TYPE_NETWORK;
+	memcpy(packet + 1, data, size);
 
-	mesh_io_send(io, &info, packet, size);
+	mesh_io_send(io, &info, packet, size + 1);
 }
 
 static void send_msg_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
@@ -2270,15 +2280,12 @@ static void send_msg_pkt(struct mesh_net *net, uint8_t *packet, uint8_t size)
 	mesh_io_send(io, &info, packet, size);
 }
 
-static void packet_received(void *user_data, const void *data, uint8_t size,
-								int8_t rssi)
+static enum _relay_advice packet_received(void *user_data,
+				uint32_t key_id, uint32_t iv_index,
+				const void *data, uint8_t size, int8_t rssi)
 {
 	struct mesh_net *net = user_data;
-	uint32_t iv_index;
-	uint8_t iv_flag;
 	const uint8_t *msg = data;
-	uint8_t *out;
-	size_t out_size;
 	uint8_t app_msg_len;
 	uint8_t net_ttl, net_key_id, net_segO, net_segN, net_opcode;
 	uint32_t net_seq, cache_cookie;
@@ -2287,50 +2294,13 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 	bool net_ctl, net_segmented, net_szmic, net_relay;
 	struct mesh_friend *net_frnd = NULL;
 	bool drop = false;
-	uint64_t hash, *isNew = NULL;
-	uint32_t key_id;
-
-	iv_flag = msg[0] >> 7;
-	iv_index = net->iv_index;
-	l_debug("%s iv_index %d NID: %2.2x", __func__, iv_index, msg[0] & 0x7f);
-
-	if (sizeof(uint16_t) <= sizeof(void *)) {
-		/* Add in additional cache to allow us to
-		 * avoid decrypting duplicatesr
-		 * Fast 64 bit Hash, Network MIC doesn't matter
-		 * With 64 bit hash, false pos chance is 1 in 1.8 * 10^19
-		 */
-		hash = l_get_le64(msg + 1) ^ l_get_le64(msg + 9);
-		isNew = check_fast_cache(net, hash);
-		if (!isNew)
-			return;
-
-		l_debug("New");
-	}
-
-	memcpy(packet + 2, data, size);
-
-	if (iv_index && (iv_index & 0x01) != iv_flag)
-		iv_index--;
 
 	/* Tester--Drop 90% of packets */
 	/* l_getrandom(&iv_flag, 1); */
 	/* if (iv_flag%10<9) drop = true; */
 
-	if (!drop)
-		print_packet("RX: Network [enc] :", data, size);
-
-	key_id = net_key_decrypt(iv_index, packet + 2, size, &out, &out_size);
 
-	if (!key_id) {
-		l_debug("Failed to decode packet");
-		/* Remove fast-cache-hash */
-		l_queue_remove(net->fast_cache, isNew);
-		l_free(isNew);
-		return;
-	}
-
-	memcpy(packet + 2, out, out_size);
+	memcpy(packet + 2, data, size);
 
 	if (!drop)
 		print_packet("RX: Network [clr] :", packet + 2, size);
@@ -2347,7 +2317,7 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 					&net_segO, &net_segN,
 					&msg, &app_msg_len)) {
 		l_error("Failed to parse packet content");
-		return;
+		return RELAY_NONE;
 	}
 
 	/* Ignore incoming packets if we are LPN and frnd bit not set */
@@ -2357,7 +2327,7 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 		subnet = l_queue_find(net->subnets, match_key_id,
 							L_UINT_TO_PTR(key_id));
 		if (subnet)
-			return;
+			return RELAY_NONE;
 
 		/* If the queue is empty, stop polling */
 		if (net_ctl && net_opcode == NET_OP_FRND_UPDATE && !msg[5])
@@ -2368,16 +2338,16 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 	} else if (net_dst == 0) {
 		l_error("illegal parms: DST: %4.4x Ctl: %d TTL: %2.2x",
 						net_dst, net_ctl, net_ttl);
-		return;
+		return RELAY_NONE;
 	}
 
 	/* Ignore if we originally sent this */
 	if (is_us(net, net_src, true))
-		return;
+		return RELAY_NONE;
 
 	if (drop) {
 		l_info("Dropping SEQ 0x%06x", net_seq);
-		return;
+		return RELAY_NONE;
 	}
 
 	l_debug("check %08x", cache_cookie);
@@ -2385,7 +2355,7 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 	/* As a Relay, suppress repeats of last N packets that pass through */
 	/* The "cache_cookie" should be unique part of App message */
 	if (msg_in_cache(net, net_src, net_seq, cache_cookie))
-		return;
+		return RELAY_NONE;
 
 	l_debug("RX: Network %04x -> %04x : TTL 0x%02x : IV : %8.8x SEQ 0x%06x",
 			net_src, net_dst, net_ttl, iv_index, net_seq);
@@ -2403,12 +2373,12 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 			if (net_opcode == NET_OP_SEG_ACKNOWLEDGE) {
 				/* Illegal to send ACK to non-Unicast Addr */
 				if (net_dst & 0x8000)
-					return;
+					return RELAY_NONE;
 
 				/* print_packet("Got ACK", msg, app_msg_len); */
 				/* Pedantic check for correct size */
 				if (app_msg_len != 7)
-					return;
+					return RELAY_NONE;
 
 				/* If this is an ACK to our friend queue-only */
 				if (is_lpn_friend(net, net_dst, true))
@@ -2468,28 +2438,37 @@ static void packet_received(void *user_data, const void *data, uint8_t size,
 		if (!!(net_frnd))
 			l_info("Ask for more data!");
 
-		/* If this is one of our Unicast addresses, don't relay */
-		if (net_dst <= 0x7fff)
-			return;
+		/* If this is one of our Unicast addresses, disallow relay */
+		if (IS_UNICAST(net_dst))
+			return RELAY_DISALLOWED;
 	}
 
+	/* If relay not enable, or no more hops allowed */
 	if (!net->relay.enable || net_ttl < 0x02 || net_frnd)
-		return;
+		return RELAY_NONE;
 
-	packet[2 + 1] = (packet[2 + 1] & ~TTL_MASK) | (net_ttl - 1);
+	/* Group or Virtual destinations should *always* be relayed */
+	if (IS_GROUP(net_dst) || IS_VIRTUAL(net_dst))
+		return RELAY_ALWAYS;
 
-	if (!net_key_encrypt(key_id, iv_index, packet + 2, size)) {
-		l_error("Failed to encode relay packet");
-		return;
-	}
+	/* Unicast destinations for other nodes *may* be relayed */
+	else if (IS_UNICAST(net_dst))
+		return RELAY_ALLOWED;
 
-	if (net->relay.enable)
-		send_relay_pkt(net, packet, size + 1);
+	/* Otherwise, do not make a relay decision */
+	else
+		return RELAY_NONE;
 }
 
 struct net_queue_data {
 	struct mesh_io_recv_info *info;
+	struct mesh_net *net;
 	const uint8_t *data;
+	uint8_t *out;
+	size_t out_size;
+	enum _relay_advice relay_advice;
+	uint32_t key_id;
+	uint32_t iv_index;
 	uint16_t len;
 };
 
@@ -2497,30 +2476,78 @@ static void net_rx(void *net_ptr, void *user_data)
 {
 	struct net_queue_data *data = user_data;
 	struct mesh_net *net = net_ptr;
+	enum _relay_advice relay_advice;
+	uint8_t *out;
+	size_t out_size;
+	uint32_t key_id;
 	int8_t rssi = 0;
 
+	key_id = net_key_decrypt(net->iv_index, data->data, data->len,
+								&out, &out_size);
+
+	if (!key_id)
+		return;
+
+	print_packet("RX: Network [enc] :", data->data, data->len);
+
 	if (data->info) {
 		net->instant = data->info->instant;
 		net->chan = data->info->chan;
 		rssi = data->info->rssi;
 	}
 
-	packet_received(net, data->data, data->len, rssi);
+	relay_advice = packet_received(net, key_id, net->iv_index,
+							out, out_size, rssi);
+	if (relay_advice > data->relay_advice) {
+		bool iv_flag = !!(net->iv_index & 1);
+		bool iv_pkt = !!(data->data[0] & 0x80);
+
+		data->iv_index = net->iv_index;
+		if (iv_pkt != iv_flag)
+			data->iv_index--;
+
+		data->relay_advice = relay_advice;
+		data->key_id = key_id;
+		data->net = net;
+		data->out = out;
+		data->out_size = out_size;
+	}
 }
 
 static void net_msg_recv(void *user_data, struct mesh_io_recv_info *info,
 					const uint8_t *data, uint16_t len)
 {
+	uint64_t hash;
+	bool isNew;
 	struct net_queue_data net_data = {
 		.info = info,
 		.data = data + 1,
 		.len = len - 1,
+		.relay_advice = RELAY_NONE,
 	};
 
-	if (len <= 2)
+	if (len < 9)
+		return;
+
+	hash = l_get_le64(data + 1);
+
+	/* Only process packet once per reception */
+	isNew = check_fast_cache(hash);
+	if (!isNew)
 		return;
 
 	l_queue_foreach(nets, net_rx, &net_data);
+
+	if (net_data.relay_advice == RELAY_ALWAYS ||
+			net_data.relay_advice == RELAY_ALLOWED) {
+		uint8_t ttl = net_data.out[1] & TTL_MASK;
+
+		net_data.out[1] &=  ~TTL_MASK;
+		net_data.out[1] |= ttl - 1;
+		net_key_encrypt(net_data.key_id, net_data.iv_index,
+					net_data.out, net_data.out_size);
+		send_relay_pkt(net_data.net, net_data.out, net_data.out_size);
+	}
 }
 
 static void set_network_beacon(void *a, void *b)
@@ -2927,6 +2954,9 @@ bool mesh_net_attach(struct mesh_net *net, struct mesh_io *io)
 		if (!nets)
 			nets = l_queue_new();
 
+		if (!fast_cache)
+			fast_cache = l_queue_new();
+
 		l_info("Register io cb");
 		mesh_io_register_recv_cb(io, MESH_IO_FILTER_BEACON,
 							beacon_recv, net);
-- 
2.14.5


^ permalink raw reply related	[flat|nested] 2+ messages in thread