diff --git a/src/modules/module-avb/aecp-vendor-unique-milan-v12.c b/src/modules/module-avb/aecp-vendor-unique-milan-v12.c new file mode 100644 index 000000000..0c5b3bf34 --- /dev/null +++ b/src/modules/module-avb/aecp-vendor-unique-milan-v12.c @@ -0,0 +1,136 @@ +/* SPDX-FileCopyrightText: Copyright © 2025 Alexandre Malki */ +/* SPDX-License-Identifier: MIT */ + +#include +#include +#include + +#include +#include + +#include "aecp.h" +#include "aecp-vendor-unique-milan-v12.h" +#include "internal.h" + +/* Milan v1.2: protocol version this implementation supports. The high 16 + * bits encode the MVU protocol major version; bits 15..0 the minor. v1.0 + * of MVU = 0x00010000. Most controllers (Hive, etc.) accept anything ≥ + * 0x00010000 and don't gate features on this value. */ +#define MILAN_MVU_PROTOCOL_VERSION 0x00010000u + +#define MILAN_MVU_FEATURES_FLAGS 0x00000000u + +#define MILAN_MVU_CERTIFICATION_VERSION 0x00000000u + +static const uint8_t mvu_protocol_id[6] = { + AVB_AECP_MVU_PROTOCOL_ID_0, + AVB_AECP_MVU_PROTOCOL_ID_1, + AVB_AECP_MVU_PROTOCOL_ID_2, + AVB_AECP_MVU_PROTOCOL_ID_3, + AVB_AECP_MVU_PROTOCOL_ID_4, + AVB_AECP_MVU_PROTOCOL_ID_5, +}; + +static int reply_mvu_status(struct aecp *aecp, uint8_t status, + const void *m, int len) +{ + struct server *server = aecp->server; + uint8_t buf[2048]; + struct avb_ethernet_header *h = (void *)buf; + struct avb_packet_aecp_header *aecp_hdr; + + if (len > (int)sizeof(buf)) + return -EMSGSIZE; + + memcpy(buf, m, len); + aecp_hdr = SPA_PTROFF(h, sizeof(*h), void); + + AVB_PACKET_AECP_SET_MESSAGE_TYPE(aecp_hdr, + AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(aecp_hdr, status); + + return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, len); +} + +static int handle_get_milan_info(struct aecp *aecp, const void *m, int len) +{ + struct server *server = aecp->server; + uint8_t buf[2048]; + struct avb_ethernet_header *h_reply; + struct avb_packet_aecp_header *aecp_hdr; + struct avb_packet_aecp_vendor_unique *vu_reply; + struct avb_packet_aecp_mvu_get_milan_info_rsp *body; + int total = (int)(sizeof(struct avb_ethernet_header) + + sizeof(struct avb_packet_aecp_vendor_unique) + + sizeof(struct avb_packet_aecp_mvu_get_milan_info_rsp)); + + if (total > (int)sizeof(buf)) + return -EMSGSIZE; + + memcpy(buf, m, len); + if (len < total) + memset(buf + len, 0, total - len); + + h_reply = (struct avb_ethernet_header *)buf; + aecp_hdr = SPA_PTROFF(h_reply, sizeof(*h_reply), void); + vu_reply = (struct avb_packet_aecp_vendor_unique *)aecp_hdr; + + AVB_PACKET_AECP_SET_MESSAGE_TYPE(aecp_hdr, + AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE); + AVB_PACKET_AECP_SET_STATUS(aecp_hdr, AVB_AECP_STATUS_SUCCESS); + + AVB_PACKET_SET_LENGTH(&aecp_hdr->hdr, + (uint16_t)(total - sizeof(struct avb_ethernet_header) - + sizeof(struct avb_packet_header))); + + /* protocol_id and command_type are echoed from the command — the U + * flag is preserved but reset for the response per Section 5.4.4.2. */ + vu_reply->command_type = htons(ntohs(vu_reply->command_type) & + AVB_AECP_MVU_CMD_TYPE_CMD_MASK); + + body = (struct avb_packet_aecp_mvu_get_milan_info_rsp *)vu_reply->payload; + body->protocol_version = htonl(MILAN_MVU_PROTOCOL_VERSION); + body->features_flags = htonl(MILAN_MVU_FEATURES_FLAGS); + body->certification_version = htonl(MILAN_MVU_CERTIFICATION_VERSION); + + pw_log_info("MVU GET_MILAN_INFO reply proto=0x%08x features=0x%08x " + "cert=0x%08x", MILAN_MVU_PROTOCOL_VERSION, + MILAN_MVU_FEATURES_FLAGS, MILAN_MVU_CERTIFICATION_VERSION); + + return avb_server_send_packet(server, h_reply->src, AVB_TSN_ETH, + buf, total); +} + +int aecp_vendor_unique_milan_v12_handle_command(struct aecp *aecp, + const void *m, int len) +{ + const struct avb_ethernet_header *h = m; + const struct avb_packet_aecp_vendor_unique *vu; + uint16_t command_type; + uint16_t cmd; + + if (len < (int)(sizeof(*h) + sizeof(*vu))) + return 0; + + vu = SPA_PTROFF(h, sizeof(*h), const void); + + if (memcmp(vu->protocol_id, mvu_protocol_id, + sizeof(mvu_protocol_id)) != 0) + return 0; + + command_type = ntohs(vu->command_type); + cmd = command_type & AVB_AECP_MVU_CMD_TYPE_CMD_MASK; + + pw_log_debug("MVU command 0x%04x (U=%d)", cmd, + (command_type & AVB_AECP_MVU_CMD_TYPE_U_FLAG_MASK) ? 1 : 0); + + switch (cmd) { + case AVB_AECP_MVU_CMD_GET_MILAN_INFO: + (void)handle_get_milan_info(aecp, m, len); + return 1; + default: + (void)reply_mvu_status(aecp, AVB_AECP_STATUS_NOT_IMPLEMENTED, + m, len); + return 1; + } +} diff --git a/src/modules/module-avb/aecp-vendor-unique-milan-v12.h b/src/modules/module-avb/aecp-vendor-unique-milan-v12.h new file mode 100644 index 000000000..019cfff82 --- /dev/null +++ b/src/modules/module-avb/aecp-vendor-unique-milan-v12.h @@ -0,0 +1,51 @@ +/* SPDX-FileCopyrightText: Copyright © 2025 Alexandre Malki */ +/* SPDX-License-Identifier: MIT */ + +#ifndef AVB_AECP_VENDOR_UNIQUE_MILAN_V12_H +#define AVB_AECP_VENDOR_UNIQUE_MILAN_V12_H + +#include + +#include "aecp.h" + +/* Milan v1.2 Section 5.4.4 — Milan Vendor Unique (MVU) protocol carried inside an + * IEEE 1722.1 AECP VENDOR_UNIQUE_COMMAND/RESPONSE (message_type 6/7). + * + * Wire layout after the standard AECP common header: + * protocol_id (6) | command_type (2) | command-specific data + * + * Milan v1.2 Section 5.4.3.2.1: protocol_id is the Avnu OUI-36 00-1B-C5-0A-C + * appended with the 12-bit MVU protocol id 0x100, giving the 6-byte + * marker 00-1B-C5-0A-C1-00. command_type is a 1-bit U flag (MSB) + + * 15-bit command code. */ + +#define AVB_AECP_MVU_PROTOCOL_ID_0 0x00 +#define AVB_AECP_MVU_PROTOCOL_ID_1 0x1B +#define AVB_AECP_MVU_PROTOCOL_ID_2 0xC5 +#define AVB_AECP_MVU_PROTOCOL_ID_3 0x0A +#define AVB_AECP_MVU_PROTOCOL_ID_4 0xC1 +#define AVB_AECP_MVU_PROTOCOL_ID_5 0x00 + +/* Milan MVU command codes (Milan v1.2 Section 5.4.4). */ +#define AVB_AECP_MVU_CMD_GET_MILAN_INFO 0x0000 + +#define AVB_AECP_MVU_CMD_TYPE_U_FLAG_MASK 0x8000 +#define AVB_AECP_MVU_CMD_TYPE_CMD_MASK 0x7FFF + +struct avb_packet_aecp_vendor_unique { + struct avb_packet_aecp_header hdr; + uint8_t protocol_id[6]; + uint16_t command_type; + uint8_t payload[0]; +} __attribute__ ((__packed__)); + +struct avb_packet_aecp_mvu_get_milan_info_rsp { + uint32_t protocol_version; + uint32_t features_flags; + uint32_t certification_version; +} __attribute__ ((__packed__)); + +int aecp_vendor_unique_milan_v12_handle_command(struct aecp *aecp, + const void *m, int len); + +#endif /* AVB_AECP_VENDOR_UNIQUE_MILAN_V12_H */