Add extracted tools: CitrineOS, OpenOCPP, ShapeShifter

- CitrineOS core extracted (CSMS OCPP 2.0.1)
- OpenOCPP extracted (firmware OCPP 1.6J/2.0.1)
- ShapeShifter library installed (pip install -e)
- ShapeShifter specification extracted
- EVerest extracted

TODO updated with progress

tools/EVerest-main/lib/everest/slac/fsm/ev/CMakeLists.txt

@@ -0,0 +1,21 @@
+add_library(slac_fsm_ev)
+add_library(slac::fsm::ev ALIAS slac_fsm_ev)
+ev_register_library_target(slac_fsm_ev)
+
+target_include_directories(slac_fsm_ev
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+)
+
+target_sources(slac_fsm_ev
+    PRIVATE
+        src/context.cpp
+        src/states/others.cpp
+        src/states/sounding.cpp
+)
+
+target_link_libraries(slac_fsm_ev
+    PUBLIC
+        slac::slac
+        fsm::fsm
+)

tools/EVerest-main/lib/everest/slac/fsm/ev/include/everest/slac/fsm/ev/context.hpp

@@ -0,0 +1,110 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EV_SLAC_CONTEXT_HPP
+#define EV_SLAC_CONTEXT_HPP
+
+#include <functional>
+#include <stdexcept>
+#include <string>
+
+#include <slac/slac.hpp>
+
+namespace slac::fsm::ev {
+
+namespace _context_detail {
+template <typename SlacMessageType> struct MMTYPE;
+
+template <> struct MMTYPE<slac::messages::cm_slac_parm_req> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SLAC_PARAM | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::cm_start_atten_char_ind> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_START_ATTEN_CHAR | slac::defs::MMTYPE_MODE_IND;
+};
+
+template <> struct MMTYPE<slac::messages::cm_mnbc_sound_ind> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_MNBC_SOUND | slac::defs::MMTYPE_MODE_IND;
+};
+
+template <> struct MMTYPE<slac::messages::cm_atten_char_rsp> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_ATTEN_CHAR | slac::defs::MMTYPE_MODE_RSP;
+};
+
+template <> struct MMTYPE<slac::messages::cm_slac_match_req> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SLAC_MATCH | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::cm_set_key_req> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SET_KEY | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <typename SlacMessageType> struct MMV {
+    // this is the default value for homeplug av 2.0 messages, which are
+    // backward compatible with homeplug av 1.1 messages
+    // non-backward (to 1.1) compatible message are CM_CHAN_EST,
+    // CM_AMP_MAP and CM_NW_STATS, these need to use AV_2_0
+    // older av 1.0 message need to use AV_1_0
+    static constexpr auto value = slac::defs::MMV::AV_1_1;
+};
+} // namespace _context_detail
+
+struct ContextCallbacks {
+    std::function<void(slac::messages::HomeplugMessage&)> send_raw_slac{nullptr};
+    std::function<void(const std::string&)> signal_state{nullptr};
+    std::function<void(const std::string&)> log_debug{nullptr};
+    std::function<void(const std::string&)> log_info{nullptr};
+    std::function<void(const std::string&)> log_warn{nullptr};
+    std::function<void(const std::string&)> log_error{nullptr};
+};
+
+struct Context {
+    static constexpr std::array<uint8_t, ETH_ALEN> BROADCAST_MAC = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
+    static constexpr std::array<uint8_t, ETH_ALEN> EV_PLC_MAC = {0x00, 0xB0, 0x52, 0x00, 0x00, 0x01};
+
+    Context(const ContextCallbacks& callbacks_, const std::array<uint8_t, ETH_ALEN>& mac) :
+        callbacks(callbacks_), ev_host_mac(mac) {
+    }
+
+    const std::array<uint8_t, ETH_ALEN> ev_host_mac{};
+
+    // event specific payloads
+    // FIXME (aw): due to the synchroneous nature of the fsm, this could be even a ptr/ref
+    slac::messages::HomeplugMessage slac_message;
+
+    // FIXME (aw): message should be const, but libslac doesn't allow for const ptr - needs changes in libslac
+    template <typename SlacMessageType>
+    void send_slac_message(const uint8_t* dest_mac, SlacMessageType const& message) {
+        slac::messages::HomeplugMessage hp_message;
+        hp_message.setup_ethernet_header(dest_mac);
+        try {
+            hp_message.setup_payload(&message, sizeof(message), _context_detail::MMTYPE<SlacMessageType>::value,
+                                     _context_detail::MMV<SlacMessageType>::value);
+        } catch (const std::runtime_error& e) {
+            const auto error_message = std::string("Could not setup SLAC payload: ") + std::string(e.what());
+            log_error(error_message);
+        }
+        callbacks.send_raw_slac(hp_message);
+    }
+
+    // signal handlers
+    void signal_state(const std::string& state);
+
+    // logging util
+    void log_debug(const std::string& text);
+    void log_info(const std::string& text);
+    void log_warn(const std::string& text);
+    void log_error(const std::string& text);
+
+private:
+    const ContextCallbacks& callbacks;
+};
+
+struct SessionParamaters {
+    SessionParamaters(const uint8_t* run_id, const uint8_t* evse_mac);
+    uint8_t run_id[slac::defs::RUN_ID_LEN];
+    uint8_t evse_mac[ETH_ALEN];
+};
+
+} // namespace slac::fsm::ev
+
+#endif // EV_SLAC_CONTEXT_HPP

tools/EVerest-main/lib/everest/slac/fsm/ev/include/everest/slac/fsm/ev/fsm.hpp

@@ -0,0 +1,29 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EV_SLAC_FSM_HPP
+#define EV_SLAC_FSM_HPP
+
+#include <fsm/fsm.hpp>
+
+#include "context.hpp"
+
+namespace slac::fsm::ev {
+
+enum class Event {
+    RESET,
+    TRIGGER_MATCHING,
+    SLAC_MESSAGE,
+
+    // internal events
+    FAILED,
+};
+
+using FSMReturnType = int;
+
+using FSM = ::fsm::FSM<Event, FSMReturnType>;
+using FSMSimpleState = ::fsm::states::StateWithContext<FSM::SimpleStateType, Context>;
+using FSMCompoundState = ::fsm::states::StateWithContext<FSM::CompoundStateType, Context>;
+
+} // namespace slac::fsm::ev
+
+#endif // EV_SLAC_FSM_HPP

tools/EVerest-main/lib/everest/slac/fsm/ev/include/everest/slac/fsm/ev/states/others.hpp

@@ -0,0 +1,103 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EV_SLAC_STATES_OTHERS_HPP
+#define EV_SLAC_STATES_OTHERS_HPP
+
+#include <chrono>
+
+#include <everest/slac/fsm/ev/fsm.hpp>
+
+namespace slac::fsm::ev {
+
+struct ResetState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+};
+
+struct InitSlacState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // only returns true, if valid
+    bool check_for_valid_parm_conf();
+
+    // sends out CM_SLAC_PARM.REQ, increases num of tries and returns the timeout until a response is expected
+    int send_parm_req();
+
+    std::chrono::time_point<std::chrono::steady_clock> next_timeout;
+
+    int num_of_tries{0};
+
+    uint8_t run_id[8];
+};
+
+struct MatchRequestState : public FSMSimpleState {
+    MatchRequestState(Context& ctx, SessionParamaters session_parameters);
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // return the pointer to the NMK, if valid
+    const uint8_t* check_for_valid_match_req_conf();
+
+    // sends out CM_SLAC_PARM.REQ, increases num of tries and returns the timeout until a response is expected
+    int send_match_req();
+
+    std::chrono::time_point<std::chrono::steady_clock> next_timeout;
+
+    int num_of_tries{0};
+
+    SessionParamaters session_parameters;
+};
+
+struct JoinNetworkState : public FSMSimpleState {
+    static constexpr auto SET_KEY_TIMEOUT_MS = 500;
+    JoinNetworkState(Context& ctx, const uint8_t* nmk);
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // only returns true, if valid
+    bool check_for_valid_set_key_conf();
+
+    std::chrono::time_point<std::chrono::steady_clock> timeout;
+
+    uint8_t nmk[slac::defs::NMK_LEN];
+};
+
+struct MatchedState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    void enter() final {
+        ctx.log_info("Entered matched state");
+        ctx.signal_state("MATCHED");
+    }
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+};
+
+struct FailedState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    void enter() final {
+        ctx.log_info("Entered failed state");
+    }
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+};
+
+} // namespace slac::fsm::ev
+
+#endif // EV_SLAC_STATES_OTHERS_HPP

tools/EVerest-main/lib/everest/slac/fsm/ev/include/everest/slac/fsm/ev/states/sounding.hpp

@@ -0,0 +1,42 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EV_SLAC_STATES_SOUNDING_HPP
+#define EV_SLAC_STATES_SOUNDING_HPP
+
+#include <chrono>
+
+#include <slac/slac.hpp>
+
+#include <everest/slac/fsm/ev/fsm.hpp>
+
+namespace slac::fsm::ev {
+
+struct SoundingState : public FSMSimpleState {
+    SoundingState(Context& ctx, SessionParamaters session_parameters);
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // returns true, if CM_ATTEN_CHAR.IND is expected and fulfills expectations and response has been send
+    bool handle_valid_atten_char_ind();
+
+    // returns true if further message will need to be send
+    bool do_sounding();
+
+    // timepoint until we need to receive the atten_char_ind message
+    std::chrono::time_point<std::chrono::steady_clock> sounding_timeout;
+
+    // timepoint until we internally want to send the next message
+    std::chrono::time_point<std::chrono::steady_clock> next_timeout;
+
+    int count_start_atten_char_sent{0};
+    int count_mnbc_sound_sent{0};
+
+    SessionParamaters session_parameters;
+};
+
+} // namespace slac::fsm::ev
+
+#endif // EV_SLAC_STATES_SOUNDING_HPP

tools/EVerest-main/lib/everest/slac/fsm/ev/src/context.cpp

@@ -0,0 +1,44 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/ev/context.hpp>
+
+#include <cstring>
+
+namespace slac::fsm::ev {
+
+void Context::signal_state(const std::string& state) {
+    if (callbacks.signal_state) {
+        callbacks.signal_state(state);
+    }
+}
+
+void Context::log_debug(const std::string& text) {
+    if (callbacks.log_debug) {
+        callbacks.log_debug(text);
+    }
+}
+
+void Context::log_info(const std::string& text) {
+    if (callbacks.log_info) {
+        callbacks.log_info(text);
+    }
+}
+
+void Context::log_warn(const std::string& text) {
+    if (callbacks.log_warn) {
+        callbacks.log_warn(text);
+    }
+}
+
+void Context::log_error(const std::string& text) {
+    if (callbacks.log_error) {
+        callbacks.log_error(text);
+    }
+}
+
+SessionParamaters::SessionParamaters(const uint8_t* run_id_, const uint8_t* evse_mac_) {
+    memcpy(run_id, run_id_, sizeof(run_id));
+    memcpy(evse_mac, evse_mac_, sizeof(evse_mac));
+}
+
+} // namespace slac::fsm::ev

tools/EVerest-main/lib/everest/slac/fsm/ev/src/states/others.cpp

@@ -0,0 +1,303 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/ev/states/others.hpp>
+
+#include <cstring>
+#include <random>
+
+#include <endian.h>
+
+#include "timing_helper.hpp"
+
+#include <everest/slac/fsm/ev/states/sounding.hpp>
+
+namespace slac::fsm::ev {
+
+void ResetState::enter() {
+    ctx.signal_state("UNMATCHED");
+    ctx.log_info("Entered Reset state");
+}
+
+FSMSimpleState::HandleEventReturnType ResetState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::TRIGGER_MATCHING) {
+        return sa.create_simple<InitSlacState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+FSMSimpleState::CallbackReturnType ResetState::callback() {
+    ctx.log_info("Called callback of ResetState");
+
+    return {};
+}
+
+FSMSimpleState::HandleEventReturnType InitSlacState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        if (check_for_valid_parm_conf()) {
+            ctx.signal_state("MATCHING");
+            return sa.create_simple<SoundingState>(ctx, SessionParamaters{run_id, ctx.slac_message.get_src_mac()});
+        }
+
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::FAILED) {
+        return sa.create_simple<FailedState>(ctx);
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+void InitSlacState::enter() {
+    ctx.log_info("Entered init state");
+
+    // generate random run_id
+    std::random_device rnd_dev;
+    std::mt19937 rng(rnd_dev());
+    std::uniform_int_distribution<std::mt19937::result_type> dist256(0, 255);
+
+    for (auto& id : this->run_id) {
+        id = dist256(rng);
+    }
+}
+
+FSMSimpleState::CallbackReturnType InitSlacState::callback() {
+    if (num_of_tries == 0) {
+        return send_parm_req();
+    }
+
+    // did already send a parm req, check for timeout
+
+    const auto now = std::chrono::steady_clock::now();
+    const auto time_left = milliseconds_left(now, next_timeout);
+
+    if (time_left > 0) {
+        // still have time
+        return time_left;
+    }
+
+    // timeout, check if we still have retries
+    if (num_of_tries < 100) {
+        // FIXME (aw): the norm says num_of_tries < slac::defs::C_EV_MATCH_RETRY
+        // but doesn't seem to work in 'real life'
+        return send_parm_req();
+    }
+
+    // no retries left fail
+    return Event::FAILED;
+}
+
+int InitSlacState::send_parm_req() {
+    slac::messages::cm_slac_parm_req msg;
+    msg.application_type = 0x0;
+    msg.security_type = 0x0;
+    memcpy(msg.run_id, run_id, sizeof(msg.run_id));
+
+    ctx.send_slac_message(ctx.BROADCAST_MAC.data(), msg);
+
+    num_of_tries++;
+
+    next_timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(slac::defs::TT_MATCH_RESPONSE_MS);
+
+    return slac::defs::TT_MATCH_RESPONSE_MS;
+}
+
+bool InitSlacState::check_for_valid_parm_conf() {
+    const auto mmtype = ctx.slac_message.get_mmtype();
+    if (mmtype != (slac::defs::MMTYPE_CM_SLAC_PARAM | slac::defs::MMTYPE_MODE_CNF)) {
+        return false;
+    }
+
+    // correct message type
+    const auto& parm_cnf = ctx.slac_message.get_payload<slac::messages::cm_slac_parm_cnf>();
+
+    // it is not clear, whether we should use the m-sound target anyhow ... or if we should really validate any of the
+    // fields except the run id
+    const auto same_run_id = (memcmp(parm_cnf.run_id, run_id, sizeof(run_id)) == 0);
+
+    return same_run_id;
+}
+
+MatchRequestState::MatchRequestState(Context& ctx, SessionParamaters session_parameters_) :
+    FSMSimpleState(ctx), session_parameters(std::move(session_parameters_)) {
+}
+
+FSMSimpleState::HandleEventReturnType MatchRequestState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        const auto nmk = check_for_valid_match_req_conf();
+        if (nmk) {
+            return sa.create_simple<JoinNetworkState>(ctx, nmk);
+        }
+
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::FAILED) {
+        return sa.create_simple<FailedState>(ctx);
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+void MatchRequestState::enter() {
+    ctx.log_info("Entered MatchRequestState state");
+}
+
+FSMSimpleState::CallbackReturnType MatchRequestState::callback() {
+    if (num_of_tries == 0) {
+        return send_match_req();
+    }
+
+    // did already send a parm req, check for timeout
+
+    const auto now = std::chrono::steady_clock::now();
+    const auto time_left = milliseconds_left(now, next_timeout);
+
+    if (time_left > 0) {
+        // still have time
+        return time_left;
+    }
+
+    // timeout, check if we still have retries
+    if (num_of_tries < slac::defs::C_EV_MATCH_RETRY) {
+        return send_match_req();
+    }
+
+    // no retries left fail
+    return Event::FAILED;
+}
+
+const uint8_t* MatchRequestState::check_for_valid_match_req_conf() {
+    if (num_of_tries == 0) {
+        return nullptr;
+    }
+
+    const auto mmtype = ctx.slac_message.get_mmtype();
+    if (mmtype != (slac::defs::MMTYPE_CM_SLAC_MATCH | slac::defs::MMTYPE_MODE_CNF)) {
+        return nullptr;
+    }
+
+    // correct message type
+    const auto& match_cnf = ctx.slac_message.get_payload<slac::messages::cm_slac_match_cnf>();
+
+    const auto run_id_match =
+        (memcmp(session_parameters.run_id, match_cnf.run_id, sizeof(session_parameters.run_id)) == 0);
+
+    if (run_id_match) {
+        return match_cnf.nmk;
+    } else {
+        return nullptr;
+    }
+}
+
+int MatchRequestState::send_match_req() {
+    slac::messages::cm_slac_match_req msg;
+    msg.application_type = 0x0;
+    msg.security_type = 0x0;
+    msg.mvf_length = htole16(0x3e); // FIXME (aw) fixed constant
+    memset(msg.pev_id, 0, sizeof(msg.pev_id));
+    memcpy(msg.pev_mac, ctx.ev_host_mac.data(), sizeof(msg.pev_mac));
+    memset(msg.evse_id, 0, sizeof(msg.evse_id));
+    memcpy(msg.evse_mac, session_parameters.evse_mac, sizeof(msg.evse_mac));
+    memcpy(msg.run_id, session_parameters.run_id, sizeof(msg.run_id));
+    memset(msg._reserved, 0, sizeof(msg._reserved));
+
+    ctx.send_slac_message(session_parameters.evse_mac, msg);
+
+    num_of_tries++;
+
+    next_timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(slac::defs::TT_MATCH_RESPONSE_MS);
+
+    return slac::defs::TT_MATCH_RESPONSE_MS;
+}
+
+JoinNetworkState::JoinNetworkState(Context& ctx, const uint8_t* nmk_) : FSMSimpleState(ctx) {
+    memcpy(nmk, nmk_, sizeof(nmk));
+}
+
+FSMSimpleState::HandleEventReturnType JoinNetworkState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        if (check_for_valid_set_key_conf()) {
+            // FIXME (aw): later on, we also need to distinguish between set_key failed
+            return sa.create_simple<MatchedState>(ctx);
+        }
+
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::FAILED) {
+        return sa.create_simple<FailedState>(ctx);
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+void JoinNetworkState::enter() {
+    ctx.log_info("Entered JoinNetwork state");
+
+    slac::messages::cm_set_key_req msg;
+    msg.key_type = slac::defs::CM_SET_KEY_REQ_KEY_TYPE_NMK;
+    msg.my_nonce = 0xAAAAAAAA;
+    msg.your_nonce = 0x00000000;
+    msg.pid = slac::defs::CM_SET_KEY_REQ_PID_HLE;
+    msg.prn = htole16(slac::defs::CM_SET_KEY_REQ_PRN_UNUSED);
+    msg.pmn = slac::defs::CM_SET_KEY_REQ_PMN_UNUSED;
+    msg.cco_capability = slac::defs::CM_SET_KEY_REQ_CCO_CAP_NONE;
+    slac::utils::generate_nid_from_nmk(msg.nid, nmk);
+    msg.new_eks = slac::defs::CM_SET_KEY_REQ_PEKS_NMK_KNOWN_TO_STA;
+    memcpy(msg.new_key, nmk, sizeof(msg.new_key));
+
+    ctx.send_slac_message(ctx.EV_PLC_MAC.data(), msg);
+
+    timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(SET_KEY_TIMEOUT_MS);
+}
+
+FSMSimpleState::CallbackReturnType JoinNetworkState::callback() {
+    const auto now = std::chrono::steady_clock::now();
+    const auto time_left = milliseconds_left(now, timeout);
+
+    if (time_left > 0) {
+        // still have time
+        return time_left;
+    }
+
+    // we reached the set key timeout
+    return Event::FAILED;
+}
+
+bool JoinNetworkState::check_for_valid_set_key_conf() {
+    const auto mmtype = ctx.slac_message.get_mmtype();
+    if (mmtype != (slac::defs::MMTYPE_CM_SET_KEY | slac::defs::MMTYPE_MODE_CNF)) {
+        return false;
+    }
+
+    // correct message type
+    const auto& set_key_cnf = ctx.slac_message.get_payload<slac::messages::cm_set_key_cnf>();
+
+    // FIXME (aw): validation of the message?
+
+    return true;
+}
+
+FSMSimpleState::HandleEventReturnType MatchedState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else if (ev == Event::SLAC_MESSAGE) {
+        return sa.HANDLED_INTERNALLY;
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+FSMSimpleState::HandleEventReturnType FailedState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else if (ev == Event::SLAC_MESSAGE) {
+        return sa.HANDLED_INTERNALLY;
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+} // namespace slac::fsm::ev

tools/EVerest-main/lib/everest/slac/fsm/ev/src/states/sounding.cpp

@@ -0,0 +1,154 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/ev/states/sounding.hpp>
+
+#include <algorithm>
+#include <cstring>
+#include <optional>
+#include <random>
+
+#include "timing_helper.hpp"
+
+#include <everest/slac/fsm/ev/states/others.hpp>
+
+namespace slac::fsm::ev {
+
+SoundingState::SoundingState(Context& ctx, SessionParamaters session_parameters_) :
+    FSMSimpleState(ctx), session_parameters(std::move(session_parameters_)) {
+}
+
+void SoundingState::enter() {
+    sounding_timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(slac::defs::TT_EV_ATTEN_RESULTS_MS);
+}
+
+FSMSimpleState::CallbackReturnType SoundingState::callback() {
+    const auto now = std::chrono::steady_clock::now();
+    const auto sounding_time_left = milliseconds_left(now, sounding_timeout);
+
+    if (sounding_time_left <= 0) {
+        return Event::FAILED;
+    }
+
+    if (count_mnbc_sound_sent == 10) {
+        // we're already done, return time left until sounding timeout
+        return sounding_time_left;
+    }
+
+    if (count_start_atten_char_sent == 0) {
+        // no sounding messages have been send yet
+        next_timeout = now;
+    }
+
+    const auto next_step_time_left = milliseconds_left(now, next_timeout);
+
+    if (next_step_time_left > 0) {
+        // just idle a bit
+        return std::min(next_step_time_left, sounding_time_left);
+    }
+
+    // need to issue the next step
+    if (do_sounding()) {
+        // FIXME (aw): how to setup the next timeout, we could send the everything right away, but waiting at least a
+        // bit seems to be a good practice
+        const auto next_step_delay_ms = 20; // FIXME (aw): which value to use? TP_EV_BATCH_MSG_INTERVAL_MS
+        next_timeout = now + std::chrono::milliseconds(next_step_delay_ms);
+        return std::min(next_step_delay_ms, static_cast<int>(sounding_time_left));
+    }
+
+    return sounding_time_left;
+}
+
+FSMSimpleState::HandleEventReturnType SoundingState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        if (handle_valid_atten_char_ind()) {
+            return sa.create_simple<MatchRequestState>(ctx, session_parameters);
+        }
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    }
+
+    return sa.PASS_ON;
+}
+
+bool SoundingState::do_sounding() {
+    if (count_start_atten_char_sent < slac::defs::C_EV_START_ATTEN_CHAR_INDS) {
+
+        slac::messages::cm_start_atten_char_ind msg;
+        msg.application_type = 0x0;
+        msg.security_type = 0x0;
+        msg.num_sounds = slac::defs::C_EV_MATCH_MNBC;
+        msg.timeout = (slac::defs::TT_EVSE_MATCH_MNBC_MS + 99) / 100; // in multiples of 100ms!
+        msg.resp_type = 0x01; // fixed value indicating 'other Green Phy station'
+        memcpy(msg.forwarding_sta, ctx.ev_host_mac.data(), sizeof(msg.forwarding_sta));
+        memcpy(msg.run_id, session_parameters.run_id, sizeof(msg.run_id));
+
+        ctx.send_slac_message(ctx.BROADCAST_MAC.data(), msg);
+
+        count_start_atten_char_sent++;
+
+    } else if (count_mnbc_sound_sent < slac::defs::C_EV_MATCH_MNBC) {
+        count_mnbc_sound_sent++;
+
+        slac::messages::cm_mnbc_sound_ind msg;
+        msg.application_type = 0x0;
+        msg.security_type = 0x0;
+        memset(msg.sender_id, 0, sizeof(msg.sender_id));
+        msg.remaining_sound_count = slac::defs::C_EV_MATCH_MNBC - count_mnbc_sound_sent;
+        memcpy(msg.run_id, session_parameters.run_id, sizeof(msg.run_id));
+        memset(msg._reserved, 0, sizeof(msg._reserved));
+
+        // FIXME (aw): does this have any performance penalties?
+        std::random_device rnd_dev;
+        std::mt19937 rng(rnd_dev());
+        std::uniform_int_distribution<std::mt19937::result_type> dist256(0, 255);
+
+        for (auto& random : msg.random) {
+            random = dist256(rng);
+        }
+
+        ctx.send_slac_message(ctx.BROADCAST_MAC.data(), msg);
+    }
+
+    return (count_mnbc_sound_sent < slac::defs::C_EV_MATCH_MNBC);
+}
+
+bool SoundingState::handle_valid_atten_char_ind() {
+    if (count_mnbc_sound_sent < slac::defs::C_EV_MATCH_MNBC) {
+        ctx.log_info("Received unexpected message while SOUNDING");
+        return false;
+    }
+
+    const auto mmtype = ctx.slac_message.get_mmtype();
+
+    if (mmtype != (slac::defs::MMTYPE_CM_ATTEN_CHAR | slac::defs::MMTYPE_MODE_IND)) {
+        ctx.log_info("Received unexpected message after SOUNDING");
+        return false;
+    }
+
+    // correct message type
+    const auto& atten_char = ctx.slac_message.get_payload<slac::messages::cm_atten_char_ind>();
+
+    const auto run_id_match =
+        (memcmp(session_parameters.run_id, atten_char.run_id, sizeof(session_parameters.run_id)) == 0);
+
+    if (run_id_match == false) {
+        return false;
+    }
+
+    // reply
+    slac::messages::cm_atten_char_rsp response;
+    response.application_type = 0x0;
+    response.security_type = 0x0;
+    memcpy(response.source_address, ctx.ev_host_mac.data(), sizeof(response.source_address));
+    memcpy(response.run_id, atten_char.run_id, sizeof(response.run_id));
+    memset(response.source_id, 0, sizeof(response.source_id));
+    memset(response.resp_id, 0, sizeof(response.resp_id));
+    response.result = 0x0;
+
+    ctx.send_slac_message(session_parameters.evse_mac, response);
+
+    return true;
+}
+
+} // namespace slac::fsm::ev

tools/EVerest-main/lib/everest/slac/fsm/ev/src/states/timing_helper.hpp

@@ -0,0 +1,12 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EV_SLAC_STATES_TIMING_HELPER_HPP
+#define EV_SLAC_STATES_TIMING_HELPER_HPP
+
+#include <chrono>
+
+template <typename TimepointType> auto milliseconds_left(const TimepointType& from, const TimepointType& to) {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(to - from).count();
+}
+
+#endif // EV_SLAC_STATES_TIMING_HELPER_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/CMakeLists.txt

@@ -0,0 +1,23 @@
+add_library(slac_fsm_evse)
+add_library(slac::fsm::evse ALIAS slac_fsm_evse)
+ev_register_library_target(slac_fsm_evse)
+
+target_include_directories(slac_fsm_evse
+    PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+)
+
+target_sources(slac_fsm_evse
+    PRIVATE
+        src/misc.cpp
+        src/context.cpp
+        src/states/others.cpp
+        src/states/matching.cpp
+        src/states/matching_handle_slac.cpp
+)
+
+target_link_libraries(slac_fsm_evse
+    PUBLIC
+        slac::slac
+        fsm::fsm
+)

tools/EVerest-main/lib/everest/slac/fsm/evse/include/everest/slac/fsm/evse/context.hpp

@@ -0,0 +1,237 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_CONTEXT_HPP
+#define EVSE_SLAC_CONTEXT_HPP
+
+#include <functional>
+#include <string>
+
+#include <slac/slac.hpp>
+
+namespace slac::fsm::evse {
+
+namespace _context_detail {
+
+template <typename SlacMessageType> struct MMTYPE;
+template <> struct MMTYPE<slac::messages::cm_slac_parm_cnf> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SLAC_PARAM | slac::defs::MMTYPE_MODE_CNF;
+};
+template <> struct MMTYPE<slac::messages::cm_atten_char_ind> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_ATTEN_CHAR | slac::defs::MMTYPE_MODE_IND;
+};
+template <> struct MMTYPE<slac::messages::cm_set_key_req> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SET_KEY | slac::defs::MMTYPE_MODE_REQ;
+};
+template <> struct MMTYPE<slac::messages::cm_validate_cnf> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_VALIDATE | slac::defs::MMTYPE_MODE_CNF;
+};
+template <> struct MMTYPE<slac::messages::cm_slac_match_cnf> {
+    static const uint16_t value = slac::defs::MMTYPE_CM_SLAC_MATCH | slac::defs::MMTYPE_MODE_CNF;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::cm_reset_device_req> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_CM_RESET_DEVICE | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::cm_reset_device_cnf> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_CM_RESET_DEVICE | slac::defs::MMTYPE_MODE_CNF;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::link_status_req> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_LINK_STATUS | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::link_status_cnf> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::op_attr_req> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_OP_ATTR | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::qualcomm::op_attr_cnf> {
+    static const uint16_t value = slac::defs::qualcomm::MMTYPE_OP_ATTR | slac::defs::MMTYPE_MODE_CNF;
+};
+
+// This message has no CNF counterpart
+template <> struct MMTYPE<slac::messages::lumissil::nscm_reset_device_req> {
+    static const uint16_t value = slac::defs::lumissil::MMTYPE_NSCM_RESET_DEVICE | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::lumissil::nscm_get_version_req> {
+    static const uint16_t value = slac::defs::lumissil::MMTYPE_NSCM_GET_VERSION | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::lumissil::nscm_get_version_cnf> {
+    static const uint16_t value = slac::defs::lumissil::MMTYPE_NSCM_GET_VERSION | slac::defs::MMTYPE_MODE_CNF;
+};
+
+template <> struct MMTYPE<slac::messages::lumissil::nscm_get_d_link_status_req> {
+    static const uint16_t value = slac::defs::lumissil::MMTYPE_NSCM_GET_D_LINK_STATUS | slac::defs::MMTYPE_MODE_REQ;
+};
+
+template <> struct MMTYPE<slac::messages::lumissil::nscm_get_d_link_status_cnf> {
+    static const uint16_t value = slac::defs::lumissil::MMTYPE_NSCM_GET_D_LINK_STATUS | slac::defs::MMTYPE_MODE_CNF;
+};
+
+template <typename SlacMessageType> struct MMV {
+    // this is the default value for homeplug av 2.0 messages, which are
+    // backward compatible with homeplug av 1.1 messages
+    // non-backward (to 1.1) compatible message are CM_CHAN_EST,
+    // CM_AMP_MAP and CM_NW_STATS, these need to use AV_2_0
+    // older av 1.0 message need to use AV_1_0
+    static constexpr auto value = slac::defs::MMV::AV_1_1;
+};
+
+template <> struct MMV<slac::messages::qualcomm::cm_reset_device_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::qualcomm::cm_reset_device_cnf> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::qualcomm::link_status_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::qualcomm::link_status_cnf> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::qualcomm::op_attr_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::qualcomm::op_attr_cnf> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0;
+};
+
+template <> struct MMV<slac::messages::lumissil::nscm_reset_device_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0; // FIXME this is unclear
+};
+
+template <> struct MMV<slac::messages::lumissil::nscm_get_version_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0; // FIXME this is unclear
+};
+
+template <> struct MMV<slac::messages::lumissil::nscm_get_version_cnf> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0; // FIXME this is unclear
+};
+
+template <> struct MMV<slac::messages::lumissil::nscm_get_d_link_status_req> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0; // FIXME this is unclear
+};
+
+template <> struct MMV<slac::messages::lumissil::nscm_get_d_link_status_cnf> {
+    static constexpr auto value = slac::defs::MMV::AV_1_0; // FIXME this is unclear
+};
+
+} // namespace _context_detail
+
+// FIXME (aw): this should be moved to common headers (in libslac)
+enum class ModemVendor {
+    Unknown,
+    Qualcomm,
+    Lumissil,
+    VertexCom,
+};
+
+struct ContextCallbacks {
+    std::function<void(slac::messages::HomeplugMessage&)> send_raw_slac{nullptr};
+    std::function<void(const std::string&)> signal_state{nullptr};
+    std::function<void(bool)> signal_dlink_ready{nullptr};
+    std::function<void()> signal_error_routine_request{nullptr};
+    std::function<void(const std::string&)> signal_ev_mac_address_parm_req{nullptr};
+    std::function<void(const std::string&)> signal_ev_mac_address_match_cnf{nullptr};
+    std::function<void(const std::string&)> log_debug{nullptr};
+    std::function<void(const std::string&)> log_info{nullptr};
+    std::function<void(const std::string&)> log_warn{nullptr};
+    std::function<void(const std::string&)> log_error{nullptr};
+};
+
+struct EvseSlacConfig {
+    // MAC address of our (EVSE) PLC modem
+    // FIXME (aw): is that used somehow?
+    uint8_t plc_peer_mac[ETH_ALEN] = {0x00, 0xB0, 0x52, 0x00, 0x00, 0x01};
+
+    // FIXME (aw): we probably want to use std::array here
+    void generate_nmk();
+    uint8_t session_nmk[slac::defs::NMK_LEN]{};
+
+    // flag for using 5% PWM in AC mode
+    bool ac_mode_five_percent{true};
+
+    // timeout for CM_SET_KEY.REQ
+    int set_key_timeout_ms = 500;
+
+    // timeout for CM_SLAC_PARM.REQ
+    int slac_init_timeout_ms = slac::defs::TT_EVSE_SLAC_INIT_MS;
+
+    // Settings CM_DEVICE_RESET.REQ
+    struct chip_reset_struct {
+        bool enabled = false;
+        int timeout_ms = 500;
+        int delay_ms = 100;
+    } chip_reset;
+
+    // Settings for LINK_STATUS detection
+    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;
+
+    int request_info_delay_ms = 100;
+
+    // offset for adjusting the calculated sounding attenuation
+    int sounding_atten_adjustment = 0;
+
+    bool reset_instead_of_fail{false};
+
+    bool regenerate_key_on_reset{true};
+};
+
+struct Context {
+    explicit Context(const ContextCallbacks& callbacks_) : callbacks(callbacks_){};
+
+    EvseSlacConfig slac_config{};
+
+    // event specific payloads
+    // FIXME (aw): due to the synchroneous nature of the fsm, this could be even a ptr/ref
+    slac::messages::HomeplugMessage slac_message_payload;
+
+    // FIXME (aw): message should be const, but libslac doesn't allow for const ptr - needs changes in libslac
+    template <typename SlacMessageType> void send_slac_message(const uint8_t* mac, SlacMessageType const& message) {
+        slac::messages::HomeplugMessage hp_message;
+        hp_message.setup_ethernet_header(mac);
+        hp_message.setup_payload(&message, sizeof(message), _context_detail::MMTYPE<SlacMessageType>::value,
+                                 _context_detail::MMV<SlacMessageType>::value);
+        callbacks.send_raw_slac(hp_message);
+    }
+
+    // signal handlers
+    void signal_cm_slac_parm_req(const uint8_t* ev_mac);
+    void signal_cm_slac_match_cnf(const uint8_t* ev_mac);
+    void signal_dlink_ready(bool value);
+    void signal_error_routine_request();
+    void signal_state(const std::string& state);
+
+    // logging util
+    void log_debug(const std::string& text);
+    void log_info(const std::string& text);
+    void log_warn(const std::string& text);
+    void log_error(const std::string& text);
+
+    ModemVendor modem_vendor{ModemVendor::Unknown};
+    uint8_t evse_mac[ETH_ALEN];
+
+private:
+    const ContextCallbacks& callbacks;
+};
+
+} // namespace slac::fsm::evse
+
+#endif // EVSE_SLAC_CONTEXT_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/include/everest/slac/fsm/evse/fsm.hpp

@@ -0,0 +1,33 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_FSM_HPP
+#define EVSE_SLAC_FSM_HPP
+
+#include <fsm/fsm.hpp>
+
+#include "context.hpp"
+
+namespace slac::fsm::evse {
+
+enum class Event {
+    RESET,
+    ENTER_BCD,
+    LEAVE_BCD,
+    SLAC_MESSAGE,
+
+    // internal events
+    RETRY_MATCHING,
+    MATCH_COMPLETE,
+    FAILED,
+    SUCCESS,
+};
+
+using FSMReturnType = int;
+
+using FSM = ::fsm::FSM<Event, FSMReturnType>;
+using FSMSimpleState = ::fsm::states::StateWithContext<FSM::SimpleStateType, Context>;
+using FSMCompoundState = ::fsm::states::StateWithContext<FSM::CompoundStateType, Context>;
+
+} // namespace slac::fsm::evse
+
+#endif // EVSE_SLAC_FSM_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/include/everest/slac/fsm/evse/states/matching.hpp

@@ -0,0 +1,93 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_STATES_MATCHING_HPP
+#define EVSE_SLAC_STATES_MATCHING_HPP
+
+#include <chrono>
+#include <memory>
+
+#include <slac/slac.hpp>
+
+#include "../fsm.hpp"
+
+namespace slac::fsm::evse {
+
+enum class MatchingSubState {
+    WAIT_FOR_START_ATTEN_CHAR,
+    SOUNDING,
+    FINALIZE_SOUNDING,
+    WAIT_FOR_ATTEN_CHAR_RSP,
+    WAIT_FOR_SLAC_MATCH,
+    RECEIVED_SLAC_MATCH,
+    MATCH_COMPLETE,
+    FAILED,
+};
+
+constexpr auto FINALIZE_SOUNDING_DELAY_MS = 45;
+
+using MatchingTimepoint = std::chrono::time_point<std::chrono::steady_clock>;
+
+struct MatchingSession {
+    MatchingSession(const uint8_t* ev_mac, const uint8_t* run_id);
+
+    // common session related
+    MatchingSubState state{MatchingSubState::WAIT_FOR_START_ATTEN_CHAR};
+    uint8_t ev_mac[ETH_ALEN];
+    uint8_t run_id[slac::defs::RUN_ID_LEN];
+
+    // timeout related
+    MatchingTimepoint next_timeout;
+    bool timeout_active{false};
+
+    // sounding related
+    int captured_sounds{0};
+    int captured_aags[slac::defs::AAG_LIST_LEN];
+    bool received_mnbc_sound{false};
+
+    int num_retries{0};
+
+    // helper functions
+    void set_next_timeout(int delay_ms);
+    void ack_timeout();
+    bool is_identified_by(const uint8_t* ev_mac, const uint8_t* run_id) const;
+    slac::messages::cm_atten_char_ind calculate_avg() const;
+};
+
+struct MatchingState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    std::vector<MatchingSession> sessions;
+
+    // FIXME (aw): this should be const ref, but some of the member functions of HomeplugMessage are not const'd
+    void handle_slac_message(slac::messages::HomeplugMessage&);
+
+    void handle_cm_slac_parm_req(const slac::messages::cm_slac_parm_req&);
+    void handle_cm_start_atten_char_ind(const slac::messages::cm_start_atten_char_ind&);
+    void handle_cm_mnbc_sound_ind(const slac::messages::cm_mnbc_sound_ind&);
+    void handle_cm_atten_profile_ind(const slac::messages::cm_atten_profile_ind&);
+    void handle_cm_atten_char_rsp(const slac::messages::cm_atten_char_rsp&);
+    void handle_cm_validate_req(const slac::messages::cm_validate_req&);
+    void handle_cm_slac_match_req(const slac::messages::cm_slac_match_req&);
+
+    void finalize_sounding(MatchingSession& session);
+
+    // FIXME (aw): this should be wrapped somewhere else
+    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;
+    int failed_count{0};
+};
+
+} // namespace slac::fsm::evse
+
+#endif // EVSE_SLAC_STATES_MATCHING_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/include/everest/slac/fsm/evse/states/others.hpp

@@ -0,0 +1,112 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_STATES_OTHERS_HPP
+#define EVSE_SLAC_STATES_OTHERS_HPP
+
+#include <chrono>
+#include <memory>
+
+#include "../fsm.hpp"
+
+namespace slac::fsm::evse {
+
+struct ResetState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // for now returns true if CM_SET_KEY_CNF is received
+    bool handle_slac_message(slac::messages::HomeplugMessage&);
+
+    bool setup_has_been_send{false};
+};
+
+struct ResetChipState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // for now returns true if CM_RESET_CNF is received
+    bool handle_slac_message(slac::messages::HomeplugMessage&);
+
+    bool reset_delay_done{false};
+    bool chip_reset_has_been_sent{false};
+
+    enum class SubState {
+        DELAY,
+        SEND_RESET,
+        DONE,
+    } sub_state{SubState::DELAY};
+};
+
+struct IdleState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+};
+
+struct MatchedState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    void leave() final;
+
+    CallbackReturnType callback() final;
+
+    bool link_status_req_sent{false};
+};
+
+struct FailedState : public FSMSimpleState {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+};
+
+struct WaitForLinkState : public FSMSimpleState {
+    WaitForLinkState(Context& ctx, std::unique_ptr<slac::messages::cm_slac_match_cnf> sent_match_cnf_message);
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    void enter() final;
+    CallbackReturnType callback() final;
+
+    // for now returns true if link up detected is received
+    bool handle_slac_message(slac::messages::HomeplugMessage&);
+
+    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 {
+    using FSMSimpleState::FSMSimpleState;
+
+    HandleEventReturnType handle_event(AllocatorType&, Event) final;
+
+    CallbackReturnType callback() final;
+
+    void handle_slac_message(slac::messages::HomeplugMessage&);
+
+    // For now we are requesting only one version info packet, but probably there will be more in the future.
+    enum class SubState {
+        QUALCOMM_OP_ATTR,
+        LUMISSIL_GET_VERSION,
+        DONE,
+    } sub_state{SubState::QUALCOMM_OP_ATTR};
+};
+
+} // namespace slac::fsm::evse
+
+#endif // EVSE_SLAC_STATES_OTHERS_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/src/context.cpp

@@ -0,0 +1,79 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/evse/context.hpp>
+
+#include <random>
+
+#include "misc.hpp"
+
+namespace slac::fsm::evse {
+
+void EvseSlacConfig::generate_nmk() {
+    const std::string CHARACTERS = "0123456789ABCDEFGHIJKLMNOPQRSTUVWXYZ";
+
+    std::random_device random_device;
+    std::mt19937 generator(random_device());
+    std::uniform_int_distribution<> distribution(0, CHARACTERS.size() - 1);
+
+    for (std::size_t i = 0; i < slac::defs::NMK_LEN; ++i) {
+        session_nmk[i] = (uint8_t)CHARACTERS[distribution(generator)];
+    }
+}
+
+void Context::signal_cm_slac_parm_req(const uint8_t* mac) {
+    if (callbacks.signal_ev_mac_address_parm_req) {
+        const auto mac_string = format_mac_addr(mac);
+        callbacks.signal_ev_mac_address_parm_req(mac_string);
+    }
+}
+
+void Context::signal_cm_slac_match_cnf(const uint8_t* mac) {
+    if (callbacks.signal_ev_mac_address_match_cnf) {
+        const auto mac_string = format_mac_addr(mac);
+        callbacks.signal_ev_mac_address_match_cnf(mac_string);
+    }
+}
+
+void Context::signal_dlink_ready(bool value) {
+    if (callbacks.signal_dlink_ready) {
+        callbacks.signal_dlink_ready(value);
+    }
+}
+
+void Context::signal_error_routine_request() {
+    if (callbacks.signal_error_routine_request) {
+        callbacks.signal_error_routine_request();
+    }
+}
+
+void Context::signal_state(const std::string& state) {
+    if (callbacks.signal_state) {
+        callbacks.signal_state(state);
+    }
+}
+
+void Context::log_debug(const std::string& text) {
+    if (callbacks.log_debug) {
+        callbacks.log_debug(text);
+    }
+}
+
+void Context::log_info(const std::string& text) {
+    if (callbacks.log_info) {
+        callbacks.log_info(text);
+    }
+}
+
+void Context::log_warn(const std::string& text) {
+    if (callbacks.log_warn) {
+        callbacks.log_warn(text);
+    }
+}
+
+void Context::log_error(const std::string& text) {
+    if (callbacks.log_error) {
+        callbacks.log_error(text);
+    }
+}
+
+} // namespace slac::fsm::evse

tools/EVerest-main/lib/everest/slac/fsm/evse/src/misc.cpp

@@ -0,0 +1,35 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include "misc.hpp"
+
+#include <net/ethernet.h>
+
+#include <slac/slac.hpp>
+
+std::string format_mac_addr(const uint8_t* mac) {
+    char string_buffer[ETH_ALEN * 2 + (ETH_ALEN - 1) + 1];
+    snprintf(string_buffer, sizeof(string_buffer), "%02X:%02X:%02X:%02X:%02X:%02X", mac[0], mac[1], mac[2], mac[3],
+             mac[4], mac[5]);
+    return string_buffer;
+}
+
+std::string format_nmk(const uint8_t* nmk) {
+    char string_buffer[slac::defs::NMK_LEN * 3];
+    for (int i = 0; i < slac::defs::NMK_LEN; i++) {
+        snprintf(string_buffer + (i * 3), sizeof(string_buffer) - (i * 3), "%02X:", nmk[i]);
+    }
+    return string_buffer;
+}
+
+std::string format_run_id(const uint8_t* run_id) {
+    char string_buffer[2 * slac::defs::RUN_ID_LEN + 1];
+    snprintf(string_buffer, sizeof(string_buffer), "%02X%02X%02X%02X%02X%02X%02X%02X", run_id[0], run_id[1], run_id[2],
+             run_id[3], run_id[4], run_id[5], run_id[6], run_id[7]);
+    return string_buffer;
+}
+
+std::string format_mmtype(const uint16_t mmtype) {
+    char string_buffer[2 + 2 * 2 + 1];
+    snprintf(string_buffer, sizeof(string_buffer), "0x%04hX", mmtype);
+    return string_buffer;
+}

tools/EVerest-main/lib/everest/slac/fsm/evse/src/misc.hpp

@@ -0,0 +1,17 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_MISC_HPP
+#define EVSE_SLAC_MISC_HPP
+
+#include <cstdint>
+#include <string>
+
+std::string format_nmk(const uint8_t* nmk);
+
+std::string format_mac_addr(const uint8_t* mac);
+
+std::string format_run_id(const uint8_t* run_id);
+
+std::string format_mmtype(const uint16_t mmtype);
+
+#endif // EVSE_SLAC_MISC_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/src/states/matching.cpp

@@ -0,0 +1,187 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/evse/states/matching.hpp>
+
+#include <cstring>
+#include <optional>
+
+#include "matching_handle_slac.hpp"
+#include <everest/slac/fsm/evse/states/others.hpp>
+
+namespace slac::fsm::evse {
+
+//
+// Helper functions
+//
+static inline auto remaining_milliseconds(const MatchingTimepoint& timeout, const MatchingTimepoint& now) {
+    return std::chrono::duration_cast<std::chrono::milliseconds>(timeout - now).count();
+}
+
+MatchingSession::MatchingSession(const uint8_t* ev_mac, const uint8_t* run_id) {
+    memcpy(this->ev_mac, ev_mac, sizeof(this->ev_mac));
+    memcpy(this->run_id, run_id, sizeof(this->run_id));
+    memset(captured_aags, 0, sizeof(captured_aags));
+}
+
+void MatchingSession::set_next_timeout(int delay_ms) {
+    next_timeout = std::chrono::steady_clock::now() + std::chrono::milliseconds(delay_ms);
+    timeout_active = true;
+}
+
+void MatchingSession::ack_timeout() {
+    timeout_active = false;
+}
+
+bool MatchingSession::is_identified_by(const uint8_t* ev_mac, const uint8_t* run_id) const {
+    if (0 != memcmp(run_id, this->run_id, sizeof(this->run_id))) {
+        return false;
+    }
+
+    if (0 != memcmp(ev_mac, this->ev_mac, sizeof(this->ev_mac))) {
+        return false;
+    }
+
+    return true;
+}
+
+bool all_sessions_failed(const std::vector<MatchingSession>& sessions) {
+    for (const auto& session : sessions) {
+        if (session.state != MatchingSubState::FAILED) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+//
+// Matching state related
+//
+void MatchingState::enter() {
+    ctx.signal_state("MATCHING");
+    ctx.log_info("Entered Matching state, waiting for CM_SLAC_PARM_REQ");
+    // timeout for getting CM_SLAC_PARM_REQ
+    timeout_slac_parm_req =
+        std::chrono::steady_clock::now() + std::chrono::milliseconds(ctx.slac_config.slac_init_timeout_ms);
+}
+
+FSMSimpleState::CallbackReturnType MatchingState::callback() {
+    // check timeouts
+    auto now_tp = std::chrono::steady_clock::now();
+    std::optional<FSMReturnType> call_back_ms;
+
+    if (!seen_slac_parm_req) {
+        if (now_tp >= timeout_slac_parm_req) {
+            ctx.log_error("CM_SLAC_PARM_REQ timed out -> FAILED");
+            return Event::FAILED;
+        }
+
+        call_back_ms = remaining_milliseconds(timeout_slac_parm_req, now_tp);
+    }
+
+    // fallthrough: CM_SLAC_PARM_REQ has been seen, check individual sessions
+
+    for (auto& session : sessions) {
+        // there should always be an active timeout, right?
+        // FIXME (aw)
+        while (session.timeout_active) {
+            // FIXME (aw): this way we only take the first one
+            if (session.state == MatchingSubState::MATCH_COMPLETE) {
+                return Event::MATCH_COMPLETE;
+            }
+
+            auto remaining_ms = remaining_milliseconds(session.next_timeout, now_tp);
+            if (remaining_ms > 0) {
+                if (call_back_ms.has_value() == false || *call_back_ms > remaining_ms) {
+                    call_back_ms = remaining_ms;
+                }
+                break;
+            }
+
+            // fall-through, timeout should be handled now
+            session.ack_timeout();
+
+            if (session.state == MatchingSubState::WAIT_FOR_START_ATTEN_CHAR) {
+                session_log(ctx, session, LogLevel::ERROR, "Waiting for CM_START_ATTEN_CHAR_IND timed out -> failed");
+                session.state = MatchingSubState::FAILED;
+            } else if (session.state == MatchingSubState::SOUNDING) {
+                session_log(ctx, session, LogLevel::WARN,
+                            "Sounding not yet complete but timed out, going to sub-state FINALIZE_SOUNDING");
+                session.state = MatchingSubState::FINALIZE_SOUNDING;
+                session.set_next_timeout(FINALIZE_SOUNDING_DELAY_MS);
+            } else if (session.state == MatchingSubState::FINALIZE_SOUNDING) {
+                finalize_sounding(session);
+                session.num_retries = 0;
+            } else if (session.state == MatchingSubState::WAIT_FOR_ATTEN_CHAR_RSP) {
+                session.num_retries++;
+                if (session.num_retries <= slac::defs::C_EV_MATCH_RETRY) {
+                    session_log(ctx, session, LogLevel::WARN,
+                                "Waiting for CM_ATTEN_CHAR_RSP timed out -> retry matching");
+                    finalize_sounding(session);
+                } else {
+                    session_log(ctx, session, LogLevel::ERROR, "Waiting for CM_ATTEN_CHAR_RSP timed out -> failed");
+                    session.state = MatchingSubState::FAILED;
+                }
+            } else if (session.state == MatchingSubState::WAIT_FOR_SLAC_MATCH) {
+                session_log(ctx, session, LogLevel::ERROR, "Wating for CM_SLAC_MATCH_REQ timed out -> failed");
+                session.state = MatchingSubState::FAILED;
+            }
+        }
+
+        if (all_sessions_failed(sessions)) {
+            return Event::FAILED;
+        }
+    }
+
+    if (call_back_ms.has_value() == false) {
+        // FIXME (aw): this should not happen, should we assert here or something similar?
+    }
+    return *call_back_ms;
+}
+
+FSMSimpleState::HandleEventReturnType MatchingState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        handle_slac_message(ctx.slac_message_payload);
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } 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, std::move(match_cnf_message));
+        } else {
+            return sa.create_simple<MatchedState>(ctx);
+        }
+    } else if (ev == Event::RETRY_MATCHING) {
+        num_retries++;
+        if (num_retries == slac::defs::C_EV_MATCH_RETRY) {
+            ctx.log_error("Reached retry limit for matching");
+            return sa.create_simple<FailedState>(ctx);
+        }
+
+        // otherwise, reset timeout
+        timeout_slac_parm_req =
+            std::chrono::steady_clock::now() + std::chrono::milliseconds(ctx.slac_config.slac_init_timeout_ms);
+        return sa.HANDLED_INTERNALLY;
+    } else if (ev == Event::FAILED) {
+        failed_count++;
+        if (ctx.slac_config.reset_instead_of_fail and failed_count < 2) {
+            ctx.log_error("Resetting MatchingState. Waiting for the next CM_SLAC_PARAM.REQ message.");
+
+            // Resetting all relevant MatchingState members
+            sessions.clear();
+            // timeout for getting CM_SLAC_PARM_REQ
+            timeout_slac_parm_req =
+                std::chrono::steady_clock::now() + std::chrono::milliseconds(ctx.slac_config.slac_init_timeout_ms);
+            seen_slac_parm_req = false;
+            num_retries = 0;
+
+            return sa.HANDLED_INTERNALLY;
+        }
+        return sa.create_simple<FailedState>(ctx);
+    }
+
+    return sa.PASS_ON;
+}
+
+} // namespace slac::fsm::evse

tools/EVerest-main/lib/everest/slac/fsm/evse/src/states/matching_handle_slac.cpp

@@ -0,0 +1,482 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include "matching_handle_slac.hpp"
+
+#include <cstring>
+#include <iomanip>
+#include <sstream>
+
+#include "../misc.hpp"
+
+namespace slac::fsm::evse {
+
+void session_log(Context& ctx, MatchingSession& session, const LogLevel level, const std::string& text) {
+    const auto run_id = format_run_id(session.run_id);
+    const auto mac = format_mac_addr(session.ev_mac);
+    std::stringstream ss;
+
+    ss << "Session (run_id=" << run_id << ", ev_mac=" << mac << "): " << text;
+
+    switch (level) {
+    case LogLevel::DEBUG:
+        ctx.log_debug(ss.str());
+        break;
+    case LogLevel::INFO:
+        ctx.log_info(ss.str());
+        break;
+    case LogLevel::WARN:
+        ctx.log_warn(ss.str());
+        break;
+    case LogLevel::ERROR:
+        ctx.log_error(ss.str());
+        break;
+    }
+}
+
+//
+// MatchingSession related
+//
+static MatchingSession* find_session(std::vector<MatchingSession>& sessions, const uint8_t* ev_mac,
+                                     const uint8_t* run_id) {
+    for (auto& session : sessions) {
+        if (session.is_identified_by(ev_mac, run_id)) {
+            return &session;
+        }
+    }
+
+    return nullptr;
+}
+
+static auto create_cm_slac_parm_cnf(const MatchingSession& session) {
+    slac::messages::cm_slac_parm_cnf param_confirm;
+
+    memcpy(param_confirm.m_sound_target, slac::defs::BROADCAST_MAC_ADDRESS, sizeof(slac::defs::BROADCAST_MAC_ADDRESS));
+    param_confirm.num_sounds = slac::defs::CM_SLAC_PARM_CNF_NUM_SOUNDS;
+    param_confirm.timeout = slac::defs::CM_SLAC_PARM_CNF_TIMEOUT;
+    param_confirm.resp_type = slac::defs::CM_SLAC_PARM_CNF_RESP_TYPE;
+    memcpy(param_confirm.forwarding_sta, session.ev_mac, sizeof(param_confirm.forwarding_sta));
+    param_confirm.application_type = slac::defs::COMMON_APPLICATION_TYPE;
+    param_confirm.security_type = slac::defs::COMMON_SECURITY_TYPE;
+    memcpy(param_confirm.run_id, session.run_id, sizeof(param_confirm.run_id));
+
+    return param_confirm;
+}
+
+static auto create_cm_atten_char_ind(const MatchingSession& session, int atten_offset = 0) {
+    slac::messages::cm_atten_char_ind atten_char_ind;
+
+    atten_char_ind.application_type = slac::defs::COMMON_APPLICATION_TYPE;
+    atten_char_ind.security_type = slac::defs::COMMON_SECURITY_TYPE;
+    memcpy(atten_char_ind.source_address, session.ev_mac, sizeof(atten_char_ind.source_address));
+    memcpy(atten_char_ind.run_id, session.run_id, sizeof(atten_char_ind.run_id));
+    // memcpy(atten_char_ind.source_id, session_ev_id, sizeof(atten_char_ind.source_id));
+    memset(atten_char_ind.source_id, 0, sizeof(atten_char_ind.source_id));
+    // memcpy(atten_char_ind.resp_id, sample_evse_vin, sizeof(atten_char_ind.resp_id));
+    memset(atten_char_ind.resp_id, 0, sizeof(atten_char_ind.resp_id));
+    atten_char_ind.num_sounds = session.captured_sounds;
+    atten_char_ind.attenuation_profile.num_groups = slac::defs::AAG_LIST_LEN;
+    if (session.captured_sounds != 0) {
+        for (int i = 0; i < slac::defs::AAG_LIST_LEN; ++i) {
+            atten_char_ind.attenuation_profile.aag[i] =
+                session.captured_aags[i] / session.captured_sounds + atten_offset;
+        }
+    } else {
+        // FIXME (aw): what to do here, if we didn't receive any sounds?
+        memset(atten_char_ind.attenuation_profile.aag, 0x01, sizeof(atten_char_ind.attenuation_profile.aag));
+    }
+
+    return atten_char_ind;
+}
+
+// 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);
+    memcpy(match_cnf.pev_id, match_req.pev_id, sizeof(match_cnf.pev_id));
+    memcpy(match_cnf.pev_mac, match_req.pev_mac, sizeof(match_cnf.pev_mac));
+    memcpy(match_cnf.evse_id, match_req.evse_id, sizeof(match_cnf.evse_id));
+    memcpy(match_cnf.evse_mac, match_req.evse_mac, sizeof(match_cnf.evse_mac));
+    memcpy(match_cnf.run_id, match_req.run_id, sizeof(match_cnf.run_id));
+    memset(match_cnf._rerserved, 0, 8);
+    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));
+}
+
+void MatchingState::handle_slac_message(slac::messages::HomeplugMessage& msg) {
+    const auto mmtype = msg.get_mmtype();
+    tmp_ev_mac = msg.get_src_mac();
+
+    switch (mmtype) {
+    case (slac::defs::MMTYPE_CM_SLAC_PARAM | slac::defs::MMTYPE_MODE_REQ):
+        handle_cm_slac_parm_req(msg.get_payload<slac::messages::cm_slac_parm_req>());
+        break;
+    case (slac::defs::MMTYPE_CM_START_ATTEN_CHAR | slac::defs::MMTYPE_MODE_IND):
+        handle_cm_start_atten_char_ind(msg.get_payload<slac::messages::cm_start_atten_char_ind>());
+        break;
+    case (slac::defs::MMTYPE_CM_MNBC_SOUND | slac::defs::MMTYPE_MODE_IND):
+        handle_cm_mnbc_sound_ind(msg.get_payload<slac::messages::cm_mnbc_sound_ind>());
+        break;
+    case (slac::defs::MMTYPE_CM_ATTEN_PROFILE | slac::defs::MMTYPE_MODE_IND):
+        handle_cm_atten_profile_ind(msg.get_payload<slac::messages::cm_atten_profile_ind>());
+        break;
+    case (slac::defs::MMTYPE_CM_ATTEN_CHAR | slac::defs::MMTYPE_MODE_RSP):
+        handle_cm_atten_char_rsp(msg.get_payload<slac::messages::cm_atten_char_rsp>());
+        break;
+    case (slac::defs::MMTYPE_CM_SLAC_MATCH | slac::defs::MMTYPE_MODE_REQ):
+        handle_cm_slac_match_req(msg.get_payload<slac::messages::cm_slac_match_req>());
+        break;
+    case (slac::defs::MMTYPE_CM_VALIDATE | slac::defs::MMTYPE_MODE_REQ):
+        handle_cm_validate_req(msg.get_payload<slac::messages::cm_validate_req>());
+        break;
+    default:
+        ctx.log_warn("Received non-expected SLAC message of type " + format_mmtype(mmtype));
+    }
+}
+
+static bool validate_cm_slac_parm_req(const slac::messages::cm_slac_parm_req& msg) {
+
+    if (msg.application_type not_eq slac::defs::COMMON_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq slac::defs::COMMON_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_warn("Invalid CM_SLAC_PARM.REQ received, ignoring");
+        return;
+    }
+
+    // set this flag to true, to disable the retry timeout
+    seen_slac_parm_req = true;
+
+    auto session = find_session(sessions, tmp_ev_mac, msg.run_id);
+    if (session) {
+        // the matching session existed already, according to [V2G3-A09-16] we should restart
+        *session = MatchingSession(tmp_ev_mac, msg.run_id);
+    } else {
+        // the session didn't exist, lets create it
+        sessions.emplace_back(MatchingSession{tmp_ev_mac, msg.run_id});
+        session = &sessions.back();
+    }
+
+    session_log(ctx, *session, LogLevel::INFO, "initialized, waiting for CM_START_ATTEN_CHAR_IND");
+
+    // timeout until we need to get cm_start_atten_char_ind
+    session->set_next_timeout(slac::defs::TT_MATCH_SEQUENCE_MS);
+
+    auto param_confirm = create_cm_slac_parm_cnf(*session);
+
+    ctx.send_slac_message(param_confirm.forwarding_sta, param_confirm);
+    ctx.signal_cm_slac_parm_req(tmp_ev_mac);
+}
+
+static bool validate_cm_start_atten_char_ind(const slac::messages::cm_start_atten_char_ind& msg) {
+
+    if (msg.application_type not_eq slac::defs::COMMON_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq slac::defs::COMMON_SECURITY_TYPE) {
+        return false;
+    }
+    if (msg.num_sounds == 0) { // Don't be strict to the ISO 15118-3
+        return false;
+    }
+    if (msg.timeout == 0) { // Don't be strict to the ISO 15118-3
+        return false;
+    }
+    if (msg.resp_type not_eq slac::defs::CM_SLAC_PARM_CNF_RESP_TYPE) {
+        return false;
+    }
+
+    return true;
+}
+
+void MatchingState::handle_cm_start_atten_char_ind(const slac::messages::cm_start_atten_char_ind& msg) {
+
+    if (not validate_cm_start_atten_char_ind(msg)) {
+        ctx.log_warn("Invalid CM_START_ATTEN_CHAR_IND received, ignoring");
+        return;
+    }
+
+    auto session = find_session(sessions, tmp_ev_mac, msg.run_id);
+    if (!session) {
+        ctx.log_warn("No session found for CM_START_ATTEN_CHAR_IND");
+        return;
+    }
+
+    if (session->state != MatchingSubState::WAIT_FOR_START_ATTEN_CHAR) {
+        if (session->state != MatchingSubState::SOUNDING)
+            session_log(ctx, *session, LogLevel::WARN,
+                        "needs to be in state WAIT_FOR_START_ATTEN_CHAR for CM_START_ATTEN_CHAR_IND");
+        return;
+    }
+
+    // go to sounding
+    session_log(ctx, *session, LogLevel::INFO, "received CM_START_ATTEN_CHAR_IND, going to substate SOUNDING");
+    session->state = MatchingSubState::SOUNDING;
+    session->set_next_timeout(slac::defs::TT_EVSE_MATCH_MNBC_MS);
+}
+
+static bool validate_cm_mnbc_sound_ind(const slac::messages::cm_mnbc_sound_ind& msg) {
+
+    if (msg.application_type not_eq slac::defs::COMMON_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq slac::defs::COMMON_SECURITY_TYPE) {
+        return false;
+    }
+
+    return true;
+}
+
+void MatchingState::handle_cm_mnbc_sound_ind(const slac::messages::cm_mnbc_sound_ind& msg) {
+
+    if (not validate_cm_mnbc_sound_ind(msg)) {
+        ctx.log_warn("Invalid CM_MNBC_SOUND_IND received, ignoring");
+        return;
+    }
+
+    auto session = find_session(sessions, tmp_ev_mac, msg.run_id);
+    if (!session) {
+        ctx.log_warn("No session found for CM_MNBC_SOUND_IND");
+        return;
+    }
+
+    if (session->state != MatchingSubState::SOUNDING) {
+        session_log(ctx, *session, LogLevel::WARN, "needs to be in state SOUNDING for CM_MNBC_SOUND_IND");
+        return;
+    }
+
+    session_log(ctx, *session, LogLevel::INFO, "received CM_MNBC_SOUND_IND");
+
+    session->received_mnbc_sound = true;
+}
+
+void MatchingState::handle_cm_atten_profile_ind(const slac::messages::cm_atten_profile_ind& msg) {
+    // cm_atten_profile_ind does not carry a run_id, so we can't exactly identify the session
+    // FIXME (aw): for now, we only take the first one found
+    MatchingSession* session = nullptr;
+
+    for (auto& session_i : sessions) {
+        if (memcmp(msg.pev_mac, session_i.ev_mac, sizeof(msg.pev_mac)) == 0) {
+            session = &session_i;
+        }
+    }
+
+    if (!session) {
+        ctx.log_warn("No session found for CM_ATTEN_PROFILE_IND");
+        return;
+    }
+
+    if (session->state != MatchingSubState::SOUNDING) {
+        session_log(ctx, *session, LogLevel::WARN, "needs to be in state SOUNDING for CM_ATTEN_PROFILE_IND");
+        return;
+    }
+
+    if (msg.num_groups != slac::defs::AAG_LIST_LEN) {
+        session_log(ctx, *session, LogLevel::WARN, "mismatch in number of AAG groups");
+        return;
+    }
+
+    session_log(ctx, *session, LogLevel::INFO, "received CM_ATTEN_PROFILE_IND");
+
+    for (int i = 0; i < slac::defs::AAG_LIST_LEN; ++i) {
+        session->captured_aags[i] += msg.aag[i];
+    }
+
+    session->captured_sounds++;
+
+    if (session->captured_sounds < slac::defs::CM_SLAC_PARM_CNF_NUM_SOUNDS) {
+        return;
+    }
+
+    // fall-through: all sounds captured
+    session_log(ctx, *session, LogLevel::INFO, "received all sounds, going to substate FINALIZE_SOUNDING");
+    session->state = MatchingSubState::FINALIZE_SOUNDING;
+    session->set_next_timeout(FINALIZE_SOUNDING_DELAY_MS);
+}
+
+static bool validate_cm_atten_char_rsp(const slac::messages::cm_atten_char_rsp& msg, const MatchingSession& session) {
+
+    if (msg.application_type not_eq slac::defs::COMMON_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq slac::defs::COMMON_SECURITY_TYPE) {
+        return false;
+    }
+    if (memcmp(msg.source_address, session.ev_mac, ETH_ALEN)) {
+        return false;
+    }
+    uint8_t source_id_ref[slac::messages::SOURCE_ID_LEN];
+    memset(source_id_ref, 0, sizeof(source_id_ref));
+    if (memcmp(source_id_ref, msg.source_id, slac::messages::SOURCE_ID_LEN)) {
+        return false;
+    }
+    uint8_t resp_id_ref[slac::messages::RESP_ID_LEN];
+    memset(resp_id_ref, 0, sizeof(resp_id_ref));
+    if (memcmp(resp_id_ref, msg.resp_id, slac::messages::RESP_ID_LEN)) {
+        return false;
+    }
+    if (msg.result not_eq slac::defs::CM_ATTEN_CHAR_RSP_RESULT) {
+        return false;
+    }
+
+    return true;
+}
+
+void MatchingState::handle_cm_atten_char_rsp(const slac::messages::cm_atten_char_rsp& msg) {
+
+    auto session = find_session(sessions, tmp_ev_mac, msg.run_id);
+    if (!session) {
+        ctx.log_warn("No session found for CM_ATTEN_CHAR_RSP");
+        return;
+    }
+
+    if (not validate_cm_atten_char_rsp(msg, *session)) {
+        session_log(ctx, *session, LogLevel::WARN, "Invalid CM_ATTEN_CHAR_RSP received, ignoring");
+        return;
+    }
+
+    if (session->state != MatchingSubState::WAIT_FOR_ATTEN_CHAR_RSP) {
+        session_log(ctx, *session, LogLevel::WARN,
+                    "needs to be in state WAIT_FOR_ATTEN_CHAR_RSP for CM_ATTEN_CHAR_RSP");
+        return;
+    }
+
+    session_log(ctx, *session, LogLevel::INFO, "received CM_ATTEN_CHAR_RSP, going to substate WAIT_FOR_SLAC_MATCH");
+    session->state = MatchingSubState::WAIT_FOR_SLAC_MATCH;
+
+    // FIXME (aw): referring to the standard, it is not clear here, if we should offset from TT_EVSE_MATCH_MNBC
+    session->set_next_timeout(slac::defs::TT_EVSE_MATCH_SESSION_MS);
+}
+
+void MatchingState::handle_cm_validate_req(const slac::messages::cm_validate_req& msg) {
+    // NOTE: CM_VALIDATE.REQ does not specify its session
+    // EVSE allowed to not implement: [V2G3-A09-51]
+    ctx.log_warn("Received CM_VALIDATE.REQ / not implemented - will return failure code");
+
+    slac::messages::cm_validate_cnf validate_cnf;
+    validate_cnf.signal_type = slac::defs::CM_VALIDATE_REQ_SIGNAL_TYPE;
+    validate_cnf.toggle_num = 0;
+    validate_cnf.result = slac::defs::CM_VALIDATE_REQ_RESULT_FAILURE;
+
+    ctx.send_slac_message(tmp_ev_mac, validate_cnf);
+}
+
+static bool validate_cm_slac_match_req(const slac::messages::cm_slac_match_req& msg, const MatchingSession& session,
+                                       const Context& ctx) {
+
+    if (msg.application_type not_eq slac::defs::COMMON_APPLICATION_TYPE) {
+        return false;
+    }
+    if (msg.security_type not_eq slac::defs::COMMON_SECURITY_TYPE) {
+        return false;
+    }
+    if (msg.mvf_length not_eq slac::defs::CM_SLAC_MATCH_REQ_MVF_LENGTH) {
+        return false;
+    }
+    // PEV ID = 0x00 TC_SECC_CMN_VTB_CmSlacMatch_013/014(?)
+    uint8_t pev_id_ref[slac::messages::PEV_ID_LEN];
+    memset(pev_id_ref, 0, sizeof(pev_id_ref));
+    if (memcmp(pev_id_ref, msg.pev_id, slac::messages::PEV_ID_LEN)) {
+        return false;
+    }
+    // PEV MAC TC_SECC_CMN_VTB_CmSlacMatch_015/016(?)
+    if (memcmp(msg.pev_mac, session.ev_mac, ETH_ALEN)) {
+        return false;
+    }
+    // EVSE ID = 0x00 TC_SECC_CMN_VTB_CmSlacMatch_017/018(?)
+    uint8_t evse_id_ref[slac::messages::EVSE_ID_LEN];
+    memset(evse_id_ref, 0, sizeof(evse_id_ref));
+    if (memcmp(evse_id_ref, msg.evse_id, slac::messages::EVSE_ID_LEN)) {
+        return false;
+    }
+    // EVSE MAC TC_SECC_CMN_VTB_CmSlacMatch_019/020
+    if (memcmp(ctx.evse_mac, msg.evse_mac, ETH_ALEN)) {
+        return false;
+    }
+    // RunID TC_SECC_CMN_VTB_CmSlacMatch_021/022
+    if (memcmp(msg.run_id, session.run_id, slac::defs::RUN_ID_LEN)) {
+        return false;
+    }
+    return true;
+}
+
+void MatchingState::handle_cm_slac_match_req(const slac::messages::cm_slac_match_req& msg) {
+
+    auto session = find_session(sessions, tmp_ev_mac, msg.run_id);
+    if (!session) {
+        ctx.log_warn("No session found for CM_SLAC_MATCH_REQ");
+        return;
+    }
+
+    if (not validate_cm_slac_match_req(msg, *session, ctx)) {
+        session_log(ctx, *session, LogLevel::WARN, "Invalid CM_SLAC_MATCH_REQ received, ignoring");
+        return;
+    }
+
+    if (session->state != MatchingSubState::WAIT_FOR_SLAC_MATCH && session->state != MatchingSubState::MATCH_COMPLETE) {
+        session_log(ctx, *session, LogLevel::WARN,
+                    "needs to be in state WAIT_FOR_SLAC_MATCH or MATCH_COMPLETE for CM_SLAC_MATCH_REQ");
+        return;
+    }
+
+    session_log(ctx, *session, LogLevel::INFO,
+                "Received CM_SLAC_MATCH_REQ, sending CM_SLAC_MATCH_CNF -> session complete");
+
+    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");
+        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
+    session->set_next_timeout(0);
+    ctx.signal_cm_slac_match_cnf(tmp_ev_mac);
+}
+
+void MatchingState::finalize_sounding(MatchingSession& session) {
+    session_log(ctx, session, LogLevel::INFO, "Finalize sounding, sending CM_ATTEN_CHAR_IND");
+    session.state = MatchingSubState::WAIT_FOR_ATTEN_CHAR_RSP;
+
+    auto atten_char = create_cm_atten_char_ind(session, ctx.slac_config.sounding_atten_adjustment);
+
+    ctx.send_slac_message(session.ev_mac, atten_char);
+
+    session.set_next_timeout(slac::defs::TT_MATCH_RESPONSE_MS);
+
+    int aag_overall_sum = 0;
+    for (size_t i = 0; i < slac::defs::AAG_LIST_LEN; ++i) {
+        aag_overall_sum += atten_char.attenuation_profile.aag[i];
+    }
+    std::ostringstream ss;
+    ss << "Avg atten.: " << std::fixed << std::setprecision(1)
+       << (static_cast<double>(aag_overall_sum) / slac::defs::AAG_LIST_LEN) << " dB";
+    if (ctx.slac_config.sounding_atten_adjustment != 0) {
+        ss << " plus offset " << std::to_string(ctx.slac_config.sounding_atten_adjustment) << " dB";
+    }
+    ss << ", from " << std::to_string(slac::defs::AAG_LIST_LEN) << " groups, " << session.captured_sounds << " sounds";
+    session_log(ctx, session, LogLevel::INFO, ss.str());
+}
+
+} // namespace slac::fsm::evse

tools/EVerest-main/lib/everest/slac/fsm/evse/src/states/matching_handle_slac.hpp

@@ -0,0 +1,21 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#ifndef EVSE_SLAC_STATES_MATCHING_HANDLE_SLAC_HPP
+#define EVSE_SLAC_STATES_MATCHING_HANDLE_SLAC_HPP
+
+#include <everest/slac/fsm/evse/states/matching.hpp>
+
+namespace slac::fsm::evse {
+
+enum class LogLevel {
+    DEBUG,
+    INFO,
+    WARN,
+    ERROR
+};
+
+void session_log(Context& ctx, MatchingSession& session, const LogLevel level, const std::string& text);
+
+} // namespace slac::fsm::evse
+
+#endif // EVSE_SLAC_STATES_MATCHING_HANDLE_SLAC_HPP

tools/EVerest-main/lib/everest/slac/fsm/evse/src/states/others.cpp

@@ -0,0 +1,421 @@
+// SPDX-License-Identifier: Apache-2.0
+// Copyright 2023 - 2023 Pionix GmbH and Contributors to EVerest
+#include <everest/slac/fsm/evse/states/others.hpp>
+
+#include <cstring>
+#include <optional>
+#include <string_view>
+
+#include <everest/slac/fsm/evse/states/matching.hpp>
+
+#include "../misc.hpp"
+
+namespace slac::fsm::evse {
+
+static auto create_cm_set_key_req(uint8_t const* session_nmk) {
+    slac::messages::cm_set_key_req set_key_req;
+
+    set_key_req.key_type = slac::defs::CM_SET_KEY_REQ_KEY_TYPE_NMK;
+    set_key_req.my_nonce = 0x00000000;
+    set_key_req.your_nonce = 0x00000000;
+    set_key_req.pid = slac::defs::CM_SET_KEY_REQ_PID_HLE;
+    set_key_req.prn = htole16(slac::defs::CM_SET_KEY_REQ_PRN_UNUSED);
+    set_key_req.pmn = slac::defs::CM_SET_KEY_REQ_PMN_UNUSED;
+    set_key_req.cco_capability = slac::defs::CM_SET_KEY_REQ_CCO_CAP_NONE;
+    slac::utils::generate_nid_from_nmk(set_key_req.nid, session_nmk);
+    set_key_req.new_eks = slac::defs::CM_SET_KEY_REQ_PEKS_NMK_KNOWN_TO_STA;
+    memcpy(set_key_req.new_key, session_nmk, sizeof(set_key_req.new_key));
+
+    return set_key_req;
+}
+
+void ResetState::enter() {
+    ctx.log_info("Entered Reset state");
+    if (ctx.slac_config.regenerate_key_on_reset) {
+        ctx.slac_config.generate_nmk();
+    }
+}
+
+FSMSimpleState::HandleEventReturnType ResetState::handle_event(AllocatorType& sa, Event ev) {
+    const auto& cfg = ctx.slac_config;
+    if (ev == Event::SLAC_MESSAGE) {
+        if (handle_slac_message(ctx.slac_message_payload)) {
+            if (cfg.chip_reset.enabled) {
+                // If chip reset is enabled in config, go to ResetChipState and from there to IdleState
+                return sa.create_simple<ResetChipState>(ctx);
+            } else {
+                // If chip reset is disabled, go to IdleState directly
+                return sa.create_simple<IdleState>(ctx);
+            }
+        } else {
+            return sa.PASS_ON;
+        }
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+FSMSimpleState::CallbackReturnType ResetState::callback() {
+    const auto& cfg = ctx.slac_config;
+    if (setup_has_been_send == false) {
+        auto set_key_req = create_cm_set_key_req(cfg.session_nmk);
+
+        ctx.log_info("New NMK key: " + format_nmk(cfg.session_nmk));
+
+        ctx.send_slac_message(cfg.plc_peer_mac, set_key_req);
+
+        setup_has_been_send = true;
+
+        return cfg.set_key_timeout_ms;
+    } else {
+        ctx.log_error("CM_SET_KEY_REQ timeout - failed to setup NMK key");
+        return {};
+    }
+}
+
+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. It is optional in the standard.
+        ctx.log_warn("Received non-expected SLAC message of type " + format_mmtype(mmtype));
+        return false;
+    } else {
+        ctx.log_info("Received CM_SET_KEY_CNF");
+        return true;
+    }
+}
+
+void ResetChipState::enter() {
+    ctx.log_info("Entered HW Chip Reset state");
+}
+
+FSMSimpleState::HandleEventReturnType ResetChipState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        if (handle_slac_message(ctx.slac_message_payload)) {
+            return sa.create_simple<IdleState>(ctx);
+        } else {
+            return sa.PASS_ON;
+        }
+    } else if (ev == Event::SUCCESS) {
+        return sa.create_simple<IdleState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+FSMSimpleState::CallbackReturnType ResetChipState::callback() {
+    const auto& cfg = ctx.slac_config;
+    if (sub_state == SubState::DELAY) {
+        sub_state = SubState::SEND_RESET;
+        return cfg.chip_reset.delay_ms;
+
+    } else if (sub_state == SubState::SEND_RESET) {
+
+        if (ctx.modem_vendor == ModemVendor::Qualcomm) {
+            slac::messages::qualcomm::cm_reset_device_req reset_req;
+            ctx.log_info("Resetting HW Chip using RS_DEV.REQ");
+            ctx.send_slac_message(cfg.plc_peer_mac, reset_req);
+            sub_state = SubState::DONE;
+            return cfg.chip_reset.timeout_ms;
+
+        } else if (ctx.modem_vendor == ModemVendor::Lumissil) {
+            slac::messages::lumissil::nscm_reset_device_req reset_req;
+            ctx.log_info("Resetting HW Chip using NSCM_RESET_DEVICE.REQ");
+            sub_state = SubState::DONE;
+            ctx.send_slac_message(cfg.plc_peer_mac, reset_req);
+            // CG5317 does not reply to the reset packet
+            return Event::SUCCESS;
+
+        } else {
+            ctx.log_info("Chip reset not supported on this chip");
+        }
+    } else {
+        ctx.log_error("Reset timeout, no response received - failed to reset the chip");
+        return {};
+    }
+    return {};
+}
+
+bool ResetChipState::handle_slac_message(slac::messages::HomeplugMessage& message) {
+    const auto mmtype = message.get_mmtype();
+    if (mmtype != (slac::defs::qualcomm::MMTYPE_CM_RESET_DEVICE | slac::defs::MMTYPE_MODE_CNF)) {
+        // unexpected message
+        ctx.log_warn("Received non-expected SLAC message of type " + format_mmtype(mmtype));
+        return false;
+    } else {
+        ctx.log_info("Received RS_DEV.CNF");
+        return true;
+    }
+}
+
+void IdleState::enter() {
+    ctx.signal_state("UNMATCHED");
+    ctx.log_info("Entered Idle state");
+}
+
+FSMSimpleState::HandleEventReturnType IdleState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::ENTER_BCD) {
+        return sa.create_simple<MatchingState>(ctx);
+    } else if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+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::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_error("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 {
+        // 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() {
+    if (ctx.slac_config.ac_mode_five_percent) {
+        ctx.signal_error_routine_request();
+    }
+    ctx.log_info("Entered Failed state");
+}
+
+FSMSimpleState::HandleEventReturnType FailedState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::RESET) {
+        return sa.create_simple<ResetState>(ctx);
+    } else {
+        return sa.PASS_ON;
+    }
+}
+
+void WaitForLinkState::enter() {
+    ctx.log_info("Waiting for Link to be ready...");
+    start_time = std::chrono::steady_clock::now();
+}
+
+FSMSimpleState::HandleEventReturnType WaitForLinkState::handle_event(AllocatorType& sa, Event ev) {
+    if (ev == Event::SLAC_MESSAGE) {
+        if (handle_slac_message(ctx.slac_message_payload)) {
+            return sa.create_simple<MatchedState>(ctx);
+        } else {
+            return sa.PASS_ON;
+        }
+    } else if (ev == Event::RETRY_MATCHING) {
+        ctx.log_error("Link could not be established");
+        // 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;
+    }
+}
+
+FSMSimpleState::CallbackReturnType WaitForLinkState::callback() {
+    const auto& cfg = ctx.slac_config;
+    if (not link_status_req_sent) {
+        link_status_req_sent = send_link_status_req(ctx);
+        return cfg.link_status.retry_ms;
+    } else {
+        // Did we timeout?
+        if (std::chrono::steady_clock::now() - start_time > std::chrono::milliseconds(cfg.link_status.timeout_ms)) {
+            return Event::RETRY_MATCHING;
+        }
+        // Link is confirmed not up yet, query again
+        link_status_req_sent = false;
+        return cfg.link_status.retry_ms;
+    }
+}
+
+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();
+
+    auto link_ok = check_link_status_cnf(ctx.modem_vendor, message);
+
+    if (link_ok.has_value() and link_ok.value()) {
+        return true;
+    }
+
+    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) {
+    if (ev == Event::SLAC_MESSAGE) {
+        handle_slac_message(ctx.slac_message_payload);
+        return sa.PASS_ON;
+    } else if (ev == Event::SUCCESS) {
+        return sa.create_simple<ResetState>(ctx);
+    }
+    return sa.PASS_ON;
+}
+
+FSMSimpleState::CallbackReturnType InitState::callback() {
+    const auto& cfg = ctx.slac_config;
+
+    if (sub_state == SubState::QUALCOMM_OP_ATTR) {
+        sub_state = SubState::LUMISSIL_GET_VERSION;
+        slac::messages::qualcomm::op_attr_req op_attr_req;
+        ctx.send_slac_message(cfg.plc_peer_mac, op_attr_req);
+        return cfg.request_info_delay_ms;
+
+    } else if (sub_state == SubState::LUMISSIL_GET_VERSION) {
+        sub_state = SubState::DONE;
+        slac::messages::lumissil::nscm_get_version_req version_req;
+        ctx.send_slac_message(cfg.plc_peer_mac, version_req);
+        return cfg.request_info_delay_ms;
+
+    } else if (sub_state == SubState::DONE) {
+        // the requested info may or may not be implemented by the chip,
+        // so we ignore timeouts here.
+        return Event::SUCCESS;
+    }
+    return {};
+}
+
+static std::string get_qualcomm_device_info(slac::messages::qualcomm::op_attr_cnf const& msg) {
+    const auto get_string_view = [](auto const& raw) constexpr {
+        static_assert(sizeof(uint8_t) == sizeof(char));
+        return std::string_view(reinterpret_cast<char const*>(raw), sizeof(raw));
+    };
+
+    std::string result("Qualcomm PLC Device Attributes:");
+    result += "\n  HW Platform: ";
+    result += get_string_view(msg.hw_platform);
+    result += "\n  SW Platform: ";
+    result += get_string_view(msg.sw_platform);
+    result += ("\n  Firmware: " + std::to_string(msg.version_major) + "." + std::to_string(msg.version_minor) + "." +
+               std::to_string(msg.version_pib) + "." + std::to_string(msg.reserved) + "-" +
+               std::to_string(msg.version_build));
+    result += "\n  Build date: ";
+    result += get_string_view(msg.build_date);
+
+    result += "\n  ZC signal: ";
+
+    // FIXME: no magic numbers
+    const auto zc_signal = (msg.line_freq_zc >> 2) & 0x03;
+    if (zc_signal == 0x01) {
+        result += "Detected";
+    } else if (zc_signal == 0x02) {
+        result += "Missing";
+    } else {
+        result += ("Unknown (" + std::to_string(zc_signal) + ")");
+    }
+
+    result += "\n  Line frequency: ";
+
+    const auto line_freq = (msg.line_freq_zc) & 0x03;
+    if (line_freq == 0x01) {
+        result += "50Hz";
+    } else if (line_freq == 0x02) {
+        result += "60Hz";
+    } else {
+        result += ("Unknown (" + std::to_string(line_freq) + ")");
+    }
+
+    return result;
+}
+
+static std::string get_lumissil_device_info(slac::messages::lumissil::nscm_get_version_cnf const& msg) {
+    return "Lumissil PLC Device Firmware version: " + std::to_string(msg.version_major) + "." +
+           std::to_string(msg.version_minor) + "." + std::to_string(msg.version_patch) + "." +
+           std::to_string(msg.version_build);
+}
+
+void InitState::handle_slac_message(slac::messages::HomeplugMessage& message) {
+    const auto mmtype = message.get_mmtype();
+    if (mmtype == (slac::defs::qualcomm::MMTYPE_OP_ATTR | slac::defs::MMTYPE_MODE_CNF)) {
+        const auto msg = message.get_payload<slac::messages::qualcomm::op_attr_cnf>();
+        const auto device_info = get_qualcomm_device_info(msg);
+        ctx.log_info(device_info);
+        // This message is only supported on Qualcomm, so we can use it to detect the Vendor
+        ctx.modem_vendor = ModemVendor::Qualcomm;
+
+    } else if (mmtype == (slac::defs::lumissil::MMTYPE_NSCM_GET_VERSION | slac::defs::MMTYPE_MODE_CNF)) {
+        const auto msg = message.get_payload<slac::messages::lumissil::nscm_get_version_cnf>();
+        const auto device_info = get_lumissil_device_info(msg);
+        ctx.log_info(device_info);
+        // This message is only supported on Qualcomm, so we can use it to detect the Vendor
+        ctx.modem_vendor = ModemVendor::Lumissil;
+    }
+}
+} // namespace slac::fsm::evse