diff --git a/spa/plugins/avb/avb-pcm-sink.c b/spa/plugins/avb/avb-pcm-sink.c index c5efc55ea..9a66aa657 100644 --- a/spa/plugins/avb/avb-pcm-sink.c +++ b/spa/plugins/avb/avb-pcm-sink.c @@ -709,7 +709,7 @@ static int impl_node_process(void *object) spa_return_val_if_fail(input != NULL, -EIO); spa_log_trace_fp(this->log, "%p: process %d %d/%d", this, input->status, - input->buffer_id, this->n_buffers); + input->buffer_id, port->n_buffers); if (this->position && this->position->clock.flags & SPA_IO_CLOCK_FLAG_FREEWHEEL) { input->status = SPA_STATUS_NEED_DATA; diff --git a/src/modules/module-avb/acmp.c b/src/modules/module-avb/acmp.c index 3683a8d52..b5f81c64d 100644 --- a/src/modules/module-avb/acmp.c +++ b/src/modules/module-avb/acmp.c @@ -129,7 +129,6 @@ static int handle_connect_tx_command(struct acmp *acmp, uint64_t now, const void const struct avb_packet_acmp *p = m; struct avb_packet_acmp *reply = (struct avb_packet_acmp*)buf; int status = AVB_ACMP_STATUS_SUCCESS; - uint8_t mac[6] = { 0x91, 0xe0, 0xf0, 0x00, 0x10, 0x00 }; struct stream *stream; if (be64toh(p->talker_guid) != server->entity_id) @@ -146,12 +145,11 @@ static int handle_connect_tx_command(struct acmp *acmp, uint64_t now, const void AVB_PACKET_ACMP_SET_MESSAGE_TYPE(reply, AVB_ACMP_MESSAGE_TYPE_CONNECT_TX_RESPONSE); reply->stream_id = htobe64(stream->id); - memcpy(stream->addr, mac, 6); + stream_activate(stream, now); memcpy(reply->stream_dest_mac, stream->addr, 6); reply->connection_count = htons(1); reply->stream_vlan_id = htons(2); - stream_activate(stream, now); done: AVB_PACKET_ACMP_SET_STATUS(reply, status); diff --git a/src/modules/module-avb/avdecc.c b/src/modules/module-avb/avdecc.c index 6adf84edc..1327f385b 100644 --- a/src/modules/module-avb/avdecc.c +++ b/src/modules/module-avb/avdecc.c @@ -285,7 +285,7 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s goto error_free; avb_aecp_register(server); - avb_maap_register(server); + server->maap = avb_maap_register(server); server->mmrp = avb_mmrp_register(server); server->msrp = avb_msrp_register(server); server->mvrp = avb_mvrp_register(server); @@ -304,6 +304,8 @@ struct server *avdecc_server_new(struct impl *impl, const char *ifname, struct s server_create_stream(server, SPA_DIRECTION_INPUT, 0); server_create_stream(server, SPA_DIRECTION_OUTPUT, 0); + avb_maap_reserve(server->maap, 1); + return server; error_free: diff --git a/src/modules/module-avb/internal.h b/src/modules/module-avb/internal.h index fd3e40be9..6bd106024 100644 --- a/src/modules/module-avb/internal.h +++ b/src/modules/module-avb/internal.h @@ -95,6 +95,7 @@ struct server { struct avb_mmrp *mmrp; struct avb_mvrp *mvrp; struct avb_msrp *msrp; + struct avb_maap *maap; struct avb_msrp_attribute *domain_attr; }; diff --git a/src/modules/module-avb/maap.c b/src/modules/module-avb/maap.c index c0d7fc536..33cf9ef05 100644 --- a/src/modules/module-avb/maap.c +++ b/src/modules/module-avb/maap.c @@ -25,14 +25,41 @@ #include #include +#include #include "maap.h" +#define MAAP_ALLOCATION_POOL_SIZE 0xFE00 +#define MAAP_ALLOCATION_POOL_BASE { 0x91, 0xe0, 0xf0, 0x00, 0x00, 0x00 } +static uint8_t maap_base[6] = MAAP_ALLOCATION_POOL_BASE; + +#define MAAP_PROBE_RETRANSMITS 3 + +#define MAAP_PROBE_INTERVAL_MS 500 +#define MAAP_PROBE_INTERVAL_VAR_MS 100 + +#define MAAP_ANNOUNCE_INTERVAL_MS 3000 +#define MAAP_ANNOUNCE_INTERVAL_VAR_MS 2000 + struct maap { struct server *server; struct spa_hook server_listener; + struct pw_properties *props; + struct spa_source *source; + +#define STATE_IDLE 0 +#define STATE_PROBE 1 +#define STATE_ANNOUNCE 2 + uint32_t state; + uint64_t timeout; + uint32_t probe_count; + + unsigned short xsubi[3]; + + uint16_t offset; + uint16_t count; }; static const char *message_type_as_string(uint8_t message_type) @@ -69,6 +96,123 @@ static void maap_message_debug(struct maap *maap, const struct avb_packet_maap * pw_log_info(" conflict-count: %d", AVB_PACKET_MAAP_GET_CONFLICT_COUNT(p)); } +#define PROBE_TIMEOUT(n) ((n) + (MAAP_PROBE_INTERVAL_MS + \ + drand48() * MAAP_PROBE_INTERVAL_VAR_MS) * SPA_NSEC_PER_MSEC) +#define ANNOUNCE_TIMEOUT(n) ((n) + (MAAP_ANNOUNCE_INTERVAL_MS + \ + drand48() * MAAP_ANNOUNCE_INTERVAL_VAR_MS) * SPA_NSEC_PER_MSEC) + +static int make_new_address(struct maap *maap, uint64_t now, int range) +{ + maap->offset = nrand48(maap->xsubi) % (MAAP_ALLOCATION_POOL_SIZE - range); + maap->count = range; + maap->state = STATE_PROBE; + maap->probe_count = MAAP_PROBE_RETRANSMITS; + maap->timeout = PROBE_TIMEOUT(now); + return 0; +} + +static uint16_t maap_check_conflict(struct maap *maap, const uint8_t request_start[6], + uint16_t request_count, uint8_t conflict_start[6]) +{ + uint16_t our_start, our_end; + uint16_t req_start, req_end; + uint16_t conf_start, conf_count = 0; + + if (memcmp(request_start, maap_base, 4) != 0) + return 0; + + our_start = maap->offset; + our_end = our_start + maap->count; + req_start = request_start[4] << 8 | request_start[5]; + req_end = req_start + request_count; + + if (our_start >= req_start && our_start <= req_end) { + conf_start = our_start; + conf_count = SPA_MIN(our_end, req_end) - our_start; + } + else if (req_start >= our_start && req_start <= our_end) { + conf_start = req_start; + conf_count = SPA_MIN(req_end, our_end) - req_start; + } + if (conf_count == 0) + return 0; + + conflict_start[4] = conf_start >> 8; + conflict_start[5] = conf_start; + return conf_count; +} + +static int send_packet(struct maap *maap, uint64_t now, + uint8_t type, const uint8_t conflict_start[6], uint16_t conflict_count) +{ + struct avb_packet_maap p; + uint8_t bmac[6] = AVB_MAAP_MAC; + int res = 0; + uint8_t start[6]; + + spa_zero(p); + memcpy(p.hdr.eth.dest, bmac, 6); + memcpy(p.hdr.eth.src, maap->server->mac_addr, 6); + p.hdr.eth.type = htons(AVB_TSN_ETH); + p.hdr.subtype = AVB_SUBTYPE_MAAP; + AVB_PACKET_SET_LENGTH(&p.hdr, sizeof(p) - sizeof(p.hdr.eth)); + + AVB_PACKET_MAAP_SET_MAAP_VERSION(&p, 1); + AVB_PACKET_MAAP_SET_MESSAGE_TYPE(&p, type); + + memcpy(start, maap_base, 4); + start[4] = maap->offset >> 8; + start[5] = maap->offset; + AVB_PACKET_MAAP_SET_REQUEST_START(&p, start); + AVB_PACKET_MAAP_SET_REQUEST_COUNT(&p, maap->count); + if (conflict_count) { + AVB_PACKET_MAAP_SET_CONFLICT_START(&p, conflict_start); + AVB_PACKET_MAAP_SET_CONFLICT_COUNT(&p, conflict_count); + } + + pw_log_info("send: %d (%s)", type, message_type_as_string(type)); + maap_message_debug(maap, &p); + + if (send(maap->source->fd, &p, sizeof(p), 0) < 0) { + res = -errno; + pw_log_warn("got send error: %m"); + } + return res; +} + +static int handle_probe(struct maap *maap, uint64_t now, const struct avb_packet_maap *p) +{ + uint8_t conflict_start[6]; + uint16_t conflict_count; + + conflict_count = maap_check_conflict(maap, p->request_start, ntohs(p->request_count), + conflict_start); + if (conflict_count == 0) + return 0; + + switch (maap->state) { + case STATE_PROBE: + make_new_address(maap, now, 8); + break; + case STATE_ANNOUNCE: + send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_DEFEND, conflict_start, conflict_count); + break; + } + return 0; +} + +static int handle_defend(struct maap *maap, uint64_t now, const struct avb_packet_maap *p) +{ + uint8_t conflict_start[6]; + uint16_t conflict_count; + + conflict_count = maap_check_conflict(maap, p->conflict_start, ntohs(p->conflict_count), + conflict_start); + if (conflict_count != 0) + make_new_address(maap, now, 8); + return 0; +} + static int maap_message(struct maap *maap, uint64_t now, const void *message, int len) { const struct avb_packet_maap *p = message; @@ -78,6 +222,15 @@ static int maap_message(struct maap *maap, uint64_t now, const void *message, in maap_message_debug(maap, p); + switch (AVB_PACKET_MAAP_GET_MESSAGE_TYPE(p)) { + case AVB_MAAP_MESSAGE_TYPE_PROBE: + handle_probe(maap, now, p); + break; + case AVB_MAAP_MESSAGE_TYPE_DEFEND: + case AVB_MAAP_MESSAGE_TYPE_ANNOUNCE: + handle_defend(maap, now, p); + break; + } return 0; } @@ -105,52 +258,117 @@ static void on_socket_data(void *data, int fd, uint32_t mask) } } +static void maap_periodic(void *data, uint64_t now) +{ + struct maap *maap = data; + + if (now < maap->timeout) + return; + + switch(maap->state) { + case STATE_IDLE: + break; + case STATE_PROBE: + send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_PROBE, NULL, 0); + if (--maap->probe_count == 0) + maap->state = STATE_ANNOUNCE; + maap->timeout = PROBE_TIMEOUT(now); + break; + case STATE_ANNOUNCE: + send_packet(maap, now, AVB_MAAP_MESSAGE_TYPE_ANNOUNCE, NULL, 0); + maap->timeout = ANNOUNCE_TIMEOUT(now); + break; + } +} + +static void maap_free(struct maap *maap) +{ + pw_loop_destroy_source(maap->server->impl->loop, maap->source); + spa_hook_remove(&maap->server_listener); + pw_properties_free(maap->props); + free(maap); +} + static void maap_destroy(void *data) { struct maap *maap = data; - pw_loop_destroy_source(maap->server->impl->loop, maap->source); - spa_hook_remove(&maap->server_listener); - free(maap); + maap_free(maap); } static const struct server_events server_events = { AVB_VERSION_SERVER_EVENTS, .destroy = maap_destroy, + .periodic = maap_periodic, }; -int avb_maap_register(struct server *server) +struct avb_maap *avb_maap_register(struct server *server) { struct maap *maap; uint8_t bmac[6] = AVB_MAAP_MAC; - int res, fd; + int fd, res; fd = avb_server_make_socket(server, AVB_TSN_ETH, bmac); - if (fd < 0) - return fd; + if (fd < 0) { + res = fd; + goto error; + } maap = calloc(1, sizeof(*maap)); if (maap == NULL) { res = -errno; goto error_close; } + maap->props = pw_properties_new(NULL, NULL); + if (maap->props == NULL) { + res = -errno; + goto error_free; + } + + pw_conf_load_state("module-avb", "maap", maap->props); maap->server = server; + pw_getrandom(maap->xsubi, sizeof(maap->xsubi), 0); + pw_log_info("%lx %d", server->entity_id, server->ifindex); maap->source = pw_loop_add_io(server->impl->loop, fd, SPA_IO_IN, true, on_socket_data, maap); if (maap->source == NULL) { res = -errno; pw_log_error("maap %p: can't create maap source: %m", maap); - goto error_no_source; + goto error_free; } avdecc_server_add_listener(server, &maap->server_listener, &server_events, maap); - return 0; + return (struct avb_maap *)maap; -error_no_source: +error_free: free(maap); error_close: close(fd); - return res; +error: + errno = -res; + return NULL; +} + +int avb_maap_reserve(struct avb_maap *m, uint32_t count) +{ + struct maap *maap = (struct maap*)m; + make_new_address(maap, 0, count); + return 0; +} + +int avb_maap_get_address(struct avb_maap *m, uint8_t addr[6], uint32_t index) +{ + struct maap *maap = (struct maap*)m; + uint16_t offset; + + if (maap->state != STATE_ANNOUNCE) + return -EAGAIN; + + memcpy(addr, maap_base, 6); + offset = maap->offset + index; + addr[4] = offset >> 8; + addr[5] = offset; + return 0; } diff --git a/src/modules/module-avb/maap.h b/src/modules/module-avb/maap.h index 680ec2fea..6e56f8e9a 100644 --- a/src/modules/module-avb/maap.h +++ b/src/modules/module-avb/maap.h @@ -60,6 +60,11 @@ struct avb_packet_maap { #define AVB_PACKET_MAAP_GET_CONFLICT_START(p) ((p)->conflict_start) #define AVB_PACKET_MAAP_GET_CONFLICT_COUNT(p) ntohs((p)->conflict_count) -int avb_maap_register(struct server *server); +struct avb_maap; + +struct avb_maap *avb_maap_register(struct server *server); + +int avb_maap_reserve(struct avb_maap *maap, uint32_t count); +int avb_maap_get_address(struct avb_maap *maap, uint8_t addr[6], uint32_t index); #endif /* AVB_MAAP_H */ diff --git a/src/modules/module-avb/mrp.c b/src/modules/module-avb/mrp.c index a66b053d9..7b6bc4646 100644 --- a/src/modules/module-avb/mrp.c +++ b/src/modules/module-avb/mrp.c @@ -315,7 +315,7 @@ void avb_mrp_attribute_update_state(struct avb_mrp_attribute *attr, uint64_t now } if (a->registrar_state != state || notify) { - pw_log_info("attr %p: %d %d -> %d %d", a, event, a->registrar_state, state, notify); + pw_log_debug("attr %p: %d %d -> %d %d", a, event, a->registrar_state, state, notify); a->registrar_state = state; } diff --git a/src/modules/module-avb/msrp.c b/src/modules/module-avb/msrp.c index 09f4f6f9c..271043eff 100644 --- a/src/modules/module-avb/msrp.c +++ b/src/modules/module-avb/msrp.c @@ -396,7 +396,7 @@ static void msrp_event(void *data, uint64_t now, uint8_t event) if (dispatch[a->attr.type].encode == NULL) continue; - pw_log_info("send %s %s", dispatch[a->attr.type].name, + pw_log_debug("send %s %s", dispatch[a->attr.type].name, avb_mrp_send_name(a->attr.mrp->pending_send)); len = dispatch[a->attr.type].encode(msrp, a, msg); diff --git a/src/modules/module-avb/stream.c b/src/modules/module-avb/stream.c index e7f61f762..70dcf7c06 100644 --- a/src/modules/module-avb/stream.c +++ b/src/modules/module-avb/stream.c @@ -534,6 +534,9 @@ int stream_activate(struct stream *stream, uint64_t now) stream->talker_attr->attr.talker.stream_id = htobe64(stream->peer_id); avb_mrp_attribute_begin(stream->talker_attr->mrp, now); } else { + if ((res = avb_maap_get_address(server->maap, stream->addr, stream->index)) < 0) + return res; + stream->listener_attr->attr.listener.stream_id = htobe64(stream->id); stream->listener_attr->param = AVB_MSRP_LISTENER_PARAM_IGNORE; avb_mrp_attribute_begin(stream->listener_attr->mrp, now); diff --git a/src/modules/module-avb/stream.h b/src/modules/module-avb/stream.h index f13340052..e09fd59ac 100644 --- a/src/modules/module-avb/stream.h +++ b/src/modules/module-avb/stream.h @@ -89,6 +89,7 @@ struct stream { #include "msrp.h" #include "mvrp.h" +#include "maap.h" struct stream *server_create_stream(struct server *server, enum spa_direction direction, uint16_t index);