From 3d7151c6c0f268d170659548d3e084d5eae25114 Mon Sep 17 00:00:00 2001
From: jflyper <shinoda@jaist.ac.jp>
Date: Sat, 23 Nov 2019 17:57:14 +0900
Subject: [PATCH] Add temporary facility of vendor specific init string to
 displayport msp

Only for debugging purposes during attribute rich displayport devices.
Should be / Will be removed from a production firmware.
---
 src/main/cli/settings.c                |  4 ++++
 src/main/io/displayport_msp.c          | 15 +++++++++++++++
 src/main/pg/displayport_profiles.h     |  5 +++++
 src/main/target/common_defaults_post.h |  5 +++++
 4 files changed, 29 insertions(+)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index bc038b80c..9bf11e2b7 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1428,6 +1428,10 @@ const clivalue_t valueTable[] = {
     { "displayport_msp_col_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -6, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, colAdjust) },
     { "displayport_msp_row_adjust", VAR_INT8    | MASTER_VALUE, .config.minmax = { -3, 0 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, rowAdjust) },
     { "displayport_msp_serial",     VAR_INT8    | MASTER_VALUE, .config.minmax = { SERIAL_PORT_NONE, SERIAL_PORT_IDENTIFIER_MAX }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, displayPortSerial) },
+#ifdef USE_DISPLAYPORT_MSP_VENDOR_SPECIFIC
+    { "displayport_msp_vendor_init", VAR_UINT8   | MASTER_VALUE | MODE_ARRAY, .config.array.length = 253, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, vendorInit) },
+    { "displayport_msp_vendor_init_length", VAR_UINT8   | MASTER_VALUE, .config.minmaxUnsigned = { 0, 252 }, PG_DISPLAY_PORT_MSP_CONFIG, offsetof(displayPortProfile_t, vendorInitLength) },
+#endif // USE_DISPLAYPORT_MSP_VENDOR_SPECIFIC
 #endif
 
 // PG_DISPLAY_PORT_MSP_CONFIG
diff --git a/src/main/io/displayport_msp.c b/src/main/io/displayport_msp.c
index 8140d9129..5c481a7eb 100644
--- a/src/main/io/displayport_msp.c
+++ b/src/main/io/displayport_msp.c
@@ -170,6 +170,21 @@ static const displayPortVTable_t mspDisplayPortVTable = {
 
 displayPort_t *displayPortMspInit(void)
 {
+#ifdef USE_DISPLAYPORT_MSP_VENDOR_SPECIFIC
+    // XXX Should handle the case that a device starts to listen after the init string is sent
+    // XXX 1. Send the init string periodically while not armed.
+    // XXX 2. Send the init string in response to device identification message from a device.
+
+    // Send vendor specific initialization string.
+    // The string start with subcommand code.
+
+    int initLength = displayPortProfileMsp()->vendorInitLength;
+
+    if (initLength) {
+        output(&mspDisplayPort, MSP_DISPLAYPORT, (uint8_t *)displayPortProfileMsp()->vendorInit, initLength);
+    }
+#endif
+
     displayInit(&mspDisplayPort, &mspDisplayPortVTable);
     resync(&mspDisplayPort);
     return &mspDisplayPort;
diff --git a/src/main/pg/displayport_profiles.h b/src/main/pg/displayport_profiles.h
index a4c0a6247..028758279 100644
--- a/src/main/pg/displayport_profiles.h
+++ b/src/main/pg/displayport_profiles.h
@@ -29,6 +29,11 @@ typedef struct displayPortProfile_s {
     uint8_t blackBrightness;
     uint8_t whiteBrightness;
     int8_t displayPortSerial;  // serialPortIdentifier_e
+
+#ifdef USE_DISPLAYPORT_MSP_VENDOR_SPECIFIC
+    uint8_t vendorInitLength;  // Actual length of vendorInit byte string
+    uint8_t vendorInit[253];   // Max 253 bytes of vendor specific initialization byte string
+#endif
 } displayPortProfile_t;
 
 PG_DECLARE(displayPortProfile_t, displayPortProfileMsp);
diff --git a/src/main/target/common_defaults_post.h b/src/main/target/common_defaults_post.h
index ea8c93990..cb642120b 100644
--- a/src/main/target/common_defaults_post.h
+++ b/src/main/target/common_defaults_post.h
@@ -677,3 +677,8 @@
 #define DSHOT_BITBANGED_TIMER_DEFAULT DSHOT_BITBANGED_TIMER_AUTO
 #endif
 #endif // USE_DSHOT_BITBANG
+
+// XXX Tentative; may be removed
+#if defined(USE_MSP_DISPLAYPORT) && defined(USE_OSD_OVER_MSP_DISPLAYPORT)
+#define USE_DISPLAYPORT_MSP_VENDOR_SPECIFIC
+#endif