- Fix TC_SECC_CMN_VTB_CmSlacParm_004/5/6

- Fix TC_SECC_CMN_VTB_CmSlacMatch_003 - Add reset event handling while waiting for Link - Add link detection to matching state and reset if link got lost Signed-off-by: Cornelius Claussen <cc@pionix.de>
EVerest · Oct 23, 2024 · bdf3672 · bdf3672
1 parent 2f9152f
commit bdf3672
Show file tree

Hide file tree

Showing 6 changed files with 109 additions and 31 deletions.
diff --git a/lib/staging/slac/fsm/evse/include/slac/fsm/evse/context.hpp b/lib/staging/slac/fsm/evse/include/slac/fsm/evse/context.hpp
@@ -173,6 +173,7 @@ struct EvseSlacConfig {
     struct link_status_struct {
         bool do_detect = false;
         int retry_ms = 100;
+        int poll_in_matched_state_ms = 1000;
         int timeout_ms = 5000;
         bool debug_simulate_failed_matching = false;
     } link_status;
@@ -212,6 +213,7 @@ struct Context {
     void log_info(const std::string& text);
 
     ModemVendor modem_vendor{ModemVendor::Unknown};
+    slac::messages::cm_slac_match_cnf match_cnf_message;
 
 private:
     const ContextCallbacks& callbacks;

diff --git a/lib/staging/slac/fsm/evse/include/slac/fsm/evse/states/others.hpp b/lib/staging/slac/fsm/evse/include/slac/fsm/evse/states/others.hpp
@@ -58,6 +58,10 @@ struct MatchedState : public FSMSimpleState {
 
     void enter() final;
     void leave() final;
+
+    CallbackReturnType callback() final;
+
+    bool link_status_req_sent{false};
 };
 
 struct FailedState : public FSMSimpleState {

diff --git a/lib/staging/slac/fsm/evse/src/states/matching.cpp b/lib/staging/slac/fsm/evse/src/states/matching.cpp
@@ -72,7 +72,8 @@ FSMSimpleState::CallbackReturnType MatchingState::callback() {
 
     if (!seen_slac_parm_req) {
         if (now_tp >= timeout_slac_parm_req) {
-            return Event::RETRY_MATCHING;
+            ctx.log_info("CM_SLAC_PARM_REQ timed out -> FAILED");
+            return Event::FAILED;
         }
 
         call_back_ms = remaining_milliseconds(timeout_slac_parm_req, now_tp);

diff --git a/lib/staging/slac/fsm/evse/src/states/matching_handle_slac.cpp b/lib/staging/slac/fsm/evse/src/states/matching_handle_slac.cpp
@@ -120,7 +120,27 @@ void MatchingState::handle_slac_message(slac::messages::HomeplugMessage& msg) {
     }
 }
 
+static bool validate_cm_slac_parm_req(const slac::messages::cm_slac_parm_req& msg) {
+    constexpr uint8_t CM_SLAC_PARM_APPLICATION_TYPE = 0x00; // EV/EVSE matching
+    constexpr uint8_t CM_SLAC_PARM_SECURITY_TYPE = 0x00;    // no security
+
+    if (msg.application_type not_eq CM_SLAC_PARM_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq CM_SLAC_PARM_SECURITY_TYPE) {
+        return false;
+    }
+
+    return true;
+}
+
 void MatchingState::handle_cm_slac_parm_req(const slac::messages::cm_slac_parm_req& msg) {
+
+    if (not validate_cm_slac_parm_req(msg)) {
+        ctx.log_info("Invalid CM_SLAC_PARM.REQ received, ignoring");
+        return;
+    }
+
     // set this flag to true, to disable the retry timeout
     seen_slac_parm_req = true;
 
@@ -275,12 +295,15 @@ void MatchingState::handle_cm_slac_match_req(const slac::messages::cm_slac_match
 
     if (not ctx.slac_config.link_status.debug_simulate_failed_matching) {
         auto match_confirm = create_cm_slac_match_cnf(*session, msg, ctx.slac_config.session_nmk);
+        // Store match confirmation in context, as the EV may retry the match.req later on
+        ctx.match_cnf_message = match_confirm;
         ctx.send_slac_message(tmp_ev_mac, match_confirm);
     } else {
         ctx.log_info("Sending wrong NMK to EV to simulate a failed link setup after match request");
         uint8_t wrong_session_nmk[16] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
                                          0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10};
         auto match_confirm = create_cm_slac_match_cnf(*session, msg, wrong_session_nmk);
+
         ctx.send_slac_message(tmp_ev_mac, match_confirm);
     }
 

diff --git a/lib/staging/slac/fsm/evse/src/states/others.cpp b/lib/staging/slac/fsm/evse/src/states/others.cpp
@@ -3,6 +3,7 @@
 #include <slac/fsm/evse/states/others.hpp>
 
 #include <cstring>
+#include <optional>
 #include <string_view>
 
 #include <slac/fsm/evse/states/matching.hpp>
@@ -76,7 +77,7 @@ bool ResetState::handle_slac_message(slac::messages::HomeplugMessage& message) {
     const auto mmtype = message.get_mmtype();
     if (mmtype != (slac::defs::MMTYPE_CM_SET_KEY | slac::defs::MMTYPE_MODE_CNF)) {
         // unexpected message
-        // FIXME (aw): need to also deal with CM_VALIDATE.REQ
+        // FIXME (aw): need to also deal with CM_VALIDATE.REQ. It is optional in the standard.
         ctx.log_info("Received non-expected SLAC message of type " + format_mmtype(mmtype));
         return false;
     } else {
@@ -163,17 +164,75 @@ FSMSimpleState::HandleEventReturnType IdleState::handle_event(AllocatorType& sa,
     }
 }
 
+static std::optional<bool> check_link_status_cnf(const slac::fsm::evse::ModemVendor modem_vendor,
+                                                 slac::messages::HomeplugMessage& message) {
+    const auto mmtype = message.get_mmtype();
+    if (modem_vendor == ModemVendor::Qualcomm &&
+        mmtype == (slac::defs::qualcomm::MMTYPE_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF)) {
+        const auto success = message.get_payload<slac::messages::qualcomm::link_status_cnf>().link_status == 0x01;
+        return {success};
+
+    } else if (modem_vendor == ModemVendor::Lumissil &&
+               mmtype == (slac::defs::lumissil::MMTYPE_NSCM_GET_D_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF)) {
+        const auto success =
+            message.get_payload<slac::messages::lumissil::nscm_get_d_link_status_cnf>().link_status == 0x01;
+        return {success};
+    }
+    return {};
+}
+
+static bool send_link_status_req(slac::fsm::evse::Context& ctx) {
+    if (ctx.modem_vendor == ModemVendor::Qualcomm) {
+        slac::messages::qualcomm::link_status_req link_status_req;
+        ctx.send_slac_message(ctx.slac_config.plc_peer_mac, link_status_req);
+        return true;
+    } else if (ctx.modem_vendor == ModemVendor::Lumissil) {
+        slac::messages::lumissil::nscm_get_d_link_status_req link_status_req;
+        ctx.send_slac_message(ctx.slac_config.plc_peer_mac, link_status_req);
+        return true;
+    } else {
+        return false;
+    }
+}
+
 void MatchedState::enter() {
     ctx.signal_state("MATCHED");
     ctx.signal_dlink_ready(true);
     ctx.log_info("Entered Matched state");
 }
 
 FSMSimpleState::HandleEventReturnType MatchedState::handle_event(AllocatorType& sa, Event ev) {
-    if (ev == Event::RESET) {
+    if (ev == Event::SLAC_MESSAGE) {
+        auto link_ok = check_link_status_cnf(ctx.modem_vendor, ctx.slac_message_payload);
+        if (link_ok.has_value()) {
+            if (link_ok.value()) {
+                return sa.PASS_ON;
+            } else {
+                ctx.log_info("Connection lost in matched state");
+                ctx.signal_error_routine_request();
+                return sa.PASS_ON;
+            }
+        }
+    } else if (ev == Event::RESET) {
         return sa.create_simple<ResetState>(ctx);
+    }
+    return sa.PASS_ON;
+}
+
+FSMSimpleState::CallbackReturnType MatchedState::callback() {
+    const auto& cfg = ctx.slac_config;
+
+    if (not ctx.slac_config.link_status.do_detect) {
+        return {};
+    }
+
+    if (not link_status_req_sent) {
+        link_status_req_sent = send_link_status_req(ctx);
+        return cfg.link_status.poll_in_matched_state_ms;
     } else {
-        return sa.PASS_ON;
+        // Link is confirmed not up yet, query again
+        link_status_req_sent = false;
+        return cfg.link_status.poll_in_matched_state_ms;
     }
 }
 
@@ -182,7 +241,9 @@ void MatchedState::leave() {
 }
 
 void FailedState::enter() {
-    ctx.signal_error_routine_request();
+    if (ctx.slac_config.ac_mode_five_percent) {
+        ctx.signal_error_routine_request();
+    }
     ctx.log_info("Entered Failed state");
 }
 
@@ -210,6 +271,8 @@ FSMSimpleState::HandleEventReturnType WaitForLinkState::handle_event(AllocatorTy
         ctx.log_info("Link could not be established, resetting...");
         // Notify higher layers to on CP signal
         return sa.create_simple<FailedState>(ctx);
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
     } else {
         return sa.PASS_ON;
     }
@@ -218,19 +281,7 @@ FSMSimpleState::HandleEventReturnType WaitForLinkState::handle_event(AllocatorTy
 FSMSimpleState::CallbackReturnType WaitForLinkState::callback() {
     const auto& cfg = ctx.slac_config;
     if (not link_status_req_sent) {
-
-        if (ctx.modem_vendor == ModemVendor::Qualcomm) {
-            slac::messages::qualcomm::link_status_req link_status_req;
-            ctx.send_slac_message(cfg.plc_peer_mac, link_status_req);
-            link_status_req_sent = true;
-        } else if (ctx.modem_vendor == ModemVendor::Lumissil) {
-            slac::messages::lumissil::nscm_get_d_link_status_req link_status_req;
-            ctx.send_slac_message(cfg.plc_peer_mac, link_status_req);
-            link_status_req_sent = true;
-        } else {
-            ctx.log_info("Link detection not supported on this chip");
-        }
-
+        link_status_req_sent = send_link_status_req(ctx);
         return cfg.link_status.retry_ms;
     } else {
         // Did we timeout?
@@ -246,22 +297,19 @@ FSMSimpleState::CallbackReturnType WaitForLinkState::callback() {
 bool WaitForLinkState::handle_slac_message(slac::messages::HomeplugMessage& message) {
     const auto mmtype = message.get_mmtype();
 
-    if (ctx.modem_vendor == ModemVendor::Qualcomm &&
-        mmtype == (slac::defs::qualcomm::MMTYPE_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF)) {
-        const auto success = message.get_payload<slac::messages::qualcomm::link_status_cnf>().link_status == 0x01;
-        return success;
+    auto link_ok = check_link_status_cnf(ctx.modem_vendor, message);
 
-    } else if (ctx.modem_vendor == ModemVendor::Lumissil &&
-               mmtype == (slac::defs::lumissil::MMTYPE_NSCM_GET_D_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF)) {
-        const auto success =
-            message.get_payload<slac::messages::lumissil::nscm_get_d_link_status_cnf>().link_status == 0x01;
-        return success;
+    if (link_ok.has_value() and link_ok.value()) {
+        return true;
+    }
 
-    } else {
-        // unexpected message
-        ctx.log_info("Received non-expected SLAC message of type " + format_mmtype(mmtype));
+    if (mmtype == (slac::defs::MMTYPE_CM_SLAC_MATCH | slac::defs::MMTYPE_MODE_REQ)) {
+        // EV retries MATCH_REQ, so we send the CNF again
+        ctx.log_info("Received CM_SLAC_MATCH.REQ retry from EV, sending out CM_SLAC_MATCH.CNF again.");
+        ctx.send_slac_message(message.get_src_mac(), ctx.match_cnf_message);
         return false;
     }
+    return false;
 }
 
 FSMSimpleState::HandleEventReturnType InitState::handle_event(AllocatorType& sa, Event ev) {

diff --git a/modules/EvseSlac/manifest.yaml b/modules/EvseSlac/manifest.yaml
@@ -19,7 +19,7 @@ provides:
       set_key_timeout_ms:
         description: Timeout for CM_SET_KEY.REQ. Default works for QCA7000/QCA7005/CG5317.
         type: integer
-        default: 500
+        default: 1000
       sounding_attenuation_adjustment:
         description: Offset in dB that should be added to the calculated sounding attenuation
         type: integer