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;
4243using ar::esp32p4::smv::runtime_profile_initialize;
4344using ar::esp32p4::smv::runtime_profile_snapshot;
4445using ar::esp32p4::smv::runtime_profile_validate;
46+ using ar::esp32p4::smv::smp_synch_lab_decision;
4547using ar::esp32p4::smv::take_start_request;
4648using ar::iec61850::ethernet::MacAddress;
4749using ar::iec61850::ethernet::VlanTag;
4850using ar::iec61850::sampled_values::SampledValueAsdu;
51+ using ar::iec61850::sampled_values::SampledValueAsduEncodeLayout;
4952using ar::iec61850::sampled_values::SampledValuesFrame;
5053using ar::iec61850::sampled_values::SampledValuesFrameCodec;
5154using ar::iec61850::sampled_values::SampledValuesPdu;
@@ -86,6 +89,7 @@ std::atomic<std::uint32_t> g_sample_tick_total{0U};
8689struct 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-
221211bool 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
252263void 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 ;
0 commit comments