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
This commit is contained in:
Eric F
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user