From f64f8cdd4da8124863b629a68516992269c8b277 Mon Sep 17 00:00:00 2001 From: Wim Taymans Date: Wed, 23 Mar 2022 19:57:25 +0100 Subject: [PATCH] avb: more work Implement generic MRP parsing. Implement more MRP messages. Implement MRP timeouts. Implement MRP join/leave. Prepare for generating and sending MRP packets. --- src/modules/module-avbtp/avdecc.c | 17 +- src/modules/module-avbtp/internal.h | 5 + src/modules/module-avbtp/mmrp.c | 145 +++++++++++++++- src/modules/module-avbtp/mmrp.h | 27 ++- src/modules/module-avbtp/mrp.c | 245 ++++++++++++++++++++++++---- src/modules/module-avbtp/mrp.h | 57 +++++-- src/modules/module-avbtp/msrp.c | 212 +++++++++++++++--------- src/modules/module-avbtp/msrp.h | 20 ++- src/modules/module-avbtp/mvrp.c | 92 ++++++++++- src/modules/module-avbtp/mvrp.h | 18 +- src/modules/module-avbtp/utils.h | 6 + 11 files changed, 707 insertions(+), 137 deletions(-) diff --git a/src/modules/module-avbtp/avdecc.c b/src/modules/module-avbtp/avdecc.c index e14dd165f..f82ef2488 100644 --- a/src/modules/module-avbtp/avdecc.c +++ b/src/modules/module-avbtp/avdecc.c @@ -217,15 +217,26 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s init_descriptors(server); server->mrp = avbtp_mrp_new(server); + if (server->mrp == NULL) + goto error_free; avbtp_aecp_register(server); avbtp_maap_register(server); - avbtp_mmrp_register(server); - avbtp_msrp_register(server); - avbtp_mvrp_register(server); + server->mmrp = avbtp_mmrp_register(server); + server->msrp = avbtp_msrp_register(server); + server->mvrp = avbtp_mvrp_register(server); avbtp_adp_register(server); avbtp_acmp_register(server); + server->domain_attr = avbtp_msrp_attribute_new(server->msrp, + AVBTP_MSRP_ATTRIBUTE_TYPE_DOMAIN); + server->domain_attr->attr.domain.sr_class_id = 6; + server->domain_attr->attr.domain.sr_class_priority = 3; + server->domain_attr->attr.domain.sr_class_vid = 2; + + avbtp_mrp_mad_begin(server->mrp, 0, server->domain_attr->mrp); + avbtp_mrp_mad_join(server->mrp, 0, server->domain_attr->mrp, true); + return server; error_free: diff --git a/src/modules/module-avbtp/internal.h b/src/modules/module-avbtp/internal.h index 305fee454..fed353ade 100644 --- a/src/modules/module-avbtp/internal.h +++ b/src/modules/module-avbtp/internal.h @@ -88,6 +88,11 @@ struct server { unsigned debug_messages:1; struct avbtp_mrp *mrp; + struct avbtp_mmrp *mmrp; + struct avbtp_mvrp *mvrp; + struct avbtp_msrp *msrp; + + struct avbtp_msrp_attribute *domain_attr; }; static inline const struct descriptor *server_find_descriptor(struct server *server, diff --git a/src/modules/module-avbtp/mmrp.c b/src/modules/module-avbtp/mmrp.c index 6b48f1b8c..3943dacf8 100644 --- a/src/modules/module-avbtp/mmrp.c +++ b/src/modules/module-avbtp/mmrp.c @@ -24,17 +24,116 @@ #include +#include "utils.h" #include "mmrp.h" static const uint8_t mac[6] = AVB_MMRP_MAC; +struct attr { + struct avbtp_mmrp_attribute attr; + struct spa_list link; + struct avbtp_mrp_attribute *a; + uint8_t addr[6]; +}; + struct mmrp { struct server *server; struct spa_hook server_listener; + + struct spa_list attributes; +}; + +static struct attr *find_attr_by_addr(struct mmrp *mmrp, const uint8_t addr[6]) +{ + struct attr *a; + spa_list_for_each(a, &mmrp->attributes, link) + if (memcmp(a->addr, addr, 6) == 0) + return a; + return NULL; +} + +static bool mmrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) +{ + const struct avbtp_packet_mmrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVBTP_MMRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = false; + return true; +} + +static int mmrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) +{ + struct mmrp *mmrp = data; + struct attr *a; + pw_log_info("leave all"); + spa_list_for_each(a, &mmrp->attributes, link) + if (a->attr.type == attribute_type) + avbtp_mrp_update_state(mmrp->server->mrp, now, a->a, event); + return 0; +} + +static int process_service_requirement(struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avbtp_packet_mmrp_service_requirement *t = m; + char buf[128]; + struct attr *a; + + pw_log_info("service requirement"); + pw_log_info(" %s", avbtp_utils_format_addr(buf, sizeof(buf), t->addr)); + + a = find_attr_by_addr(mmrp, t->addr); + if (a) + avbtp_mrp_rx_event(mmrp->server->mrp, now, a->a, event); + return 0; +} + +static int process_mac(struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avbtp_packet_mmrp_mac *t = m; + char buf[128]; + struct attr *a; + + pw_log_info("mac"); + pw_log_info(" %s", avbtp_utils_format_addr(buf, sizeof(buf), t->addr)); + + a = find_attr_by_addr(mmrp, t->addr); + if (a) + avbtp_mrp_rx_event(mmrp->server->mrp, now, a->a, event); + return 0; +} + +static const struct { + int (*dispatch) (struct mmrp *mmrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); +} dispatch[] = { + [AVBTP_MMRP_ATTRIBUTE_TYPE_SERVICE_REQUIREMENT] = { process_service_requirement, }, + [AVBTP_MMRP_ATTRIBUTE_TYPE_MAC] = { process_mac, }, +}; + +static int mmrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct mmrp *mmrp = data; + return dispatch[attribute_type].dispatch(mmrp, now, + attribute_type, value, event, param, index); +} + +static const struct avbtp_mrp_parse_info info = { + AVBTP_VERSION_MRP_PARSE_INFO, + .check_header = mmrp_check_header, + .attr_event = mmrp_attr_event, + .process = mmrp_process, }; static int mmrp_message(void *data, uint64_t now, const void *message, int len) { + struct mmrp *mmrp = data; const struct avbtp_packet_mrp *p = message; if (ntohs(p->eth.type) != AVB_MMRP_ETH) @@ -43,8 +142,8 @@ static int mmrp_message(void *data, uint64_t now, const void *message, int len) return 0; pw_log_info("MMRP"); - - return 0; + return avbtp_mrp_parse_packet(mmrp->server->mrp, + now, message, len, &info, mmrp); } static void mmrp_destroy(void *data) @@ -60,17 +159,53 @@ static const struct server_events server_events = { .message = mmrp_message }; -int avbtp_mmrp_register(struct server *server) +static int mmrp_attr_compare(void *data, struct avbtp_mrp_attribute *a, struct avbtp_mrp_attribute *b) +{ + return 0; +} + +static int mmrp_attr_merge(void *data, struct avbtp_mrp_attribute *a, int vector) +{ + return 0; +} + +static const struct avbtp_mrp_attribute_callbacks attr_cb = { + AVBTP_VERSION_MRP_ATTRIBUTE_CALLBACKS, + .compare = mmrp_attr_compare, + .merge = mmrp_attr_merge +}; + +struct avbtp_mmrp_attribute *avbtp_mmrp_attribute_new(struct avbtp_mmrp *m, + uint8_t type) +{ + struct mmrp *mmrp = (struct mmrp*)m; + struct avbtp_mrp_attribute *attr; + struct attr *a; + + attr = avbtp_mrp_attribute_new(mmrp->server->mrp, + &attr_cb, mmrp, sizeof(struct attr)); + + a = attr->user_data; + a->a = attr; + a->attr.type = type; + + spa_list_append(&mmrp->attributes, &a->link); + + return &a->attr; +} + +struct avbtp_mmrp *avbtp_mmrp_register(struct server *server) { struct mmrp *mmrp; mmrp = calloc(1, sizeof(*mmrp)); if (mmrp == NULL) - return -errno; + return NULL; mmrp->server = server; + spa_list_init(&mmrp->attributes); avdecc_server_add_listener(server, &mmrp->server_listener, &server_events, mmrp); - return 0; + return (struct avbtp_mmrp*)mmrp; } diff --git a/src/modules/module-avbtp/mmrp.h b/src/modules/module-avbtp/mmrp.h index 14a4aa5c8..f74e12a01 100644 --- a/src/modules/module-avbtp/mmrp.h +++ b/src/modules/module-avbtp/mmrp.h @@ -31,12 +31,37 @@ #define AVB_MMRP_ETH 0x88f6 #define AVB_MMRP_MAC { 0x01, 0x80, 0xc2, 0x00, 0x00, 0x20 } +#define AVBTP_MMRP_ATTRIBUTE_TYPE_SERVICE_REQUIREMENT 1 +#define AVBTP_MMRP_ATTRIBUTE_TYPE_MAC 2 +#define AVBTP_MMRP_ATTRIBUTE_TYPE_VALID(t) ((t)>=1 && (t)<=2) + struct avbtp_packet_mmrp_msg { uint8_t attribute_type; uint8_t attribute_length; uint8_t attribute_list[0]; } __attribute__ ((__packed__)); -int avbtp_mmrp_register(struct server *server); +struct avbtp_packet_mmrp_service_requirement { + unsigned char addr[6]; +} __attribute__ ((__packed__)); + +struct avbtp_packet_mmrp_mac { + unsigned char addr[6]; +} __attribute__ ((__packed__)); + +struct avbtp_mmrp; + +struct avbtp_mmrp_attribute { + uint8_t type; + union { + struct avbtp_packet_mmrp_service_requirement service_requirement; + struct avbtp_packet_mmrp_mac mac; + } attr; +}; + +struct avbtp_mmrp_attribute *avbtp_mmrp_attribute_new(struct avbtp_mmrp *mmrp, + uint8_t type); + +struct avbtp_mmrp *avbtp_mmrp_register(struct server *server); #endif /* AVBTP_MMRP_H */ diff --git a/src/modules/module-avbtp/mrp.c b/src/modules/module-avbtp/mrp.c index 44099a0c5..5384a5bef 100644 --- a/src/modules/module-avbtp/mrp.c +++ b/src/modules/module-avbtp/mrp.c @@ -26,18 +26,35 @@ #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 avbtp_mrp_events, m, v, ##__VA_ARGS__) +#define mrp_emit_tx_event(s,e,st) mrp_emit(s, tx_event, 0, e, st) + struct attribute { struct avbtp_mrp_attribute attr; struct spa_list link; uint8_t applicant_state; uint8_t registrar_state; uint16_t pending_indications; + uint64_t leave_timeout; struct spa_callbacks cb; }; struct mrp { struct server *server; struct spa_hook server_listener; + + struct spa_hook_list listener_list; + + struct spa_list attributes; + + uint64_t periodic_timeout; + uint64_t leave_all_timeout; + uint64_t join_timeout; }; static void mrp_destroy(void *data) @@ -47,39 +64,199 @@ static void mrp_destroy(void *data) 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) + avbtp_mrp_update_state((struct avbtp_mrp*)mrp, now, &a->attr, event); +} + +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) { + mrp->periodic_timeout = now + MRP_PERIODTIMER_MS * SPA_NSEC_PER_MSEC; + + global_event(mrp, now, AVBTP_MRP_EVENT_PERIODIC); + } + if (now > mrp->leave_all_timeout) { + mrp->leave_all_timeout = now + MRP_LVATIMER_MS * SPA_NSEC_PER_MSEC; + + global_event(mrp, now, AVBTP_MRP_EVENT_RX_LVA); + leave_all = true; + } + + if (now > mrp->join_timeout) { + uint8_t event = leave_all ? AVBTP_MRP_EVENT_TX_LVA : AVBTP_MRP_EVENT_TX; + + mrp->join_timeout = now + MRP_JOINTIMER_MS * SPA_NSEC_PER_MSEC; + + mrp_emit_tx_event(mrp, event, true); + + global_event(mrp, now, event); + + mrp_emit_tx_event(mrp, event, false); + } + + spa_list_for_each(a, &mrp->attributes, link) { + if (now > a->leave_timeout) { + a->leave_timeout = 0; + avbtp_mrp_update_state((struct avbtp_mrp*)mrp, now, &a->attr, AVBTP_MRP_EVENT_LV_TIMER); + } + } + + +} + static const struct server_events server_events = { AVBTP_VERSION_SERVER_EVENTS, .destroy = mrp_destroy, + .periodic = mrp_periodic, }; -struct avbtp_mrp_attribute *avbtp_mrp_attribute_new(struct avbtp_mrp *mrp, - uint8_t type, uint8_t param, - struct avbtp_mrp_attribute_methods *methods, void *data, +int avbtp_mrp_parse_packet(struct avbtp_mrp *mrp, uint64_t now, const void *pkt, int len, + const struct avbtp_mrp_parse_info *info, void *data) +{ + uint8_t *e = SPA_PTROFF(pkt, len, uint8_t); + uint8_t *m = SPA_PTROFF(pkt, sizeof(struct avbtp_packet_mrp), uint8_t); + + while (m < e && (m[0] != 0 || m[1] != 0)) { + const struct avbtp_packet_mrp_hdr *hdr = (const struct avbtp_packet_mrp_hdr*)m; + uint8_t attr_len = hdr->attribute_length; + uint8_t attr_type = hdr->attribute_type; + 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 avbtp_packet_mrp_vector *v = + (const struct avbtp_packet_mrp_vector*)m; + uint16_t i, num_values = AVBTP_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 len = sizeof(*v) + attr_len + event_len + param_len; + const uint8_t *first = v->first_value; + uint8_t event[3], param[4] = { 0, }; + + if (m + len > e) + return -EPROTO; + + if (v->lva) + info->attr_event(data, now, attr_type, AVBTP_MRP_EVENT_RX_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 += len; + } + } + return 0; +} + +struct avbtp_mrp_attribute *avbtp_mrp_attribute_new(struct avbtp_mrp *m, + const struct avbtp_mrp_attribute_callbacks *cb, void *data, 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->attr.type = type; - a->attr.param = param; a->attr.user_data = SPA_PTROFF(a, sizeof(*a), void); - a->cb = SPA_CALLBACKS_INIT(methods, data); + a->cb = SPA_CALLBACKS_INIT(cb, data); + spa_list_append(&mrp->attributes, &a->link); return &a->attr; } -static void stop_avb_timer(void) -{ -} -static void start_avb_timer(void) +static uint8_t get_tx_event(struct avbtp_mrp *mrp, struct attribute *a, + int event, bool leave_all) { + uint8_t ev = 0; + + switch (a->applicant_state) { + case AVBTP_MRP_VP: + case AVBTP_MRP_AA: + case AVBTP_MRP_AP: + case AVBTP_MRP_QA: + case AVBTP_MRP_QP: + if (leave_all && a->applicant_state == AVBTP_MRP_VP) { + switch (a->registrar_state) { + case AVBTP_MRP_IN: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_IN; + break; + default: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_MT; + break; + } + } else if (leave_all || a->applicant_state != AVBTP_MRP_QP) { + switch (a->registrar_state) { + case AVBTP_MRP_IN: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_JOININ; + break; + default: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_JOINMT; + break; + } + } + break; + case AVBTP_MRP_VN: + case AVBTP_MRP_AN: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_NEW; + break; + case AVBTP_MRP_LA: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_LV; + break; + case AVBTP_MRP_LO: + switch (a->registrar_state) { + case AVBTP_MRP_IN: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_IN; + break; + default: + ev = AVBTP_MRP_ATTRIBUTE_EVENT_MT; + break; + } + break; + } + return ev; } -void avbtp_mrp_update_state(struct avbtp_mrp *mrp, - struct avbtp_mrp_attribute *attr, int event, uint8_t param) +static int do_tx(struct avbtp_mrp *mrp, struct attribute *a, + int event, bool leave_all) +{ + uint8_t ev = get_tx_event(mrp, a, event, leave_all); + + spa_callbacks_call(&a->cb, struct avbtp_mrp_attribute_callbacks, + merge, 0, &a->attr, ev); + + return 0; +} + +void avbtp_mrp_update_state(struct avbtp_mrp *mrp, uint64_t now, + struct avbtp_mrp_attribute *attr, int event) { struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); switch (event) { @@ -88,18 +265,16 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, break; case AVBTP_MRP_EVENT_RX_NEW: if (a->registrar_state == AVBTP_MRP_LV) - stop_avb_timer(); + a->leave_timeout = 0; a->registrar_state = AVBTP_MRP_IN; a->pending_indications |= AVBTP_PENDING_JOIN_NEW; - a->attr.param = param; break; case AVBTP_MRP_EVENT_RX_JOININ: case AVBTP_MRP_EVENT_RX_JOINMT: if (a->registrar_state == AVBTP_MRP_LV) - stop_avb_timer(); + a->leave_timeout = 0; if (a->registrar_state == AVBTP_MRP_MT) { a->pending_indications |= AVBTP_PENDING_JOIN; - a->attr.param = param; } a->registrar_state = AVBTP_MRP_IN; break; @@ -108,7 +283,7 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, case AVBTP_MRP_EVENT_TX_LVA: case AVBTP_MRP_EVENT_REDECLARE: if (a->registrar_state == AVBTP_MRP_IN) { - start_avb_timer(); + a->leave_timeout = now + MRP_LVTIMER_MS * SPA_NSEC_PER_MSEC; a->registrar_state = AVBTP_MRP_LV; } break; @@ -116,7 +291,6 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, case AVBTP_MRP_EVENT_FLUSH: if (a->registrar_state == AVBTP_MRP_LV) { a->pending_indications |= AVBTP_PENDING_LEAVE; - a->attr.param = param; } a->registrar_state = AVBTP_MRP_MT; break; @@ -249,8 +423,7 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, case AVBTP_MRP_LA: case AVBTP_MRP_AP: case AVBTP_MRP_LO: -// int vector = makeTxEvent(e, st, 0); -// doTx(st, vector); + do_tx(mrp, a, event, false); break; } switch (a->applicant_state) { @@ -282,8 +455,7 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, case AVBTP_MRP_QA: case AVBTP_MRP_AP: case AVBTP_MRP_QP: -// int vector = makeTxEvent(e, st, 1); -// doTx(st, vector); + do_tx(mrp, a, event, true); } switch (a->applicant_state) { case AVBTP_MRP_VO: @@ -309,8 +481,8 @@ void avbtp_mrp_update_state(struct avbtp_mrp *mrp, } } -void avbtp_mrp_event(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr, - uint8_t event, uint8_t param) +void avbtp_mrp_rx_event(struct avbtp_mrp *mrp, uint64_t now, + struct avbtp_mrp_attribute *attr, uint8_t event) { static const int map[] = { [AVBTP_MRP_ATTRIBUTE_EVENT_NEW] = AVBTP_MRP_EVENT_RX_NEW, @@ -320,19 +492,27 @@ void avbtp_mrp_event(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr, [AVBTP_MRP_ATTRIBUTE_EVENT_MT] = AVBTP_MRP_EVENT_RX_MT, [AVBTP_MRP_ATTRIBUTE_EVENT_LV] = AVBTP_MRP_EVENT_RX_LV, }; - avbtp_mrp_update_state(mrp, attr, map[event], param); + avbtp_mrp_update_state(mrp, now, attr, map[event]); } -void avbtp_mrp_mad_begin(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr) +void avbtp_mrp_mad_begin(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr) { + struct attribute *a = SPA_CONTAINER_OF(attr, struct attribute, attr); + a->leave_timeout = 0; + avbtp_mrp_update_state(mrp, now, attr, AVBTP_MRP_EVENT_BEGIN); } -void avbtp_mrp_mad_join(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr, bool is_new) +void avbtp_mrp_mad_join(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr, bool is_new) { + if (is_new) + avbtp_mrp_update_state(mrp, now, attr, AVBTP_MRP_EVENT_NEW); + else + avbtp_mrp_update_state(mrp, now, attr, AVBTP_MRP_EVENT_JOIN); } -void avbtp_mrp_mad_leave(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr) +void avbtp_mrp_mad_leave(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr) { + avbtp_mrp_update_state(mrp, now, attr, AVBTP_MRP_EVENT_LV); } void avbtp_mrp_destroy(struct avbtp_mrp *mrp) @@ -349,8 +529,17 @@ struct avbtp_mrp *avbtp_mrp_new(struct server *server) 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 avbtp_mrp*)mrp; } + +void avbtp_mrp_add_listener(struct avbtp_mrp *m, struct spa_hook *listener, + const struct avbtp_mrp_events *events, void *data) +{ + struct mrp *mrp = (struct mrp*)m; + spa_hook_list_append(&mrp->listener_list, listener, events, data); +} diff --git a/src/modules/module-avbtp/mrp.h b/src/modules/module-avbtp/mrp.h index 8d79dcfc4..4d0ea8a83 100644 --- a/src/modules/module-avbtp/mrp.h +++ b/src/modules/module-avbtp/mrp.h @@ -33,6 +33,11 @@ struct avbtp_packet_mrp { uint8_t version; } __attribute__ ((__packed__)); +struct avbtp_packet_mrp_hdr { + uint8_t attribute_type; + uint8_t attribute_length; +} __attribute__ ((__packed__)); + struct avbtp_packet_mrp_vector { #if __BYTE_ORDER == __BIG_ENDIAN unsigned lva:3; @@ -104,37 +109,63 @@ struct avbtp_packet_mrp_footer { #define AVBTP_PENDING_JOIN (1u<<1) #define AVBTP_PENDING_LEAVE (1u<<2) +struct avbtp_mrp_events { +#define AVBTP_VERSION_MRP_ATTRIBUTE_CALLBACKS 0 + uint32_t version; + + int (*tx_event) (void *data, uint8_t event, bool start); +}; + struct avbtp_mrp_attribute { + uint16_t domain; uint8_t type; - uint8_t param; void *user_data; }; -struct avbtp_mrp_attribute_methods { -#define AVBTP_VERSION_ATTRIBUTE_METHODS 0 +struct avbtp_mrp_attribute_callbacks { +#define AVBTP_VERSION_MRP_ATTRIBUTE_CALLBACKS 0 uint32_t version; int (*compare) (void *data, struct avbtp_mrp_attribute *a, struct avbtp_mrp_attribute *b); - int (*merge) (void *data, struct avbtp_mrp_attribute *a, uint8_t *msg, int vector); + int (*merge) (void *data, struct avbtp_mrp_attribute *a, int vector); }; + +struct avbtp_mrp_parse_info { +#define AVBTP_VERSION_MRP_PARSE_INFO 0 + uint32_t version; + + bool (*check_header) (void *data, const void *hdr, size_t *hdr_size, bool *has_params); + + int (*attr_event) (void *data, uint64_t now, uint8_t attribute_type, uint8_t event); + + int (*process) (void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index); +}; + + +int avbtp_mrp_parse_packet(struct avbtp_mrp *mrp, uint64_t now, const void *pkt, int size, + const struct avbtp_mrp_parse_info *cb, void *data); + struct avbtp_mrp_attribute *avbtp_mrp_attribute_new(struct avbtp_mrp *mrp, - uint8_t type, uint8_t param, - struct avbtp_mrp_attribute_methods *methods, void *data, + const struct avbtp_mrp_attribute_callbacks *cb, void *data, size_t user_size); -void avbtp_mrp_update_state(struct avbtp_mrp *mrp, - struct avbtp_mrp_attribute *attr, int event, uint8_t param); +void avbtp_mrp_update_state(struct avbtp_mrp *mrp, uint64_t now, + struct avbtp_mrp_attribute *attr, int event); -void avbtp_mrp_event(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr, - uint8_t event, uint8_t param); +void avbtp_mrp_rx_event(struct avbtp_mrp *mrp, uint64_t now, + struct avbtp_mrp_attribute *attr, uint8_t event); -void avbtp_mrp_mad_begin(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr); -void avbtp_mrp_mad_join(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr, bool is_new); -void avbtp_mrp_mad_leave(struct avbtp_mrp *mrp, struct avbtp_mrp_attribute *attr); +void avbtp_mrp_mad_begin(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr); +void avbtp_mrp_mad_join(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr, bool is_new); +void avbtp_mrp_mad_leave(struct avbtp_mrp *mrp, uint64_t now, struct avbtp_mrp_attribute *attr); struct avbtp_mrp *avbtp_mrp_new(struct server *server); void avbtp_mrp_destroy(struct avbtp_mrp *mrp); +void avbtp_mrp_add_listener(struct avbtp_mrp *mrp, struct spa_hook *listener, + const struct avbtp_mrp_events *events, void *data); + #endif /* AVBTP_MRP_H */ diff --git a/src/modules/module-avbtp/msrp.c b/src/modules/module-avbtp/msrp.c index 410a7c71f..d838df347 100644 --- a/src/modules/module-avbtp/msrp.c +++ b/src/modules/module-avbtp/msrp.c @@ -32,14 +32,14 @@ static const uint8_t mac[6] = AVB_MSRP_MAC; struct attr { + struct avbtp_msrp_attribute attr; struct spa_list link; - struct avbtp_mrp_attribute attr; - uint64_t stream_id; }; struct msrp { struct server *server; struct spa_hook server_listener; + struct spa_hook mrp_listener; struct spa_list attributes; }; @@ -48,64 +48,90 @@ static struct attr *find_attr_by_stream_id(struct msrp *msrp, uint64_t stream_id { struct attr *a; spa_list_for_each(a, &msrp->attributes, link) - if (a->stream_id == stream_id) + if (a->attr.attr.talker.stream_id == stream_id) return a; return NULL; } -static void attr_event(struct msrp *msrp, uint8_t type, int event) +static void debug_msrp_talker(const struct avbtp_packet_msrp_talker *t) { - struct attr *a; - spa_list_for_each(a, &msrp->attributes, link) - if (a->attr.type == type) - avbtp_mrp_update_state(msrp->server->mrp, &a->attr, event, 0); + char buf[128]; + pw_log_info(" stream-id: %s", avbtp_utils_format_id(buf, sizeof(buf), be64toh(t->stream_id))); + pw_log_info(" dest-addr: %s", avbtp_utils_format_addr(buf, sizeof(buf), t->dest_addr)); + pw_log_info(" vlan-id: %d", ntohs(t->vlan_id)); + pw_log_info(" tspec-max-frame-size: %d", ntohs(t->tspec_max_frame_size)); + pw_log_info(" tspec-max-interval-frames: %d", ntohs(t->tspec_max_interval_frames)); + pw_log_info(" priority: %d", t->priority); + pw_log_info(" rank: %d", t->rank); + pw_log_info(" accumulated-latency: %d", ntohl(t->accumulated_latency)); } -static void process_talker(struct msrp *msrp, uint8_t attr_type, +static int process_talker(struct msrp *msrp, uint64_t now, uint8_t attr_type, const void *m, uint8_t event, uint8_t param, int num) { const struct avbtp_packet_msrp_talker *t = m; - char buf[128]; struct attr *a; pw_log_info("talker"); - pw_log_info(" %s", avbtp_utils_format_id(buf, sizeof(buf), t->stream_id)); + debug_msrp_talker(t); a = find_attr_by_stream_id(msrp, be64toh(t->stream_id)); if (a) - avbtp_mrp_event(msrp->server->mrp, &a->attr, event, param); + avbtp_mrp_rx_event(msrp->server->mrp, now, a->attr.mrp, event); + return 0; } -static void process_talker_fail(struct msrp *msrp, uint8_t attr_type, +static void debug_msrp_talker_fail(const struct avbtp_packet_msrp_talker_fail *t) +{ + char buf[128]; + debug_msrp_talker(&t->talker); + pw_log_info(" bridge-id: %s", avbtp_utils_format_id(buf, sizeof(buf), be64toh(t->bridge_id))); + pw_log_info(" failure-code: %d", t->failure_code); +} + +static int process_talker_fail(struct msrp *msrp, uint64_t now, uint8_t attr_type, const void *m, uint8_t event, uint8_t param, int num) { const struct avbtp_packet_msrp_talker_fail *t = m; - char buf[128]; pw_log_info("talker fail"); - pw_log_info(" %s", avbtp_utils_format_id(buf, sizeof(buf), t->talker.stream_id)); + debug_msrp_talker_fail(t); + return 0; } -static void process_listener(struct msrp *msrp, uint8_t attr_type, - const void *m, uint8_t event, uint8_t param, int num) +static void debug_msrp_listener(const struct avbtp_packet_msrp_listener *l) { - const struct avbtp_packet_msrp_listener *l = m; char buf[128]; - pw_log_info("listener"); pw_log_info(" %s", avbtp_utils_format_id(buf, sizeof(buf), l->stream_id)); } -static void process_domain(struct msrp *msrp, uint8_t attr_type, +static int process_listener(struct msrp *msrp, uint64_t now, uint8_t attr_type, const void *m, uint8_t event, uint8_t param, int num) { - const struct avbtp_packet_msrp_domain *d = m; - pw_log_info("domain"); + const struct avbtp_packet_msrp_listener *l = m; + pw_log_info("listener"); + debug_msrp_listener(l); + return 0; +} + +static void debug_msrp_domain(const struct avbtp_packet_msrp_domain *d) +{ pw_log_info(" %d", d->sr_class_id); pw_log_info(" %d", d->sr_class_priority); pw_log_info(" %d", ntohs(d->sr_class_vid)); } +static int process_domain(struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avbtp_packet_msrp_domain *d = m; + pw_log_info("domain"); + debug_msrp_domain(d); + return 0; +} + static const struct { - void (*dispatch) (struct msrp *msrp, uint8_t attr_type, const void *m, uint8_t event, uint8_t param, int num); + int (*dispatch) (struct msrp *msrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); } dispatch[] = { [AVBTP_MSRP_ATTRIBUTE_TYPE_TALKER_ADVERTISE] = { process_talker, }, [AVBTP_MSRP_ATTRIBUTE_TYPE_TALKER_FAILED] = { process_talker_fail, }, @@ -113,66 +139,44 @@ static const struct { [AVBTP_MSRP_ATTRIBUTE_TYPE_DOMAIN] = { process_domain, }, }; -static inline bool has_params(uint16_t type) +static bool msrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) { - return type == AVBTP_MSRP_ATTRIBUTE_TYPE_LISTENER; + const struct avbtp_packet_msrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVBTP_MSRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = attr_type == AVBTP_MSRP_ATTRIBUTE_TYPE_LISTENER; + return true; } -static int process(struct msrp *msrp, uint64_t now, const void *message, int len) +static int msrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) { - uint8_t *e = SPA_PTROFF(message, len, uint8_t); - uint8_t *m = SPA_PTROFF(message, sizeof(struct avbtp_packet_mrp), uint8_t); - - while (m < e && (m[0] != 0 || m[1] != 0)) { - const struct avbtp_packet_msrp_msg *msg = (const struct avbtp_packet_msrp_msg*)m; - uint8_t attr_len = msg->attribute_length; - uint8_t attr_type = msg->attribute_type; - bool has_param = has_params(attr_type); - - if (!AVBTP_MSRP_ATTRIBUTE_TYPE_VALID(attr_type)) - return -EINVAL; - - m += sizeof(*msg); - - while (m < e && (m[0] != 0 || m[1] != 0)) { - const struct avbtp_packet_mrp_vector *v = - (const struct avbtp_packet_mrp_vector*)m; - uint16_t i, num_values = AVBTP_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 len = sizeof(*v) + attr_len + event_len + param_len; - const uint8_t *first = v->first_value; - uint8_t event[3], param[4] = { 0, }; - - if (m + len > e) - return -EPROTO; - - if (v->lva) - attr_event(msrp, attr_type, AVBTP_MRP_EVENT_RX_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; - } - dispatch[attr_type].dispatch(msrp, - attr_type, first, event[i%3], param[i%4], i); - } - m += len; - } - } + struct msrp *msrp = data; + struct attr *a; + spa_list_for_each(a, &msrp->attributes, link) + if (a->attr.type == attribute_type) + avbtp_mrp_update_state(msrp->server->mrp, now, a->attr.mrp, event); return 0; } +static int msrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct msrp *msrp = data; + return dispatch[attribute_type].dispatch(msrp, now, + attribute_type, value, event, param, index); +} + +static const struct avbtp_mrp_parse_info info = { + AVBTP_VERSION_MRP_PARSE_INFO, + .check_header = msrp_check_header, + .attr_event = msrp_attr_event, + .process = msrp_process, +}; + static int msrp_message(void *data, uint64_t now, const void *message, int len) { @@ -185,7 +189,8 @@ static int msrp_message(void *data, uint64_t now, const void *message, int len) return 0; pw_log_info("MSRP"); - return process(msrp, now, message, len); + return avbtp_mrp_parse_packet(msrp->server->mrp, + now, message, len, &info, msrp); } static void msrp_destroy(void *data) @@ -201,18 +206,65 @@ static const struct server_events server_events = { .message = msrp_message }; -int avbtp_msrp_register(struct server *server) +static int msrp_attr_compare(void *data, struct avbtp_mrp_attribute *a, struct avbtp_mrp_attribute *b) +{ + return 0; +} + +static int msrp_attr_merge(void *data, struct avbtp_mrp_attribute *a, int vector) +{ + pw_log_info("attr merge"); + return 0; +} + +static const struct avbtp_mrp_attribute_callbacks attr_cb = { + AVBTP_VERSION_MRP_ATTRIBUTE_CALLBACKS, + .compare = msrp_attr_compare, + .merge = msrp_attr_merge +}; + +struct avbtp_msrp_attribute *avbtp_msrp_attribute_new(struct avbtp_msrp *m, + uint8_t type) +{ + struct msrp *msrp = (struct msrp*)m; + struct avbtp_mrp_attribute *attr; + struct attr *a; + + attr = avbtp_mrp_attribute_new(msrp->server->mrp, + &attr_cb, msrp, sizeof(struct attr)); + + a = attr->user_data; + a->attr.mrp = attr; + spa_list_append(&msrp->attributes, &a->link); + a->attr.type = type; + + return &a->attr; +} + +static int msrp_tx_event(void *data, uint8_t event, bool start) +{ + pw_log_info("tx %s", start ? "start" : "stop"); + return 0; +} + +static const struct avbtp_mrp_events mrp_events = { + AVBTP_VERSION_MRP_ATTRIBUTE_CALLBACKS, + .tx_event = msrp_tx_event +}; + +struct avbtp_msrp *avbtp_msrp_register(struct server *server) { struct msrp *msrp; msrp = calloc(1, sizeof(*msrp)); if (msrp == NULL) - return -errno; + return NULL; msrp->server = server; spa_list_init(&msrp->attributes); avdecc_server_add_listener(server, &msrp->server_listener, &server_events, msrp); + avbtp_mrp_add_listener(server->mrp, &msrp->mrp_listener, &mrp_events, msrp); - return 0; + return (struct avbtp_msrp*)msrp; } diff --git a/src/modules/module-avbtp/msrp.h b/src/modules/module-avbtp/msrp.h index 977c1dcc9..83a28cbfd 100644 --- a/src/modules/module-avbtp/msrp.h +++ b/src/modules/module-avbtp/msrp.h @@ -104,6 +104,24 @@ struct avbtp_packet_msrp_domain { uint16_t sr_class_vid; } __attribute__ ((__packed__)); -int avbtp_msrp_register(struct server *server); + +struct avbtp_msrp_attribute { + struct avbtp_mrp_attribute *mrp; + uint8_t type; + uint8_t param; + union { + struct avbtp_packet_msrp_talker talker; + struct avbtp_packet_msrp_talker_fail talker_fail; + struct avbtp_packet_msrp_listener listener; + struct avbtp_packet_msrp_domain domain; + } attr; +}; + +struct avbtp_msrp; + +struct avbtp_msrp_attribute *avbtp_msrp_attribute_new(struct avbtp_msrp *msrp, + uint8_t type); + +struct avbtp_msrp *avbtp_msrp_register(struct server *server); #endif /* AVBTP_MSRP_H */ diff --git a/src/modules/module-avbtp/mvrp.c b/src/modules/module-avbtp/mvrp.c index a7f7c5ac6..66012d28c 100644 --- a/src/modules/module-avbtp/mvrp.c +++ b/src/modules/module-avbtp/mvrp.c @@ -28,13 +28,94 @@ static const uint8_t mac[6] = AVB_MVRP_MAC; +struct attr { + struct avbtp_mvrp_attribute attr; + struct spa_list link; + struct avbtp_mrp_attribute *a; + uint16_t vlan; +}; + struct mvrp { struct server *server; struct spa_hook server_listener; + + struct spa_list attributes; +}; + +static struct attr *find_attr_by_vlan(struct mvrp *mvrp, uint16_t vlan) +{ + struct attr *a; + spa_list_for_each(a, &mvrp->attributes, link) + if (a->vlan == vlan) + return a; + return NULL; +} + +static bool mvrp_check_header(void *data, const void *hdr, size_t *hdr_size, bool *has_params) +{ + const struct avbtp_packet_mvrp_msg *msg = hdr; + uint8_t attr_type = msg->attribute_type; + + if (!AVBTP_MVRP_ATTRIBUTE_TYPE_VALID(attr_type)) + return false; + + *hdr_size = sizeof(*msg); + *has_params = false; + return true; +} + +static int mvrp_attr_event(void *data, uint64_t now, uint8_t attribute_type, uint8_t event) +{ + struct mvrp *mvrp = data; + struct attr *a; + pw_log_info("leave all"); + spa_list_for_each(a, &mvrp->attributes, link) + if (a->attr.type == attribute_type) + avbtp_mrp_update_state(mvrp->server->mrp, now, a->a, event); + return 0; +} + +static int process_vid(struct mvrp *mvrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num) +{ + const struct avbtp_packet_mvrp_vid *t = m; + struct attr *a; + uint16_t vlan = ntohs(t->vlan); + + pw_log_info("vid"); + pw_log_info(" %d", vlan); + + a = find_attr_by_vlan(mvrp, vlan); + if (a) + avbtp_mrp_rx_event(mvrp->server->mrp, now, a->a, event); + return 0; +} + +static const struct { + int (*dispatch) (struct mvrp *mvrp, uint64_t now, uint8_t attr_type, + const void *m, uint8_t event, uint8_t param, int num); +} dispatch[] = { + [AVBTP_MVRP_ATTRIBUTE_TYPE_VID] = { process_vid, }, +}; + +static int mvrp_process(void *data, uint64_t now, uint8_t attribute_type, const void *value, + uint8_t event, uint8_t param, int index) +{ + struct mvrp *mvrp = data; + return dispatch[attribute_type].dispatch(mvrp, now, + attribute_type, value, event, param, index); +} + +static const struct avbtp_mrp_parse_info info = { + AVBTP_VERSION_MRP_PARSE_INFO, + .check_header = mvrp_check_header, + .attr_event = mvrp_attr_event, + .process = mvrp_process, }; static int mvrp_message(void *data, uint64_t now, const void *message, int len) { + struct mvrp *mvrp = data; const struct avbtp_packet_mrp *p = message; if (ntohs(p->eth.type) != AVB_MVRP_ETH) @@ -43,8 +124,8 @@ static int mvrp_message(void *data, uint64_t now, const void *message, int len) return 0; pw_log_info("MVRP"); - - return 0; + return avbtp_mrp_parse_packet(mvrp->server->mrp, + now, message, len, &info, mvrp); } static void mvrp_destroy(void *data) @@ -60,17 +141,18 @@ static const struct server_events server_events = { .message = mvrp_message }; -int avbtp_mvrp_register(struct server *server) +struct avbtp_mvrp *avbtp_mvrp_register(struct server *server) { struct mvrp *mvrp; mvrp = calloc(1, sizeof(*mvrp)); if (mvrp == NULL) - return -errno; + return NULL; mvrp->server = server; + spa_list_init(&mvrp->attributes); avdecc_server_add_listener(server, &mvrp->server_listener, &server_events, mvrp); - return 0; + return (struct avbtp_mvrp*)mvrp; } diff --git a/src/modules/module-avbtp/mvrp.h b/src/modules/module-avbtp/mvrp.h index 63bab9efb..76bceef4a 100644 --- a/src/modules/module-avbtp/mvrp.h +++ b/src/modules/module-avbtp/mvrp.h @@ -37,6 +37,22 @@ struct avbtp_packet_mvrp_msg { uint8_t attribute_list[0]; } __attribute__ ((__packed__)); -int avbtp_mvrp_register(struct server *server); +#define AVBTP_MVRP_ATTRIBUTE_TYPE_VID 1 +#define AVBTP_MVRP_ATTRIBUTE_TYPE_VALID(t) ((t)==1) + +struct avbtp_packet_mvrp_vid { + uint16_t vlan; +} __attribute__ ((__packed__)); + +struct avbtp_mvrp; + +struct avbtp_mvrp_attribute { + uint8_t type; + union { + struct avbtp_packet_mvrp_vid vid; + } attr; +}; + +struct avbtp_mvrp *avbtp_mvrp_register(struct server *server); #endif /* AVBTP_MVRP_H */ diff --git a/src/modules/module-avbtp/utils.h b/src/modules/module-avbtp/utils.h index adbf3ea3f..784af5876 100644 --- a/src/modules/module-avbtp/utils.h +++ b/src/modules/module-avbtp/utils.h @@ -64,5 +64,11 @@ static inline int avbtp_utils_parse_id(const char *str, int len, uint64_t *id) return 0; } +static inline char *avbtp_utils_format_addr(char *str, size_t size, const uint8_t addr[6]) +{ + snprintf(str, size, "%02x:%02x:%02x:%02x:%02x:%02x", + addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); + return str; +} #endif /* AVBTP_UTILS_H */