bluez5_utils-5.62/mesh/mesh-io-unit.c

// SPDX-License-Identifier: LGPL-2.1-or-later
/*
 *
 *  BlueZ - Bluetooth protocol stack for Linux
 *
 *  Copyright (C) 2021  Intel Corporation. All rights reserved.
 *
 *
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif

#include <errno.h>
#include <string.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <sys/un.h>
#include <unistd.h>
#include <stdio.h>
#include <ell/ell.h>

#include "mesh/mesh-defs.h"
#include "mesh/dbus.h"
#include "mesh/mesh-io.h"
#include "mesh/mesh-io-api.h"
#include "mesh/mesh-io-generic.h"

struct mesh_io_private {
	struct l_io *sio;
	void *user_data;
	char *unique_name;
	mesh_io_ready_func_t ready_callback;
	struct l_timeout *tx_timeout;
	struct l_queue *rx_regs;
	struct l_queue *tx_pkts;
	struct sockaddr_un addr;
	int fd;
	uint16_t interval;
};

struct pvt_rx_reg {
	mesh_io_recv_func_t cb;
	void *user_data;
	uint8_t len;
	uint8_t filter[0];
};

struct process_data {
	struct mesh_io_private		*pvt;
	const uint8_t			*data;
	uint8_t				len;
	struct mesh_io_recv_info	info;
};

struct tx_pkt {
	struct mesh_io_send_info	info;
	bool				delete;
	uint8_t				len;
	uint8_t				pkt[30];
};

struct tx_pattern {
	const uint8_t			*data;
	uint8_t				len;
};

static uint32_t get_instant(void)
{
	struct timeval tm;
	uint32_t instant;

	gettimeofday(&tm, NULL);
	instant = tm.tv_sec * 1000;
	instant += tm.tv_usec / 1000;

	return instant;
}

static uint32_t instant_remaining_ms(uint32_t instant)
{
	instant -= get_instant();
	return instant;
}

static void process_rx_callbacks(void *v_reg, void *v_rx)
{
	struct pvt_rx_reg *rx_reg = v_reg;
	struct process_data *rx = v_rx;

	if (!memcmp(rx->data, rx_reg->filter, rx_reg->len))
		rx_reg->cb(rx_reg->user_data, &rx->info, rx->data, rx->len);
}

static void process_rx(struct mesh_io_private *pvt, int8_t rssi,
					uint32_t instant, const uint8_t *addr,
					const uint8_t *data, uint8_t len)
{
	struct process_data rx = {
		.pvt = pvt,
		.data = data,
		.len = len,
		.info.instant = instant,
		.info.addr = addr,
		.info.chan = 7,
		.info.rssi = rssi,
	};

	l_queue_foreach(pvt->rx_regs, process_rx_callbacks, &rx);
}

static bool incoming(struct l_io *sio, void *user_data)
{
	struct mesh_io_private *pvt = user_data;
	uint32_t instant;
	uint8_t buf[31];
	size_t size;

	instant = get_instant();

	size = recv(pvt->fd, buf, sizeof(buf), MSG_DONTWAIT);

	if (size > 9 && buf[0]) {
		process_rx(pvt, -20, instant, NULL, buf + 1, (uint8_t)size);
	} else if (size == 1 && !buf[0] && pvt->unique_name) {

		/* Return DBUS unique name */
		size = strlen(pvt->unique_name);

		if (size > sizeof(buf) - 2)
			return true;

		buf[0] = 0;
		memcpy(buf + 1, pvt->unique_name, size + 1);
		send(pvt->fd, buf, size + 2, MSG_DONTWAIT);
	}

	return true;
}

static bool find_by_ad_type(const void *a, const void *b)
{
	const struct tx_pkt *tx = a;
	uint8_t ad_type = L_PTR_TO_UINT(b);

	return !ad_type || ad_type == tx->pkt[0];
}

static bool find_by_pattern(const void *a, const void *b)
{
	const struct tx_pkt *tx = a;
	const struct tx_pattern *pattern = b;

	if (tx->len < pattern->len)
		return false;

	return (!memcmp(tx->pkt, pattern->data, pattern->len));
}

static void free_socket(struct mesh_io_private *pvt)
{
	l_io_destroy(pvt->sio);
	close(pvt->fd);
	unlink(pvt->addr.sun_path);
}

static void hello_callback(struct l_dbus_message *msg, void *user_data)
{
	struct mesh_io_private *pvt = user_data;

	pvt->unique_name = l_strdup(l_dbus_message_get_destination(msg));
	l_debug("User-Daemon unique name: %s", pvt->unique_name);
}

static void get_name(struct l_timeout *timeout, void *user_data)
{
	struct mesh_io_private *pvt = user_data;
	struct l_dbus *dbus = dbus_get_bus();
	struct l_dbus_message *msg;

	l_timeout_remove(timeout);
	if (!dbus) {
		l_timeout_create_ms(20, get_name, pvt, NULL);
		return;
	}

	/* Retrieve unique name */
	msg = l_dbus_message_new_method_call(dbus, "org.freedesktop.DBus",
							"/org/freedesktop/DBus",
							"org.freedesktop.DBus",
							"GetId");

	l_dbus_message_set_arguments(msg, "");

	l_dbus_send_with_reply(dbus, msg, hello_callback, pvt, NULL);
}

static void unit_up(void *user_data)
{
	struct mesh_io_private *pvt = user_data;

	l_debug("Started io-unit");

	if (pvt->ready_callback)
		pvt->ready_callback(pvt->user_data, true);

	l_timeout_create_ms(1, get_name, pvt, NULL);
}

static bool unit_init(struct mesh_io *io, void *opt,
				mesh_io_ready_func_t cb, void *user_data)
{
	struct mesh_io_private *pvt;
	char *sk_path;
	size_t size;

	l_debug("Starting Unit test IO");
	if (!io || io->pvt)
		return false;

	sk_path = (char *) opt;

	pvt = l_new(struct mesh_io_private, 1);

	pvt->addr.sun_family = AF_LOCAL;
	snprintf(pvt->addr.sun_path, sizeof(pvt->addr.sun_path), "%s",
								sk_path);

	pvt->fd = socket(PF_LOCAL, SOCK_DGRAM | SOCK_CLOEXEC, 0);
	if (pvt->fd < 0)
		goto fail;

	unlink(pvt->addr.sun_path);
	size = offsetof(struct sockaddr_un, sun_path) +
						strlen(pvt->addr.sun_path);

	if (bind(pvt->fd, (struct sockaddr *) &pvt->addr, size) < 0)
		goto fail;

	/* Setup socket handlers */
	pvt->sio = l_io_new(pvt->fd);
	if (!l_io_set_read_handler(pvt->sio, incoming, pvt, NULL))
		goto fail;

	pvt->rx_regs = l_queue_new();
	pvt->tx_pkts = l_queue_new();

	pvt->ready_callback = cb;
	pvt->user_data = user_data;

	io->pvt = pvt;

	l_idle_oneshot(unit_up, pvt, NULL);

	return true;

fail:
	l_error("Failed to bind Unit Test socket");
	free_socket(pvt);
	l_free(pvt);

	return false;
}

static bool unit_destroy(struct mesh_io *io)
{
	struct mesh_io_private *pvt = io->pvt;

	if (!pvt)
		return true;

	l_free(pvt->unique_name);
	l_timeout_remove(pvt->tx_timeout);
	l_queue_destroy(pvt->rx_regs, l_free);
	l_queue_destroy(pvt->tx_pkts, l_free);

	free_socket(pvt);

	l_free(pvt);
	io->pvt = NULL;

	return true;
}

static bool unit_caps(struct mesh_io *io, struct mesh_io_caps *caps)
{
	struct mesh_io_private *pvt = io->pvt;

	if (!pvt || !caps)
		return false;

	caps->max_num_filters = 255;
	caps->window_accuracy = 50;

	return true;
}

static bool simple_match(const void *a, const void *b)
{
	return a == b;
}

static void send_pkt(struct mesh_io_private *pvt, struct tx_pkt *tx,
							uint16_t interval)
{
	send(pvt->fd, tx->pkt, tx->len, MSG_DONTWAIT);

	if (tx->delete) {
		l_queue_remove_if(pvt->tx_pkts, simple_match, tx);
		l_free(tx);
	}
}

static void tx_to(struct l_timeout *timeout, void *user_data)
{
	struct mesh_io_private *pvt = user_data;
	struct tx_pkt *tx;
	uint16_t ms;
	uint8_t count;

	if (!pvt)
		return;

	tx = l_queue_pop_head(pvt->tx_pkts);
	if (!tx) {
		l_timeout_remove(timeout);
		pvt->tx_timeout = NULL;
		return;
	}

	if (tx->info.type == MESH_IO_TIMING_TYPE_GENERAL) {
		ms = tx->info.u.gen.interval;
		count = tx->info.u.gen.cnt;
		if (count != MESH_IO_TX_COUNT_UNLIMITED)
			tx->info.u.gen.cnt--;
	} else {
		ms = 25;
		count = 1;
	}

	tx->delete = !!(count == 1);

	send_pkt(pvt, tx, ms);

	if (count == 1) {
		/* Recalculate wakeup if we are responding to POLL */
		tx = l_queue_peek_head(pvt->tx_pkts);

		if (tx && tx->info.type == MESH_IO_TIMING_TYPE_POLL_RSP) {
			ms = instant_remaining_ms(tx->info.u.poll_rsp.instant +
						tx->info.u.poll_rsp.delay);
		}
	} else
		l_queue_push_tail(pvt->tx_pkts, tx);

	if (timeout) {
		pvt->tx_timeout = timeout;
		l_timeout_modify_ms(timeout, ms);
	} else
		pvt->tx_timeout = l_timeout_create_ms(ms, tx_to, pvt, NULL);
}

static void tx_worker(void *user_data)
{
	struct mesh_io_private *pvt = user_data;
	struct tx_pkt *tx;
	uint32_t delay;

	tx = l_queue_peek_head(pvt->tx_pkts);
	if (!tx)
		return;

	switch (tx->info.type) {
	case MESH_IO_TIMING_TYPE_GENERAL:
		if (tx->info.u.gen.min_delay == tx->info.u.gen.max_delay)
			delay = tx->info.u.gen.min_delay;
		else {
			l_getrandom(&delay, sizeof(delay));
			delay %= tx->info.u.gen.max_delay -
						tx->info.u.gen.min_delay;
			delay += tx->info.u.gen.min_delay;
		}
		break;

	case MESH_IO_TIMING_TYPE_POLL:
		if (tx->info.u.poll.min_delay == tx->info.u.poll.max_delay)
			delay = tx->info.u.poll.min_delay;
		else {
			l_getrandom(&delay, sizeof(delay));
			delay %= tx->info.u.poll.max_delay -
						tx->info.u.poll.min_delay;
			delay += tx->info.u.poll.min_delay;
		}
		break;

	case MESH_IO_TIMING_TYPE_POLL_RSP:
		/* Delay until Instant + Delay */
		delay = instant_remaining_ms(tx->info.u.poll_rsp.instant +
						tx->info.u.poll_rsp.delay);
		if (delay > 255)
			delay = 0;
		break;

	default:
		return;
	}

	if (!delay)
		tx_to(pvt->tx_timeout, pvt);
	else if (pvt->tx_timeout)
		l_timeout_modify_ms(pvt->tx_timeout, delay);
	else
		pvt->tx_timeout = l_timeout_create_ms(delay, tx_to, pvt, NULL);
}

static bool send_tx(struct mesh_io *io, struct mesh_io_send_info *info,
					const uint8_t *data, uint16_t len)
{
	struct mesh_io_private *pvt = io->pvt;
	struct tx_pkt *tx;
	bool sending = false;

	if (!info || !data || !len || len > sizeof(tx->pkt))
		return false;

	tx = l_new(struct tx_pkt, 1);

	memcpy(&tx->info, info, sizeof(tx->info));
	memcpy(&tx->pkt, data, len);
	tx->len = len;

	if (info->type == MESH_IO_TIMING_TYPE_POLL_RSP)
		l_queue_push_head(pvt->tx_pkts, tx);
	else {
		sending = !l_queue_isempty(pvt->tx_pkts);

		l_queue_push_tail(pvt->tx_pkts, tx);
	}

	if (!sending) {
		l_timeout_remove(pvt->tx_timeout);
		pvt->tx_timeout = NULL;
		l_idle_oneshot(tx_worker, pvt, NULL);
	}

	return true;
}

static bool tx_cancel(struct mesh_io *io, const uint8_t *data, uint8_t len)
{
	struct mesh_io_private *pvt = io->pvt;
	struct tx_pkt *tx;

	if (!data)
		return false;

	if (len == 1) {
		do {
			tx = l_queue_remove_if(pvt->tx_pkts, find_by_ad_type,
							L_UINT_TO_PTR(data[0]));
			l_free(tx);

		} while (tx);
	} else {
		struct tx_pattern pattern = {
			.data = data,
			.len = len
		};

		do {
			tx = l_queue_remove_if(pvt->tx_pkts, find_by_pattern,
								&pattern);
			l_free(tx);

		} while (tx);
	}

	if (l_queue_isempty(pvt->tx_pkts)) {
		l_timeout_remove(pvt->tx_timeout);
		pvt->tx_timeout = NULL;
	}

	return true;
}

static bool find_by_filter(const void *a, const void *b)
{
	const struct pvt_rx_reg *rx_reg = a;
	const uint8_t *filter = b;

	return !memcmp(rx_reg->filter, filter, rx_reg->len);
}

static bool recv_register(struct mesh_io *io, const uint8_t *filter,
			uint8_t len, mesh_io_recv_func_t cb, void *user_data)
{
	struct mesh_io_private *pvt = io->pvt;
	struct pvt_rx_reg *rx_reg;

	if (!cb || !filter || !len)
		return false;

	rx_reg = l_queue_remove_if(pvt->rx_regs, find_by_filter, filter);

	l_free(rx_reg);
	rx_reg = l_malloc(sizeof(*rx_reg) + len);

	memcpy(rx_reg->filter, filter, len);
	rx_reg->len = len;
	rx_reg->cb = cb;
	rx_reg->user_data = user_data;

	l_queue_push_head(pvt->rx_regs, rx_reg);

	return true;
}

static bool recv_deregister(struct mesh_io *io, const uint8_t *filter,
								uint8_t len)
{
	return true;
}

const struct mesh_io_api mesh_io_unit = {
	.init = unit_init,
	.destroy = unit_destroy,
	.caps = unit_caps,
	.send = send_tx,
	.reg = recv_register,
	.dereg = recv_deregister,
	.cancel = tx_cancel,
};