Skip to content

Commit f3a107e

Browse files
authored
P3-H P0.1: encoder-produced deterministic SV WirePlan (#141)
P0 is already merged. Retargeted diff against current main contains only P0.1. Exact-head CI is green. Merge encoder-owned field layout, direct smpSynch patching, and realtime authority lock. Tracks #139.
2 parents ea42c68 + d63cec7 commit f3a107e

10 files changed

Lines changed: 275 additions & 154 deletions

File tree

‎.github/workflows/esp32p4-smv-injector.yml‎

Lines changed: 11 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,16 @@ jobs:
8484
from pathlib import Path
8585
8686
source = Path('main/app_main.cpp').read_text(encoding='utf-8')
87+
cmake = Path('main/CMakeLists.txt').read_text(encoding='utf-8')
88+
smp_synch = Path('main/smp_synch_lab.cpp').read_text(encoding='utf-8')
89+
assert 'find_fixed_field' not in source
90+
assert 'AR_SMP_SYNCH_INTERCEPT_TX' not in cmake
91+
assert 'encode_into_with_layout' in source
92+
assert 'sample_synchronization_offset' in source
93+
assert 'smp_synch_lab_decision()' in source
94+
assert 'wire_field_offsets.hpp' not in smp_synch
95+
assert 'esp_eth_transmit' not in smp_synch
96+
assert 'observe_canonical_tx_result' not in smp_synch
8797
start_begin = source.index('if (take_start_request())')
8898
start_end = source.index('if (!live_tx_running())', start_begin)
8999
start_block = source[start_begin:start_end]
@@ -99,7 +109,7 @@ jobs:
99109
assert 'kTransportFailureStopThreshold = 400U' in source
100110
assert 'live_control_force_stop();' in source
101111
assert 'SV transport fault:' in source
102-
print('Realtime boundary policy: PASS - post-log timer start + elapsed-time telemetry + 100 ms fail-closed canonical TX')
112+
print('Realtime boundary policy: PASS - encoder-owned WirePlan + direct smpSynch patch + one canonical TX failure authority')
103113
PY
104114
105115
- name: Build firmware

‎embedded/esp32p4_smv_injector/main/CMakeLists.txt‎

Lines changed: 0 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,3 @@ if(CONFIG_AR_PTP_LAB_TX)
3838
"-Dar_ptp_lab_get_status=ar_ptp_source_get_status")
3939
endif()
4040

41-
# Only the deterministic SV publisher translation unit is intercepted. The shim
42-
# patches the already encoded smpSynch byte immediately before transmit; PTP and
43-
# every other Ethernet user continue calling ESP-IDF's esp_eth_transmit directly.
44-
set_property(SOURCE "app_main.cpp" APPEND PROPERTY COMPILE_OPTIONS
45-
"-include" "${CMAKE_CURRENT_LIST_DIR}/smp_synch_lab.hpp"
46-
"-DAR_SMP_SYNCH_INTERCEPT_TX=1")

‎embedded/esp32p4_smv_injector/main/app_main.cpp‎

Lines changed: 47 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -10,6 +10,7 @@
1010
#include "ethernet_port.h"
1111
#include "live_control.hpp"
1212
#include "runtime_profile.hpp"
13+
#include "smp_synch_lab.hpp"
1314
#include "esp_err.h"
1415
#include "esp_eth.h"
1516
#include "esp_event.h"
@@ -42,10 +43,12 @@ using ar::esp32p4::smv::live_tx_running;
4243
using ar::esp32p4::smv::runtime_profile_initialize;
4344
using ar::esp32p4::smv::runtime_profile_snapshot;
4445
using ar::esp32p4::smv::runtime_profile_validate;
46+
using ar::esp32p4::smv::smp_synch_lab_decision;
4547
using ar::esp32p4::smv::take_start_request;
4648
using ar::iec61850::ethernet::MacAddress;
4749
using ar::iec61850::ethernet::VlanTag;
4850
using ar::iec61850::sampled_values::SampledValueAsdu;
51+
using ar::iec61850::sampled_values::SampledValueAsduEncodeLayout;
4952
using ar::iec61850::sampled_values::SampledValuesFrame;
5053
using ar::iec61850::sampled_values::SampledValuesFrameCodec;
5154
using ar::iec61850::sampled_values::SampledValuesPdu;
@@ -86,6 +89,7 @@ std::atomic<std::uint32_t> g_sample_tick_total{0U};
8689
struct PacketTemplate final {
8790
std::vector<std::uint8_t> bytes;
8891
std::size_t sample_count_offset{std::numeric_limits<std::size_t>::max()};
92+
std::size_t sample_synchronization_offset{std::numeric_limits<std::size_t>::max()};
8993
std::size_t sample_payload_offset{std::numeric_limits<std::size_t>::max()};
9094
};
9195

@@ -204,57 +208,66 @@ SampledValuesFrame make_runtime_frame(
204208
return frame;
205209
}
206210

207-
bool find_fixed_field(
208-
const std::vector<std::uint8_t>& bytes,
209-
const std::uint8_t tag,
210-
const std::uint8_t length,
211-
std::size_t& value_offset) noexcept {
212-
for (std::size_t index = 20U; index + 2U + length <= bytes.size(); ++index) {
213-
if (bytes[index] == tag && bytes[index + 1U] == length) {
214-
value_offset = index + 2U;
215-
return true;
216-
}
217-
}
218-
return false;
219-
}
220-
221211
bool build_packet_template(
222212
const std::array<std::uint8_t, 6>& source_mac,
223213
const RuntimePublisherProfile& profile,
224214
const bool broadcast_destination,
225215
const bool diagnostic_mirror,
226216
PacketTemplate& packet) {
227217
packet = {};
228-
packet.bytes = SampledValuesFrameCodec::encode(
229-
make_runtime_frame(source_mac, profile, diagnostic_mirror));
230-
231-
if (broadcast_destination) {
232-
std::fill_n(packet.bytes.begin(), 6U, std::uint8_t{0xFFU});
218+
const auto frame = make_runtime_frame(source_mac, profile, diagnostic_mirror);
219+
const auto required = SampledValuesFrameCodec::encoded_size(frame);
220+
if (!required.has_value()) {
221+
ESP_LOGE(kTag, "SV template size validation failed");
222+
return false;
233223
}
234224

235-
std::size_t conf_rev_offset = 0U;
236-
if (!find_fixed_field(packet.bytes, 0x82U, 2U, packet.sample_count_offset) ||
237-
!find_fixed_field(packet.bytes, 0x83U, 4U, conf_rev_offset) ||
238-
!find_fixed_field(packet.bytes, 0x87U, 64U, packet.sample_payload_offset)) {
239-
ESP_LOGE(kTag, "Failed to locate fixed SV patch fields in encoded template");
225+
packet.bytes.resize(*required);
226+
std::array<SampledValueAsduEncodeLayout, 1> layouts{};
227+
const auto encoded = SampledValuesFrameCodec::encode_into_with_layout(
228+
frame, packet.bytes, layouts);
229+
if (!encoded.success() || encoded.bytes_written != packet.bytes.size()) {
230+
ESP_LOGE(kTag, "SV template encode/layout generation failed");
240231
return false;
241232
}
242-
if (packet.sample_payload_offset + 64U > packet.bytes.size()) {
243-
ESP_LOGE(kTag, "SV payload patch region exceeds encoded frame");
233+
234+
const auto& layout = layouts.front();
235+
const auto exact_region = [&packet](const auto& region, const std::size_t expected_size) {
236+
return region.present && region.value_size == expected_size &&
237+
region.value_offset <= packet.bytes.size() &&
238+
region.value_size <= packet.bytes.size() - region.value_offset;
239+
};
240+
if (!exact_region(layout.sample_count, 2U) ||
241+
!exact_region(layout.configuration_revision, 4U) ||
242+
!exact_region(layout.sample_synchronization, 1U) ||
243+
!exact_region(layout.sample_payload, 64U)) {
244+
ESP_LOGE(kTag, "SV template patch layout does not match the bounded runtime contract");
244245
return false;
245246
}
246247

248+
packet.sample_count_offset = layout.sample_count.value_offset;
249+
packet.sample_synchronization_offset = layout.sample_synchronization.value_offset;
250+
packet.sample_payload_offset = layout.sample_payload.value_offset;
251+
252+
if (broadcast_destination) {
253+
std::fill_n(packet.bytes.begin(), 6U, std::uint8_t{0xFFU});
254+
}
255+
247256
write_u16_be(packet.bytes.data() + packet.sample_count_offset, 0U);
248-
write_u32_be(packet.bytes.data() + conf_rev_offset, profile.configuration_revision);
257+
write_u32_be(
258+
packet.bytes.data() + layout.configuration_revision.value_offset,
259+
profile.configuration_revision);
249260
return true;
250261
}
251262

252263
void patch_packet(
253264
PacketTemplate& packet,
254265
const std::uint16_t sample_count,
266+
const std::uint8_t sample_synchronization,
255267
const WaveformRow& values,
256268
const SvLiveSignalState& signal_state) noexcept {
257269
write_u16_be(packet.bytes.data() + packet.sample_count_offset, sample_count);
270+
packet.bytes[packet.sample_synchronization_offset] = sample_synchronization;
258271
for (std::size_t channel = 0U; channel < values.size(); ++channel) {
259272
const auto base = packet.sample_payload_offset + channel * 8U;
260273
write_i32_be(packet.bytes.data() + base, values[channel]);
@@ -541,7 +554,11 @@ void publisher_task(void* argument) {
541554
++local.mirror_fail;
542555
#endif
543556
} else {
544-
patch_packet(canonical, canonical_sample_count, row, signal_state);
557+
const auto sync_decision = smp_synch_lab_decision();
558+
const auto sample_synchronization =
559+
static_cast<std::uint8_t>(sync_decision.value);
560+
patch_packet(
561+
canonical, canonical_sample_count, sample_synchronization, row, signal_state);
545562
const esp_err_t canonical_result =
546563
esp_eth_transmit(eth_handle, canonical.bytes.data(), canonical.bytes.size());
547564
if (canonical_result == ESP_OK) {
@@ -555,7 +572,8 @@ void publisher_task(void* argument) {
555572
}
556573

557574
#if CONFIG_AR_SMV_BROADCAST_MIRROR
558-
patch_packet(mirror, mirror_sample_count, row, signal_state);
575+
patch_packet(
576+
mirror, mirror_sample_count, sample_synchronization, row, signal_state);
559577
const esp_err_t mirror_result =
560578
esp_eth_transmit(eth_handle, mirror.bytes.data(), mirror.bytes.size());
561579
if (mirror_result == ESP_OK) ++local.mirror_ok;
Lines changed: 14 additions & 85 deletions
Original file line numberDiff line numberDiff line change
@@ -1,36 +1,20 @@
11
// SPDX-License-Identifier: GPL-3.0-or-later
22

33
#include "smp_synch_lab.hpp"
4-
#include "live_control.hpp"
5-
6-
#include "ariec61850/sampled_values/wire_field_offsets.hpp"
7-
8-
#include "esp_log.h"
94

105
#include <atomic>
11-
#include <cstddef>
126
#include <cstdint>
137
#include <optional>
14-
#include <span>
158

169
namespace ar::esp32p4::smv {
1710
namespace {
1811

19-
using ar::iec61850::sampled_values::find_smp_synch_value_offset;
2012
using ar::iec61850::time_sync::resolve_sv_sync_policy;
2113

22-
constexpr char kTag[] = "ar_smv_txguard";
23-
constexpr std::uint16_t kDiagnosticMirrorAppId = 0x4F01U;
24-
// At the production 4000 fps profile this trips after 100 ms of uninterrupted
25-
// canonical SV transport failure. Mirror failures never drive this guard.
26-
constexpr std::uint32_t kCanonicalFailureStopThreshold = 400U;
27-
2814
std::atomic<std::uint8_t> g_mode{
2915
static_cast<std::uint8_t>(SvSyncPolicyMode::external_ptp_auto)};
3016
// -1 means PTP-P2 has not supplied measured lock evidence yet.
3117
std::atomic<int> g_measured_value{-1};
32-
std::atomic<std::uint32_t> g_consecutive_canonical_tx_failures{0U};
33-
std::atomic_bool g_transport_fault_reported{false};
3418

3519
[[nodiscard]] std::optional<SmpSynchValue> measured_value() noexcept {
3620
const int raw = g_measured_value.load(std::memory_order_acquire);
@@ -46,51 +30,9 @@ std::atomic_bool g_transport_fault_reported{false};
4630
}
4731
}
4832

49-
[[nodiscard]] SmpSynchDecision current_decision() noexcept {
50-
const auto mode = static_cast<SvSyncPolicyMode>(
33+
[[nodiscard]] SvSyncPolicyMode current_mode() noexcept {
34+
return static_cast<SvSyncPolicyMode>(
5135
g_mode.load(std::memory_order_acquire));
52-
return resolve_sv_sync_policy(mode, measured_value());
53-
}
54-
55-
[[nodiscard]] bool diagnostic_mirror_frame(const std::span<const std::uint8_t> bytes) noexcept {
56-
// Untagged IEC 61850-9-2: EtherType 0x88BA, APPID immediately follows.
57-
if (bytes.size() >= 16U && bytes[12] == 0x88U && bytes[13] == 0xBAU) {
58-
const auto app_id = static_cast<std::uint16_t>(
59-
(static_cast<std::uint16_t>(bytes[14]) << 8U) | bytes[15]);
60-
return app_id == kDiagnosticMirrorAppId;
61-
}
62-
63-
// 802.1Q tagged IEC 61850-9-2: inner EtherType 0x88BA, then APPID.
64-
if (bytes.size() >= 20U && bytes[12] == 0x81U && bytes[13] == 0x00U &&
65-
bytes[16] == 0x88U && bytes[17] == 0xBAU) {
66-
const auto app_id = static_cast<std::uint16_t>(
67-
(static_cast<std::uint16_t>(bytes[18]) << 8U) | bytes[19]);
68-
return app_id == kDiagnosticMirrorAppId;
69-
}
70-
return false;
71-
}
72-
73-
void observe_canonical_tx_result(const esp_err_t result) noexcept {
74-
if (result == ESP_OK) {
75-
g_consecutive_canonical_tx_failures.store(0U, std::memory_order_release);
76-
g_transport_fault_reported.store(false, std::memory_order_release);
77-
return;
78-
}
79-
80-
const auto failures =
81-
g_consecutive_canonical_tx_failures.fetch_add(1U, std::memory_order_acq_rel) + 1U;
82-
if (failures < kCanonicalFailureStopThreshold) return;
83-
84-
// The sample clock can be perfectly healthy while the Ethernet data plane is
85-
// completely dead. Fail closed instead of leaving Studio green at
86-
// "Injection running" with txFail=4000.
87-
if (!g_transport_fault_reported.exchange(true, std::memory_order_acq_rel)) {
88-
ESP_LOGE(kTag,
89-
"Canonical SV transport fault: %lu consecutive TX failures (%s); forcing STOP",
90-
static_cast<unsigned long>(failures),
91-
esp_err_to_name(result));
92-
}
93-
live_control_force_stop();
9436
}
9537

9638
} // namespace
@@ -100,42 +42,29 @@ void smp_synch_lab_set_mode(const SvSyncPolicyMode mode) noexcept {
10042
}
10143

10244
SvSyncPolicyMode smp_synch_lab_mode() noexcept {
103-
return static_cast<SvSyncPolicyMode>(g_mode.load(std::memory_order_acquire));
45+
return current_mode();
46+
}
47+
48+
SmpSynchDecision smp_synch_lab_decision() noexcept {
49+
return resolve_sv_sync_policy(current_mode(), measured_value());
10450
}
10551

10652
SmpSynchLabStatus smp_synch_lab_status() noexcept {
10753
const auto measured = measured_value();
108-
return {current_decision(), measured.has_value()};
54+
return {
55+
resolve_sv_sync_policy(current_mode(), measured),
56+
measured.has_value(),
57+
};
10958
}
11059

11160
void smp_synch_lab_set_measured(const std::optional<SmpSynchValue> value) noexcept {
11261
if (!value.has_value()) {
11362
g_measured_value.store(-1, std::memory_order_release);
11463
return;
11564
}
116-
g_measured_value.store(static_cast<int>(static_cast<std::uint8_t>(*value)),
117-
std::memory_order_release);
118-
}
119-
120-
esp_err_t smp_synch_lab_transmit(
121-
const esp_eth_handle_t handle,
122-
void* buffer,
123-
const std::size_t length) noexcept {
124-
auto bytes = std::span<std::uint8_t>{};
125-
if (buffer != nullptr) {
126-
bytes = std::span<std::uint8_t>{static_cast<std::uint8_t*>(buffer), length};
127-
const auto value_offset = find_smp_synch_value_offset(bytes);
128-
if (value_offset.has_value()) {
129-
const auto decision = current_decision();
130-
bytes[*value_offset] = static_cast<std::uint8_t>(decision.value);
131-
}
132-
}
133-
134-
const auto result = ::esp_eth_transmit(handle, buffer, length);
135-
if (!diagnostic_mirror_frame(bytes)) {
136-
observe_canonical_tx_result(result);
137-
}
138-
return result;
65+
g_measured_value.store(
66+
static_cast<int>(static_cast<std::uint8_t>(*value)),
67+
std::memory_order_release);
13968
}
14069

14170
} // namespace ar::esp32p4::smv

‎embedded/esp32p4_smv_injector/main/smp_synch_lab.hpp‎

Lines changed: 4 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -4,10 +4,6 @@
44

55
#include "ariec61850/time_sync/smp_synch_policy.hpp"
66

7-
#include "esp_eth.h"
8-
9-
#include <cstddef>
10-
#include <cstdint>
117
#include <optional>
128

139
namespace ar::esp32p4::smv {
@@ -25,28 +21,14 @@ struct SmpSynchLabStatus final {
2521
void smp_synch_lab_set_mode(SvSyncPolicyMode mode) noexcept;
2622

2723
[[nodiscard]] SvSyncPolicyMode smp_synch_lab_mode() noexcept;
24+
[[nodiscard]] SmpSynchDecision smp_synch_lab_decision() noexcept;
2825
[[nodiscard]] SmpSynchLabStatus smp_synch_lab_status() noexcept;
2926

3027
/**
31-
* Future PTP-P2 hook. AUTO consumes this value only when the discipline/lock
32-
* engine has real measured evidence. Passing std::nullopt removes that evidence
33-
* and immediately returns AUTO to the safe smpSynch=0 default.
28+
* PTP-P2 measurement hook. AUTO consumes this value only when the
29+
* discipline/lock engine has real measured evidence. Passing std::nullopt
30+
* removes that evidence and immediately returns AUTO to smpSynch=0.
3431
*/
3532
void smp_synch_lab_set_measured(std::optional<SmpSynchValue> value) noexcept;
3633

37-
/**
38-
* Transmit shim used only by app_main.cpp. It patches the already-encoded SV
39-
* smpSynch TLV immediately before TX, allowing live lab transitions without
40-
* rebuilding the deterministic packet template. Non-SV frames are untouched.
41-
*/
42-
esp_err_t smp_synch_lab_transmit(
43-
esp_eth_handle_t handle,
44-
void* buffer,
45-
std::size_t length) noexcept;
46-
4734
} // namespace ar::esp32p4::smv
48-
49-
#if defined(AR_SMP_SYNCH_INTERCEPT_TX) && AR_SMP_SYNCH_INTERCEPT_TX
50-
#define esp_eth_transmit(handle, buffer, length) \
51-
::ar::esp32p4::smv::smp_synch_lab_transmit((handle), (buffer), (length))
52-
#endif

‎include/ariec61850/sampled_values/frame_codec.hpp‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -3,6 +3,7 @@
33
#pragma once
44

55
#include "ariec61850/sampled_values/frame.hpp"
6+
#include "ariec61850/sampled_values/pdu_codec.hpp"
67
#include "ariec61850/wire/encode_result.hpp"
78

89
#include <cstddef>
@@ -22,6 +23,13 @@ class SampledValuesFrameCodec final {
2223
const SampledValuesFrame& frame,
2324
std::span<std::uint8_t> destination) noexcept;
2425

26+
// Layout offsets returned here are absolute offsets into the encoded
27+
// Ethernet frame. Empty layout span disables metadata collection.
28+
[[nodiscard]] static wire::EncodeResult encode_into_with_layout(
29+
const SampledValuesFrame& frame,
30+
std::span<std::uint8_t> destination,
31+
std::span<SampledValueAsduEncodeLayout> layouts) noexcept;
32+
2533
// Host convenience wrapper. Embedded SV publishers should use encode_into
2634
// with a persistent Ethernet-sized buffer.
2735
[[nodiscard]] static std::vector<std::uint8_t> encode(const SampledValuesFrame& frame);

0 commit comments

Comments
 (0)