pipewire/src/modules/module-avb/mrp.c

/* AVB support */
/* SPDX-FileCopyrightText: Copyright © 2022 Wim Taymans */
/* SPDX-FileCopyrightText: Copyright © 2025 Kebag-Logic */
/* SPDX-FileCopyrightText: Copyright © 2025 Alexandre Malki <alexandre.malki@kebag-logic.com> */
/* SPDX-License-Identifier: MIT */

#include <pipewire/pipewire.h>

#include "mrp.h"

#define MRP_JOINTIMER_MS	100
#define MRP_LVTIMER_MS		1000
#define MRP_LVATIMER_MS		10000
#define MRP_PERIODTIMER_MS	1000

#define mrp_emit(s,m,v,...)		spa_hook_list_call(&s->listener_list, struct avb_mrp_events, m, v, ##__VA_ARGS__)
#define mrp_emit_event(s,n,e)		mrp_emit(s,event,0,n,e)
#define mrp_emit_notify(s,n,a,e)	mrp_emit(s,notify,0,n,a,e)

#define mrp_attribute_emit(a,m,v,...)		spa_hook_list_call(&a->listener_list, struct avb_mrp_attribute_events, m, v, ##__VA_ARGS__)
#define mrp_attribute_emit_notify(a,n,e)	mrp_attribute_emit(a,notify,0,n,e)


struct mrp;

struct attribute {
	struct avb_mrp_attribute attr;
	struct mrp *mrp;
	struct spa_list link;
	uint8_t applicant_state;
	uint8_t registrar_state;
	uint64_t leave_timeout;
	unsigned joined:1;
	struct spa_hook_list listener_list;
};

enum fsm_lva {
	FSM_LVA_ACTIVE,
	FSM_LVA_PASSIVE
};

struct fsm_leave_all_timer {
	enum fsm_lva state;
	uint64_t leave_all_timeout;
};

struct mrp {
	struct server *server;
	struct spa_hook server_listener;

	struct spa_hook_list listener_list;

	struct spa_list attributes;

	uint64_t periodic_timeout;
	struct fsm_leave_all_timer lva_timer;
	uint64_t join_timeout;
};

static void mrp_destroy(void *data)
{
	struct mrp *mrp = data;
	struct attribute *a, *t;
	spa_hook_remove(&mrp->server_listener);

        spa_list_for_each_safe(a, t, &mrp->attributes, link) {
		spa_list_remove(&a->link);
		free(a);
        }

	free(mrp);
}

static void global_event(struct mrp *mrp, uint64_t now, uint8_t event)
{
	struct attribute *a;
	spa_list_for_each(a, &mrp->attributes, link)
		avb_mrp_attribute_update_state(&a->attr, now, event);
	mrp_emit_event(mrp, now, event);
}

static void mrp_set_update_lva(struct mrp *mrp, uint64_t now, bool force_send)
{
	if (!force_send) {
		mrp->lva_timer.leave_all_timeout = now
				+ (MRP_LVATIMER_MS + (pw_rand32() % (MRP_LVATIMER_MS / 2)))
				* SPA_NSEC_PER_MSEC;
	} else {
		mrp->lva_timer.leave_all_timeout = now;
	}
}

static void mrp_periodic(void *data, uint64_t now)
{
	struct mrp *mrp = data;
	bool leave_all = false;
	struct attribute *a;

	if (now > mrp->periodic_timeout) {
		if (mrp->periodic_timeout > 0)
			global_event(mrp, now, AVB_MRP_EVENT_PERIODIC);
		mrp->periodic_timeout = now + MRP_PERIODTIMER_MS * SPA_NSEC_PER_MSEC;
	}


	if (now > mrp->lva_timer.leave_all_timeout) {
		/* 802.1Q-2014 Table 10-5 */
		mrp->lva_timer.state = FSM_LVA_ACTIVE;
		if (mrp->lva_timer.leave_all_timeout > 0) {
			global_event(mrp, now, AVB_MRP_EVENT_RX_LVA);
			leave_all = true;
		}
	}

	if (now > mrp->join_timeout) {
		if (mrp->join_timeout > 0) {
			uint8_t event = leave_all ? AVB_MRP_EVENT_TX_LVA : AVB_MRP_EVENT_TX;
			global_event(mrp, now, event);
		}
		mrp->join_timeout = now + MRP_JOINTIMER_MS * SPA_NSEC_PER_MSEC;
	}

	spa_list_for_each(a, &mrp->attributes, link) {
		// 802.1Q Clause 10.7.4.2
		if (a->leave_timeout > 0 && now > a->leave_timeout && a->registrar_state ==
			AVB_MRP_LV) {
			a->leave_timeout = 0;
			avb_mrp_attribute_update_state(&a->attr, now, AVB_MRP_EVENT_LV_TIMER);
		}
	}
}

static const struct server_events server_events = {
	AVB_VERSION_SERVER_EVENTS,
	.destroy = mrp_destroy,
	.periodic = mrp_periodic,
};

int avb_mrp_parse_packet(struct avb_mrp *mrp, uint64_t now, const void *pkt, int len,
		const struct avb_mrp_parse_info *info, void *data)
{
	uint8_t *e = SPA_PTROFF(pkt, len, uint8_t);
	uint8_t *m = SPA_PTROFF(pkt, sizeof(struct avb_packet_mrp), uint8_t);

	while (m < e && (m[0] != 0 || m[1] != 0)) {
		const struct avb_packet_mrp_hdr *hdr = (const struct avb_packet_mrp_hdr*)m;
		uint8_t attr_type = hdr->attribute_type;
		uint8_t attr_len = hdr->attribute_length;
		size_t hdr_size;
		bool has_param;

		if (!info->check_header(data, hdr, &hdr_size, &has_param))
			return -EINVAL;

		m += hdr_size;

		while (m < e && (m[0] != 0 || m[1] != 0)) {
			const struct avb_packet_mrp_vector *v =
				(const struct avb_packet_mrp_vector*)m;
			uint16_t i, num_values = AVB_MRP_VECTOR_GET_NUM_VALUES(v);
			uint8_t event_len = (num_values+2)/3;
			uint8_t param_len = has_param ? (num_values+3)/4 : 0;
			int plen = sizeof(*v) + attr_len + event_len + param_len;
			const uint8_t *first = v->first_value;
			uint8_t event[3], param[4] = { 0, };

			if (m + plen > e)
				return -EPROTO;

			if (v->lva)
				info->attr_event(data, now, attr_type, AVB_MRP_ATTRIBUTE_EVENT_LVA);

			for (i = 0; i < num_values; i++) {
				if (i % 3 == 0) {
					uint8_t ep = first[attr_len + i/3];
					event[2] = ep % 6; ep /= 6;
					event[1] = ep % 6; ep /= 6;
					event[0] = ep % 6;
				}
				if (has_param && (i % 4 == 0)) {
					uint8_t ep = first[attr_len + event_len + i/4];
					param[3] = ep % 4; ep /= 4;
					param[2] = ep % 4; ep /= 4;
					param[1] = ep % 4; ep /= 4;
					param[0] = ep % 4;
				}
				info->process(data, now, attr_type, first,
						event[i%3], param[i%4], i);
			}
			m += plen;
		}
		m += 2;
	}
	return 0;
}

const char *avb_applicant_state_name(uint8_t state)
{
	switch (state) {
	case AVB_MRP_VO:
		return "VO";
	case AVB_MRP_VP:
		return "VP";
	case AVB_MRP_VN:
		return "VN";
	case AVB_MRP_AN:
		return "AN";
	case AVB_MRP_AA:
		return "AA";
	case AVB_MRP_QA:
		return "QA";
	case AVB_MRP_LA:
		return "LA";
	case AVB_MRP_AO:
		return "AO";
	case AVB_MRP_QO:
		return "QO";
	case AVB_MRP_AP:
		return "AP";
	case AVB_MRP_QP:
		return "QP";
	case AVB_MRP_LO:
		return "LO";
	}

    return "unknown-applicant-state";
}

const char *avb_registrar_state_name(uint8_t state)
{
	switch (state) {
	case AVB_MRP_IN:
		return "IN";
	case AVB_MRP_LV:
		return "LV";
	case AVB_MRP_MT:
		return "MT";
	}

	return "unknown-registrar-state";
}

const char *avb_mrp_event_name(uint8_t event)
{
	switch (event) {
	case AVB_MRP_EVENT_BEGIN:
		return "begin";
	case AVB_MRP_EVENT_NEW:
		return "new";
	case AVB_MRP_EVENT_JOIN:
		return "join";
	case AVB_MRP_EVENT_LV:
		return "lv";
	case AVB_MRP_EVENT_TX:
		return "tx";
	case AVB_MRP_EVENT_TX_LVA:
		return "tx_lva";
	case AVB_MRP_EVENT_TX_LVAF:
		return "tx_lvaf";
	case AVB_MRP_EVENT_RX_NEW:
		return "rx_new";
	case AVB_MRP_EVENT_RX_JOININ:
		return "rx_joinin";
	case AVB_MRP_EVENT_RX_IN:
		return "rx_in";
	case AVB_MRP_EVENT_RX_JOINMT:
		return "rx_joinmt";
	case AVB_MRP_EVENT_RX_MT:
		return "rx_mt";
	case AVB_MRP_EVENT_RX_LV:
		return "rx_lv";
	case AVB_MRP_EVENT_RX_LVA:
		return "rx_lva";
	case AVB_MRP_EVENT_FLUSH:
		return "flush";
	case AVB_MRP_EVENT_REDECLARE:
		return "redeclare";
	case AVB_MRP_EVENT_PERIODIC:
		return "periodic";
	case AVB_MRP_EVENT_LV_TIMER:
		return "lv_timer";
	case AVB_MRP_EVENT_LVA_TIMER:
		return "lva_timer";
	}

	return "unknown-event";
}

const char *avb_mrp_notify_name(uint8_t notify)
{
	switch(notify) {
	case AVB_MRP_NOTIFY_NEW:
		return "new";
	case AVB_MRP_NOTIFY_JOIN:
		return "join";
	case AVB_MRP_NOTIFY_LEAVE:
		return "leave";
	}
	return "unknown";
}

const char *avb_mrp_send_name(uint8_t send)
{
	switch(send) {
	case 0:
		return "none";
	case AVB_MRP_SEND_NEW:
		return "new";
	case AVB_MRP_SEND_JOININ:
		return "joinin";
	case AVB_MRP_SEND_IN:
		return "in";
	case AVB_MRP_SEND_JOINMT:
		return "joinmt";
	case AVB_MRP_SEND_MT:
		return "mt";
	case AVB_MRP_SEND_LV:
		return "leave";
	}
	return "unknown";
}

struct avb_mrp_attribute *avb_mrp_attribute_new(struct avb_mrp *m,
		size_t user_size)
{
	struct mrp *mrp = (struct mrp*)m;
	struct attribute *a;

	a = calloc(1, sizeof(*a) + user_size);
	if (a == NULL)
		return NULL;

	a->mrp = mrp;
	a->attr.user_data = SPA_PTROFF(a, sizeof(*a), void);
	spa_hook_list_init(&a->listener_list);
	spa_list_append(&mrp->attributes, &a->link);

	return &a->attr;
}

void avb_mrp_attribute_destroy(struct avb_mrp_attribute *attr)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	spa_list_remove(&a->link);
	free(a);
}

void avb_mrp_attribute_add_listener(struct avb_mrp_attribute *attr, struct spa_hook *listener,
		const struct avb_mrp_attribute_events *events, void *data)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	spa_hook_list_append(&a->listener_list, listener, events, data);
}

void avb_mrp_attribute_update_state(struct avb_mrp_attribute *attr, uint64_t now,
		int event)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	struct mrp *mrp = a->mrp;
	uint8_t notify = 0, state;
	uint8_t send = 0;

	// Handle the LVA timer FSM
	switch (event) {
		case AVB_MRP_EVENT_RX_LVA:
			mrp_set_update_lva(mrp, now, false);
			mrp->lva_timer.state = FSM_LVA_PASSIVE;
		break;
		case AVB_MRP_EVENT_TX:
			if (mrp->lva_timer.state == FSM_LVA_ACTIVE) {
				mrp_set_update_lva(mrp, now, true);
			}
			mrp->lva_timer.state = FSM_LVA_PASSIVE;
		break;
		default:
		break;
	}

	state = a->registrar_state;

	switch (event) {
	case AVB_MRP_EVENT_BEGIN:
		state = AVB_MRP_MT;
		break;
	case AVB_MRP_EVENT_RX_NEW:
		notify = AVB_MRP_NOTIFY_NEW;
		switch (state) {
		case AVB_MRP_LV:
			a->leave_timeout = INT64_MAX;
			break;
		}
		state = AVB_MRP_IN;
		break;
	case AVB_MRP_EVENT_RX_JOININ:
	case AVB_MRP_EVENT_RX_JOINMT:
		switch (state) {
		case AVB_MRP_LV:
			a->leave_timeout = INT64_MAX;
                        break;
		case AVB_MRP_MT:
			notify = AVB_MRP_NOTIFY_JOIN;
			break;
		}
		state = AVB_MRP_IN;
		break;
	case AVB_MRP_EVENT_RX_LV:
	case AVB_MRP_EVENT_RX_LVA:
	case AVB_MRP_EVENT_TX_LVA:
	case AVB_MRP_EVENT_REDECLARE:
		switch (state) {
		case AVB_MRP_IN:
			a->leave_timeout = now + MRP_LVTIMER_MS * SPA_NSEC_PER_MSEC;
			state = AVB_MRP_LV;
			break;
		}
		break;
	case AVB_MRP_EVENT_FLUSH:
		switch (state) {
		case AVB_MRP_LV:
			notify = AVB_MRP_NOTIFY_LEAVE;
			break;
		}
		state = AVB_MRP_MT;
		break;
	case AVB_MRP_EVENT_LV_TIMER:
		switch (state) {
		case AVB_MRP_LV:
			notify = AVB_MRP_NOTIFY_LEAVE;
			state = AVB_MRP_MT;
			break;
		}
		break;
	default:
		break;
	}
	if (notify) {
		mrp_attribute_emit_notify(a, now, notify);
		mrp_emit_notify(mrp, now, &a->attr, notify);
	}

	if (a->registrar_state != state || notify) {
		pw_log_debug("REG: attr %p: %s %s %s -> %s %s notify? %s", a, a->attr.name,
			avb_mrp_event_name(event), avb_registrar_state_name(a->registrar_state),
			avb_registrar_state_name(state), avb_mrp_send_name(notify),
			notify ? "YES":"NO");
		a->registrar_state = state;
	}

	state = a->applicant_state;

	switch (event) {
	case AVB_MRP_EVENT_BEGIN:
		state = AVB_MRP_VO;
		break;
	case AVB_MRP_EVENT_NEW:
		switch (state) {
		case AVB_MRP_VN:
		case AVB_MRP_AN:
			break;
		default:
			state = AVB_MRP_VN;
			break;
		}
		break;
	case AVB_MRP_EVENT_JOIN:
		switch (state) {
		case AVB_MRP_VO:
		case AVB_MRP_LO:
			state = AVB_MRP_VP;
			break;
		case AVB_MRP_LA:
			state = AVB_MRP_AA;
			break;
		case AVB_MRP_AO:
			state = AVB_MRP_AP;
			break;
		case AVB_MRP_QO:
			state = AVB_MRP_QP;
			break;
		}
		break;
	case AVB_MRP_EVENT_LV:
		switch (state) {
		case AVB_MRP_VP:
			state = AVB_MRP_VO;
			break;
		case AVB_MRP_VN:
		case AVB_MRP_AN:
		case AVB_MRP_AA:
		case AVB_MRP_QA:
			state = AVB_MRP_LA;
			break;
		case AVB_MRP_AP:
			state = AVB_MRP_AO;
			break;
		case AVB_MRP_QP:
			state = AVB_MRP_QO;
			break;
		}
		break;
	case AVB_MRP_EVENT_RX_JOININ:
		switch (state) {
		case AVB_MRP_VO:
			state = AVB_MRP_AO;
			break;
		case AVB_MRP_VP:
			state = AVB_MRP_AP;
			break;
		case AVB_MRP_AA:
			state = AVB_MRP_QA;
			break;
		case AVB_MRP_AO:
			state = AVB_MRP_QO;
			break;
		case AVB_MRP_AP:
			state = AVB_MRP_QP;
			break;
		}
		SPA_FALLTHROUGH;
	case AVB_MRP_EVENT_RX_IN:
		switch (state) {
		case AVB_MRP_AA:
			state = AVB_MRP_QA;
			break;
		}
		break;
	case AVB_MRP_EVENT_RX_JOINMT:
	case AVB_MRP_EVENT_RX_MT:
		switch (state) {
		case AVB_MRP_QA:
			state = AVB_MRP_AA;
			break;
		case AVB_MRP_QO:
			state = AVB_MRP_AO;
			break;
		case AVB_MRP_QP:
			state = AVB_MRP_AP;
			break;
		case AVB_MRP_LO:
			state = AVB_MRP_VO;
			break;
		}
		break;
	case AVB_MRP_EVENT_RX_LV:
	case AVB_MRP_EVENT_RX_LVA:
	case AVB_MRP_EVENT_REDECLARE:
		switch (state) {
		case AVB_MRP_VO:
		case AVB_MRP_AO:
		case AVB_MRP_QO:
			state = AVB_MRP_LO;
			break;
		case AVB_MRP_AN:
			state = AVB_MRP_VN;
			break;
		case AVB_MRP_AA:
		case AVB_MRP_QA:
		case AVB_MRP_AP:
		case AVB_MRP_QP:
			state = AVB_MRP_VP;
			break;
		}
		break;
	case AVB_MRP_EVENT_PERIODIC:
		switch (state) {
		case AVB_MRP_QA:
			state = AVB_MRP_AA;
			break;
		case AVB_MRP_QP:
			state = AVB_MRP_AP;
			break;
		}
		break;
	case AVB_MRP_EVENT_TX:
		switch (state) {
		case AVB_MRP_VP:
		case AVB_MRP_AA:
		case AVB_MRP_AP:
			if (a->registrar_state == AVB_MRP_IN)
				send = AVB_MRP_SEND_JOININ;
			else
				send = AVB_MRP_SEND_JOINMT;
			break;
		case AVB_MRP_VN:
		case AVB_MRP_AN:
			send = AVB_MRP_SEND_NEW;
			break;
		case AVB_MRP_LA:
			send = AVB_MRP_SEND_LV;
			break;
		case AVB_MRP_LO:
			if (a->registrar_state == AVB_MRP_IN)
				send = AVB_MRP_SEND_IN;
			else
				send = AVB_MRP_SEND_MT;
			break;
		}
		switch (state) {
		case AVB_MRP_VP:
			state = AVB_MRP_AA;
			break;
		case AVB_MRP_VN:
			state = AVB_MRP_AN;
			break;
		case AVB_MRP_AN:
			if(a->registrar_state == AVB_MRP_IN)
				state = AVB_MRP_QA;
			else
				state = AVB_MRP_AA;
			break;
		case AVB_MRP_AA:
		case AVB_MRP_AP:
			state = AVB_MRP_QA;
			break;
		case AVB_MRP_LA:
		case AVB_MRP_LO:
			state = AVB_MRP_VO;
			break;
		}
		break;
	case AVB_MRP_EVENT_TX_LVA:
	{
		switch (state) {
		case AVB_MRP_VP:
			if (a->registrar_state == AVB_MRP_IN)
				send = AVB_MRP_SEND_IN;
			else
				send = AVB_MRP_SEND_MT;
			break;
		case AVB_MRP_VN:
		case AVB_MRP_AN:
			send = AVB_MRP_SEND_NEW;
			break;
                case AVB_MRP_AA:
                case AVB_MRP_QA:
                case AVB_MRP_AP:
		case AVB_MRP_QP:
			if (a->registrar_state == AVB_MRP_IN)
				send = AVB_MRP_SEND_JOININ;
			else
				send = AVB_MRP_SEND_JOINMT;
			break;
		}
		switch (state) {
		case AVB_MRP_VO:
		case AVB_MRP_LA:
		case AVB_MRP_AO:
		case AVB_MRP_QO:
			state = AVB_MRP_LO;
			break;
		case AVB_MRP_VP:
			state = AVB_MRP_AA;
			break;
		case AVB_MRP_VN:
			state = AVB_MRP_AN;
			break;
		case AVB_MRP_AN:
		case AVB_MRP_AA:
		case AVB_MRP_AP:
		case AVB_MRP_QP:
			state = AVB_MRP_QA;
			break;
		}
		break;
	}
	default:
		break;
	}

	if (a->applicant_state != state || send) {
		pw_log_debug("APP: attr %p: %s %s %s -> %s %d:%s joined? %s", a, a->attr.name,
			avb_mrp_event_name(event), avb_applicant_state_name(a->applicant_state),
			avb_registrar_state_name(state), send, avb_mrp_send_name(send),
			a->joined? "YES" : " NO");
		a->applicant_state = state;
	}

	if (a->joined)
		a->attr.pending_send = send;
}

void avb_mrp_attribute_rx_event(struct avb_mrp_attribute *attr, uint64_t now, uint8_t event)
{
	static const int map[] = {
		[AVB_MRP_ATTRIBUTE_EVENT_NEW] = AVB_MRP_EVENT_RX_NEW,
		[AVB_MRP_ATTRIBUTE_EVENT_JOININ] = AVB_MRP_EVENT_RX_JOININ,
		[AVB_MRP_ATTRIBUTE_EVENT_IN] = AVB_MRP_EVENT_RX_IN,
		[AVB_MRP_ATTRIBUTE_EVENT_JOINMT] = AVB_MRP_EVENT_RX_JOINMT,
		[AVB_MRP_ATTRIBUTE_EVENT_MT] = AVB_MRP_EVENT_RX_MT,
		[AVB_MRP_ATTRIBUTE_EVENT_LV] = AVB_MRP_EVENT_RX_LV,
		[AVB_MRP_ATTRIBUTE_EVENT_LVA] = AVB_MRP_EVENT_RX_LVA,
	};
	avb_mrp_attribute_update_state(attr, now, map[event]);
}

void avb_mrp_attribute_begin(struct avb_mrp_attribute *attr, uint64_t now)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	a->leave_timeout = 0;
	avb_mrp_attribute_update_state(attr, now, AVB_MRP_EVENT_BEGIN);
}

void avb_mrp_attribute_join(struct avb_mrp_attribute *attr, uint64_t now, bool is_new)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	a->joined = true;
	int event = is_new ? AVB_MRP_EVENT_NEW : AVB_MRP_EVENT_JOIN;
	avb_mrp_attribute_update_state(attr, now, event);
}

void avb_mrp_attribute_leave(struct avb_mrp_attribute *attr, uint64_t now)
{
	struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr);
	avb_mrp_attribute_update_state(attr, now, AVB_MRP_EVENT_LV);
	a->joined = false;
}

void avb_mrp_destroy(struct avb_mrp *mrp)
{
	mrp_destroy(mrp);
}

struct avb_mrp *avb_mrp_new(struct server *server)
{
	struct mrp *mrp;

	mrp = calloc(1, sizeof(*mrp));
	if (mrp == NULL)
		return NULL;

	mrp->server = server;
	spa_list_init(&mrp->attributes);
	spa_hook_list_init(&mrp->listener_list);

	avdecc_server_add_listener(server, &mrp->server_listener, &server_events, mrp);

	return (struct avb_mrp*)mrp;
}

void avb_mrp_add_listener(struct avb_mrp *m, struct spa_hook *listener,
		const struct avb_mrp_events *events, void *data)
{
	struct mrp *mrp = (struct mrp*)m;
	spa_hook_list_append(&mrp->listener_list, listener, events, data);
}