#51 get milan info

src/modules/meson.build

@@ -740,6 +740,7 @@ if build_module_avb
       'module-avb/acmp.c',
       'module-avb/aecp.c',
       'module-avb/aecp-aem.c',
+      'module-avb/aecp-mvu.c',
       'module-avb/aecp-aem-state.c',
       'module-avb/avdecc.c',
       'module-avb/maap.c',

src/modules/module-avb/aecp-aem.c

@@ -486,30 +486,5 @@ int avb_aecp_aem_handle_response(struct aecp *aecp, const void *m, int len)
 
 #ifdef USE_MILAN
 
-static uint64_t avb_general_48_char_to_64bit(const uint8_t *input48)
-{
-	uint64_t output64 = 0;
-	for (uint32_t pos = 0; pos < 6; pos++) {
-		output64 |=  input48[pos] << (pos << 3);
-	}
-
-	return  output64;
-}
-
-int avb_aecp_vendor_unique_command(struct aecp *aecp, const void *m, int len)
-{
-	const struct avb_ethernet_header *h = m;
-	const struct avb_packet_aecp_milan_vendor_unique *p = SPA_PTROFF(h, sizeof(*h), void);
-	uint64_t mvu = avb_general_48_char_to_64bit(p->protocol_id);
-
-	pw_log_warn("Retrieve value of %lu\n", mvu);
-
-	return 0;
-}
-
-int avb_aecp_vendor_unique_response(struct aecp *aecp, const void *m, int len)
-{
-	return 0;
-}
 
 #endif // USE_MILAN

src/modules/module-avb/aecp-aem.h

@@ -245,25 +245,6 @@ struct avb_packet_aecp_aem {
 	uint8_t payload[0];
 } __attribute__ ((__packed__));
 
-#ifdef USE_MILAN
-
-#define AECP_AVB_VENDOR_UNIQUE_PROTOCOL_ID_MILAN (0x001BC50AC100ULL)
-
-struct avb_packet_aecp_milan_vendor_unique {
-	struct avb_packet_aecp_header aecp;
-	// TODO thing how to improve
-	uint8_t protocol_id[6];
-#if __BYTE_ORDER == __BIG_ENDIAN
-	unsigned r:1;
-	unsigned command_type:15;
-#elif __BYTE_ORDER == __LITTLE_ENDIAN
-	unsigned command_type:15;
-	unsigned r:1;
-#endif
-	uint8_t payload[0];
-} __attribute__ ((__packed__));
-
-#endif
 
 #define AVB_PACKET_AEM_SET_COMMAND_TYPE(p,v)		((p)->cmd1 = ((v) >> 8),(p)->cmd2 = (v))
 
@@ -272,7 +253,5 @@ struct avb_packet_aecp_milan_vendor_unique {
 int avb_aecp_aem_handle_timeouts(struct aecp *aecp, uint64_t now);
 int avb_aecp_aem_handle_command(struct aecp *aecp, const void *m, int len);
 int avb_aecp_aem_handle_response(struct aecp *aecp, const void *m, int len);
-int avb_aecp_vendor_unique_command(struct aecp *aecp, const void *m, int len);
-int avb_aecp_vendor_unique_response(struct aecp *aecp, const void *m, int len);
 
 #endif /* AVB_AEM_H */

src/modules/module-avb/aecp-mvu.c

@@ -0,0 +1,174 @@
+#include "aecp-mvu.h"
+#include "aecp-cmd-resp/aecp-aem-helpers.h"
+#include "utils.h"
+
+static int reply_mvu_status(struct aecp *aecp, int status, const void *m,
+    int len)
+{
+	struct server *server = aecp->server;
+	uint8_t buf[2048];
+    char debug_str[1024];
+	struct avb_ethernet_header *h = (void*)buf;
+	struct avb_packet_aecp_milan_vendor_unique *reply = SPA_PTROFF(h, sizeof(*h), void);
+    int rc;
+    int written = 0;
+	memcpy(buf, m, len);
+	AVB_PACKET_AECP_SET_MESSAGE_TYPE(&reply->aecp,
+        AVB_AECP_MESSAGE_TYPE_VENDOR_UNIQUE_RESPONSE);
+
+	AVB_PACKET_AECP_SET_STATUS(&reply->aecp, status);
+
+    pw_log_warn("Sending packet of size %d \n", len);
+
+    for (int i = 0; i < len; i++) {
+        rc = sprintf(&debug_str[written], "%c%02x",
+                                 !(len % 16) ? '\n' : ':', ((uint8_t*)buf)[i]);
+        if (rc < 0) {
+            break;
+        }
+        written += rc;
+    //    pw_log_error("[%u]%02x", i, buf[i]);
+    }
+
+    pw_log_error("%s", debug_str);
+
+	return avb_server_send_packet(server, h->src, AVB_TSN_ETH, buf, len);
+}
+
+static int reply_mvu_bad_arguments(struct aecp *aecp, const void *m , int len)
+{
+    return reply_mvu_status(aecp, AVB_AECP_AEM_STATUS_BAD_ARGUMENTS, m, len);
+}
+
+static int reply_mvu_not_implemented(struct aecp *aecp, const void *m, int len)
+{
+	return reply_mvu_status(aecp, AVB_AECP_AEM_STATUS_NOT_IMPLEMENTED, m, len);
+}
+
+static int reply_mvu_not_supported(struct aecp *aecp, const void *m, int len)
+{
+	return reply_mvu_status(aecp, AVB_AECP_AEM_STATUS_NOT_SUPPORTED, m, len);
+}
+
+static int reply_mvu_success(struct aecp *aecp, const void *m, int len)
+{
+	return reply_mvu_status(aecp, AVB_AECP_AEM_STATUS_SUCCESS, m, len);
+}
+
+static int mvu_get_milan_info(struct aecp *aecp, int64_t now, const void *m,
+    int len)
+{
+    uint8_t buf[128];
+    struct avb_ethernet_header *h = (struct avb_ethernet_header *)buf;
+    struct avb_packet_aecp_milan_vendor_unique *p = SPA_PTROFF(h, sizeof(*h), void);
+    struct avb_packet_mvu_get_milan_info gmi = {0}, *gmi_cpy;
+    gmi_cpy = (struct avb_packet_mvu_get_milan_info *) p->payload;
+    uint16_t control_data_length;
+
+    memset(buf, 0, sizeof(buf));
+    memcpy(buf, m, len);
+
+    control_data_length = AVB_PACKET_GET_LENGTH(&p->aecp.hdr);
+    control_data_length += sizeof(gmi);
+
+    /* This is fake we do not have certification ahah */
+    gmi.certification_version = htonl(AECP_MVU_GET_MILAN_INFO_CERT_VERSION);
+    gmi.feature_flags = htonl(AECP_MVU_GET_MILAN_INFO_FLAGS);
+    gmi.protocol_version = htonl(AECP_MVU_GET_MILAN_INFO_PROTO_VERSION);
+
+    memcpy(gmi_cpy, &gmi, sizeof(gmi));
+
+    len = len + sizeof(gmi);
+    AVB_PACKET_SET_LENGTH(&p->aecp.hdr, control_data_length + sizeof(gmi));
+
+    return reply_mvu_success(aecp, buf, len);
+}
+
+static int mvu_set_system_unique_id(struct aecp *aecp, int64_t now, const void *m,
+    int len)
+{
+    return reply_mvu_not_supported(aecp, m, len);
+}
+
+static int mvu_get_system_unique_id(struct aecp *aecp, int64_t now, const void *m,
+    int len)
+{
+    return reply_mvu_not_supported(aecp, m, len);
+}
+
+static int mvu_set_media_clk_ref_info(struct aecp *aecp, int64_t now, const void *m,
+    int len)
+{
+    return reply_mvu_not_supported(aecp, m, len);
+}
+
+static int mvu_get_media_clk_ref_info(struct aecp *aecp, int64_t now, const void *m,
+    int len)
+{
+    return reply_mvu_not_supported(aecp, m, len);
+}
+
+struct cmd_info {
+	const char *name;
+	const bool is_readonly;
+	int (*handle_command) (struct aecp *aecp, int64_t now, const void *m,
+		 int len);
+	int (*handle_response) (struct aecp *aecp, int64_t now, const void *m,
+		 int len);
+};
+
+#define AECP_AEM_HANDLE_CMD(cmd, readonly_desc, name_str, handle_exec)			\
+	[cmd] = { .name = name_str, .is_readonly = readonly_desc, 	 				\
+				.handle_command = handle_exec }
+
+static const struct cmd_info mvu_cmd [] = {
+    AECP_AEM_HANDLE_CMD(AECP_MILAN_VENDOR_UNIQUE_GET_MILAN_INFO, true,
+        "milan-vu-get-info", mvu_get_milan_info),
+
+    AECP_AEM_HANDLE_CMD(AECP_MILAN_VENDOR_UNIQUE_SET_SYSTEM_UNIQUE_ID, true,
+        "milan-vu-set-system-unique-id", mvu_set_system_unique_id),
+
+    AECP_AEM_HANDLE_CMD(AECP_MILAN_VENDOR_UNIQUE_GET_SYSTEM_UNIQUE_ID, true,
+        "milan-vu-get-system-unique-id", mvu_get_system_unique_id),
+
+    AECP_AEM_HANDLE_CMD(AECP_MILAN_VENDOR_UNIQUE_SET_MEDIA_CLOCK_REF_INFO, true,
+        "milan-vu-set-clock-ref-info", mvu_set_media_clk_ref_info),
+
+    AECP_AEM_HANDLE_CMD(AECP_MILAN_VENDOR_UNIQUE_GET_MEDIA_CLOCK_REF_INFO, true,
+        "milan-vu-get-clock-ref-info", mvu_get_media_clk_ref_info)
+};
+
+int avb_aecp_vendor_unique_command(struct aecp *aecp, const void *m, int len)
+{
+	const struct avb_ethernet_header *h = m;
+	const struct avb_packet_aecp_milan_vendor_unique *p = SPA_PTROFF(h, sizeof(*h), void);
+	uint64_t mvu = AECP_VENDOR_UNIQUE_PROTO_ID(p);
+    uint16_t cmd_type;
+
+    if (len < (int) (sizeof(*h) + sizeof(*p))) {
+        pw_log_error("packet size too small %d\n", len);
+        return reply_mvu_bad_arguments(aecp, m, len);
+    }
+
+	pw_log_info("Vendor unique 0x%lx\n", (mvu));
+    cmd_type = htons(p->command_type);
+
+    if (mvu == AECP_MVU_MILAN_VENDOR_UNIQUE) {
+        if (cmd_type >= ARRAY_SIZE(mvu_cmd)) {
+            pw_log_error("invalid command %d\n", cmd_type);
+            return reply_mvu_not_implemented(aecp, m, len);
+        }
+
+        pw_log_info("Milan VU command %u\n", cmd_type);
+        return mvu_cmd[cmd_type].handle_command(aecp, 0, m, len);
+    } else {
+        pw_log_warn("Unsupported Vendor Unique protocol_id 0x%lx\n", mvu);
+    }
+
+	return reply_mvu_not_supported(aecp, m, len);
+}
+
+int avb_aecp_vendor_unique_response(struct aecp *aecp, const void *m, int len)
+{
+	return reply_not_supported(aecp, m, len);
+}

src/modules/module-avb/aecp-mvu.h

@@ -0,0 +1,55 @@
+#ifndef __AECP_MVU_H__
+#define __AECP_MVU_H__
+
+#include <stdio.h>
+#include "aecp.h"
+
+
+#define AECP_AVB_VENDOR_UNIQUE_PROTOCOL_ID_MILAN (0x001BC50AC100ULL)
+
+
+/* Milan v1.2 Clause 5.4.3.2.1 protocol_id field */
+#define AECP_MVU_MILAN_VENDOR_UNIQUE	((uint64_t)0x1bc50ac100)
+
+/** These are hardcoded value for now */
+/* Milan v1.2 Table 5.20: GET_MILAN */
+#define AECP_MVU_GET_MILAN_INFO_PROTO_VERSION	(1)
+#define AECP_MVU_GET_MILAN_INFO_CERT_VERSION	(0x01020000)
+#define AECP_MVU_GET_MILAN_INFO_FLAGS			(0)
+
+
+// Milan v1.2 Table 5.18
+#define AECP_MILAN_VENDOR_UNIQUE_GET_MILAN_INFO             (0x0000)
+#define AECP_MILAN_VENDOR_UNIQUE_SET_SYSTEM_UNIQUE_ID       (0x0001)
+#define AECP_MILAN_VENDOR_UNIQUE_GET_SYSTEM_UNIQUE_ID       (0x0002)
+#define AECP_MILAN_VENDOR_UNIQUE_SET_MEDIA_CLOCK_REF_INFO   (0x0003)
+#define AECP_MILAN_VENDOR_UNIQUE_GET_MEDIA_CLOCK_REF_INFO   (0x0004)
+
+
+struct avb_packet_mvu_get_milan_info {
+	uint32_t protocol_version;
+	uint32_t feature_flags;
+	uint32_t certification_version;
+} __attribute__ ((__packed__));
+
+#define AVB_PACKET_AECP_MVU_SET_MESSAGE_TYPE(p,v)	(&(p)->r = v)
+
+struct avb_packet_aecp_milan_vendor_unique {
+	struct avb_packet_aecp_header aecp;
+	uint16_t proto1;
+    uint32_t proto2;
+#if __BYTE_ORDER == __BIG_ENDIAN
+	unsigned r:1;
+	unsigned command_type:15;
+#elif __BYTE_ORDER == __LITTLE_ENDIAN
+	unsigned command_type:15;
+	unsigned r:1;
+#endif
+	uint8_t payload[0];
+} __attribute__ ((__packed__));
+
+
+int avb_aecp_vendor_unique_command(struct aecp *aecp, const void *m, int len);
+int avb_aecp_vendor_unique_response(struct aecp *aecp, const void *m, int len);
+
+#endif // __AECP_MVU_H__

src/modules/module-avb/aecp.c

@@ -11,6 +11,7 @@
 
 #include "aecp.h"
 #include "aecp-aem.h"
+#include "aecp-mvu.h"
 #include "aecp-aem-controls.h"
 #include "internal.h"
 

src/modules/module-avb/aecp.h

@@ -35,6 +35,9 @@ struct avb_packet_aecp_header {
 #define AVB_PACKET_AECP_GET_MESSAGE_TYPE(p)		AVB_PACKET_GET_SUB1(&(p)->hdr)
 #define AVB_PACKET_AECP_GET_STATUS(p)			AVB_PACKET_GET_SUB2(&(p)->hdr)
 
+#define AECP_VENDOR_UNIQUE_PROTO_ID(p)        (((uint64_t)htons((p)->proto1)) << 32 | \
+											  (uint64_t) htonl((p)->proto2));
+
 struct avb_aecp *avb_aecp_register(struct server *server);
 
 #endif /* AVB_AECP_H */