- Fix TC_SECC_CMN_VTB_CmSlacParm_004/5/6

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;

lib/staging/slac/fsm/evse/include/slac/fsm/evse/states/matching.hpp

@@ -4,6 +4,7 @@
 #define EVSE_SLAC_STATES_MATCHING_HPP
 
 #include <chrono>
+#include <memory>
 
 #include <slac/slac.hpp>
 
@@ -77,8 +78,11 @@ struct MatchingState : public FSMSimpleState {
     const uint8_t* tmp_ev_mac;
 
     MatchingTimepoint timeout_slac_parm_req;
+
     bool seen_slac_parm_req{false};
     int num_retries{0};
+
+    std::unique_ptr<slac::messages::cm_slac_match_cnf> match_cnf_message;
 };
 
 } // namespace slac::fsm::evse

lib/staging/slac/fsm/evse/include/slac/fsm/evse/states/others.hpp

@@ -3,8 +3,10 @@
 #ifndef EVSE_SLAC_STATES_OTHERS_HPP
 #define EVSE_SLAC_STATES_OTHERS_HPP
 
-#include "../fsm.hpp"
 #include <chrono>
+#include <memory>
+
+#include "../fsm.hpp"
 
 namespace slac::fsm::evse {
 
@@ -58,6 +60,10 @@ struct MatchedState : public FSMSimpleState {
 
     void enter() final;
     void leave() final;
+
+    CallbackReturnType callback() final;
+
+    bool link_status_req_sent{false};
 };
 
 struct FailedState : public FSMSimpleState {
@@ -69,7 +75,7 @@ struct FailedState : public FSMSimpleState {
 };
 
 struct WaitForLinkState : public FSMSimpleState {
-    using FSMSimpleState::FSMSimpleState;
+    WaitForLinkState(Context& ctx, std::unique_ptr<slac::messages::cm_slac_match_cnf> sent_match_cnf_message);
 
     HandleEventReturnType handle_event(AllocatorType&, Event) final;
 
@@ -81,6 +87,7 @@ struct WaitForLinkState : public FSMSimpleState {
 
     bool link_status_req_sent{false};
     std::chrono::steady_clock::time_point start_time;
+    std::unique_ptr<slac::messages::cm_slac_match_cnf> match_cnf_message;
 };
 
 struct InitState : public FSMSimpleState {

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);
@@ -139,7 +140,7 @@ FSMSimpleState::HandleEventReturnType MatchingState::handle_event(AllocatorType&
     } else if (ev == Event::MATCH_COMPLETE) {
         // Wait for link up to be confirmed before going to MATCHED state if enabled in config
         if (ctx.slac_config.link_status.do_detect) {
-            return sa.create_simple<WaitForLinkState>(ctx);
+            return sa.create_simple<WaitForLinkState>(ctx, std::move(match_cnf_message));
         } else {
             return sa.create_simple<MatchedState>(ctx);
         }

lib/staging/slac/fsm/evse/src/states/matching_handle_slac.cpp

@@ -69,10 +69,9 @@ static auto create_cm_atten_char_ind(const MatchingSession& session, int atten_o
     return atten_char_ind;
 }
 
-static auto create_cm_slac_match_cnf(const MatchingSession& session, const slac::messages::cm_slac_match_req& match_req,
-                                     const uint8_t* session_nmk) {
-    slac::messages::cm_slac_match_cnf match_cnf;
-
+// Note (aw): this function doesn't return by value in order to optimize for fewer copies
+static void create_cm_slac_match_cnf(slac::messages::cm_slac_match_cnf& match_cnf, const MatchingSession& session,
+                                     const slac::messages::cm_slac_match_req& match_req, const uint8_t* session_nmk) {
     match_cnf.application_type = slac::defs::COMMON_APPLICATION_TYPE;
     match_cnf.security_type = slac::defs::COMMON_SECURITY_TYPE;
     match_cnf.mvf_length = htole16(slac::defs::CM_SLAC_MATCH_CNF_MVF_LENGTH);
@@ -85,8 +84,6 @@ static auto create_cm_slac_match_cnf(const MatchingSession& session, const slac:
     match_cnf._reserved2 = 0;
     slac::utils::generate_nid_from_nmk(match_cnf.nid, session_nmk);
     memcpy(match_cnf.nmk, session_nmk, sizeof(match_cnf.nmk));
-
-    return match_cnf;
 }
 
 void MatchingState::handle_slac_message(slac::messages::HomeplugMessage& msg) {
@@ -120,7 +117,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;
 
@@ -273,17 +290,21 @@ void MatchingState::handle_cm_slac_match_req(const slac::messages::cm_slac_match
 
     session_log(ctx, *session, "Received CM_SLAC_MATCH_REQ, sending CM_SLAC_MATCH_CNF -> session complete");
 
-    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);
-        ctx.send_slac_message(tmp_ev_mac, match_confirm);
-    } else {
+    static constexpr uint8_t wrong_session_nmk[16] = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08,
+                                                      0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F, 0x10};
+
+    auto const* session_nmk = ctx.slac_config.session_nmk;
+
+    if (ctx.slac_config.link_status.debug_simulate_failed_matching) {
         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);
+        session_nmk = wrong_session_nmk;
     }
 
+    match_cnf_message = std::make_unique<slac::messages::cm_slac_match_cnf>();
+    create_cm_slac_match_cnf(*match_cnf_message, *session, msg, session_nmk);
+
+    ctx.send_slac_message(tmp_ev_mac, *match_cnf_message);
+
     session->state = MatchingSubState::MATCH_COMPLETE;
 
     // call this immediately again in MatchedState::callback to handle things

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,26 +164,86 @@ 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;
+    }
+
+    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& link_status = ctx.slac_config.link_status;
+
+    if (not link_status.do_detect) {
+        return {};
+    }
+
+    if (not link_status_req_sent) {
+        link_status_req_sent = send_link_status_req(ctx);
     } else {
-        return sa.PASS_ON;
+        // Link is confirmed not up yet, query again
+        link_status_req_sent = false;
     }
+
+    return link_status.poll_in_matched_state_ms;
 }
 
 void MatchedState::leave() {
     ctx.signal_dlink_ready(false);
 }
 
 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?
@@ -243,25 +294,27 @@ FSMSimpleState::CallbackReturnType WaitForLinkState::callback() {
     }
 }
 
+WaitForLinkState::WaitForLinkState(Context& ctx,
+                                   std::unique_ptr<slac::messages::cm_slac_match_cnf> sent_match_cnf_message) :
+    FSMSimpleState(ctx), match_cnf_message(std::move(sent_match_cnf_message)) {
+}
+
 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(), *match_cnf_message);
         return false;
     }
+    return false;
 }
 
 FSMSimpleState::HandleEventReturnType InitState::handle_event(AllocatorType& sa, Event ev) {

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