From cbfff0f16885a9e87e556d1166cc61b1e5998f00 Mon Sep 17 00:00:00 2001
From: Raphael Coeffic <raphael.coeffic@frafos.com>
Date: Mon, 19 Apr 2021 11:08:21 +0200
Subject: [PATCH] yaml: switch warning state (de-)serialize

---
 radio/src/datastructs.h                       |  2 +-
 .../storage/yaml/yaml_datastructs_funcs.cpp   | 74 +++++++++++++++++--
 .../src/storage/yaml/yaml_datastructs_x10.cpp |  2 +-
 .../storage/yaml/yaml_datastructs_x12s.cpp    |  2 +-
 4 files changed, 72 insertions(+), 8 deletions(-)

diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h
index 64ad53f58..c7dc781eb 100644
--- a/radio/src/datastructs.h
+++ b/radio/src/datastructs.h
@@ -559,7 +559,7 @@ typedef uint8_t swarnenable_t;
 
 #if defined(COLORLCD)
   #define SWITCHES_WARNING_DATA \
-    NOBACKUP(swarnstate_t  switchWarningState);
+    NOBACKUP(swarnstate_t  switchWarningState CUST(r_swtchWarn,w_swtchWarn));
 #else
   #define SWITCHES_WARNING_DATA \
     swarnstate_t  switchWarningState; \
diff --git a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp
index bba5c8ce5..b9d21b662 100644
--- a/radio/src/storage/yaml/yaml_datastructs_funcs.cpp
+++ b/radio/src/storage/yaml/yaml_datastructs_funcs.cpp
@@ -383,7 +383,8 @@ static bool fmd_is_active(uint8_t* data, uint32_t bitoffs)
 
 static uint32_t r_swtchWarn(const YamlNode* node, const char* val, uint8_t val_len)
 {
-    //TODO: read from string like 'AdBuC-'
+    // Read from string like 'AdBuC-':
+    //
     // -> reads:
     //    - Switch A: must be DOWN
     //    - Switch B: must be UP
@@ -391,13 +392,76 @@ static uint32_t r_swtchWarn(const YamlNode* node, const char* val, uint8_t val_l
     //
     // -> switches not in the list shall not be checked
     //
-    // -> refer to getSwitchWarningString() (strhelpers.cpp)
-    return 0;
+    swarnstate_t swtchWarn = 0;
+    while(val_len--) {
+
+        signed swtch = *(val++) - 'A';
+        if (swtch >= NUM_SWITCHES)
+            break;
+
+        unsigned state = 0;
+        switch (*(val++)) {
+        case 'u':
+            state = 1;
+            break;
+        case '-':
+            state = 2;
+            break;
+        case 'd':
+            state = 3;
+            break;
+        default:
+            // no check
+            break;
+        }
+
+        // 3 bits per switch
+        swtchWarn |= (state << (3*swtch));
+    }
+    
+    return swtchWarn;
 }
 
 static bool w_swtchWarn(const YamlNode* node, uint32_t val, yaml_writer_func wf, void* opaque)
 {
-    //TODO:
-    // -> refer to getSwitchWarningString() (strhelpers.cpp)
+    for (int i = 0; i < NUM_SWITCHES; i++) {
+        if (SWITCH_EXISTS(i)) {
+
+            // decode check state
+            // -> 3 bits per switch
+            auto state = (val >> (3*i)) & 0x07;
+
+            // state == 0 -> no check
+            // state == 1 -> UP
+            // state == 2 -> MIDDLE
+            // state == 3 -> DOWN
+            char swtchWarn[2] = {(char)('A' + i), 0};
+
+            switch (state) {
+            case 0:
+                break;
+            case 1:
+                swtchWarn[1] = 'u';
+                break;
+            case 2:
+                swtchWarn[1] = '-';
+                break;
+            case 3:
+                swtchWarn[1] = 'd';
+                break;
+            default:
+                // this should never happen
+                swtchWarn[1] = 'x';
+                break;
+            }
+
+            if (swtchWarn[1] != 0) {
+                if (!wf(opaque, swtchWarn, 2)) {
+                    return false;
+                }
+            }
+        }
+    }
+
     return true;
 }
diff --git a/radio/src/storage/yaml/yaml_datastructs_x10.cpp b/radio/src/storage/yaml/yaml_datastructs_x10.cpp
index 29cc08b86..d476bc22e 100644
--- a/radio/src/storage/yaml/yaml_datastructs_x10.cpp
+++ b/radio/src/storage/yaml/yaml_datastructs_x10.cpp
@@ -747,7 +747,7 @@ static const struct YamlNode struct_ModelData[] = {
   YAML_STRUCT("swashR", 64, struct_SwashRingData, NULL),
   YAML_ARRAY("flightModeData", 352, 9, struct_FlightModeData, fmd_is_active),
   YAML_UNSIGNED( "thrTraceSrc", 8 ),
-  YAML_UNSIGNED( "switchWarningState", 32 ),
+  YAML_UNSIGNED_CUST( "switchWarningState", 32, r_swtchWarn, w_swtchWarn ),
   YAML_ARRAY("gvars", 56, 9, struct_GVarData, NULL),
   YAML_STRUCT("varioData", 40, struct_VarioData, NULL),
   YAML_UNSIGNED( "rssiSource", 8 ),
diff --git a/radio/src/storage/yaml/yaml_datastructs_x12s.cpp b/radio/src/storage/yaml/yaml_datastructs_x12s.cpp
index adba40f99..824249a3c 100644
--- a/radio/src/storage/yaml/yaml_datastructs_x12s.cpp
+++ b/radio/src/storage/yaml/yaml_datastructs_x12s.cpp
@@ -748,7 +748,7 @@ static const struct YamlNode struct_ModelData[] = {
   YAML_STRUCT("swashR", 64, struct_SwashRingData, NULL),
   YAML_ARRAY("flightModeData", 352, 9, struct_FlightModeData, fmd_is_active),
   YAML_UNSIGNED( "thrTraceSrc", 8 ),
-  YAML_UNSIGNED( "switchWarningState", 32 ),
+  YAML_UNSIGNED_CUST( "switchWarningState", 32, r_swtchWarn, w_swtchWarn ),
   YAML_ARRAY("gvars", 56, 9, struct_GVarData, NULL),
   YAML_STRUCT("varioData", 40, struct_VarioData, NULL),
   YAML_UNSIGNED( "rssiSource", 8 ),