#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,
struct l_queue *frnd_msgs;
struct l_queue *friends;
struct l_queue *destinations;
- struct l_queue *fast_cache;
uint8_t prov_priv_key[32];
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)
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();
if (!nets)
nets = l_queue_new();
+ if (!fast_cache)
+ fast_cache = l_queue_new();
+
return 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);
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)
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)
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,
};
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)
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;
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);
&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 */
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])
} 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);
/* 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);
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))
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;
};
{
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)
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);
projects / platform / upstream / bluez.git / commitdiff