//This file was generated by the LASAL2 CodeGenerator  -- 
//Please, do not edit this file (it might be overwritten by the next generator run)

//{{LSL_DEFINES
#define SM_LogBufferSize 1000
#define SM_MaxObjNameLen  256
//}}LSL_DEFINES

//{{LSL_DECLARATION
#include "..\..\Class\SafetyManager\SafetyManager.h"
#include "..\..\Class\HwControl\HwControl.h"
#include "..\..\Source\interfaces\lsl_st_hardwaretree.h"
#include "..\..\Source\interfaces\lsl_st_safetydll.h"
#include "..\..\Source\interfaces\lsl_st_systrace.h"

(*!
<Class
	Name               = "SafetyManager"
	Revision           = "2.28"
	GUID               = "{186EBAC0-5F71-4A3F-BA36-ED9EA68EA059}"
	RealtimeTask       = "false"
	CyclicTask         = "false"
	DefCyclictime      = "10 ms"
	BackgroundTask     = "false"
	Sigmatek           = "true"
	OSInterface        = "false"
	HighPriority       = "false"
	Automatic          = "false"
	UpdateMode         = "Prescan"
	IconPath           = ".\Class\Safety.ico"
	SharedCommandTable = "true"
	Objectsize         = "(646,120)"
	Comment            = "Class to handle:&#13;&#10; - safety communication between domains&#13;&#10; - time synchronisation of safety modules">
	<Channels>
		<Server Name="ClassState" Comment="Shows state of safety functionality initialisation&#13;&#10;&#13;&#10; 1..OK&#13;&#10; 0..not active or initializing&#13;&#10;-1..failed to allocate memory&#13;&#10;-2..invalid domain number set in this system&#13;&#10;-3..newer OS version required for domain number support&#13;&#10;-4..method to determine domain number not provided by lsl_st_hardwaretree.h&#13;&#10;-5..only 1 instance of safety manager allowed&#13;&#10;-6..a slave system can only have 1 master. check hw class configuration&#13;&#10;-7..invalid module type tried to log in&#13;&#10;-8..couldn&apos;t find InterfaceCRC which is requested by other domain. Check SafetyDesigner projects, most likely the interface variables doesn&apos;t match.&#13;&#10;-9..failed to add a new copy data object to varan manager&#13;&#10;-10..a non-optional module or domain is not available&#13;&#10;-11..timeout in initialisation (see debugger trace or logfile for further details)&#13;&#10;-12..invalid interface domain configuration (no safety module found, only interface domains)&#13;&#10;-13..failed to install callbacks for cyclic and realtime task">
		</Server>
		<Server Name="LoginCnt" GUID="{5FC5C929-C7F8-4BFB-8DF3-09B84403273D}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Server Name="MaxSyncDuration" GUID="{7517B303-9F59-4B9A-AD1A-8766B8C07194}" Visualized="true" Initialize="false" WriteProtected="false" Retentive="false" Comment="Shows maximum value of server SyncDuration"/>
		<Server Name="MinSyncDuration" GUID="{697A1FC7-1D68-4539-AE59-3800B4CF8373}" Visualized="true" Initialize="false" WriteProtected="false" Retentive="false" Comment="Shows minimum value of server SyncDuration"/>
		<Server Name="SyncDuration" GUID="{58439908-A73E-4D89-8216-1483D81B8572}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Time of last complete synchronisation cycle in ms (only the Master has this information)"/>
		<Client Name="SafetyDomainTimeout" Required="false" Internal="false" Comment="[ms] the class has to wait for optional interface domains (leave 0 if none available)&#13;&#10;Set this client to 1 when using FlexCon functionality (without interface domains) where it&apos;s possible that not all participants are available at all times"/>
		<Client Name="To_HwControl" Required="true" Internal="false"/>
		<Client Name="To_MultiTask" Required="false" Internal="false"/>
		<Client Name="To_OSKernel" Required="false" Internal="false"/>
		<Client Name="UdpReceiveData" Required="true" Internal="true"/>
		<Client Name="UdpSafetyRequestCycle" Required="false" Internal="false" DefValue="UDP_MIN_REQUEST_TIME" Comment="Extended setting which may only be made in consultation with Sigmatek! &#13;&#10;The UDP Safety Requests are made every x-th cycle (client setting default value = 10) from the read to the write interface. The current time interval in which requests are sent is displayed in the variable sUdpVars.ActTimeRequestSheduler.  If, for example, the processing time is 1.3 ms and the default value of 10 is entered at the client, then the time interval at which requests are sent is ~13 ms. Depending on the processing time, the response timeout on the side of the write interface may have to be adjusted.&#13;&#10;The minimum value is 10 (UDP_MIN_REQUEST_TIME) and the maximum value is sUdpVars.CalculatedRequestTimeout/2."/>
		<Client Name="UdpSafetyResponseTimeout" Required="false" Internal="false" DefValue="UDP_MIN_RESPONSE_TIMEOUT" Comment="Extended setting which may only be made in consultation with Sigmatek!&#13;&#10;With this client the UDP Safety Response Timeout can be changed. The client default value is 100ms. The increase of the Response Timeout at the Write Interface is necessary if the Request Time of the Read Interface (see UdpSafetyRequestCycle) has been increased. This prevents unnecessary response retries."/>
		<Client Name="UdpSendData" Required="true" Internal="true"/>
	</Channels>
	<Dependencies>
		<Files>
			<File Path=".\Class\SafetyManager\SafetyManager.h" Include="true"/>
			<File Path=".\Class\HwControl\HwControl.h" Include="true"/>
			<File Path=".\Source\interfaces\lsl_st_hardwaretree.h" Include="true"/>
			<File Path=".\Source\interfaces\lsl_st_safetydll.h" Include="true"/>
			<File Path=".\Source\interfaces\lsl_st_systrace.h" Include="true"/>
		</Files>
	</Dependencies>
	<RevDoku>
		<Owner Company="Sigmatek" Author="RamAnd"/>
		<Dokumentation Revision="2.28" Date="27.09.2024" Author="RamAnd" Company="Sigmatek" Description="SafetyDomainTimeout now also sets the timeout to wait for optional UDP interface frames.&#13;&#10;Mapping of PDOs in the VBC/VMC memory could have been wrong which would prevent the routing of the safety PDOs.&#13;&#10;Prevented access to uninitialized pointer when an optional reading interface frame via VBC/VMC was not available that could have lead to a crash.&#13;&#10;Prevent sending the current interface frame configuration to the counterparts too fast which could cause problems with the SDO buffer.&#13;&#10;The startup of multiple stations connected via VBC/VMC has been reworked to speed up the connection establishment.&#13;&#10;The number of possible internal interface frames has been increased from 10 to 100.&#13;&#10;The maximum number of interface frame informations from other stations has been increased from 150 to 500."/>
		<Dokumentation Revision="2.27" Date="11.09.2024" Author="RamAnd" Company="Sigmatek" Description="Corrected collecting module information of safety interface frame communication partners that could have lead to a wrong restriction of the number of possible modules."/>
		<Dokumentation Revision="2.26" Date="30.09.2022" Author="RamAnd" Company="Sigmatek" Description="The busy state of the safety CPU wasn&apos;t detected correctly when establishing UDP connections."/>
		<Dokumentation Revision="2.25" Date="08.07.2022" Author="RamAnd" Company="Sigmatek" Description="Try again to establish a safety UDP connection if the attempt failed because the safe CPU was busy."/>
		<Dokumentation Revision="2.24" Date="22.03.2022" Author="RamAnd" Company="Sigmatek" Description="Added support for the new OS Interface to quit errors"/>
		<Dokumentation Revision="2.23" Date="08.03.2022" Author="RamAnd" Company="Sigmatek" Description="Added Drive Plug &amp; Play (FSoE connections are being established without reinitializing the complete Safety infrastructure)"/>
		<Dokumentation Revision="2.21" Date="17.01.2022" Author="EisMic" Company="Sigmatek" Description="Testversion: Added newinst commando CMD_HWC_CONVERT_COMPRESSED_HW_PATH which can be called via HwControl.&#13;&#10;This commando can be used to hand over the compressed hw path and returns the uncompressed hw path."/>
		<Dokumentation Revision="2.20" Date="30.11.2021" Author="EisMic" Company="Sigmatek" Description="Cyclic initialisation now waits until all local modules without config which are not optional are available."/>
		<Dokumentation Revision="2.19" Date="23.10.2020" Author="PieSte" Company="Sigmatek" Description="Added support for new blocktype and extended routing of FSoE data for this type.&#13;&#10;Corrected allocate of udp buffers at &quot;AddModule()&quot;.&#13;&#10;Don&apos;t Log AddDynSlave error if RetCode is zero at GetDestSafetyOfDO() Methode."/>
		<Dokumentation Revision="2.18" Date="02.03.2020" Author="FurDom" Company="Sigmatek" Description="Reworked to support newer CPUs (address space &gt; 0x80000000). Needs SafetyUDP class version &gt;= 1.4!"/>
		<Dokumentation Revision="2.17" Date="18.07.2019" Author="RamAnd" Company="Sigmatek" Description="Corrected that SDO forwarding would not work properly in some cases.&#13;&#10;Prevent writing to an invalid pointer when resetting UDP connections that could have lead to a crash."/>
		<Dokumentation Revision="2.16" Date="25.06.2019" Author="RamAnd" Company="Sigmatek" Description="Added support for FlexCon functionality."/>
		<Dokumentation Revision="2.15" Date="14.03.2019" Author="RamAnd" Company="Sigmatek" Description="Removed limitations of formerly unallowed safety topologies (since v2.0)"/>
		<Dokumentation Revision="2.14" Date="25.01.2019" Author="PieSte" Company="Sigmatek" Description="The Sequence Number for Udp Safety communication has been extended from 1 to 4 bytes. This increases the availability of the Udp Safety connection. Furthermore two clients have been added to set the Request Cycle (Default 10) and the Response Timeout (Default 100ms) to optimize the Safety UDP bus load. This setting may only be changed after consultation with Sigmatek.  (SA43141)"/>
		<Dokumentation Revision="2.13" Date="18.10.2018" Author="LanSte" Company="Sigmatek" Description="Added support for custom FSoE Connections."/>
		<Dokumentation Revision="2.12" Date="24.08.2018" Author="PieSte" Company="Sigmatek" Description="Behaviour of safety udp communication when resetting the bSequenceCntReceived flag after maximum of retries corrected."/>
		<Dokumentation Revision="2.11" Date="05.07.2018" Author="PieSte&#13;&#10;EisMic" Company="Sigmatek" Description="Added support for HGW1033."/>
		<Dokumentation Revision="2.10" Date="27.03.2018" Author="EisMic" Company="Sigmatek" Description="Implemented New-Inst command CMD_POST_INIT_DISTRIBUTE_TASKTIMES which is called from HwControl."/>
		<Dokumentation Revision="2.9" Date="06.03.2018" Author="EisMic" Company="Sigmatek" Description="Attached header files no longer point to the installation directory of LASAL but instead now point to the the source/interfaces folder."/>
		<Dokumentation Revision="2.8" Date="23.01.2018" Author="RamAnd" Company="Sigmatek" Description="Log messages have been standardized for easier maintenance, easier use in any class and to allow spam filtering. Code for the logging functions is now only located in HwControl."/>
		<Dokumentation Revision="2.7" Date="05.01.2018" Author="PieSte" Company="Sigmatek" Description="The safety UDP communication has been made more reliable. Now only new packages are allowed and old packages are discarded."/>
		<Dokumentation Revision="2.6" Date="22.08.2017" Author="RamAnd" Company="Sigmatek" Description="Fixed an error which prevented two SCP111s on the same PLC from exchanging interfaceframes in both directions."/>
		<Dokumentation Revision="2.5" Date="22.06.2017" Author="RamAnd" Company="Sigmatek" Description="If a disconnect happens in a multi master system with safety communication (more than 2 systems connected and exchanging safety data) now the other masters are notified about it to reestablish the connection."/>
		<Dokumentation Revision="2.4" Date="28.02.2017" Author="EisMic" Company="Sigmatek" Description="Corrected an error in the handling of optional modules."/>
		<Dokumentation Revision="2.3" Date="28.01.2016" Author="RamAnd" Company="Sigmatek" Description="Support for FSoE via IP added."/>
		<Dokumentation Revision="2.2" Date="06.06.2016" Author="EisMic" Company="Sigmatek" Description="Corrected error if distributed safety modules are connected after the initialisation phase."/>
		<Dokumentation Revision="2.1" Date="11.05.2016" Author="RamAnd" Company="Sigmatek" Description="Suppressed warning that would occur with target ARM on compiler version 30 or higher."/>
		<Dokumentation Revision="2.0" Date="30.11.2015" Author="ZoePat&#13;&#10;LanSte" Company="Sigmatek" Description="Check for unallowed safety topology."/>
		<Dokumentation Revision="1.48" Date="25.09.2014" Author="RamAnd" Company="Sigmatek" Description="Support for MS012 added"/>
		<Dokumentation Revision="1.47" Date="19.05.2015" Author="RamAnd" Company="Sigmatek" Description="If two systems with FSoE safety were connected via VBC/VMC an error in the connection establishment could cause the FSoE connection to fail."/>
		<Dokumentation Revision="1.46" Date="15.04.2015" Author="RamAnd" Company="Sigmatek" Description="Added another step in initialisation to prevent watchdog error."/>
		<Dokumentation Revision="1.45" Date="16.10.2014&#13;&#10;25.03.2015" Author="RamAnd&#13;&#10;PieSte" Company="Sigmatek" Description="Support for SCP111 added.&#13;&#10;Support for SDD1300, SDD1400, SDD1500 and SDD1600 added."/>
		<Dokumentation Revision="1.44" Date="30.07.2014" Author="LanSte" Company="Sigmatek" Description="Added Support to ignore hardware diagnose and hardwaretree entry commands."/>
		<Dokumentation Revision="1.43" Date="17.06.2014" Author="RamAnd" Company="Sigmatek" Description="Added possibility to start a quit error via OS interface.&#13;&#10;Corrected algorithm for finding SDO target at interface frame communication."/>
		<Dokumentation Revision="1.42" Date="14.04.2014" Author="RamAnd" Company="Sigmatek" Description="Prevent crash if the safety configs are not available at startup."/>
		<Dokumentation Revision="1.41" Date="31.03.2014" Author="LanSte" Company="Sigmatek" Description="Improved Error checking and Logging."/>
		<Dokumentation Revision="1.40" Date="05.12.2013" Author="RamAnd" Company="Sigmatek" Description="Removed domain support&#13;&#10;Support for SCP011 added"/>
		<Dokumentation Revision="1.30" Date="18.07.2013" Author="RamAnd" Company="Sigmatek" Description="Updated LogValue method to show also the actual object name."/>
		<Dokumentation Revision="1.20" Date="07.03.2013" Author="RamAnd&#13;&#10;RamAnd&#13;&#10;ZoePat" Company="Sigmatek" Description="Changed range of the safety NewInst commands. Range for HW-Classes: 0x8000-0x8FFF; Range for Safety HW-Classes: 0x8100-0x81FF&#13;&#10;Corrected order of managing time sync elements. It could have happened, that a subdomain returns the token to the master before sending it to their subdomains.&#13;&#10;Corrected minimal length of configuration."/>
		<Dokumentation Revision="1.10" Date="22.01.2013" Author="ZoePat&#13;&#10;RamAnd&#13;&#10;RamAnd&#13;&#10;RamAnd&#13;&#10;RamAnd" Company="Sigmatek" Description="Support for SCP010 added&#13;&#10;Corrected error in unsafe variable handling if a new safety project is downloaded, while the hardware classes are running.&#13;&#10;Send messages only to managing modules which are synchron.&#13;&#10;Added functionality for domain internal interface frames.&#13;&#10;Fixed memory leak in recreating the time sync list."/>
		<Dokumentation Revision="1.0" Date="03.09.2012" Author="RamAnd" Company="Sigmatek" Description="Initial version"/>
	</RevDoku>
	<Network Name="SafetyManager">
		<!-- List of Components in this network -->
		<Components>
			<Object
				Name       = "_base"
				GUID       = "{3632CBBA-7E99-4714-B216-B99B262A49F9}"
				Class      = "SafetyUDP"
				Position   = "(218,120)"
				Visualized = "true">
				<Channels>
					<Server Name="ClassState"/>
					<Server Name="InitFault"/>
					<Server Name="InitSSM"/>
					<Server Name="sControl"/>
					<Server Name="sErrorCount"/>
					<Server Name="sLastErrorMessage"/>
					<Server Name="sLastErrorNoOS"/>
					<Server Name="sTReceive"/>
					<Client Name="InterfaceNo"/>
					<Client Name="To_StdLib"/>
					<Client Name="UdpSafetyPort" Value="10823"/>
				</Channels>
			</Object>
			<Object
				Name       = "UdpReceiveData"
				GUID       = "{AEB21C96-66F3-41BB-AF8B-4D8748F3E760}"
				Class      = "CriticalSection"
				Position   = "(330,870)"
				Visualized = "false">
				<Channels>
					<Server Name="ClassSvr"/>
				</Channels>
			</Object>
			<Object
				Name       = "UdpSendData"
				GUID       = "{4D3B3492-CD23-4D10-950E-7B1E0CD8A442}"
				Class      = "CriticalSection"
				Position   = "(330,720)"
				Visualized = "false">
				<Channels>
					<Server Name="ClassSvr"/>
				</Channels>
			</Object>
		</Components>
		<Comments>
		</Comments>
		<!-- List of Connections in this network -->
		<Connections>
			<Connection Source="this.ClassState" Destination="_base.ClassState" Vertices="(920,210),(748,210),"/>
			<Connection Source="this.sControl" Destination="_base.sControl" Vertices="(920,270),(748,270),"/>
			<Connection Source="this.sErrorCount" Destination="_base.sErrorCount" Vertices="(920,330),(748,330),"/>
			<Connection Source="this.sLastErrorMessage" Destination="_base.sLastErrorMessage" Vertices="(920,390),(748,390),"/>
			<Connection Source="this.sLastErrorNoOS" Destination="_base.sLastErrorNoOS" Vertices="(920,450),(748,450),"/>
			<Connection Source="_base.UdpSafetyPort" Destination="this.UdpSafetyPort" Vertices="(218,270),(38,210),"/>
			<Connection Source="this.InitSSM" Destination="_base.InitSSM" Vertices="(920,510),(748,510),"/>
			<Connection Source="this.InitFault" Destination="_base.InitFault" Vertices="(920,570),(748,570),"/>
			<Connection Source="this.sTReceive" Destination="_base.sTReceive" Vertices="(920,630),(748,630),"/>
			<Connection Source="_base.InterfaceNo" Destination="this.InterfaceNo" Vertices="(218,330),(38,270),"/>
			<Connection Source="this.UdpSendData" Destination="UdpSendData.ClassSvr"/>
			<Connection Source="this.UdpReceiveData" Destination="UdpReceiveData.ClassSvr"/>
		</Connections>
		<!-- Headerfiles -->
		<Options>
		</Options>
	</Network>
</Class>
*)
#pragma using SafetyUDP

SafetyManager : CLASS
: SafetyUDP
	TYPE
#pragma pack(push, 1)
	  t_AddDynSlaveRsp : STRUCT
	    Header : SafetyCDIAS_Base::SDOHeader;
	    Retcode : USINT;
	    ConnectionIndex : UINT;
	    SafeInOffset : UINT;
	    SafeInLength : UINT;
	    SafeOutOffset : UINT;
	    SafeOutLength : UINT;
	    CommunicationID : UINT;
	  END_STRUCT;
#pragma pack(pop)
	  t_UdpConInfoBits : BSINT
	  [
	    1 IsOptional,
	    2 OptionalValid,
	    3 PdosInitialized,
	    8 Disconnecting,
	  ];
#pragma pack(push, 1)
	  t_UdpConnectionBuffer : STRUCT
	    udSequenceNumber : UDINT;
	    usSequenceNumber : USINT;
	    PdoLength : UINT;
	    PdoData : ARRAY [0..UDP_MAX_PDO_SIZE-1] OF USINT;
	    SdoLength : USINT;
	    SdoData : ARRAY [0..UDP_MAX_SDO_SIZE-1] OF USINT;
	    usHeaderVersion : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpConnection : STRUCT
	    IP : HDINT;
	    IFCRC : HDINT;
	    SafetyNbr : HDINT;
	    SafetyNbrOtherSide : HDINT;
	    InfoBits : t_UdpConInfoBits;
	    WaitForResponse : BOOL;
	    SendResponse : BOOL;
	    MaxRetryCnt : UDINT;
	    TLastRequest : UDINT;
	    LastResponseTime : UDINT;
	    MaxResponseTime : UDINT;
	    RequestRetries : UDINT;
	    SentPackages : UDINT;
	    ReceivedPackages : UDINT;
	    ActSendData : t_UdpConnectionBuffer;
	    ActReceiveData : t_UdpConnectionBuffer;
	    ThreadSafePdoReceiveBuffer : ARRAY [0..UDP_MAX_PDO_SIZE-1] OF USINT;
	    bSequenceCntReceived : BOOL;
	    TLastValidRequest : UDINT;
	    bs_SingleLog : BSINT
	    [
	      1 OldHeaderVersionOfResponseIF,
	      2 OldHeaderVersionOfRequestIF,
	      3 Bit3,
	      4 Bit4,
	      5 Bit5,
	      6 Bit6,
	      7 Bit7,
	      8 Bit8,
	    ];
	  END_STRUCT;
#pragma pack(pop)
	  t_aUdpConnections : ARRAY [0..MAX_UDP_IF_ALLOWED-1] OF t_UdpConnection;
	  t_ClearRoutingInfoSSW :
	  (
	    _PrepareUdpDisconnect,
	    _SendDisconnect,
	    _Wait4DisconnectConfirm:=2,
	    _FinishEverythingElse:=3
	  )$UDINT;
#pragma pack(push, 1)
	  t_CopyCmdDO : STRUCT
	    pHandle : ^LSL_VARANFRAME;
	    pSettings : ^t_VaranDOSettingsMov;
	    b_Ready4Action : BOOL;
	  END_STRUCT;
#pragma pack(pop)
	  t_FeatureEnable : BDINT
	  [
	    1 TransmissionTimeInConfig,
	    2 FSoEFlags_4ByteSize,
	  ];
	  t_FlexConRole :
	  (
	    _NoFlexCon,
	    _FlexConHost,
	    _FlexConNode,
	    _InvalidConstellation
	  )$UDINT;
#pragma pack(push, 1)
	  t_ModuleInfo : STRUCT
	    SafetyNbr : HDINT;
	    InfoBits : BSINT
	    [
	      1 IsFSoEMaster,
	      2 IsFSoESlave,
	    ];
	    Reserved : ARRAY [0..2] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_FSoECopyCmd : STRUCT
	    pSource : pVoid;
	    pDest : pVoid;
	    Size : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_SafetyComponent : STRUCT
	    ModuleType : t_e_ModuleType;
	    ObjName : ARRAY [0..SM_MaxObjNameLen-1] OF CHAR;
	    pThis : pVirtualBase;
	    SafetyNbr : HDINT;
	    FW_Version : HDINT;
	    FPGA_Version : HSINT;
	    NewConfigType : t_ConfigStateType;
	    InfoBits : BSINT
	    [
	      1 OnlyInputs,
	      2 IsAddressService,
	      6 IsOptional,
	    ];
	    PathLen : USINT;
	    pPath : ^USINT;
	    pNext : ^t_SafetyComponent;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_InputPDOs : STRUCT
	    pSafety : ^t_SafetyComponent;
	    pMem : DINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_ManagingFlexCon : STRUCT
	    SafetyNumber : HDINT;
	    pFSoEConnection : ^t_FSoEConnectionDetails;
	    pCopyCmd : ^t_FSoECopyCmd;
	    pInputPdo : ^t_InputPDOs;
	    ReadOffset : UINT;
	    ReadLen : UINT;
	    WriteOffset : UINT;
	    WriteLen : UINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_ManagingComponent : STRUCT
	    ModuleType : t_e_ModuleType;
	    ObjName : ARRAY [0..SM_MaxObjNameLen-1] OF CHAR;
	    pThis : pVirtualBase;
	    InfoBits : BINT
	    [
	      1 HasBeenFlushed,
	      2 NewTimeSyncPossible,
	      3 IsSynchron,
	      4 SubDomainsSynchron,
	      5 GotIFInfo,
	      6 IsOptional,
	      8 IsConnectionToMaster,
	      9 LocalConfigReady,
	      10 IncompleteFSoEConnections,
	    ];
	    DomainNr : USINT;
	    ModuleNo : USINT;
	    ModuleInfo : ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_ModuleInfo;
	    FlexCon : t_ManagingFlexCon;
	    PDOWriteMemUsed : UDINT;
	    PDOWriteMemAvailable : UDINT;
	    pPDOWriteSpace : ^USINT;
	    pPDOWriteSpaceDPRAM : ^USINT;
	    ud_HandlePDOWriteSpaceDPRAM : UDINT;
	    PDOWriteSpaceDPRAM : DINT;
	    PDOReadMemUsed : UDINT;
	    PDOReadMemAvailable : UDINT;
	    pPDOReadSpace : ^USINT;
	    pPDOReadSpaceDPRAM : ^USINT;
	    ud_HandlePDOReadSpaceDPRAM : UDINT;
	    pModulePath : ^USINT;
	    pNext : ^t_ManagingComponent;
	  END_STRUCT;
#pragma pack(pop)
	  t_pManagingComponent : ^t_ManagingComponent;
#pragma pack(push, 1)
	  t_FlexCon : STRUCT
	    Role : t_FlexConRole;
	    pManaging2Host : t_pManagingComponent;
	    pSafetyHost : ^t_SafetyComponent;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_IFConnectionInfo : STRUCT
	    Flags : t_FSoEFlags;
	    SlaveAddress : UINT;
	    SafetyNbr : HDINT;
	    MasterReadSize : UINT;
	    MasterWriteSize : UINT;
	    ConnectionID : UINT;
	    InterfaceCRC : HDINT;
	    FollowUps : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_FSoEConnInfo : STRUCT
	    FromOtherSide : t_IFConnectionInfo;
	    pManaging : ^t_ManagingComponent;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_GenericDynamicComponent : STRUCT
	    ModuleType : t_e_ModuleType;
	    ObjName : ARRAY [0..SM_MaxObjNameLen-1] OF CHAR;
	    pThis : pVirtualBase;
	    pModulePath : ^USINT;
	    pNext : ^t_GenericDynamicComponent;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_InterfaceFrameInfo : STRUCT
	    ModuleInfo : t_ModuleInfo;
	    InterfaceCRC : HDINT;
	    IFPdoLen : USINT;
	    OffsetInDomainModule : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_IFInfo : STRUCT
	    IsResponse : BOOL;
	    aModuleInfos : ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_InterfaceFrameInfo;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_IFInfoFSoE : STRUCT
	    IsResponse : BOOL;
	    aConnectionInfos : ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_FSoEConnInfo;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_IFInfoFSoESend : STRUCT
	    IsResponse : BOOL;
	    aConnectionInfos : ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_IFConnectionInfo;
	  END_STRUCT;
#pragma pack(pop)
	  t_InitSSW :
	  (
	    _NotMaster,
	    _ClearRoutingInfos:=5,
	    _FirstWait4SynchronManagingNodes,
	    _FirstWait4SynchronSubdomains,
	    _SendDomainRoutingInfo,
	    _SetConnectionOk,
	    _Wait4ReInit:=10,
	    _CheckLocalCPUConfigs:=13,
	    _Wait4FSoEConnectionsLocal:=14,
	    _Wait4SynchronManagingNodes:=15,
	    _Wait4SynchronSubdomains:=20,
	    _Wait4LocalInit:=32,
	    _Wait4CPUConfigs:=35,
	    _SendInterfaceConfigs:=40,
	    _Wait4InterfaceConfigs:=45,
	    _Wait4FSoEConnectionsExternal:=47,
	    _SendNextUnsafeConfig:=50,
	    _Wait4UnsafeConfigAck:=55,
	    _SendNextPDO:=60,
	    _Wait4PDOAcks:=65,
	    _Wait4CopyCmdStart:=70,
	    _NoDomains:=75,
	    _FinishInitialisation:=80,
	    _InitFinished:=85
	  )$USINT;
#pragma pack(push, 1)
	  t_InterfaceFrameInfo_2 : STRUCT
	    Info : t_InterfaceFrameInfo;
	    pThisManaging : pVirtualBase;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_LocalIFs : STRUCT
	    InterfaceCRC : HDINT;
	    pSafetyModule : ^t_SafetyComponent;
	    MasterSafetyNbr : HDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_LogBuf : STRUCT
	    OldState : t_InitSSW;
	    NewState : t_InitSSW;
	    Command : t_AsyCMDType;
	    TimeSinceLastLog_us : UDINT;
	  END_STRUCT;
#pragma pack(pop)
	  t_LogBufCmd :
	  (
	    Cmd_None:=0,
	    Cmd_StartTimeSync:=10,
	    Cmd_SetNewSyncMode:=20,
	    Cmd_InitFinished:=21,
	    Cmd_SDO2Module:=22,
	    Cmd_TDO2Module:=23,
	    Cmd_GetSafetyNr:=24,
	    Cmd_AddInputPDO:=25,
	    Cmd_AddOutputPDO:=26,
	    Cmd_RemoveInputPDO:=27,
	    Cmd_SyncCheck:=30,
	    Cmd_IFPossible:=31,
	    Cmd_PDO_MemUsageWr:=32,
	    Cmd_PDO_MemUsageRd:=33,
	    Cmd_PDO_CopyMode:=34,
	    Cmd_TimeSyncFinished:=40,
	    Cmd_NewSafetyProject:=41,
	    Cmd_AddModule:=50,
	    Cmd_AddModuleInfo:=51,
	    Cmd_SDO2Manager:=52,
	    Cmd_TDO2Manager:=53,
	    Cmd_CPU_Cfg2Manager:=54,
	    Cmd_UnsafeCfg2Manager:=55,
	    Cmd_NewCfgType:=56,
	    Cmd_Disconnect:=60,
	    Cmd_AddManagingInfo:=61,
	    Cmd_TDO_TokenFwd:=70,
	    Cmd_DomainNrSend:=71,
	    Cmd_DomainNrFwd:=72,
	    Cmd_StopAndFlushCfg:=73,
	    Cmd_StoppedAndFlushed:=74,
	    Cmd_SpreadCfgAndStart:=75,
	    Cmd_SubdomainsSynchron:=76,
	    Cmd_CPU_Cfg2MasterMng:=77,
	    Cmd_PDO_MappingBroadcast:=78,
	    Cmd_PDO_MappingAck:=79,
	    Cmd_PDO_SafetyNr2Dom:=80,
	    Cmd_SDO_InitInformation:=81,
	    Cmd_ToP_SDO2Master:=82,
	    Cmd_ToP_SDO2Module:=83,
	    Cmd_SendUnsafeVarCfg:=84,
	    Cmd_UnsafeVarCfgAck:=85,
	    Cmd_UnsafeVarNewValue:=86,
	    Cmd_SendSubdomainSync:=87,
	    Cmd_IF_ModuleInfo:=90
	  )$UINT;
	  t_LogMsgs : BDINT
	  [
	    1 NoTDOTargetToManager,
	    2 NoTDOTargetToModule,
	    3 InvalidCommandReceived,
	    4 UnkownCopySource,
	    5 InvalidIFConfiguration,
	    6 TooManyOptionalMissing,
	    7 UdpReceivedInvalidCommand,
	    8 UdpReceivedInvalidState,
	    9 UdpNoMem4Connections,
	  ];
#pragma pack(push, 1)
	  t_PDOModuleInfo : STRUCT
	    pPDOMem : ^USINT;
	    pPDOMemDPRAM : ^USINT;
	    SafetyNbr : HDINT;
	    DomainNr : USINT;
	    PathLen : UINT;
	    pPath : ^USINT;
	    HandlePDODPRAM : UDINT;  //! <Type Comment="Handle of the VARAN DO. &#13;&#10;Needed to be passed through with Payload Support because the Handle can not be derived from pPDOMemDPRAM in this case.&#13;&#10;The Handle is needed to start/stop the DO and for creating the Payload Move DO." Name="t_PDOModuleInfo.HandlePDODPRAM"/>
	    OffsetPDODPRAM : UDINT;  //! <Type Comment="Offset the VARAN DO. &#13;&#10;Needed for creating the Payload Move DO." Name="t_PDOModuleInfo.OffsetPDODPRAM"/>
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_PDOElement : STRUCT
	    PDO_ID : USINT;
	    Length : USINT;
	    ThisDomainsRole : t_SourceDest;
	    SourceInfo : t_PDOModuleInfo;
	    DestInfo : t_PDOModuleInfo;
	    pSafety : ^t_SafetyComponent;
	    CopyCmdIndex : INT;
	    pManagingSource : ^t_ManagingComponent;
	    pManagingDest : ^t_ManagingComponent;
	  END_STRUCT;
#pragma pack(pop)
	  t_pUdpConnection : ^t_UdpConnection;
#pragma pack(push, 1)
	  t_RemDynSlaveRsp : STRUCT
	    Header : SafetyCDIAS_Base::SDOHeader;
	    Retcode : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_SDOLog : STRUCT
	    SDOCmd : t_AsyCMDType;
	    Cmd : USINT;
	    IsResponse : BOOL;
	  END_STRUCT;
#pragma pack(pop)
	  t_SingleLogMng : BDINT
	  [
	    1 SendUdpRspRetryFailed,
	    2 SendUdpRspFailed,
	    3 SendUdpReqRetryFailed,
	    4 SendUdpReqFailed,
	    5 SetUdpRequestCycleFailed,
	    6 SetUdpResponseTimeoutFailed,
	    7 NoOptionalTimeoutSetUDP,
	    8 NoOptionalTimeoutSetVxC,
	  ];
	  t_TimeSyncInfoBits : BSINT
	  [
	    1 IsConnectionToMaster,  //! <Type Comment="if the this-pointer matches the this-pointer of the master connection" Name="t_TimeSyncInfoBits.IsConnectionToMaster"/>
	    2 IsManagingModule,
	    3 IsInterfaceDomain,
	    4 IsMissingOptional,
	  ];
#pragma pack(push, 1)
	  t_TimeSyncElement : STRUCT
	    pThis : pVirtualBase;
	    InfoBits : t_TimeSyncInfoBits;
	    pManaging : ^t_ManagingComponent;
	    pSafety : ^t_SafetyComponent;
	    pPrev : ^t_TimeSyncElement;
	    pNext : ^t_TimeSyncElement;
	  END_STRUCT;
#pragma pack(pop)
	  t_UdpCommand :
	  (
	    UdpCmd_Invalid,
	    UdpCmd_DataReq,
	    UdpCmd_DataRsp,
	    UdpCmd_ReInitialize,
	    UdpCmd_ReInitConfirm
	  )$USINT;
	  t_UdpFrameState : BSINT
	  [
	    1 HeaderVersionInfo1,
	    2 HeaderVersionInfo2,
	    3 HeaderVersionInfo3,
	    4 HeaderVersionInfo4,
	    7 IsFromConsumer,
	    8 IsFromProducer,
	  ];
	  t_UdpInit : BSINT
	  [
	    1 FSoEAvailable,
	    2 ProvidesIF,
	    3 CallBackInstalled,
	  ];
#pragma pack(push, 1)
	  t_UdpPacketHeaderV1 : STRUCT
	    Command : t_UdpCommand;
	    State : t_UdpFrameState;
	    IFCRC : HDINT;
	    SafetyNbr : HDINT;
	    SequenceNumber : USINT;
	    PdoDataLength : UINT;
	    SdoDataLength : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpPacketHeaderV2 : STRUCT
	    Command : t_UdpCommand;
	    State : t_UdpFrameState;
	    IFCRC : HDINT;
	    SafetyNbr : HDINT;
	    SequenceNumber : UDINT;
	    PdoDataLength : UINT;
	    SdoDataLength : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpPacketV1 : STRUCT
	    Header : t_UdpPacketHeaderV1;
	    Data : ARRAY [0..UDP_MAX_PDO_SIZE+UDP_MAX_SDO_SIZE-1] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpPacketV2 : STRUCT
	    Header : t_UdpPacketHeaderV2;
	    Data : ARRAY [0..UDP_MAX_PDO_SIZE+UDP_MAX_SDO_SIZE-1] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpScheduler : STRUCT
	    CyCnt : UDINT;
	    ActRequestedIndex : UDINT;
	    LastTime : UDINT;
	    ActRuntime : UDINT;
	    MinRuntime : UDINT;
	    MaxRuntime : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_UdpVariables : STRUCT
	    UdpInit : t_UdpInit;
	    ProducerConnectionNo : UDINT;
	    paConnectionsToProducers : ^t_aUdpConnections;
	    ConsumerConnectionNo : UDINT;
	    paConnectionsToConsumers : ^t_aUdpConnections;
	    RequestScheduler : t_UdpScheduler;
	    ResponseScheduler : t_UdpScheduler;
	    pTempSendBufferCy : ^t_UdpPacketV2;
	    pTempSendBufferIRQ : ^t_UdpPacketV2;
	    ConnectionsInitialized : BOOL;
	    SendCopyCmdNo : UDINT;
	    aSendCopyCmds : ARRAY [0..MAX_UDP_IF_ALLOWED-1] OF t_FSoECopyCmd;
	    ReceiveCopyCmdNo : UDINT;
	    aReceiveCopyCmds : ARRAY [0..MAX_UDP_IF_ALLOWED-1] OF t_FSoECopyCmd;
	    LowestTransmissionTime_ms : UDINT;
	    ClearRoutingInfoSSW : t_ClearRoutingInfoSSW;
	    CalculatedRequestTime : UDINT;
	    CalculatedRequestTimeout : UDINT;
	    CalculatedResponseTime : UDINT;
	    CalculatedResponseTimeout : UDINT;
	    ReceivedUnexpectedResponse : UDINT;
	    InvalidResponseSettingAtFowardingProducerList : UDINT;
	    InvalidResponseSettingAtFowardingConsumerList : UDINT;
	    WrongSequenceInRequestCnt : UDINT;
	    WrongSequenceInResponseCnt : UDINT;
	    WrongReceiveUDPLength : UDINT;
	    UnhandledReceivePacketCnt : UDINT;
	    ActTimeBetweenReceiveData : UDINT;
	    MaxTimeBetweenReceiveData : UDINT;
	    LastReceivedTimeStamp : UDINT;
	    ActTimeRequestScheduler : UDINT;
	    MaxTimeRequestScheduler : UDINT;
	    LastRequestTimeScheduler : UDINT;
	    bSetFirstMaxRequestSchedulerTime : BOOL;
	    SendCnt : UDINT;
	    ReceiveCnt : UDINT;
	    SendErrorCnt : UDINT;
	    LastSendError : DINT;
	    ReInitSenderIP : HDINT;
	    SdoAlreadyUsedCnt : UDINT;
	    UdpTaskStarted : BOOL;
	    udAdvancedRequestCycle : UDINT;
	    udAdvancedResponseTimeout : UDINT;
	  END_STRUCT;
#pragma pack(pop)
	END_TYPE
  //Servers:
	LoginCnt 	: SvrCh_DINT;
	MinSyncDuration 	: SvrCh_UDINT;
	SyncDuration 	: SvrCh_UDINT;
	MaxSyncDuration 	: SvrCh_UDINT;
  //Clients:
	To_HwControl 	: CltChCmd_HwControlBase;
	To_OSKernel 	: CltChCmd__OSKernel;
	To_MultiTask 	: CltChCmd__MultiTask;
	SafetyDomainTimeout 	: CltCh_UDINT;
	UdpSafetyRequestCycle 	: CltCh_UDINT;
	UdpSafetyResponseTimeout 	: CltCh_UDINT;
	UdpSendData 	: CltChCmd_CriticalSection;
	UdpReceiveData 	: CltChCmd_CriticalSection;
  //Variables:
		InitSSW 	: t_InitSSW;			//! <Variable Comment="initialisation state machine (mostly relevant for the master) which is handled in CyWork()" Name="InitSSW"/>
		pFirstManagingInterfaceComponent 	: ^t_ManagingComponent;			//! <Variable Comment="linked list with all logged in management modules (for interface frames)" Name="pFirstManagingInterfaceComponent"/>
		pFirstSafetyComponent 	: ^t_SafetyComponent;			//! <Variable Comment="linked list with all logged in safety modules" Name="pFirstSafetyComponent"/>
		pFirstGenericDynamic 	: ^t_GenericDynamicComponent;
		us_SafetyModuleNo 	: USINT;			//! <Variable Comment="number of safety modules logged in" Name="us_SafetyModuleNo"/>
		b_NewSyncMode 	: BOOL;			//! <Variable Comment="remembers if there is a new time synchronisation (master safetymanager controls time synchronisation of single safety modules) or an old (every single safety module controls it&apos;s own synchronisation)" Name="b_NewSyncMode"/>
		LastTimeSyncThis 	: pVirtualBase;			//! <Variable Comment="remembers the this pointer of the module where the time synchronisation is active" Name="LastTimeSyncThis"/>
		TimeSyncTimeStamp 	: UDINT;			//! <Variable Comment="ms timestamp of last time a module finished synchronisation or when the synchronisation process started" Name="TimeSyncTimeStamp"/>
		TimeSyncCycleStart 	: UDINT;			//! <Variable Comment="ms timestamp of when we started the time synchronisation cycle (the first module of the master)" Name="TimeSyncCycleStart"/>
		b_CheckTimeSyncTimeout 	: BOOL;			//! <Variable Comment="this variable tells if the timeout of time synchronisation is active or not" Name="b_CheckTimeSyncTimeout"/>
		pNeedThisClass4GlobVarDef 	: ^SafetyCDIAS_Base;			//! <Variable Comment="this variable is needed to make a dependency to class SafetyCDIAS_Base, because we need the global variable declaration of it" Name="pNeedThisClass4GlobVarDef"/>
		pHardwareTree 	: ^LSL_HARDWARETREE;			//! <Variable Comment="this variable is needed to use the hardwaretree commands of OS" Name="pHardwareTree"/>
		ActPDO_ID 	: USINT;			//! <Variable Comment="actual PDO ID, to definitely identify a PDO" Name="ActPDO_ID"/>
		ConfigNo 	: USINT;			//! <Variable Comment="number of configs stored in pConfigs pointer" Name="ConfigNo"/>
		pConfigs 	: ^t_CfgListElem;			//! <Variable Comment="linked list with the configurations of all safety CPUs in the system" Name="pConfigs"/>
		pSortedConnections 	: ^t_FSoEConnectionDetails;
		SafeCPU_Nbr_No 	: USINT;
		aSafetyCPU_Nbr : ARRAY [0..99] OF HDINT;

		LocalCPUNo 	: USINT;
		LocalCPUCfgNo 	: USINT;
		pFirstLocalCPUCfg 	: ^t_LocalCfgListElem;
		pActualSendCfg 	: ^t_CfgListElem;
		pActualSendPDO 	: ^t_PdoListElem;
		PDOElementNo 	: UDINT;
		aPDOElements : ARRAY [0..MAX_PDO_CONFIGS-1] OF t_PDOElement;

		InterfaceFrameNo 	: UDINT;
		aInterfaceFrameInfos : ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_InterfaceFrameInfo_2;

		IFFSoENo 	: UDINT;
		aIFFSoEInfos : ARRAY [0..MAX_MODULES_FSOE_IF_INFOS-1] OF t_FSoEConnInfo;

		b_StartTimeSync 	: BOOL;
		b_SlaveSyncActive 	: BOOL;
		ud_InitTimeOut 	: UDINT;
		CopyCmdsAvailable 	: USINT;			//! <Variable Comment="number of copy commands which have been added to VM" Name="CopyCmdsAvailable"/>
		CopyCmdsUsed 	: USINT;			//! <Variable Comment="number of copy commands used in the VM (can be maximum the number of available copy commands)" Name="CopyCmdsUsed"/>
		aCopyCmds : ARRAY [0..MAX_PDO_COPY_CMDS-1] OF t_CopyCmdDO;

		b_SynchroTimeoutDone 	: BOOL;
		b_SynchroTimeoutSet 	: BOOL;
		b_SyncTimeoutExceeded 	: BOOL;
		SynchroTimeout 	: UDINT;
		pFirstTimeSyncElement 	: ^t_TimeSyncElement;
		pActTimeSyncElement 	: ^t_TimeSyncElement;
		pMasterTimeSyncElement 	: ^t_TimeSyncElement;
		OldInitSSW 	: t_InitSSW;
		LogBuf : ARRAY [0..SM_LogBufferSize-1] OF t_LogBuf;

		ActLogIndex 	: UDINT;
		TimeStampLastLog_us 	: UDINT;
		LocalIFNo 	: USINT;
		aLocalIFs : ARRAY [0..MAX_INTERNAL_IF_MODULES-1] OF t_LocalIFs;

		s_TimeRt 	: t_TimeMeasurementVaran;
		s_TimeRtPostScan 	: t_TimeMeasurementVaran;
		s_TimeCy 	: t_TimeMeasurementVaran;
		SyncTimeoutCounter 	: UDINT;
		bd_LogRtMessagesInCy 	: t_LogMsgs;
		ud_SizeTempMemCy 	: UDINT;
		pTempMemCy 	: pVoid;
		IsTimeSyncMaster 	: DINT;
		QuitErrorAvailable 	: DINT;
		pSafetyDll 	: ^OS_SAFETY_DLL;
		pSafetyDll2 	: ^OS_SAFETY_DLL2;
		b_GotFSoEModules 	: BOOL;
		FSoECopyCmdNo 	: UDINT;
		aFSoECopyCmds : ARRAY [0..MAX_FSOE_CONNECTIONS-1] OF t_FSoECopyCmd;

		FSoEInputPDONo 	: UDINT;
		aFSoEInputPDOs : ARRAY [0..MAX_FSOE_CONNECTIONS-1] OF t_InputPDOs;

		b_MandatoryStuffAvailable 	: BOOL;
		LastCfgReceiveTime 	: UDINT;
		b_AllLocalSlaveAddressesValid 	: BOOL;
		b_GotIncompleteFSoEConnections 	: BOOL;
		b_CheckSafetyTopology 	: DINT;			//! <Variable Comment="Not used anymore but we leave it here for compatibility reasons. Customers could have a derivation of the class to disable the check. In this case deleting the variable would lead to a error when compiling." Name="b_CheckSafetyTopology"/>
		bdFeatureEnable 	: t_FeatureEnable;
		ActRtTime 	: UDINT;
		ActCyTime 	: UDINT;
		sUdpVars 	: t_UdpVariables;
		bd_SingleLog 	: t_SingleLogMng;
		sFlexCon 	: t_FlexCon;
		b_PostInitFinished 	: BOOL;
		LastIfCfgDistribution 	: UDINT;
		LastUdpIfCfgDistribution 	: UDINT;
		LocalInitStartTime 	: UDINT;
  //Functions:
	
	FUNCTION VIRTUAL GLOBAL Init;
	
	FUNCTION VIRTUAL GLOBAL CyWork
		VAR_INPUT
			EAX 	: UDINT;
		END_VAR
		VAR_OUTPUT
			state (EAX) 	: UDINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL RtWork
		VAR_INPUT
			EAX 	: UDINT;
		END_VAR
		VAR_OUTPUT
			state (EAX) 	: UDINT;
		END_VAR;
	
	FUNCTION SafetyManager
		VAR_OUTPUT
			ret_code 	: ConfStates;
		END_VAR;
	
	FUNCTION AddModule
		VAR_INPUT
			pPara 	: ^CmdStruct;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION AddSafetyModule
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
			pThis 	: pVirtualBase;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION AddManagingModule
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
			pThis 	: pVirtualBase;
			pObjectPath 	: ^USINT;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION AddSafetyNbr
		VAR_INPUT
			pThis 	: pVirtualBase;
			ud_SafetyNbr 	: HDINT;
			FirmwareVersion 	: HDINT;
			FPGAVersion 	: HSINT;
			PathLen 	: USINT;
			pPath 	: ^USINT;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION GetModuleTypeNo
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
		END_VAR
		VAR_OUTPUT
			ModuleNo 	: UDINT;
		END_VAR;
	
	FUNCTION NewTimeSyncPossible
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION SetNewSyncMode
		VAR_INPUT
			b_NewMode 	: BOOL;
		END_VAR;
	
	FUNCTION TellAllSafetyModules
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pResult 	: ^results;
		END_VAR;
	
	FUNCTION TellAllInterfaceManagingModules
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pResult 	: ^results;
			OverrideSync 	: BOOL := FALSE;
		END_VAR;
	
	FUNCTION StartTimeSync;
	
	FUNCTION NextTimeSyncModule;
	
	FUNCTION GiveTokenToNextModule
		VAR_OUTPUT
			RestartCycle 	: BOOL;
		END_VAR;
	
	FUNCTION AllManagingNodesSynchron
		VAR_OUTPUT
			AllReady 	: BOOL;
		END_VAR;
	
	FUNCTION AddConfiguration
		VAR_INPUT
			Length 	: UINT;
			pData 	: ^void;
			IsFSoEConfig 	: BOOL;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION FreeCfgLists;
	
	FUNCTION AddOwnCPUsToList;
	
	FUNCTION AllCPUConfigsArrived
		VAR_OUTPUT
			AllReady 	: BOOL;
		END_VAR;
	
	FUNCTION AddLocalCPUCfg
		VAR_INPUT
			Length 	: UINT;
			pData 	: ^void;
			IsFSoECfg 	: BOOL := FALSE;
		END_VAR;
	
	FUNCTION RemoveLocalCPUCfg
		VAR_INPUT
			SafetyNumber 	: HDINT;
		END_VAR;
	
	FUNCTION SendLocalCfgsToMaster;
	
	FUNCTION BroadCastActPDO;
	
	FUNCTION IsAffectedByPDO
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pSourceDest 	: ^t_SourceDest;
			pSafetyNbr 	: ^HDINT;
		END_VAR
		VAR_OUTPUT
			IsAffected 	: BOOL;
		END_VAR;
				//! <Function Comment="for SDOs and TDOs" Name="GetDestManagingOfDO"/>
	FUNCTION GetDestManagingOfDO
		VAR_INPUT
			pDO 	: ^SafetyCDIAS_Base::SDOHeader;
		END_VAR
		VAR_OUTPUT
			pDestManaging 	: ^t_ManagingComponent;
		END_VAR;
	
	FUNCTION GetDestSafetyOfDO
		VAR_INPUT
			pDO 	: ^SafetyCDIAS_Base::SDOHeader;
		END_VAR
		VAR_OUTPUT
			pDestSafety 	: ^t_SafetyComponent;
		END_VAR;
	
	FUNCTION CheckTopSDO
		VAR_INPUT
			pPara 	: ^CmdStruct;
		END_VAR;
	
	FUNCTION TopSDODest
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pResult 	: ^results;
		END_VAR
		VAR_OUTPUT
			IsDestDomain 	: BOOL;
		END_VAR;
	
	FUNCTION PDOLengthCorrection;
	
	FUNCTION DoublePDOEntry
		VAR_INPUT
			NewPDO 	: t_PdoCfg;
			pNewCfg 	: ^t_CfgListElem;
		END_VAR
		VAR_OUTPUT
			IsDoubleEntry 	: BOOL;
		END_VAR;
	
	FUNCTION StoreInterfaceRoutingList
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pResult 	: ^results;
			pHelpManaging 	: ^t_ManagingComponent;
			IsFSoE 	: BOOL;
		END_VAR
		VAR_OUTPUT
			IFDataChanged 	: BOOL;
		END_VAR;
	
	FUNCTION AllInterfaceCfgsArrived
		VAR_OUTPUT
			AllReady 	: BOOL;
		END_VAR;
	
	FUNCTION SendInterfaceConfigs
		VAR_INPUT
			DestinationThis 	: pVirtualBase := NIL;
			IsResponse 	: BOOL := FALSE;
			IsFSoE 	: BOOL;
		END_VAR;
	
	FUNCTION GetIFInfoByCRC
		VAR_INPUT
			InterfaceCRC 	: HDINT;
			pMemOffset 	: ^UDINT := NIL;
			pLength 	: ^USINT := NIL;
		END_VAR
		VAR_OUTPUT
			SafetyNbr 	: HDINT;
		END_VAR;
	
	FUNCTION SetTimeout;
	
	FUNCTION CheckTimeout;
	
	FUNCTION AllNonOptionalIFDomainsSynchron
		VAR_OUTPUT
			EverythingSynchron 	: BOOL;
		END_VAR;
	
	FUNCTION VIRTUAL GetThisViaServerName
		VAR_INPUT
			pObjectDotServer 	: ^CHAR;
		END_VAR
		VAR_OUTPUT
			ThisPtr 	: pVoid;
		END_VAR;
	
	FUNCTION GetNewMovDO
		VAR_OUTPUT
			Index 	: INT;
		END_VAR;
	
	FUNCTION CheckCPUConfigs
		VAR_INPUT
			OnlyOptionalCheck 	: BOOL := FALSE;			//! <Variable Comment="Set this true if you only want to set the correct optional state for each managing component based on the PDOs.&#13;&#10;If this is false the method will also throw an error if the managing module of a non-optional interface frame is missing." Name="CheckCPUConfigs.OnlyOptionalCheck"/>
		END_VAR
		VAR_OUTPUT
			ConfigsOK 	: BOOL;
		END_VAR;
	
	FUNCTION ClearRoutingInformations
		VAR_OUTPUT
			finished 	: BOOL;
		END_VAR;
	
	FUNCTION CheckPDORecycle
		VAR_INPUT
			SourceDest 	: t_SourceDest;
			pNewPDO 	: ^t_PDOElement;
		END_VAR
		VAR_OUTPUT
			pRecyclePDO 	: ^t_PDOElement;
		END_VAR;
	
	FUNCTION AddManagingInfo
		VAR_INPUT
			pPara 	: ^CmdStruct;
		END_VAR;
	
	FUNCTION CheckModuleUsage
		VAR_INPUT
			Length 	: UINT;
			pData 	: ^void;
			IsFSoEConfig 	: BOOL;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION SetModuleUsed
		VAR_INPUT
			PathLen 	: UINT;
			pPath 	: ^USINT;
			IgnoreOptional 	: BOOL;
			IsOptional 	: BOOL;
			IsInterfaceDomain 	: BOOL;
		END_VAR;
	
	FUNCTION CreateTimeSyncList;
	
	FUNCTION AddTimeSyncElement
		VAR_INPUT
			pManaging 	: ^t_ManagingComponent;
			pSafety 	: ^t_SafetyComponent;
			IsInterfaceDomain 	: BOOL;
		END_VAR;
	
	FUNCTION CreateVirtualPDOs;
	
	FUNCTION AddVirtualPDOToCfg
		VAR_INPUT
			pCfg 	: ^t_CfgListElem;
			pNewPDO 	: ^t_PdoCfg;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION AddToLocalIFs
		VAR_INPUT
			pHelpPDO 	: ^t_PdoCfg;
		END_VAR;
	
	FUNCTION StartTimeMeasurement
		VAR_INPUT
			usMode (EAX) 	: DINT;
		END_VAR;
	
	FUNCTION StopTimeMeasurement
		VAR_INPUT
			usMode (EAX) 	: DINT;
		END_VAR;
	
	FUNCTION AnyIFDomainSynchron
		VAR_OUTPUT
			AnythingSynchron 	: BOOL;
		END_VAR;
	
	FUNCTION LocalIFsAvailable
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION IsCPUModule
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION GetMasterConnection
		VAR_OUTPUT
			pMasterConn 	: ^t_ManagingComponent;
		END_VAR;
	
	FUNCTION IsFSoEModule
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION CreateFSoEPDOs;
	
	FUNCTION ShareConnectionInfos
		VAR_INPUT
			pMasterConnection 	: ^t_FSoEConnectionDetails;
			pMasterConfig 	: ^t_CfgListElem;
			pSlaveConnection 	: ^t_FSoEConnectionDetails;
			pSlaveConfig 	: ^t_CfgListElem;
		END_VAR;
	
	FUNCTION ShareSingleConnectionInfos
		VAR_INPUT
			pMasterConnection 	: ^t_FSoEConnectionDetails;
			pModuleConfig 	: ^t_ModuleListElem;
		END_VAR;
	
	FUNCTION MySort
		VAR_INPUT
			src 	: ^USINT;
			no 	: UDINT;
			itemsize 	: UDINT;
			sortfunct 	: ^void;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
	
	FUNCTION ConnectionSort
		VAR_INPUT
			pCon1 	: ^pVoid;
			pCon2 	: ^pVoid;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION FsoEInfoSort
		VAR_INPUT
			pInfo1 	: ^pVoid;
			pInfo2 	: ^pVoid;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION FinalizePDOMapping;
	
	FUNCTION AddInternalIFFSoE
		VAR_INPUT
			pMasterConnection 	: ^t_FSoEConnectionDetails;
			pMasterConfig 	: ^t_CfgListElem;
			pSlaveConnection 	: ^t_FSoEConnectionDetails;
			pSlaveConfig 	: ^t_CfgListElem;
		END_VAR;
	
	FUNCTION CheckOptional;
	
	FUNCTION UpdateConnectionInLocalCfg
		VAR_INPUT
			SafetyNbr 	: HDINT;
			ConnectionNo 	: UDINT;
			pFirstConnection 	: ^t_FSoEConnectionDetails;
		END_VAR;
	
	FUNCTION FSoEConInitialized
		VAR_INPUT
			SafetyNbr 	: HDINT;
			ConnectionNo 	: UDINT;
			pFirstConnection 	: ^t_FSoEConnectionDetails;
		END_VAR;
	
	FUNCTION CheckLocalSlaveAddresses;
	
	FUNCTION AddUdpConnection
		VAR_INPUT
			IP 	: HDINT;
			IFCRC 	: HDINT;
			SafetyNumber 	: HDINT;
			ProducerList 	: BOOL;
			ppConnection 	: ^t_pUdpConnection := NIL;
			pInfoBits 	: ^t_UdpConInfoBits := NIL;
		END_VAR
		VAR_OUTPUT
			success 	: BOOL;
		END_VAR;
	
	FUNCTION UdpRequestScheduler;
	
	FUNCTION UdpResponseScheduler;
	
	FUNCTION UdpSetTimeMeasurement;
	
	FUNCTION SendUdpRequest
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION SendUdpResponse
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
		END_VAR
		VAR_OUTPUT
			retcode 	: DINT;
		END_VAR;
	
	FUNCTION ReceiveUdpData
		VAR_INPUT
			pData 	: ^void;
			udLength 	: UDINT;
			udIPAddr 	: HDINT;
			udPort 	: UDINT;
		END_VAR;
	
	FUNCTION ForwardReceivedUdpSDOs
		VAR_INPUT
			paConnections 	: ^t_aUdpConnections;
			ConnectionNo 	: UDINT;
		END_VAR;
	
	FUNCTION ForwardSDO2UdpBySafetyNbr
		VAR_INPUT
			SDOLength 	: UINT;
			pSDO 	: ^USINT;
		END_VAR
		VAR_OUTPUT
			FoundTarged 	: BOOL;
		END_VAR;
	
	FUNCTION ShareUdpConnectionInfos;
	
	FUNCTION SetUdpSchedulerGrid;
	
	FUNCTION UdpTask;
	
	FUNCTION TimeSyncModuleAvailable
		VAR_INPUT
			pTimeSyncElement 	: ^t_TimeSyncElement;
		END_VAR
		VAR_OUTPUT
			Available 	: BOOL;
		END_VAR;
	
	FUNCTION ReinitEverything;
				//! <Function Comment="This method checks if the passed connection and FSoE Interface are a Valid custom connection&#13;&#10;This check replaces the slave address check for normal FSoE Interface frames. (s. FinalizePDOMapping)&#13;&#10;(The slave addresses may not valid for custom connections at this point)" Name="CheckCustomConnectionValid"/>
	FUNCTION CheckCustomConnectionValid
		VAR_INPUT
			pConnection 	: ^t_FSoEConnectionDetails;
			pIFFSoEInfo 	: ^t_FSoEConnInfo;
		END_VAR
		VAR_OUTPUT
			bIsCustomConnValid 	: BOOL;
		END_VAR;
	
	FUNCTION HandleUdpResponse
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
			pPacket 	: ^t_UdpPacketV1;
			udLength 	: UDINT;
		END_VAR;
	
	FUNCTION HandleUdpResponseV2
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
			pPacket 	: ^t_UdpPacketV2;
			udLength 	: UDINT;
		END_VAR;
	
	FUNCTION HandleUdpRequest
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
			pPacket 	: ^t_UdpPacketV1;
			udLength 	: UDINT;
		END_VAR;
	
	FUNCTION HandleUdpRequestV2
		VAR_INPUT
			pConnection 	: ^t_UdpConnection;
			pPacket 	: ^t_UdpPacketV2;
			udLength 	: UDINT;
		END_VAR;
	
	FUNCTION AddGenericDynamic
		VAR_INPUT
			ModuleType 	: t_e_ModuleType;
			pThis 	: pVirtualBase;
			pObjectPath 	: ^USINT;
		END_VAR
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
				//! <Function Comment="Get the name of an object via its this pointer" Name="GetObjName"/>
	FUNCTION GetObjName
		VAR_INPUT
			pThis 	: pVirtualBase;
			pTargetString 	: ^CHAR;
			TargetStringSize 	: UDINT;
		END_VAR;
				//! <Function Name="ClassState.NewInst" Hidden="true"/>
	FUNCTION VIRTUAL GLOBAL ClassState::NewInst
		VAR_INPUT
			pPara 	: ^CmdStruct;
			pResult 	: ^results;
		END_VAR
		VAR_OUTPUT
			ret_code 	: iprStates;
		END_VAR;
  //Tables:
	FUNCTION @STD
		VAR_OUTPUT
			ret_code	: CONFSTATES;
		END_VAR;
	FUNCTION GLOBAL TAB @CT_;
END_CLASS;

#pragma using CriticalSection
#pragma usingLtd _MultiTask
#pragma usingLtd _OSKernel
#pragma usingLtd HwControlBase


//}}LSL_DECLARATION


FUNCTION GLOBAL TAB SafetyManager::@CT_
0$UINT,
2#0100000000000000$UINT, //TY_SAFETYMANAGER
2$UINT, 28$UINT, (SIZEOF(::SafetyManager))$UINT, 
4$UINT, 8$UINT, 0$UINT, 
TO_UDINT(1799751072), "SafetyManager", //Class
TO_UDINT(3727715967), "SafetyUDP", 1$UINT, 4$UINT, //Baseclass
//Servers:
(::SafetyManager.LoginCnt.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(3842434630), "LoginCnt", 
(::SafetyManager.MinSyncDuration.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1133920085), "MinSyncDuration", 
(::SafetyManager.SyncDuration.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1910597126), "SyncDuration", 
(::SafetyManager.MaxSyncDuration.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1680693348), "MaxSyncDuration", 
//Clients:
(::SafetyManager.To_HwControl.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000010$UINT, TO_UDINT(1012711711), "To_HwControl", TO_UDINT(1193052171), "HwControlBase", 1$UINT, 10$UINT, 
(::SafetyManager.To_OSKernel.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000000$UINT, TO_UDINT(2681344549), "To_OSKernel", TO_UDINT(3045737964), "_OSKernel", 0$UINT, 5$UINT, 
(::SafetyManager.To_MultiTask.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000000$UINT, TO_UDINT(2145650779), "To_MultiTask", TO_UDINT(2641766936), "_MultiTask", 1$UINT, 1$UINT, 
(::SafetyManager.SafetyDomainTimeout.pCh)$UINT, _CH_CLT_DATA$UINT, 2#0000000000000000$UINT, TO_UDINT(1863305286), "SafetyDomainTimeout", 
(::SafetyManager.UdpSafetyRequestCycle.pCh)$UINT, _CH_CLT_DATA$UINT, 2#0000000000000000$UINT, TO_UDINT(3137205385), "UdpSafetyRequestCycle", 
(::SafetyManager.UdpSafetyResponseTimeout.pCh)$UINT, _CH_CLT_DATA$UINT, 2#0000000000000000$UINT, TO_UDINT(3847699041), "UdpSafetyResponseTimeout", 
(::SafetyManager.UdpSendData.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000010$UINT, TO_UDINT(1760877699), "UdpSendData", TO_UDINT(794617671), "CriticalSection", 0$UINT, 4$UINT, 
(::SafetyManager.UdpReceiveData.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000010$UINT, TO_UDINT(3423267838), "UdpReceiveData", TO_UDINT(794617671), "CriticalSection", 0$UINT, 4$UINT, 
END_FUNCTION


#define USER_CNT_SafetyManager 13

TYPE
	_LSL_STD_VMETH	: STRUCT
			CmdTable	: CMDMETH;
			UserFcts	: ARRAY[0..USER_CNT_SafetyManager] OF ^Void;
	END_STRUCT;
END_TYPE

FUNCTION SafetyManager::@STD
	VAR_OUTPUT
		ret_code	: CONFSTATES;
	END_VAR
	VAR
		vmt	: _LSL_STD_VMETH;
		nCmdSize	: UINT;
	END_VAR

	ret_code	:= SafetyUDP::@STD();
	IF ret_code <> C_OK THEN
		RETURN;
	END_IF;
	nCmdSize		:= SafetyUDP::ClassState.pMeth^.nCmds$UINT*SIZEOF(pVoid) + CMDMETH.Init;

	_memcpy((#vmt.CmdTable)$^USINT, SafetyUDP::ClassState.pMeth, nCmdSize);
	vmt.CmdTable.nCmds		:= nSTDCMD + USER_CNT_SafetyManager;
	vmt.CmdTable.NewInstr		:= #ClassState::NewInst();
	vmt.CmdTable.Init		:= #Init();
	vmt.CmdTable.CyWork		:= #CyWork();
	vmt.CmdTable.RtWork		:= #RtWork();
#pragma warning (disable : 74)
	vmt.UserFcts[12]		:= #GetThisViaServerName();

#pragma warning (default : 74)
	SafetyUDP::ClassState.pMeth		:= StoreCmd (pCmd := #vmt.CmdTable, SHARED);

	IF SafetyUDP::ClassState.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	MinSyncDuration.pMeth			:= StoreMethod( #M_RD_DIRECT(), #M_WR_DIRECT() );
	IF MinSyncDuration.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	MaxSyncDuration.pMeth			:= StoreMethod( #M_RD_DIRECT(), #M_WR_DIRECT() );
	IF MaxSyncDuration.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	ret_code	:= SafetyManager();

END_FUNCTION

#pragma usingLtd SafetyCDIAS_Base

//{{LSL_IMPLEMENTATION
VAR_EXTERNAL
  pSafetyManagerThis   : pVirtualBase;
  b_SafetyOnLocalCDIAS : BOOL;
  ModuleSyncNo : DINT; // Number of modules which need to be synchron before everyone sends the set bus ready signal
END_VAR
VAR_PRIVATE
  pVaran  : ^LSL_VARAN;
END_VAR

FUNCTION VIRTUAL GLOBAL SafetyManager::Init
  VAR
    MasterObjects : UDINT;
    tempCmd    : CmdStruct;
    _result    : results;
  END_VAR

  // the base increases the init counter
  SafetyUDP::Init();

  case us_FirstScan of
//**********************************************************************************************************************************************************
    1:
      // in first init we ensure that the pointer is NIL (and not any value that is in the memory)
      pSafetyManagerThis := NIL;
      
      SafetyDomainTimeout := SafetyDomainTimeout.Read();


//**********************************************************************************************************************************************************    
    2:
      if pSafetyManagerThis = NIL then
        pSafetyManagerThis := THIS;
//        pVaranManager := (To_HwControl.AddSafetyManager(pThis:=THIS))$^VaranManager;
      else
        LogError("@0432 (SafetyManager::Init) Only 1 Instance of SafetyManager possible");
        ClassState := -5;
      end_if;
//**********************************************************************************************************************************************************
  // in init nr.3 the managing master objects log in
  // in init nr.4 the managing slave objects log in (to be the last one called in the token ring => give the token back to master)
//**********************************************************************************************************************************************************
    5:
  
      //read clients for udp request time and response timeout
      UdpSafetyRequestCycle              := UdpSafetyRequestCycle.Read();
      UdpSafetyResponseTimeout           := UdpSafetyResponseTimeout.Read();
      sUdpVars.udAdvancedRequestCycle    := UdpSafetyRequestCycle;
      sUdpVars.udAdvancedResponseTimeout := UdpSafetyResponseTimeout;
        
      if b_GotFSoEModules then
        tempCmd.uiCmd := CMD_SM_FSOE_AVAILABLE;
        tempCmd.aPara[0] := this$DINT;
        TellAllInterfaceManagingModules(pPara:=#tempCmd, pResult:=#_result, OverrideSync:=TRUE);
      end_if;

      // now we check if we're on the topmost level
      MasterObjects := GetModuleTypeNo(_VBC021Slave) + GetModuleTypeNo(_VaranManager4VMC) + GetModuleTypeNo(_GenericTimeSyncSlave);
      if MasterObjects = 0 then
        // if there are no modules which makes us a slave => we're the master
        IsTimeSyncMaster := 1;
        
        // initialize everything
        InitSSW := _ClearRoutingInfos;
      elsif MasterObjects > 1 then
        // it's not possible to be a slave to more than 1 master (sync forwarding doesn't work then)
        LogError("@0433 (SafetyManager::Init) A Slave-System can't have more than 1 Master-System! Please check your HW class configuration!");
        ClassState := -6;
      else
        // we're sync-slave when using interface frames
        GetMasterConnection();
       
        // if there are more domains => initialize them
        InitSSW := _ClearRoutingInfos;
      end_if;

//**********************************************************************************************************************************************************
    12:
      // if no safety modules are logged in, disable the tasks
      if (pFirstSafetyComponent = NIL) & (pFirstManagingInterfaceComponent = NIL) then
//        To_HwControl.DisableSafetyManager();
        ClassState := 0;
      else
        // Install the callbacks
        tempCmd.uiCmd := CMD_HWCONTROL_LOGIN;
        tempCmd.aPara[0] := THIS$DINT;
        tempCmd.aPara[1] := CALL_OPTION_RT_PRESCAN  OR
                            CALL_OPTION_RT_POSTSCAN OR
                            CALL_OPTION_CYCLIC;
        tempCmd.aPara[2] := 0;  // Escape sequence, not needed for safety
        
        if To_HwControl.NewInst(#tempCmd, #_result) <> READY then
          LogError("@0434 (SafetyManager::Init) Failed to Install callbacks at HwControl");
          ClassState := -13;
        elsif _result.uiLng >= (2*sizeof(UDINT)) then
          ActRtTime := _result.aData[0]$UDINT;
          ActCyTime := _result.aData[4]$UDINT;
        end_if;
        
      end_if;

//**********************************************************************************************************************************************************
  end_case;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL SafetyManager::ClassState::NewInst
  VAR_INPUT
    pPara 	: ^CmdStruct;
    pResult 	: ^results;
  END_VAR
  VAR_OUTPUT
    ret_code 	: iprStates;
  END_VAR
  VAR
    i                   : UDINT;
    pHelpManaging       : ^t_ManagingComponent;
    pHelpSafety         : ^t_SafetyComponent;
    pHelpGeneric        : ^t_GenericDynamicComponent;
    pHelpHDINT          : ^HDINT;
    pHelpTimeSync       : ^t_TimeSyncElement;
    pDO 	              : ^SafetyCDIAS_Base::SDOHeader;
    IsOptionalIFDomain  : BOOL;
    sd_retval           : DINT;
    pHelp               : ^UDINT;
    MyPara 	            : CmdStruct;
    MyResult	          : results;
    pHelpModuleConf     : ^t_ModuleListElem;
    pHelpFsoeConn       : ^t_FSoEConnectionDetails;
    PHelpSlaveCfg       : ^t_SlaveListElem;
    pHelpConfigs        : ^t_CfgListElem;
    pConnectionHandle   : ^pVoid;
    quitRemoteModules   : USINT;
    IFDataChanged       : BOOL;
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED            
    LogStr              : ARRAY [0..255] OF CHAR;
#endif
  END_VAR

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
  // don't log the TDO and SDO events (would be too much for logging)
  CASE pPara^.uiCmd OF
    CMD_SM_TDO_TO_MANAGER, CMD_SM_TDO_TO_MODULE, CMD_SM_TDO_TOKEN_FWD, CMD_SM_TIME_SYNC_FINISHED,
    CMD_SM_SDO_TO_MANAGER, CMD_SM_SDO_TO_MODULE, CMD_SM_TOP_SDO_TO_MASTER, CMD_SM_TOP_SDO_TO_MODULE:
  ELSE
#ifdef SAFETY_DEBUG_TRACING_ACTIVATED
    LogValue("@DBG (SafetyManager::ClassState::NewInst) Incoming Command: {0}", Value:=pPara^.uiCmd);  
#endif
    LogBuf[ActLogIndex].NewState := _NotMaster;
    LogBuf[ActLogIndex].OldState := _NotMaster;
    LogBuf[ActLogIndex].Command := pPara^.uiCmd$t_LogBufCmd;
    LogBuf[ActLogIndex].TimeSinceLastLog_us := OS_READMICROSEC();
    LogBuf[ActLogIndex].TimeSinceLastLog_us -= TimeStampLastLog_us;
    TimeStampLastLog_us := OS_READMICROSEC();
    if ActLogIndex = (SM_LogBufferSize - 1) then
      ActLogIndex := 0;
    else
      ActLogIndex += 1;
    end_if;
  END_CASE;
#endif

  CASE pPara^.uiCmd OF
    
//**********************************************************************************************************************************************************
    CMD_SM_ADD_MODULE:
      pResult^.aData[0]$BOOL := AddModule( pPara );

      pResult^.uiLng := sizeof(pResult^.uiLng) + sizeof(BOOL);
  
//**********************************************************************************************************************************************************
    CMD_SM_ADD_MODULE_INFO:
      pResult^.aData[0]$BOOL := AddSafetyNbr( pThis           := pPara^.aPara[0]$pVirtualBase
                                            , ud_SafetyNbr    := pPara^.aPara[1]$UDINT
                                            , FirmwareVersion := pPara^.aPara[2]$HDINT
                                            , FPGAVersion     := pPara^.aPara[3]$HSINT
                                            , PathLen         := pPara^.aPara[4]$USINT
                                            , pPath           := pPara^.aPara[5]$^USINT
                                            );

      pResult^.uiLng := sizeof(pResult^.uiLng) + sizeof(BOOL);
      
//**********************************************************************************************************************************************************
    CMD_SM_ADD_MANAGING_INFO:
      AddManagingInfo(pPara);

//**********************************************************************************************************************************************************
    CMD_SM_TIME_SYNC_FINISHED:
      // check if the time synchronisation is still active
      if pActTimeSyncElement then      
        NextTimeSyncModule();
      end_if;
      
//**********************************************************************************************************************************************************
    CMD_SM_SDO_TO_MANAGER:
        pDO := (pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader;
        
        // check if it's addressed via topology path
        if (pDO^.StdHeader.Typ AND SSDO_MASK_TYPE) = FRM_TYPECODE_SSDO_MOD_TP then
          // if so, send it to the master, because only he knows who is the addressee
      
          pPara^.aPara[2] := this$DINT;
          
          // change the command, because now we forward it to the master
          pPara^.uiCmd := CMD_SM_TOP_SDO_TO_MASTER;

          // call our own newinst to forward the message to the master
          NewInst(pPara, pResult);

        elsif (pDO^.StdHeader.Typ AND SSDO_MASK_TYPE) = FRM_TYPECODE_DEVADDR then
          
          if (pDO^.StdHeader.Typ AND SSDO_MASK_RESPONSE) = 0 then
            // request

            // check if it's meant for the address service or for a device address
            if pDO^.DstAdr AND SM_DEVADDR_MASK_NO_ADDR_COMPARE then
              // it's a special destination => only head allowed
              if pDO^.DstAdr = SM_DEVADDR_ADDR_HEAD then
              
                // if we're host
                if sFlexCon.Role = _FlexConHost then
                  // send it to the module that provides the address service
                  pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
                  
                  // if we have a host module
                  if sFlexCon.pSafetyHost then
                    // send it there
                    sFlexCon.pSafetyHost^.pThis^.NewInst(pPara, pResult);
                    
                    // and store the source safety number with the managing component (to route the response)
                    pHelpManaging := pFirstManagingInterfaceComponent;
                    while pHelpManaging do
                      if pHelpManaging^.pThis = pPara^.aPara[2]$pVirtualBase then
                        pHelpManaging^.FlexCon.SafetyNumber := pDO^.SrcAdr;
                        exit;
                      end_if;
                    
                      pHelpManaging := pHelpManaging^.pNext;
                    end_while;
                  end_if;

                // if we're node
                elsif sFlexCon.Role = _FlexConNode then
                  // send it to the host
                  if sFlexCon.pManaging2Host then
                    sFlexCon.pManaging2Host^.pThis^.NewInst(pPara, pResult);                 
                  end_if;
                else
                  // SSDO to head station detected, but no FlexCon role => send the SDO to all modules with address service
                  pHelpSafety := pFirstSafetyComponent;
                  while pHelpSafety do
                    if pHelpSafety^.InfoBits.IsAddressService then      
                      pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
                      pHelpSafety^.pThis^.NewInst(pPara, pResult);
                    end_if;
                    pHelpSafety := pHelpSafety^.pNext;
                  end_while;
                  
                  // no other special destinations possible so far
                  if pPara^.uiCmd <> CMD_SM_SDO_TO_MODULE then
                    LogError("@05D5 (SafetyManager::ClassState::NewInst) Found SDO with special destination device address (HEAD) but no module with address service");
                  end_if;
                end_if;
              elsif pDO^.DstAdr = SM_DEVADDR_ADDR_ASSIGNMENT_SERVICE then
                pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
                // send address information to all modules
                TellAllInterfaceManagingModules(pPara, pResult);
              else
                pHelpSafety := pFirstSafetyComponent;
                while pHelpSafety do
                  if pHelpSafety^.SafetyNbr = pDO^.DstAdr then
                    pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
                    pHelpSafety^.pThis^.NewInst(pPara, pResult);
                    return;
                  end_if;
                end_while;
                // no other special destinations possible so far
                LogError("@05D6 (SafetyManager::ClassState::NewInst) Found SDO with invalid special destination device address");           
              end_if;
            else
              LogError("@05D7 (SafetyManager::ClassState::NewInst) Found SDO generic SDO.");           
            end_if;          
            
          else
            // response

            // if we're node
            if sFlexCon.Role = _FlexConNode then
              // send it to the module addressed by the package
              pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                if pHelpSafety^.SafetyNbr = pDO^.DstAdr then
                  // forward the SDO
                  pHelpSafety^.pThis^.NewInst(pPara, pResult);
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;

              pHelp := #pDO^.SessId;
              // skip session ID 
              pHelp += sizeof(pDO^.SessId);

              // and if it's a get safety number command and it succeeded
              if (pDO^.StdHeader.Cmd = SM_SDO_COMMAND_GET_SAFENBR) & (pHelp^$USINT = 0) then
              
                // skip return code
                pHelp += 1;
              
                // store the source safety number with the managing component (to route further packages)
                pHelpManaging := pFirstManagingInterfaceComponent;
                while pHelpManaging do
                  if pHelpManaging^.pThis = pPara^.aPara[2]$pVirtualBase then
                    pHelpManaging^.FlexCon.SafetyNumber := (#pDO^.SessId + sizeof(pDO^.SessId) + 1)^;
                    exit;
                  end_if;
                
                  pHelpManaging := pHelpManaging^.pNext;
                end_while;
              end_if;

            // if we're host
            elsif sFlexCon.Role = _FlexConHost then
              // send it to the node (search the safety number in the managing devices)
              
              pHelpManaging := pFirstManagingInterfaceComponent;
              while pHelpManaging do
                if pHelpManaging^.FlexCon.SafetyNumber = pDO^.DstAdr then
                  pHelpManaging^.pThis^.NewInst(pPara, pResult);                 
                  exit;
                end_if;
              
                pHelpManaging := pHelpManaging^.pNext;
              end_while;
                    
            else
              // SSDO from head station detected, but no FlexCon role => check generic dynamic devices
              if pDO^.DstAdr & pFirstGenericDynamic then
                pHelpGeneric := pFirstGenericDynamic;
                while pHelpGeneric do
                  // forward the SDO to all generic dynamic FSoE managing modules
                  if pHelpGeneric^.ModuleType = _GenericDynamicFSoE then
                    pHelpGeneric^.pThis^.NewInst(pPara, pResult);
                  end_if;
                  
                  pHelpGeneric := pHelpGeneric^.pNext;
                end_while;
              else
                if pDO^.DstAdr = 0 then
                  // zero is not allowed as device address
                  LogError("@05D8 (SafetyManager::ClassState::NewInst) Found SDO with invalid destination device address 0");
                else
                  // no generic dynamic device available to send it to
                  LogError("@05D9 (SafetyManager::ClassState::NewInst) Found SDO with invalid destination device address");
                end_if;
              end_if;
            end_if;
          end_if;
        else
          // change the command, because now we forward it to the receiver
          pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
        
          // check if we got a module which could be the destination of this
          pHelpManaging := GetDestManagingOfDO(pDO);

          if pHelpManaging then
            // check synchronicity here to make sure the info is up to date before checking it
            MyPara.uiCmd := CMD_SM_SYNC_CHECK;
            if pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult) = ERROR then
              pHelpManaging^.InfoBits.IsSynchron := FALSE;
            else
              pHelpManaging^.InfoBits.IsSynchron := TRUE;
            end_if;
            
            // if we're synchron, we can send the SDO to the managing module
            if pHelpManaging^.InfoBits.IsSynchron then
              pHelpManaging^.pThis^.NewInst(pPara, pResult);
            end_if;
          else
            // search in the local safety modules
            pHelpSafety := GetDestSafetyOfDO(pDO);
            
            if pHelpSafety then
              pHelpSafety^.pThis^.NewInst(pPara, pResult);
            else
              // it may be a udp connection
              if ((sUdpVars.ProducerConnectionNo = 0) & (sUdpVars.ConsumerConnectionNo = 0)) |
                 (ForwardSDO2UdpBySafetyNbr(pPara^.aPara[0]$UINT, pPara^.aPara[1]$^USINT) = FALSE) then
                LogError("@0435 (SafetyManager::ClassState::NewInst) Couldn't find target of SDO (CMD_SM_SDO_TO_MANAGER)");
                LogValue("SrcAddr: {0} / DestAddr: {1}", Value:=pDO^.SrcAdr, Value2:=pDO^.DstAdr);            
              end_if;
            end_if;
          end_if;
        end_if;

//**********************************************************************************************************************************************************
    CMD_SM_SDO_TO_MODULE:
      pDO := (pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader;

      // check if we got a module which could be the destination of this
      pHelpSafety := GetDestSafetyOfDO(pDO);
      
      if pHelpSafety then
        pHelpSafety^.pThis^.NewInst(pPara, pResult);
      else
        // if we have no such module or it's addressed via topology path we check if there is a domain which knows that module
        pHelpManaging := GetDestManagingOfDO(pDO);

        // if we found a domain for this module, forward it
        if pHelpManaging then
          if pHelpManaging^.InfoBits.IsSynchron then
            pHelpManaging^.pThis^.NewInst(pPara, pResult);
          end_if;
        else
          LogError("@0436 (SafetyManager::ClassState::NewInst) Couldn't find target of SDO (CMD_SM_SDO_TO_MODULE)");
          LogValue("SrcAddr: {0} / DestAddr: {1}", Value:=pDO^.SrcAdr, Value2:=pDO^.DstAdr);
        end_if;
      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_TOP_SDO_TO_MASTER:
      // if we're the master, we check the message and send the sdo with uncompressed path to it's addressee
      CheckTopSDO(pPara);

//**********************************************************************************************************************************************************
    CMD_SM_TOP_SDO_TO_MODULE:
      // if this is the destination domain, send the SDO to the right module
      if TopSDODest(pPara, pResult) = FALSE then

      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_TDO_TOKEN_FWD:
      // we sent the token to our slave and now we got it back
      if b_SlaveSyncActive then
        b_SlaveSyncActive := FALSE;
        
        // if we have no act time sync element we're maybe reconfiguring the system
        if pActTimeSyncElement then      
          // if we started the syncing, take the next module
          NextTimeSyncModule();
        end_if;
      else
        // otherwise we got the token from a superior system => start timesyncing
        b_StartTimeSync := TRUE;
      end_if;
      
//**********************************************************************************************************************************************************      
    CMD_SM_TDO_TO_MANAGER:
      // change the command, because now we forward it to the receiver
      pPara^.uiCmd := CMD_SM_TDO_TO_MODULE;

      // check if we got a module which could be the destination of this
      pHelpSafety := GetDestSafetyOfDO((pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader);
      
      if pHelpSafety then
        pHelpSafety^.pThis^.NewInst(pPara, pResult);
      else
        pHelpManaging := GetDestManagingOfDO((pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader);

        if pHelpManaging then
          if pHelpManaging^.InfoBits.IsSynchron then
            pHelpManaging^.pThis^.NewInst(pPara, pResult);
          end_if;
        else
          bd_LogRtMessagesInCy.NoTDOTargetToManager := TRUE;
        end_if;
      end_if;
    
//**********************************************************************************************************************************************************      
    CMD_SM_TDO_TO_MODULE:
      // check if we got a module which could be the destination of this
      pHelpSafety := GetDestSafetyOfDO((pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader);
      
      if pHelpSafety then
        pHelpSafety^.pThis^.NewInst(pPara, pResult);
      else
        // if we have no such module or it's addressed via topology path we check if there is a domain which knows that module
        pHelpManaging := GetDestManagingOfDO((pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader);

        // if we found a domain for this module, forward it
        if pHelpManaging then
          if pHelpManaging^.InfoBits.IsSynchron then
            pHelpManaging^.pThis^.NewInst(pPara, pResult);
          end_if;
        else
          bd_LogRtMessagesInCy.NoTDOTargetToModule := TRUE;
        end_if;
      end_if;

//**********************************************************************************************************************************************************      
    CMD_SM_DOMAIN_NR_SEND,
    CMD_SM_DOMAIN_NR_FWD:  // not sent anymore, but we leave this in here to get a defined reaction if versions get mixed (otherwise a log message with invalid command will be generated)
      // remember to which domain the managing module connects us and tell the other managing modules (but with a different cmd because they are not next door neighbors)
      
//**********************************************************************************************************************************************************      
    CMD_SM_DISCONNECT:
      // a domain has lost it's connection to the actual domain

      // check if it's a flexcon connection and no interface connection
      if pFirstManagingInterfaceComponent & b_GotFSoEModules then
        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          if pHelpManaging^.pThis = pPara^.aPara[2]$pVirtualBase then

            // delete the connected safety number
            pHelpManaging^.FlexCon.SafetyNumber := 0;

            if ((sFlexCon.Role = _FlexConNode) & (pHelpManaging = sFlexCon.pManaging2Host)) | (sFlexCon.Role = _FlexConHost) then
              // remove the InputPDO from the Safety CPU
              if pHelpManaging^.FlexCon.pInputPdo & pHelpManaging^.FlexCon.pInputPdo^.pMem & pHelpManaging^.FlexCon.pInputPdo^.pSafety then
                // unregister this PDO
                MyPara.uiCmd := CMD_SM_REMOVE_INPUT_PDO;
                MyPara.aPara[0] := pHelpManaging^.FlexCon.pInputPdo^.pMem;
                MyPara.aPara[1] := 0;
                pHelpManaging^.FlexCon.pInputPdo^.pSafety^.pThis^.NewInst(#MyPara, #MyResult);              
              end_if;

              if pHelpManaging^.FlexCon.pFSoEConnection then
                pHelpManaging^.FlexCon.pFSoEConnection := NIL;             
              end_if;

              return;
            end_if;
          end_if;
        
          pHelpManaging := pHelpManaging^.pNext;
        end_while;
      end_if;

      // stop all available copy commands
      if CopyCmdsAvailable then
        for i := 0 to (CopyCmdsAvailable-1) do
          aCopyCmds[i].pHandle^.ucCtrlStat := VARAN_DO_NO_RUN;
        end_for;        
      end_if;

      // check if it was an interface domain
      IsOptionalIFDomain := FALSE;
      if pFirstManagingInterfaceComponent & (b_GotFSoEModules = FALSE) then
        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          if pHelpManaging^.pThis = pPara^.aPara[2]$pVirtualBase then
            if pHelpManaging^.InfoBits.IsOptional then
              IsOptionalIFDomain := TRUE;
#ifdef SAFETY_DEBUG_TRACING_ACTIVATED
              LogError("@DBG (SafetyManager::ClassState::NewInst) Disconnect of optional interface frame domain detected!");
#endif
            end_if;
            exit;
          end_if;
        
          pHelpManaging := pHelpManaging^.pNext;
        end_while;
      end_if;

      // if the disconnected domain was no optional interface domain => tell all other to disconnect and prepare for reinitialisation
      if IsOptionalIFDomain = FALSE then
        pPara^.aPara[0] := 0; // set length of further parameters to 0
        TellAllSafetyModules(pPara, pResult);
      end_if;

      // tell the other managing modules that are still connected
      pHelpManaging := pFirstManagingInterfaceComponent;
      while pHelpManaging do
        // call the newinst with the given parameters (not to the one that called us now)
        if (pHelpManaging^.pThis <> pPara^.aPara[2]$pVirtualBase) then
          pHelpManaging^.pThis^.NewInst(pPara, pResult);
        end_if;
        
        pHelpManaging := pHelpManaging^.pNext;
      end_while;

      // reset module sync no (no more modules synchron)
      ModuleSyncNo := 0;

      ClassState := 0;

      // wait until we're synchron again
      InitSSW := _ClearRoutingInfos;        

//**********************************************************************************************************************************************************      
    CMD_SM_NEW_SAFETY_PROJECT:
      // remove the old config from the local memory
      if pPara^.aPara[0] = 4 then
        RemoveLocalCPUCfg((pPara^.aPara[4]$^HDINT)^);
      end_if;
    
      // new cpu config => reinitalize everything
      ReinitEverything();

//**********************************************************************************************************************************************************      
//    CMD_SM_STOP_AND_FLUSH_CFG:
//      // prevent interface-frame participants from flushing each other
//      if pMasterConnection then
//
//        // send all available local configurations to the master
//        SendLocalCfgsToMaster();
//
//        // the master tells us to stop working and flush our configuration => do it and tell our slaves
//        InitSSW := _Wait4ConfigFlush;        
//        SetTimeout();
//      end_if;

//**********************************************************************************************************************************************************      
    CMD_SM_STOPPED_AND_FLUSHED:
      
      // if we're the master, we have to remember the safety numbers of the safe CPUs (without a safety number there's only 1 byte with the time sync info
      if (pPara^.aPara[0] > 1) then
      
        // set the pointer to the first safety number
        pHelpHDINT := (pPara^.aPara[1] + sizeof(BOOL))$^HDINT;
 
        for i := 1 to ((pPara^.aPara[0]$UDINT-1)/4) do
          
          if pHelpHDINT & pHelpHDINT^ then
            aSafetyCPU_Nbr[SafeCPU_Nbr_No] := pHelpHDINT^;
            SafeCPU_Nbr_No += 1;
          end_if;

          pHelpHDINT += sizeof(HDINT);
        end_for;
      end_if;

//**********************************************************************************************************************************************************      
//    CMD_SM_FIRST_CONNECT_READY:
//      // tell everyone it's on!
//
//      // prevent interface-frame participants from finishing each others initialisation
//      if pMasterConnection then
//        InitSSW := _SetConnectionOk;
//      end_if;
            
//**********************************************************************************************************************************************************      
    CMD_SM_CPU_CFG_TO_MANAGER:     
      // if master or not we add the configuration to the local config memory, which is send on connect or disconnect&reconnect
      AddLocalCPUCfg((pPara^.aPara[0])$UINT, (pPara^.aPara[1])$^void);

//**********************************************************************************************************************************************************      
    CMD_SM_FSOE_CFG_TO_MANAGER:     
      // if master or not we add the configuration to the local config memory, which is send on connect or disconnect&reconnect
      AddLocalCPUCfg((pPara^.aPara[0])$UINT, (pPara^.aPara[1])$^void, TRUE);
        
//**********************************************************************************************************************************************************      
    CMD_SM_IF_MODULE_INFO,
    CMD_SM_IF_MODULE_INFO_FSOE,
    CMD_SM_IF_MODULE_INFO_FSOE_OK: 
      // the interface domain sent us their safety-numbers

      pHelpManaging := pFirstManagingInterfaceComponent;
      while pHelpManaging do
        // if we found the sender of this list in our list of interfaceframe components, remember the safety numbers
        if pHelpManaging^.pThis$DINT = pPara^.aPara[2] then

          // if we wait for it => safe them
          IFDataChanged := StoreInterfaceRoutingList(pPara, pResult, pHelpManaging, pPara^.uiCmd <> CMD_SM_IF_MODULE_INFO);
          
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
          if IFDataChanged = FALSE then
            if (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE) then
              _strcpy(dest:=#LogStr[0], src:="@DBG --- incomplete connections --- Received FSOE INFO with the same data from ");
            elsif (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE_OK) then
              _strcpy(dest:=#LogStr[0], src:="@DBG Received FSOE INFO OK with the same data from ");
            else  // CMD_SM_IF_MODULE_INFO
              _strcpy(dest:=#LogStr[0], src:="@DBG Received IF - INFO OK with the same data from ");
            end_if;
          else
            if (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE) then
              _strcpy(dest:=#LogStr[0], src:="@DBG --- incomplete connections --- Received FSOE INFO with new data from ");
            elsif (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE_OK) then
              _strcpy(dest:=#LogStr[0], src:="@DBG Received FSOE INFO OK with new data from ");
            else  // CMD_SM_IF_MODULE_INFO
              _strcpy(dest:=#LogStr[0], src:="@DBG Received IF - INFO OK with new data from ");
            end_if;
          
          end_if;

          _strcat(dest:=#LogStr[0], src:=#pHelpManaging^.ObjName[0]);
          LogError(e_msg:=#LogStr[0]);
#endif

          // resend the info to the module, which sent us the info so that bootup order doesn't matter (only if the response info in the first byte is false)
          if (InitSSW >= _Wait4InterfaceConfigs) & ((pPara^.aPara[1]$^BOOL)^ = FALSE) then
            // if not and we already have the config of the other system => the other domain waits for our config => send it
            SendInterfaceConfigs(pPara^.aPara[2]$pVirtualBase, TRUE, pPara^.uiCmd <> CMD_SM_IF_MODULE_INFO);
            
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
            _strcpy(dest:=#LogStr[0], src:="@DBG Sent FSOE INFO as answer back to ");
            _strcat(dest:=#LogStr[0], src:=#pHelpManaging^.ObjName[0]);
            LogError(e_msg:=#LogStr[0]);
#endif
          end_if;
                      
          // if we have new interface data or if the info is now complete (connections at other side establide) => reinitialize
          if IFDataChanged | ((pHelpManaging^.InfoBits.IncompleteFSoEConnections) & (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE_OK) & (InitSSW >= _Wait4InterfaceConfigs)) then
            pHelpManaging^.InfoBits.IncompleteFSoEConnections := FALSE;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED            
            _strcpy(dest:=#LogStr[0], src:="@DBG ");
            _strcat(dest:=#LogStr[0], src:=#pHelpManaging^.ObjName[0]);
            _strcat(dest:=#LogStr[0], src:=" triggered _ClearRoutingInfos ---------------- / InfoBits: %V2, Cmd: %V2, InitSSW: %V1, NewData: %V1");
            HWC_LogPrintf(this,#LogStr[0],#pHelpManaging^.InfoBits, #pPara^.uiCmd, #InitSSW, #IFDataChanged);
#endif
          
            // restart configuration to remove the non-complete FSoE connection settings
            InitSSW := _ClearRoutingInfos;          

          // if the connection info is not complete => remember it. we have to reconfigure as soon as we have the complete info
          elsif (pPara^.uiCmd = CMD_SM_IF_MODULE_INFO_FSOE) then
            pHelpManaging^.InfoBits.IncompleteFSoEConnections := TRUE;
          end_if;
          
          // found the sender and handled the data => get out of the while loop
          exit;
        end_if;
        pHelpManaging := pHelpManaging^.pNext;
      end_while;
      
//**********************************************************************************************************************************************************      
    CMD_SM_NEW_CFG_TYPE:
      // search for the right safety component
      pHelpSafety := pFirstSafetyComponent;
      while pHelpSafety do
        // if we found it => remember the actual config type
        if pHelpSafety^.SafetyNbr = pPara^.aPara[0]$HDINT then
          pHelpSafety^.NewConfigType := pPara^.aPara[1]$t_ConfigStateType;
          exit;
        end_if;
        
        pHelpSafety := pHelpSafety^.pNext;
      end_while;      

//**********************************************************************************************************************************************************      
    CMD_SM_MODULE_DISCONNECT:
      // deactivate entry in time sync list (if it's there)

      if pFirstTimeSyncElement then
        // check if we got this already
        pHelpTimeSync := pFirstTimeSyncElement;
        repeat
          if pHelpTimeSync^.pSafety & (pHelpTimeSync^.pSafety^.pThis = pPara^.aPara[0]$pVirtualBase) then
            // found this pointer in time sync list
            
            // mark the module missing, if it's optional
            if pHelpTimeSync^.pSafety^.InfoBits.IsOptional then
              pHelpTimeSync^.InfoBits.IsMissingOptional := TRUE;
            end_if;

            // if the actual time syncing module has been disconnected => select the next one
            if pActTimeSyncElement = pHelpTimeSync then      
              NextTimeSyncModule();
            end_if;

            exit;
          end_if;

          if pHelpTimeSync^.pManaging & (pHelpTimeSync^.pManaging^.pThis = pPara^.aPara[0]$pVirtualBase)  then
            // found this pointer in time sync list

            // mark the module missing, if it's optional
            if pHelpTimeSync^.pManaging^.InfoBits.IsOptional then
              pHelpTimeSync^.InfoBits.IsMissingOptional := TRUE;
            end_if;

            // if the actual time syncing module has been disconnected => select the next one
            if pActTimeSyncElement = pHelpTimeSync then      
              NextTimeSyncModule();
            end_if;

            exit;
          end_if;
          
          pHelpTimeSync := pHelpTimeSync^.pNext;
        until (pHelpTimeSync = pFirstTimeSyncElement) end_repeat;      
      end_if;

//**********************************************************************************************************************************************************      
    CMD_SM_MODULE_RECONNECT:
      // reactivate entry in time sync list (if it's there)

      if pFirstTimeSyncElement then
        // check if we got this already
        pHelpTimeSync := pFirstTimeSyncElement;
        repeat
          if pHelpTimeSync^.pSafety & (pHelpTimeSync^.pSafety^.pThis = pPara^.aPara[0]$pVirtualBase) then
            // found this pointer in time sync list
            
            // unmark the module if it's optional and was missing
            if pHelpTimeSync^.InfoBits.IsMissingOptional then
              pHelpTimeSync^.InfoBits.IsMissingOptional := FALSE;
            end_if;

            exit;
          end_if;

          if pHelpTimeSync^.pManaging & (pHelpTimeSync^.pManaging^.pThis = pPara^.aPara[0]$pVirtualBase)  then
            // found this pointer in time sync list

            // unmark the module if it's optional and was missing
            if pHelpTimeSync^.InfoBits.IsMissingOptional then
              pHelpTimeSync^.InfoBits.IsMissingOptional := FALSE;
            end_if;

            exit;
          end_if;
          
          pHelpTimeSync := pHelpTimeSync^.pNext;
        until (pHelpTimeSync = pFirstTimeSyncElement) end_repeat;      
      end_if;

//**********************************************************************************************************************************************************      
    CMD_SM_MODULE_REINIT:
      TellAllSafetyModules(pPara, pResult);
      
//**********************************************************************************************************************************************************      
    CMD_SM_QUIT_ERROR_AVAILABLE: // return if quit error via OS is possible (0..not possible, 1..possible, 2..possible with CIL v2) and also the version of the DLL
      pResult^.aData[0]$DINT := QuitErrorAvailable;
      
      if pSafetyDll2 then
        // return version of the new CIL interface
        pResult^.aData[4]$UDINT := pSafetyDll2^.version;
      elsif pSafetyDll then
        // return version of the old CIL interface
        pResult^.aData[4]$UDINT := pSafetyDll^.version;
      else
        // feature not available
        pResult^.aData[4]$UDINT := 0;
      end_if;

//**********************************************************************************************************************************************************      
    CMD_SM_QUIT_ERROR_VIA_OS:  // quit error at specified module via OS interface
      if (QuitErrorAvailable = 2) & pSafetyDll2 then
        
        // initialize the pointer
        pConnectionHandle := NIL;
        
        sd_retval := SAFETY_ALLOC_STATE2(pSafetyDll2, #pConnectionHandle);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -1;
          LogValue("@0437 (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_ALLOC_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          return;
        end_if;

        sd_retval := SAFETY_DISABLE_USERPROMPT_CHECK2(pSafetyDll2, pConnectionHandle);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -2;
          LogValue("@0439 (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DISABLE_USERPROMPT_CHECK2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_FREE_STATE2(pSafetyDll2, #pConnectionHandle);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_FREE_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;
        
        sd_retval := SAFETY_SET_SAFETY_NBR2(pSafetyDll2, pConnectionHandle, pPara^.aPara[1]$UDINT);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -3;
          LogValue("@043B (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_SET_SAFETY_NBR2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_FREE_STATE2(pSafetyDll2, #pConnectionHandle);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_FREE_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;

        sd_retval := SAFETY_OPEN_CONNECTION2(pSafetyDll2, pConnectionHandle);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -4;
          LogValue("@043D (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_OPEN_CONNECTION2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_FREE_STATE2(pSafetyDll2, #pConnectionHandle);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_FREE_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;

        // determine the quit error parameter according to the NewInst parameters
        
        // default is with remote modules
        quitRemoteModules := 1;
        
        // if the validity mark, version and parameter value are correct => take the parameter value (so far only 0 possible other than the default value)
        if (pPara^.aPara[2]$UDINT = SM_IF_VALIDITY_MARK) & (pPara^.aPara[3]$UDINT = SM_QUIT_ERROR_CMD_VERSION) & (pPara^.aPara[4]$USINT = 0) then
          quitRemoteModules := pPara^.aPara[4]$USINT;
        end_if;

        sd_retval := SAFETY_QUIT_ERROR2(pSafetyDll2, pConnectionHandle, quitRemoteModules);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -5;
          HWC_LogPrintf(this,"@043F (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_QUIT_ERROR2 with ErrorCode %d ",#sd_retval);

          // delete state for next action
          sd_retval := SAFETY_FREE_STATE2(pSafetyDll2, #pConnectionHandle);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_FREE_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;
        
        sd_retval := SAFETY_FREE_STATE2(pSafetyDll2, #pConnectionHandle);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -6;
          LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_FREE_STATE2 with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          return;
        end_if;

        pResult^.aData[0]$DINT := 0;

      elsif (QuitErrorAvailable = 1) & pSafetyDll then
        sd_retval := SAFETY_NEW_STATE(pSafetyDll);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -1;
          LogValue("@0437 (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_NEW_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          return;
        end_if;

        sd_retval := SAFETY_DISABLE_USERPROMPT_CHECK(pSafetyDll);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -2;
          LogValue("@0439 (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DISABLE_USERPROMPT_CHECK with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_DELETE_STATE(pSafetyDll);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DELETE_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;
        
        sd_retval := SAFETY_SET_SAFETY_NBR(pSafetyDll, pPara^.aPara[1]$UDINT);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -3;
          LogValue("@043B (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_SET_SAFETY_NBR with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_DELETE_STATE(pSafetyDll);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DELETE_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;

        sd_retval := SAFETY_OPEN_CONNECTION(pSafetyDll);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -4;
          LogValue("@043D (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_OPEN_CONNECTION with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);

          // delete state for next action
          sd_retval := SAFETY_DELETE_STATE(pSafetyDll);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DELETE_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;

        // determine the quit error parameter according to the NewInst parameters
        
        // default is with remote modules
        quitRemoteModules := 1;
        
        // if the validity mark, version and parameter value are correct => take the parameter value (so far only 0 possible other than the default value)
        if (pPara^.aPara[2]$UDINT = SM_IF_VALIDITY_MARK) & (pPara^.aPara[3]$UDINT = SM_QUIT_ERROR_CMD_VERSION) & (pPara^.aPara[4]$USINT = 0) then
          quitRemoteModules := pPara^.aPara[4]$USINT;
        end_if;

        sd_retval := SAFETY_QUIT_ERROR(pSafetyDll, quitRemoteModules);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -5;
          HWC_LogPrintf(this,"@043F (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_QUIT_ERROR with ErrorCode %d ",#sd_retval);

          // delete state for next action
          sd_retval := SAFETY_DELETE_STATE(pSafetyDll);
          if sd_retval then
            ret_code := ERROR;
            pResult^.aData[0]$DINT := -6;
            LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DELETE_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          end_if;

          return;
        end_if;
        
        sd_retval := SAFETY_DELETE_STATE(pSafetyDll);
        if sd_retval then
          ret_code := ERROR;
          pResult^.aData[0]$DINT := -6;
          LogValue("@043A (SafetyManager::ClassState::NewInst) QuitError failed at SAFETY_DELETE_STATE with ErrorCode 0x{0}", Value:=(sd_retval)$UDINT);
          return;
        end_if;

        pResult^.aData[0]$DINT := 0;
      else
        ret_code := ERROR;
        return;
      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_FSOE_AVAILABLE:
      b_GotFSoEModules := TRUE;

//**********************************************************************************************************************************************************      
    CMD_SM_FSOE_CON_INITIALIZED:
      FSoEConInitialized(SafetyNbr:=pPara^.aPara[0]$HDINT, ConnectionNo:=pPara^.aPara[1]$UDINT, pFirstConnection:=pPara^.aPara[2]$^t_FSoEConnectionDetails);
      
//**********************************************************************************************************************************************************      
    CMD_SM_POST_INIT_FINISHED: // post init is finished => time to get the configs of the safety CPUs
      // check all safety components for CPUs
      pHelpSafety := pFirstSafetyComponent;
      while pHelpSafety do
        // if it's a CPU or drive
        if IsCPUModule(pHelpSafety^.ModuleType) | (pHelpSafety^.ModuleType = _ModuleWithoutConfig) then
          // forward the command
          pHelpSafety^.pThis^.NewInst(pPara, pResult);
        end_if;
        
        pHelpSafety := pHelpSafety^.pNext;
      end_while;
      
      b_PostInitFinished := TRUE;
      
//**********************************************************************************************************************************************************      
    CMD_SM_CHECK_COMPATIBILITY: // check if a new feature is available
      if pPara^.aPara[0]$t_FeatureEnable.TransmissionTimeInConfig then
        bdFeatureEnable.TransmissionTimeInConfig := TRUE;
        
        // reset the bit in the request
        pPara^.aPara[0]$t_FeatureEnable.TransmissionTimeInConfig := FALSE;
      end_if;
      
      
      if pPara^.aPara[0]$t_FeatureEnable.FSoEFlags_4ByteSize then
        bdFeatureEnable.FSoEFlags_4ByteSize := TRUE;
        
        // reset the bit in the request
        pPara^.aPara[0]$t_FeatureEnable.FSoEFlags_4ByteSize := FALSE;
      end_if;

      
      // any features requested that are not supported?
      if pPara^.aPara[0]$BDINT then
        LogError("@0443 (SafetyManager::ClassState::NewInst) At least one requested feature is not available. Check if your safety classes are up-to-date!");
      end_if;
      
      pResult^.uiLng := 4;
      pResult^.aData[0]$t_FeatureEnable := bdFeatureEnable;
      
      //**********************************************************************************************************************************************************      
    CMD_POST_INIT_DISTRIBUTE_TASKTIMES:
    
      //called from hw control in postinit
      //Nothing to do here (only that no invalid command is received)
    
//**********************************************************************************************************************************************************      
    CMD_SM_UPDATE_FLEX_CON_HOST:
      // check if we're flexconhost and there is a number of connections in the parameter
      if (sFlexCon.Role = _FlexConHost) & (pPara^.aPara[0]) then
        // prepare parameters
        MyPara.uiCmd := CMD_SM_UPDATE_FLEX_CON_HOST;
      
        // go through all the managing nodes to see if they have a connection
        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          // initialize the safety number with 0 = no connection
          MyPara.aPara[0] := 0;
          
          // if it has an active connection
          if pHelpManaging^.FlexCon.pFSoEConnection then
            // check if one of the safety number is the list match the one of the managing module
            pHelpHDINT := pPara^.aPara[1]$^HDINT;
            for i := 0 to (pPara^.aPara[0]-1)$UDINT do
              // if the connected number from the HWK is found in the list of connected numbers from the SCP => remember it to send it to the managing module
              if pHelpManaging^.FlexCon.SafetyNumber = pHelpHDINT^ then
                MyPara.aPara[0] := pHelpHDINT^$DINT;
                
                // stop searching for this managing device
                exit;
              end_if;

              pHelpHDINT += sizeof(HDINT);
            end_for;
          end_if;

          // send the current number for this device
          pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult);              
        
          pHelpManaging := pHelpManaging^.pNext;
        end_while;        
      end_if;
      
//**********************************************************************************************************************************************************      
    CMD_HWC_CONVERT_COMPRESSED_HWPATH:
      
      //Method is used to search through the configuration for the handed over compressed hw path and returns the respective uncompressed hw path
      
      //pPara^.aPara[0] .. version
      //pPara^.aPara[1] .. comp hw path
      //pPara^.aPara[2] .. comp hw path length
      //pPara^.aPara[3] .. pointer to buffer where hw path will be copied
      //pPara^.aPara[4] .. size of buffer where data will be copied

      //pResult^.aData[0]$DINT .. version
      //pResult^.aData[4]$DINT .. retcode when error (negative) or amount of bytes of hw path
      
      //Check for valid pointer and data lengths
      if (pPara^.aPara[1] = NIL) 
       | (pPara^.aPara[2] = 0) 
       | (pPara^.aPara[3] = NIL) 
       | (pPara^.aPara[4] = 0) then
       
        ret_code := ERROR;
        pResult^.uiLng := 8;
        pResult^.aData[0]$DINT := 1;  //version of command
        pResult^.aData[4]$DINT := -1; //Invalid pointer or length
        return;
      end_if;
      
      if pConfigs = NIL  then        
        ret_code := ERROR;
        pResult^.uiLng := 8;
        pResult^.aData[0]$DINT := 1;  //version of command        
        pResult^.aData[4]$DINT := -2; //no configs yet
        return;
      end_if;    
      
      pHelpConfigs := pConfigs;
      
      while pHelpConfigs do
      
        //Go through module configs
        //=============================================================================================================
        if pHelpConfigs^.pFirstModuleCfg then
          pHelpModuleConf := pHelpConfigs^.pFirstModuleCfg;
          
          while pHelpModuleConf do
            
            //Check if we got a valid path and len
            if pHelpModuleConf^.ModuleInfo.pCompPath & pHelpModuleConf^.ModuleInfo.CompPathLen then
              
              //Check if we found the passed path
              if _memcmp(ptr1:=pPara^.aPara[1]$^void, ptr2:=pHelpModuleConf^.ModuleInfo.pCompPath, cntr:=pPara^.aPara[2]$UDINT) = 0 then
                
                //we found the compressed hw path, return pointer to normal hw path
                if pPara^.aPara[4] > pHelpModuleConf^.ModuleInfo.PathLen then
                
                  //Copy path to user pointer
                  _memcpy(ptr1:=pPara^.aPara[3]$^void, ptr2:=pHelpModuleConf^.ModuleInfo.pPath, cntr:=pHelpModuleConf^.ModuleInfo.PathLen);
                
                  ret_code := READY;
                  pResult^.uiLng := 8;
                  
                  pResult^.aData[0]$DINT := 1;  //version
                  pResult^.aData[4]$DINT := pHelpModuleConf^.ModuleInfo.PathLen;
                  return;
                  
                else
                  ret_code := ERROR;
                  pResult^.uiLng := 8;
                  pResult^.aData[0]$DINT := 1;   //version
                  pResult^.aData[4]$DINT := -3;  //not enough space to copy path
                  return;

                end_if;

              end_if;
            end_if;
            pHelpModuleConf := pHelpModuleConf^.pNext;
          end_while;
        end_if;

        //Go through slave config
        //=============================================================================================================
        if pHelpConfigs^.pFirstSlaveCfg then
        
          pHelpSlaveCfg := pHelpConfigs^.pFirstSlaveCfg;
          
          while pHelpSlaveCfg do
          
            //Check if we got a valid path and len
            if pHelpSlaveCfg^.SlaveInfo.pCompPath & pHelpSlaveCfg^.SlaveInfo.CompPathLen then
              
              //Check if we found the passed path
              if _memcmp(ptr1:=pPara^.aPara[1]$^void, ptr2:=pHelpSlaveCfg^.SlaveInfo.pCompPath, cntr:=pPara^.aPara[2]$UDINT) = 0 then
                

                //we found the compressed hw path, return pointer to normal hw path
                if pPara^.aPara[4] > pHelpSlaveCfg^.SlaveInfo.PathLen then
                
                  //Copy path to user pointer
                  _memcpy(ptr1:=pPara^.aPara[3]$^void, ptr2:=pHelpSlaveCfg^.SlaveInfo.pPath, cntr:=pHelpSlaveCfg^.SlaveInfo.PathLen);
                
                  ret_code := READY;
                  pResult^.uiLng := 8;
                  
                  pResult^.aData[0]$DINT := 1;  //Path was found
                  pResult^.aData[4]$DINT := pHelpSlaveCfg^.SlaveInfo.PathLen;
                  return;
                  
                else
                  ret_code := ERROR;
                  pResult^.uiLng := 8;
                  pResult^.aData[0]$DINT := 1;   //version
                  pResult^.aData[4]$DINT := -3;  //not enough space to copy path
                  return;

                end_if;
              end_if;
            end_if;
            pHelpSlaveCfg := pHelpSlaveCfg^.pNext;
          end_while;
        end_if;
        
        //Go through fsoe connections
        //=============================================================================================================
        if pHelpConfigs^.pFirstFSoEConnection then
          pHelpFsoeConn := pHelpConfigs^.pFirstFSoEConnection;
          
          while pHelpFsoeConn do
          
            //Check if we got a valid path and len
            if pHelpFsoeConn^.pSlavePathComp & pHelpFsoeConn^.SlavePathLen then
              
              //Check if we found the passed path
              if _memcmp(ptr1:=pPara^.aPara[1]$^void, ptr2:=pHelpFsoeConn^.pSlavePathComp, cntr:=pPara^.aPara[2]$UDINT) = 0 then
                
                //we found the compressed hw path, return pointer to normal hw path
                if pPara^.aPara[4] > pHelpFsoeConn^.SlavePathLen then
                
                  //Copy path to user pointer
                  _memcpy(ptr1:=pPara^.aPara[3]$^void, ptr2:=pHelpFsoeConn^.pSlavePath, cntr:=pHelpFsoeConn^.SlavePathLen);
                
                  ret_code := READY;
                  pResult^.uiLng := 8;
                  
                  pResult^.aData[0]$DINT := 1;  //Path was found
                  pResult^.aData[4]$DINT := pHelpFsoeConn^.SlavePathLen;
                  return;
                  
                else
                  ret_code := ERROR;
                  pResult^.uiLng := 8;
                  pResult^.aData[0]$DINT := 1;   //version
                  pResult^.aData[4]$DINT := -3;  //not enough space to copy path
                  return;
                end_if;
              end_if;

            end_if;
            pHelpFsoeConn := pHelpFsoeConn^.pNext;
          end_while;
        end_if;
        
        pHelpConfigs := pHelpConfigs^.pNext;
      end_while;

      //comp hw path was not found in config
      ret_code := ERROR;
      pResult^.uiLng := 8;
      pResult^.aData[0]$DINT := 1;   //version
      pResult^.aData[4]$DINT := -4;  //passed path was not found in configuration list
      
//**********************************************************************************************************************************************************      
  else
    bd_LogRtMessagesInCy.InvalidCommandReceived := TRUE;
  END_CASE;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
  if InitSSW <> OldInitSSW then
#ifdef SAFETY_DEBUG_TRACING_ACTIVATED
    LogValue("@DBG (SafetyManager::ClassState::NewInst) InitSSW changed from {0} to {1}", Value:=OldInitSSW, Value2:=InitSSW);
#endif
    LogBuf[ActLogIndex].NewState := InitSSW;
    LogBuf[ActLogIndex].OldState := OldInitSSW;
    LogBuf[ActLogIndex].Command := 0;
    LogBuf[ActLogIndex].TimeSinceLastLog_us := OS_READMICROSEC();
    LogBuf[ActLogIndex].TimeSinceLastLog_us -= TimeStampLastLog_us;
    TimeStampLastLog_us := OS_READMICROSEC();
    if ActLogIndex = (SM_LogBufferSize - 1) then
      ActLogIndex := 0;
    else
      ActLogIndex += 1;
    end_if;
  end_if;
  OldInitSSW := InitSSW;
#endif

  ret_code := READY;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL SafetyManager::CyWork
  VAR_INPUT
    EAX 	: UDINT;
  END_VAR
  VAR_OUTPUT
    state (EAX) 	: UDINT;
  END_VAR
  VAR
    MyPara 	      : CmdStruct;
    MyResult	    : results;
    pHelpSafety   : ^t_SafetyComponent;
    pHelpManaging : ^t_ManagingComponent;
    i             : UDINT;
    Synchron      : BOOL;
    AllReady      : BOOL;
    pHelpCfg      : ^t_LocalCfgListElem;
    pHelpConfig   : ^t_CfgListElem;
  END_VAR

//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StartTimeMeasurement(VARANTIME_CY);
#endif
//=================================================================================================
  // if we got UDP stuff going on => start the udp task
  if (sUdpVars.UdpTaskStarted = FALSE) & (sUdpVars.ProducerConnectionNo | ((sUdpVars.UdpInit AND 2#11) = (t_UdpInit.FSoEAvailable OR t_UdpInit.ProvidesIF))) then
    To_MultiTask.CREATETHREAD(taskfunction0 := #UdpTask()
                            , priority0     := 14           // same priority as cyclic task
                            , stackSize0    := 0x4000
                            , flags0        := MT_TASK_SAVETHIS
                            , parameter0    := THIS
                            , name0         := "SafetyUDP");
    
    sUdpVars.UdpTaskStarted := TRUE;
  end_if;

  case InitSSW of
//**********************************************************************************************************************************************************
    _NotMaster: // don't start any actions, because we're not the master. but if the master wants something from us, we help out

//**********************************************************************************************************************************************************
    _ClearRoutingInfos: // initialize the structures or clean up an old configuration
      if ClearRoutingInformations() then

        // if we still need to wait for optional modules
        if b_SynchroTimeoutDone = FALSE then
          // reactivate the domain synchronicity timeout
          b_SynchroTimeoutSet := FALSE;
        end_if;

        InitSSW := _FirstWait4SynchronManagingNodes;
      end_if;

//**********************************************************************************************************************************************************
    _FirstWait4SynchronManagingNodes: // wait for managing nodes to be syncron (all nodes are optionaly for this case, the will be checked later again)
      if SafetyDomainTimeout & (b_SynchroTimeoutSet = FALSE) then
        b_SynchroTimeoutSet := TRUE;
        SynchroTimeout := ops.tAbsolute;
      end_if;
    
      Synchron := AllManagingNodesSynchron();
                  
      if Synchron then
        if b_SyncTimeoutExceeded then
          b_SyncTimeoutExceeded := FALSE;
          if (b_SynchroTimeoutDone = FALSE) then
            LogError("@0444 (SafetyManager::CyWork) Timeout for managing module (VBC, VMC,..) exceeded! Initialisation will continue to check if it's optional!");
          end_if;
        end_if;

        InitSSW := _SetConnectionOk;
      end_if;

//**********************************************************************************************************************************************************
    _SetConnectionOk:
      // send hw-connection ok to start SDOs
      MyPara.uiCmd := CMD_SM_FIRST_CONNECT_READY;
      MyPara.aPara[0] := 0; // length of parameters

      TellAllSafetyModules(#MyPara, #MyResult);
      
      b_MandatoryStuffAvailable := FALSE;
      LastCfgReceiveTime := ops.tAbsolute;
      
      InitSSW := _Wait4ReInit;

//**********************************************************************************************************************************************************
    _Wait4ReInit: // wait until all local safe cpus have a valid configuration before we wait to be synchron with other domains

      // add our local configurations if there are any
      SendLocalCfgsToMaster();
     
      // wait until we got a valid config of every local safety cpu
      if (LocalCPUCfgNo = LocalCPUNo) | b_MandatoryStuffAvailable then
        AllReady := TRUE;

        if LocalCPUCfgNo then
          pHelpCfg := pFirstLocalCPUCfg;
          
          // for is used to ensure thread safety
          for i := 0 to (LocalCPUCfgNo-1) do
            if pHelpCfg^.ConfigSize = 0 then
              AllReady := FALSE;
              exit;
            end_if;
            
            pHelpCfg := pHelpCfg^.pNext;
          end_for;
        end_if;
        
        // if we have a config from every safe cpu, we start the initialisation
        if AllReady then
          InitSSW := _CheckLocalCPUConfigs;
        end_if;
      else
        // if we already got a config => check if there is something optional
        if LocalCPUCfgNo & ((ops.tAbsolute - LastCfgReceiveTime) > 5000) then
          CheckOptional();
        end_if;
      
        InitSSW := _SendInterfaceConfigs;
      end_if;

//**********************************************************************************************************************************************************
    _CheckLocalCPUConfigs: // check for unused and optional modules in the local configurations (needed for correct time synchronisation)
      // if we have a configuration of every safety cpu => check if the managing modules are optional or any of the safety/managing components is unused (not involved in any PDO)
      if (LocalCPUCfgNo = LocalCPUNo) | b_MandatoryStuffAvailable then

        // set the IsOptional-Bit of all modules (safety and managing components) to TRUE
        // in CheckModuleUsage the the non-optional will get the IsOptional-Bit set to FALSE
        pHelpSafety := pFirstSafetyComponent;
        while pHelpSafety do
          pHelpSafety^.InfoBits.IsOptional    := TRUE;
          
          pHelpSafety := pHelpSafety^.pNext;
        end_while;

        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          pHelpManaging^.InfoBits.IsOptional    := TRUE;

          pHelpManaging := pHelpManaging^.pNext;
        end_while;

        // check all configurations which modules are used
        if LocalCPUCfgNo then
          pHelpCfg := pFirstLocalCPUCfg;
          
          for i := 0 to (LocalCPUCfgNo-1) do
            // check for used modules and mark them in the infobits
            CheckModuleUsage(pHelpCfg^.ConfigSize, pHelpCfg^.pConfig, pHelpCfg^.IsFSoECfg);
            
            pHelpCfg := pHelpCfg^.pNext;
          end_for;
        end_if;
        
        // now create the linked list for the time synchronisation
        CreateTimeSyncList();


        if (pFirstManagingInterfaceComponent = NIL) then
          // if we have FSoE Modules => check the connection between the modules and tell them what to copy
          if b_GotFSoEModules then
            CreateFSoEPDOs();

            if b_AllLocalSlaveAddressesValid then
              InitSSW := _NoDomains;
            else
              // wait for all local connections to be established
              SetTimeout();
              InitSSW := _Wait4FSoEConnectionsLocal;
            end_if;
          else
            InitSSW := _NoDomains;                        
          end_if;
        else
          // if we still need to wait for optional modules
          if b_SynchroTimeoutDone = FALSE then
            // reactivate the domain synchronicity timeout, because now we know what is really optional
            b_SynchroTimeoutSet := FALSE;
          end_if;

          InitSSW := _Wait4SynchronManagingNodes;
        end_if;
        
      end_if;      

//**********************************************************************************************************************************************************
    _Wait4FSoEConnectionsLocal:
      CheckTimeout();
      if b_AllLocalSlaveAddressesValid then
        CreateFSoEPDOs();
        // success => continue
        InitSSW := _NoDomains;
      end_if;

//**********************************************************************************************************************************************************
    _Wait4SynchronManagingNodes: // wait until all managing modules are synchron with the other stations
      if SafetyDomainTimeout & (b_SynchroTimeoutSet = FALSE) then
        b_SynchroTimeoutSet := TRUE;
        SynchroTimeout := ops.tAbsolute;
      end_if;
    
      Synchron := AllManagingNodesSynchron();

      if Synchron then
        if b_SyncTimeoutExceeded then
          b_SyncTimeoutExceeded := FALSE;
          if (b_SynchroTimeoutDone = FALSE) then         
            LogError("@0445 (SafetyManager::CyWork) Timeout for optional managing module (VBC, VMC,..) exceeded! Initialisation will continue without it!");
          end_if;
        end_if;
  
        // add our own safety cpus to the config list
        AddOwnCPUsToList();

        if NewTimeSyncPossible() then
          // tell every safety module, that it has to initialize the memory for writing the time synchronisation
          SetNewSyncMode(TRUE);
          MyPara.aPara[3] := 1;
        else
          SetNewSyncMode(FALSE);
          MyPara.aPara[3] := 0;
        end_if;

        LocalInitStartTime := ops.tAbsolute;
        
        InitSSW := _Wait4LocalInit;
      end_if;

//**********************************************************************************************************************************************************
    _Wait4LocalInit:
      // wait for the local PDOs to fall into watchdog error which will be later automatically quit via set bus ready command
      // if we don't wait and they fall into watchdog error after receiving the set bus ready command, the watchdog error would have to be quit manually
      if (ops.tAbsolute - LocalInitStartTime) > SM_LOCAL_INIT_WAIT_TIME then
        InitSSW := _Wait4CPUConfigs;
        SetTimeout();
      end_if;

//**********************************************************************************************************************************************************
    _Wait4CPUConfigs:
      CheckTimeout();
      if AllCPUConfigsArrived() then
        // if all needed modules are available (all non-optional modules or interface frames)
        if CheckCPUConfigs() then
          // check if there are PDO configs to broadcast
          if ActPDO_ID then
            // before we broadcast the PDOs, we check if the length informations are correct on every entry
            PDOLengthCorrection();

            // if we have interface-connections, we wait for the safety numbers of it
            if pFirstManagingInterfaceComponent then 
              if AnyIFDomainSynchron() then
                // we have a connection via managing module (interface frame) => share our interface infos
                InitSSW := _SendInterfaceConfigs;
              else
                // otherwise => finalize initialisation
                InitSSW := _Wait4CopyCmdStart;
              end_if;
            elsif pActualSendPDO then
              InitSSW := _SendNextPDO;
            end_if;
          else
            if pFirstManagingInterfaceComponent & b_GotFSoEModules then
              // we have some FSoE Connections
              InitSSW := _SendInterfaceConfigs;
            else
              // otherwise => finalize initialisation
              InitSSW := _Wait4CopyCmdStart;
            end_if;
          end_if;
        else
          // if configs are not ok => set everything to finished so that the firmware can show the error information (otherwise the firmware will not start doing anything)
          InitSSW := _Wait4CopyCmdStart;
        end_if;
      end_if;
    
//**********************************************************************************************************************************************************
    _SendInterfaceConfigs:
      // if all domain managing modules needed for non-optional pdos are synchron
      if AllNonOptionalIFDomainsSynchron() & ((LastIfCfgDistribution = 0) | ((ops.tAbsolute - LastIfCfgDistribution) > SM_INTERFACE_CONFIG_SEND_PAUSE)) then
        if b_GotFSoEModules then
          CreateFSoEPDOs();
        end_if;
        
        // send our configs to all inferface domains
        SendInterfaceConfigs(NIL, FALSE, b_GotFSoEModules);
        
        // remember when the config has been distributed, to not spam the other sides
        LastIfCfgDistribution := ops.tAbsolute;
        
        InitSSW := _Wait4InterfaceConfigs;
        SetTimeout();
      end_if;

//**********************************************************************************************************************************************************
    _Wait4InterfaceConfigs:
      CheckTimeout();
      if AllInterfaceCfgsArrived() then
        if (LocalCPUCfgNo = LocalCPUNo) | b_MandatoryStuffAvailable then
          if pActualSendPDO then
            InitSSW := _SendNextPDO;
          else
            if b_GotFSoEModules then
              SetTimeout();
              InitSSW := _Wait4FSoEConnectionsExternal;
            else
              InitSSW := _Wait4ReInit;
            end_if;
          end_if;
        else
          InitSSW := _Wait4ReInit;
        end_If;
      end_if;

//**********************************************************************************************************************************************************
    _Wait4FSoEConnectionsExternal:
      CheckTimeout();
      if b_AllLocalSlaveAddressesValid then
        CreateFSoEPDOs();
        FinalizePDOMapping();
        InitSSW := _FinishInitialisation;
      end_if;
    
//**********************************************************************************************************************************************************
    _SendNextPDO:
      // broadcast the PDO
      BroadCastActPDO();
      
      InitSSW := _Wait4PDOAcks;
      SetTimeout();
      
//**********************************************************************************************************************************************************
    _Wait4PDOAcks:
      // wait until all acknowledges arrive (all modules which are needed for PDO transport have to answer!)
      CheckTimeout();

      // check if there is another PDO configuration to send
      repeat
        if pActualSendPDO^.pNext then
          pActualSendPDO := pActualSendPDO^.pNext;
        else
          if pActualSendCfg^.pNext then
            pActualSendCfg := pActualSendCfg^.pNext;
            pActualSendPDO := pActualSendCfg^.pFirstPdoCfg;
            
            // if there is no pdo config in this config, go to the next config
            while (pActualSendPDO = NIL) & pActualSendCfg^.pNext do
              pActualSendCfg := pActualSendCfg^.pNext;
              pActualSendPDO := pActualSendCfg^.pFirstPdoCfg;
            end_while;
          else
            pActualSendCfg := NIL;
            pActualSendPDO := NIL;
          end_if;
        end_if;
        
      // skip double entries
      until ((pActualSendPDO = NIL) | (pActualSendPDO^.PdoInfo.Info.DoublePDOEntry = FALSE)) end_repeat;
      
      // if we found another PDO => send it
      if pActualSendPDO then
        InitSSW := _SendNextPDO;
      else
        // otherwise => finalize initialisation
        InitSSW := _Wait4CopyCmdStart;
      end_if;      
      
//**********************************************************************************************************************************************************
    _Wait4CopyCmdStart:
      // this step is handlet in RT because the DOs are isochronous

//**********************************************************************************************************************************************************
    _NoDomains:
      // No domains available => just check if there are old modules or new modules used
      if NewTimeSyncPossible() then
        // tell every safety module, that it has to initialize the memory for writing the time synchronisation
        SetNewSyncMode(TRUE);
      else
        SetNewSyncMode(FALSE);
      end_if;

      // we may have internal interface frames and therefore we have to create a list with the local CRCs
      if LocalIFsAvailable() then
        // add local IFs for easier SDO/TDO forwarding
        CheckCPUConfigs();

        // check if there are PDO configs to broadcast
        if ActPDO_ID & (b_GotFSoEModules = FALSE) then
          // before we broadcast the PDOs, we check if the length informations are correct on every entry
          PDOLengthCorrection();

          // distribute PDOs locally
          pHelpConfig := pConfigs;
          while pHelpConfig do
            pActualSendCfg := pHelpConfig;
            pActualSendPDO := pHelpConfig^.pFirstPdoCfg;
            while pActualSendPDO do
              BroadCastActPDO();
            
              pActualSendPDO := pActualSendPDO^.pNext;
            end_while;
          
            pHelpConfig := pHelpConfig^.pNext;
          end_while;
        end_if;
      end_if;
      
      InitSSW := _FinishInitialisation;
      
//**********************************************************************************************************************************************************
    _FinishInitialisation:
      // if it's the new mode also start the sync process at the first safety device
      MyPara.uiCmd := CMD_SM_INIT_FINISHED;
      MyPara.aPara[0] := 0; // length of parameters

      // sent set bus ready signal (quit error 199 if active)
      TellAllSafetyModules(#MyPara, #MyResult);
            
      // if we have the new sync mode => start the time synchronisation here
      if b_NewSyncMode then
        b_StartTimeSync := TRUE;
      end_if;

      MinSyncDuration := 16#7FFFFFFF;
      
      // remember to not wait again for optional modules if the safety manager is initialized again (ClearRoutingInfos)
      b_SynchroTimeoutDone := TRUE;
      
      // if there was an error and we still reached this point, don't conceal the error (except if it's a timeout in this init sequence or a SDO problem which could have been fixed)
      if (ClassState = 0) | (ClassState = -11) | (ClassState = -8) then
        ClassState := 1;
      end_if;
      
      InitSSW := _InitFinished;
      
//**********************************************************************************************************************************************************
    _InitFinished: // finished with initialisation, now start our cyclic business

      // check sync timeout counter
      if SyncTimeoutCounter >= TIMESYNC_TIMEOUTS_BEFORE_LOGMSG then
        LogError("@0448 (SafetyManager::CyWork) Timeout while Timesynchronizing");
        
        // reset counter
        SyncTimeoutCounter := 0;
      end_if;
      
      // print log messages from realtime here to prevent rt-runtime errors
      if bd_LogRtMessagesInCy then
        if bd_LogRtMessagesInCy.NoTDOTargetToManager then
          bd_LogRtMessagesInCy.NoTDOTargetToManager := FALSE;
          LogError("@0449 (SafetyManager::ClassState::NewInst) Couldn't find target of TDO on the way to the manager");
        end_if;

        if bd_LogRtMessagesInCy.NoTDOTargetToModule then
          bd_LogRtMessagesInCy.NoTDOTargetToModule := FALSE;
          LogError("@044A (SafetyManager::ClassState::NewInst) Couldn't find target of TDO on the way to the module");
        end_if;

        if bd_LogRtMessagesInCy.InvalidCommandReceived then
          bd_LogRtMessagesInCy.InvalidCommandReceived := FALSE;
          LogError("@044B (SafetyManager::State::NewInst) Invalid command received");
        end_if;

        if bd_LogRtMessagesInCy.UnkownCopySource then
          bd_LogRtMessagesInCy.UnkownCopySource := FALSE;
          LogError("@044D (SafetyManager::RtWork) Could not activate copy command, because source address is unknown (waiting for safety number info)");
        end_if;

        if bd_LogRtMessagesInCy.InvalidIFConfiguration then
          bd_LogRtMessagesInCy.InvalidIFConfiguration := FALSE;
          LogError("@044E (SafetyManager::GiveTokenToNextModule) Only non-synchron interface domains found => need at least one safe CPU!");
        end_if;

        if bd_LogRtMessagesInCy.TooManyOptionalMissing then
          bd_LogRtMessagesInCy.TooManyOptionalMissing := FALSE;
          LogError("@044F (SafetyManager::GiveTokenToNextModule) Too many optional modules missing for a correct time sync cycle");
        end_if;

        if bd_LogRtMessagesInCy.UdpReceivedInvalidCommand then
          bd_LogRtMessagesInCy.UdpReceivedInvalidCommand := FALSE;
          LogError("@0450 (SafetyManager::ReceiveUdpData) Received invalid command via UDP");
        end_if;

        if bd_LogRtMessagesInCy.UdpReceivedInvalidState then
          bd_LogRtMessagesInCy.UdpReceivedInvalidState := FALSE;
          LogError("@0451 (SafetyManager::ReceiveUdpData) Received invalid state via UDP");
        end_if;
        
        if bd_LogRtMessagesInCy.UdpNoMem4Connections then
          bd_LogRtMessagesInCy.UdpNoMem4Connections := FALSE;
          LogError("@0452 (SafetyManager::AddIpConnection) Can't add udp connection, since the memory is not allocated");
        end_if;
      end_if;
//**********************************************************************************************************************************************************
  end_case;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
  if InitSSW <> OldInitSSW then
#ifdef SAFETY_DEBUG_TRACING_ACTIVATED
    LogValue("@DBG (SafetyManager::CyWork) InitSSW changed from {0} to {1}", Value:=OldInitSSW, Value2:=InitSSW);
#endif
    LogBuf[ActLogIndex].NewState := InitSSW;
    LogBuf[ActLogIndex].OldState := OldInitSSW;
    LogBuf[ActLogIndex].Command := 0;
    LogBuf[ActLogIndex].TimeSinceLastLog_us := OS_READMICROSEC();
    LogBuf[ActLogIndex].TimeSinceLastLog_us -= TimeStampLastLog_us;
    TimeStampLastLog_us := OS_READMICROSEC();
    if ActLogIndex = (SM_LogBufferSize - 1) then
      ActLogIndex := 0;
    else
      ActLogIndex += 1;
    end_if;
  end_if;
  OldInitSSW := InitSSW;
#endif

  state := READY;

//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StopTimeMeasurement(VARANTIME_CY);
#endif
//=================================================================================================

END_FUNCTION


FUNCTION SafetyManager::AddModule
  VAR_INPUT
    pPara 	: ^CmdStruct;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    ModuleType : t_e_ModuleType;
  END_VAR

  retcode := FALSE;

  ModuleType := pPara^.aPara[0]$t_e_ModuleType;

  // we differ between real safety components and managing components
  case ModuleType of
    _CSCP01x,
    _CSDI16x,
    _CSTO08x,
    _SCP010,
    _SCP011,
    _SCP111,
    _SCP211,
    _MS011,
    _MS012,
    _SDD1300,
    _SDD1400,
    _SDD1500,
    _SDD1600,
    _ModuleWithoutConfig:
        retcode := AddSafetyModule(ModuleType, pPara^.aPara[1]$pVirtualBase);
        
        if IsFSoEModule(ModuleType) then
        
          //only allocate memory if it is not done before
          if sUdpVars.pTempSendBufferCy = NIL then
            // allocate memory for the temporary cyclic send buffer
            sUdpVars.pTempSendBufferCy$pVoid := To_StdLib.Malloc(size:=sizeof(t_UdpPacketV2));
            
            if sUdpVars.pTempSendBufferCy then
              _memset(dest:=sUdpVars.pTempSendBufferCy, usByte:=0, cntr:=sizeof(t_UdpPacketV2));
            else
              SetError(bError:=TRUE, ErrorMessage:=_NO_MEMORY_SENDBUFFER_UDP_ERROR, dError:=0);
              LogError("@0453 (SafetyManager::AddModule) Failed to allocate memory for udp send buffer");
            end_if;          
          end_if;
          
          //only allocate memory if it is not done before
          if sUdpVars.pTempSendBufferIRQ = NIL then
            // allocate memory for the temporary irq send buffer
            sUdpVars.pTempSendBufferIRQ$pVoid := To_StdLib.Malloc(size:=sizeof(t_UdpPacketV2));
            
            if sUdpVars.pTempSendBufferIRQ then
              _memset(dest:=sUdpVars.pTempSendBufferIRQ, usByte:=0, cntr:=sizeof(t_UdpPacketV2));
            else
              SetError(bError:=TRUE, ErrorMessage:=_NO_MEMORY_SENDBUFFER_UDP_ERROR, dError:=0);
              LogError("@0453 (SafetyManager::AddModule) Failed to allocate memory for udp send buffer");
            end_if;          
          end_if;

          //only allocate memory if it is not done before
          if sUdpVars.paConnectionsToProducers = NIL then
            sUdpVars.paConnectionsToProducers$pVoid := To_StdLib.Malloc(size:=sizeof(t_aUdpConnections));
        
            if sUdpVars.paConnectionsToProducers then
              _memset(dest:=sUdpVars.paConnectionsToProducers, usByte:=0, cntr:=sizeof(t_aUdpConnections));
            else
              SetError(bError:=TRUE, ErrorMessage:=_NO_MEMORY_CONNECTION_BUFFER_UDP_ERROR, dError:=0);
              LogError("@0454 (SafetyManager::AddModule) Failed to allocate memory for udp receive buffer");
            end_if;
          end_if;
          
          //only allocate memory if it is not done before
          if sUdpVars.paConnectionsToConsumers = NIL then
            sUdpVars.paConnectionsToConsumers$pVoid := To_StdLib.Malloc(size:=sizeof(t_aUdpConnections));
        
            if sUdpVars.paConnectionsToConsumers then
              _memset(dest:=sUdpVars.paConnectionsToConsumers, usByte:=0, cntr:=sizeof(t_aUdpConnections));
            else
              SetError(bError:=TRUE, ErrorMessage:=_NO_MEMORY_CONNECTION_BUFFER_UDP_ERROR, dError:=0);
              LogError("@0454 (SafetyManager::AddModule) Failed to allocate memory for udp receive buffer");
            end_if;
          end_if;
        end_if;
    
    _VBC021Master,
    _VBC021Slave, // (is limited to 1 per system, because the system can't sync to different systems at the same time)
    _VMC052,
    _VaranManager4VMC, // VaranManager (VMC052 counterpart is also limited to 1 per system, because the system can't sync to different systems at the same time)
    _GenericTimeSyncMaster,
    _GenericTimeSyncSlave,
    _GenericFSoE:
        retcode := AddManagingModule(ModuleType, pPara^.aPara[1]$pVirtualBase, pPara^.aPara[2]$^USINT);
    _GenericDynamicFSoE:
        retcode := AddGenericDynamic(ModuleType, pPara^.aPara[1]$pVirtualBase, pPara^.aPara[2]$^USINT);
  else
    // invalid module type
    ClassState := -7;
    LogError("@0455 (SafetyManager::AddModule) Invalid ModuleType");
  end_case;

  if retcode then
    LoginCnt += 1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddSafetyModule
  VAR_INPUT
    ModuleType 	: t_e_ModuleType;
    pThis 	: pVirtualBase;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    pHelpSafety : ^t_SafetyComponent;
  END_VAR

  if pFirstSafetyComponent = NIL then
    pFirstSafetyComponent$pVoid := To_StdLib.Malloc(size:=sizeof(t_SafetyComponent));
    pHelpSafety := pFirstSafetyComponent;    
  else
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety^.pNext <> NIL do
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
    pHelpSafety^.pNext$pVoid := To_StdLib.Malloc(size:=sizeof(t_SafetyComponent));
    pHelpSafety := pHelpSafety^.pNext;
  end_if;
  
  if pHelpSafety <> NIL then
    _memset(dest:=pHelpSafety, usByte:=0, cntr:=sizeof(t_SafetyComponent));
    pHelpSafety^.ModuleType   := ModuleType;
    
    // get the object name for easier debugging
    GetObjName(pThis, #pHelpSafety^.ObjName[0], sizeof(t_SafetyComponent.ObjName));
    
    pHelpSafety^.pThis        := pThis;

//    pHelpSafety^.pNext        := NIL;  // not necessary because of memset
    
    case ModuleType of
//**********************************************************************************************************************************************************
      _CSCP01x,
      _SCP010,
      _SCP011,
      _SCP111,
      _SCP211,
      _SDD1300,
      _SDD1400,
      _SDD1500,
      _SDD1600:
        LocalCPUNo += 1;
       
//**********************************************************************************************************************************************************
      _CSDI16x,
      _MS011,
      _MS012:
        pHelpSafety^.InfoBits.OnlyInputs := TRUE;
//**********************************************************************************************************************************************************
    end_case;
        
    us_SafetyModuleNo += 1;
    
    retcode := TRUE;
  else
    ClassState := -1;

    retcode := FALSE;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddManagingModule
  VAR_INPUT
    ModuleType 	: t_e_ModuleType;
    pThis 	: pVirtualBase;
    pObjectPath 	: ^USINT;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
    pMyPath : ^USINT;
  END_VAR

  if pFirstManagingInterfaceComponent = NIL then
    pFirstManagingInterfaceComponent$pVoid := To_StdLib.Malloc(size:=sizeof(t_ManagingComponent));
    pHelpManaging := pFirstManagingInterfaceComponent;    
  else
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging^.pNext <> NIL do
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
    pHelpManaging^.pNext$pVoid := To_StdLib.Malloc(size:=sizeof(t_ManagingComponent));
    pHelpManaging := pHelpManaging^.pNext;
  end_if;
  
  if pHelpManaging <> NIL then
    _memset(dest:=pHelpManaging, usByte:=0, cntr:=sizeof(t_ManagingComponent));
    pHelpManaging^.ModuleType     := ModuleType;

    // get the object name for easier debugging
    GetObjName(pThis, #pHelpManaging^.ObjName[0], sizeof(t_ManagingComponent.ObjName));
    
    pHelpManaging^.pThis          := pThis;
    pHelpManaging^.DomainNr       := 16#FF;
    
    if pObjectPath then
      pMyPath$^void := To_StdLib.Malloc(size:=pObjectPath^ + 2);
      
      // convert 1 byte length info into 2 byte length info
      if pMyPath then
        pMyPath^$UINT := pObjectPath^$USINT;
        To_StdLib.MemCpy(dest:=pMyPath + 2, source:=pObjectPath + 1, size:=pObjectPath^);
      end_if;
    end_if;

    pHelpManaging^.pModulePath := pMyPath;
    
  //pHelpManaging^.pNext          := NIL;  // not necessary because of memset
        
    retcode := TRUE;
  else
    ClassState := -1;

    retcode := FALSE;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddGenericDynamic
  VAR_INPUT
    ModuleType 	: t_e_ModuleType;
    pThis 	: pVirtualBase;
    pObjectPath 	: ^USINT;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_GenericDynamicComponent;
    pMyPath : ^USINT;
  END_VAR

  if pFirstGenericDynamic = NIL then
    pFirstGenericDynamic$pVoid := To_StdLib.Malloc(size:=sizeof(t_GenericDynamicComponent));
    pHelpManaging := pFirstGenericDynamic;    
  else
    pHelpManaging := pFirstGenericDynamic;
    while pHelpManaging^.pNext <> NIL do
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
    pHelpManaging^.pNext$pVoid := To_StdLib.Malloc(size:=sizeof(t_GenericDynamicComponent));
    pHelpManaging := pHelpManaging^.pNext;
  end_if;
  
  if pHelpManaging <> NIL then
    _memset(dest:=pHelpManaging, usByte:=0, cntr:=sizeof(t_GenericDynamicComponent));
    pHelpManaging^.ModuleType     := ModuleType;

    // get the object name for easier debugging
    GetObjName(pThis, #pHelpManaging^.ObjName[0], sizeof(t_GenericDynamicComponent.ObjName));

    pHelpManaging^.pThis          := pThis;
    
    if pObjectPath then
      pMyPath$^void := To_StdLib.Malloc(size:=pObjectPath^ + 2);
      
      // convert 1 byte length info into 2 byte length info
      if pMyPath then
        pMyPath^$UINT := pObjectPath^$USINT;
        To_StdLib.MemCpy(dest:=pMyPath + 2, source:=pObjectPath + 1, size:=pObjectPath^);
      end_if;
    end_if;

    pHelpManaging^.pModulePath := pMyPath;
    
  //pHelpManaging^.pNext          := NIL;  // not necessary because of memset
        
    retcode := TRUE;
  else
    ClassState := -1;

    retcode := FALSE;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddSafetyNbr
	VAR_INPUT
		pThis 	: pVirtualBase;
		ud_SafetyNbr 	: HDINT;
		FirmwareVersion 	: HDINT;
		FPGAVersion 	: HSINT;
		PathLen 	: USINT;
		pPath 	: ^USINT;
	END_VAR
	VAR_OUTPUT
		retcode 	: BOOL;
	END_VAR
  VAR
    pHelpSafety             : ^t_SafetyComponent;
    pHelpConfigManager      : ^t_CfgListElem;
    pHelpConnectionManager  : ^t_FSoEConnectionDetails;
    pHelpModuleCfg          : ^t_ModuleListElem;
    MyPara 	                : CmdStruct;
    MyResult	              : results;
    pOwnerSafety            : ^t_SafetyComponent;
  END_VAR
    
  retcode := FALSE;
  
  if pFirstSafetyComponent then
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety do
      if pHelpSafety^.pThis = pThis then
        pHelpSafety^.SafetyNbr    := ud_SafetyNbr;
        pHelpSafety^.FW_Version   := FirmwareVersion;
        pHelpSafety^.FPGA_Version := FPGAVersion;
        pHelpSafety^.PathLen      := PathLen;
        pHelpSafety^.pPath        := pPath;


        if pHelpSafety^.ModuleType = _ModuleWithoutConfig then
          
          // if the post init is finished => module added during runtime
          if b_PostInitFinished then

            // check all configs
            pHelpConfigManager := pConfigs;
            while pHelpConfigManager do
            
              // check for FSoE connections that haven't been initialized
              pHelpConnectionManager := pHelpConfigManager^.pFirstFSoEConnection;
              while pHelpConnectionManager do
                                
                if pHelpConnectionManager^.Flags.FSoESlaveHasNoConfig & pHelpConnectionManager^.Flags.IsMaster then
                  if pHelpConnectionManager^.InternalConnectionState = FCS_RESET then
                    ShareSingleConnectionInfos(pHelpConnectionManager, pHelpConfigManager^.pFirstModuleCfg);
                  end_if;
                end_if;

                pHelpConnectionManager := pHelpConnectionManager^.pNext;
              end_while;
              
              // check all module configs for a match of the path and inform the owner that there is a new safety number
              pHelpModuleCfg := pHelpConfigManager^.pFirstModuleCfg;
              while pHelpModuleCfg do
                if (pHelpModuleCfg^.ModuleInfo.pPath^ = pPath^) & 
                   (To_StdLib.MemCmp(pPath + 1, pHelpModuleCfg^.ModuleInfo.pPath + 2 + pHelpModuleCfg^.ModuleInfo.pPath^ - pPath^, pPath^) = 0) then

                  // found a match => tell the owner of the configuration we found the module in
                
                  pOwnerSafety := pFirstSafetyComponent;
                  while pOwnerSafety do
                    if pOwnerSafety^.SafetyNbr = pHelpConfigManager^.SafetyNbr then
                      MyPara.uiCmd    := CMD_SM_UPDATE_SAFETY_NBR;
                      MyPara.aPara[0] := pHelpModuleCfg^.ModuleInfo.CompPathLen;    // length of HW path
                      MyPara.aPara[1] := pHelpModuleCfg^.ModuleInfo.pCompPath$DINT; // pointer to HW path
                      MyPara.aPara[2] := ud_SafetyNbr$DINT;                         // new safety number
                      pOwnerSafety^.pThis^.NewInst(#MyPara, #MyResult);      
                    end_if;
                    
                    pOwnerSafety := pOwnerSafety^.pNext;
                  end_while;
                end_if;

                pHelpModuleCfg := pHelpModuleCfg^.pNext;
              end_while;              
              
              pHelpConfigManager := pHelpConfigManager^.pNext;
            end_while;
          end_if;
        end_if;

        retcode := TRUE;
        return;
      end_if;
      
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::GetModuleTypeNo
  VAR_INPUT
    ModuleType 	: t_e_ModuleType;
  END_VAR
  VAR_OUTPUT
    ModuleNo 	: UDINT;
  END_VAR
  VAR
    pHelpSafety   : ^t_SafetyComponent;
    pHelpManaging : ^t_ManagingComponent;
    pHelpGeneric        : ^t_GenericDynamicComponent;
  END_VAR
  
  ModuleNo := 0;
  
  if pFirstSafetyComponent then
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety do
      if pHelpSafety^.ModuleType = ModuleType then
        ModuleNo += 1;
      end_if;
      
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
  end_if;

  if pFirstManagingInterfaceComponent then
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      if pHelpManaging^.ModuleType = ModuleType then
        ModuleNo += 1;
      end_if;
      
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
  end_if;
  
  if pFirstGenericDynamic then
    pHelpGeneric := pFirstGenericDynamic;
    
    while pHelpGeneric do
      if pHelpGeneric^.ModuleType = ModuleType then
        ModuleNo += 1;
      end_if;
      
      pHelpGeneric := pHelpGeneric^.pNext;
    end_while;
    
  end_if;


END_FUNCTION


FUNCTION SafetyManager::NewTimeSyncPossible
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    pHelpSafety   : ^t_SafetyComponent;
  END_VAR

  // if there is FSoE in the project => no timesync
  if b_GotFSoEModules then
    retcode := FALSE;
    return;
  end_if;

  // default is: we can => check if there is any module with too old firmware or fpga
  retcode := TRUE;
  
  if pFirstSafetyComponent then
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety do
      
      // check all modules for correct versions
      if ((pHelpSafety^.ModuleType = _CSCP01x) & (   (pHelpSafety^.FPGA_Version    < 16#14)     // if its a CSCP and the FPGA Version is lower than 1.4 
                                                   | (pHelpSafety^.FW_Version$UINT < 100)) &    // or the firmware version is lower than 100
                                                      pHelpSafety^.SafetyNbr)                   // only if there is a safety number available (maybe the module is optional and not available)
      |  (pHelpSafety^.NewConfigType = ConfigState_Old) then                                    // or the module has an old config type

        retcode := FALSE;
      end_if;
      
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::SetNewSyncMode
  VAR_INPUT
    b_NewMode 	: BOOL;
  END_VAR
  VAR
    MyPara 	  : CmdStruct;
    MyResult	: results;
  END_VAR

  b_NewSyncMode := b_NewMode;

  MyPara.uiCmd := CMD_SM_SET_NEW_SYNC_MODE;
  MyPara.aPara[0] := b_NewSyncMode;
  TellAllSafetyModules(#MyPara, #MyResult);
  
END_FUNCTION


FUNCTION SafetyManager::TellAllSafetyModules
  VAR_INPUT
    pPara 	: ^CmdStruct;
    pResult 	: ^results;
  END_VAR
  VAR
    pHelpSafety : ^t_SafetyComponent;
  END_VAR
    
  if pFirstSafetyComponent then
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety do
      // call the newinst with the given parameters
      pHelpSafety^.pThis^.NewInst(pPara, pResult);
      
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
  end_if;  

END_FUNCTION


FUNCTION SafetyManager::StartTimeSync
  
  if pFirstTimeSyncElement then
    pActTimeSyncElement := pFirstTimeSyncElement;

    // send the token to the right module    
    GiveTokenToNextModule();
    
    // remember the start time for calculating the time of a complete synchronisation
    TimeSyncCycleStart := ops.tAbsolute;    
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::NextTimeSyncModule

  pActTimeSyncElement := pActTimeSyncElement^.pNext;

  // must be done after selecting the new element
  if GiveTokenToNextModule() then
    // we're finished with our synchronisation cycle and just started a new one (must be done after selecting the new element)
    if IsTimeSyncMaster then      
      SyncDuration := ops.tAbsolute - TimeSyncCycleStart;
      if SyncDuration > MaxSyncDuration then
        MaxSyncDuration := SyncDuration;
      end_if;
      if SyncDuration < MinSyncDuration then
        MinSyncDuration := SyncDuration;
      end_if;

      // if a cycle has been finished => reset the timeout counter
      SyncTimeoutCounter := 0;

      // remember the start time for calculating the time of a complete synchronisation
      TimeSyncCycleStart := ops.tAbsolute;
    end_if;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::GiveTokenToNextModule
  VAR_OUTPUT
    RestartCycle 	: BOOL;
  END_VAR
  VAR
    MyPara 	  : CmdStruct;
    MyResult	: results;
    pTemp : ^t_TimeSyncElement;
  END_VAR

  if pActTimeSyncElement = pFirstTimeSyncElement then
    RestartCycle := TRUE;
  else
    RestartCycle := FALSE;
  end_if;

  // check if we have to skip something (missing optional modules)
  if TimeSyncModuleAvailable(pActTimeSyncElement) = FALSE then
    // save the actual position
    pTemp := pActTimeSyncElement;
    
    // if it's a missing optional module
    while (TimeSyncModuleAvailable(pActTimeSyncElement) = FALSE) do
      // skip it
      pActTimeSyncElement := pActTimeSyncElement^.pNext;
      
      // if we jumped on the first, we have to restart the cycle (time measurement and timeout surveillance)
      if pActTimeSyncElement = pFirstTimeSyncElement then
        RestartCycle := TRUE;
      end_if;
      
      // if we're at the start of the rotation => stop rotating and print error message
      if pActTimeSyncElement = pTemp then
        bd_LogRtMessagesInCy.TooManyOptionalMissing := TRUE;
        ClassState := -12;
        exit;
      end_if;
    end_while;
  end_if;

  // check if we have to skip something
  if pActTimeSyncElement^.InfoBits.IsInterfaceDomain then
    // save the actual position
    pTemp := pActTimeSyncElement;

    // if it's an interface managing module and it's not synchron, skip it (there has to be also a Safety CPU in the system, therefore we don't ask for it
    while pActTimeSyncElement^.InfoBits.IsInterfaceDomain & (pActTimeSyncElement^.pManaging^.InfoBits.IsSynchron = FALSE) do
      // skip it
      pActTimeSyncElement := pActTimeSyncElement^.pNext;
      
      // if we jumped on the first, we have to restart the cycle (time measurement and timeout surveillance)
      if pActTimeSyncElement = pFirstTimeSyncElement then
        RestartCycle := TRUE;
      end_if;
      
      // if we're at the start of the rotation => stop rotating and print error message
      if pActTimeSyncElement = pTemp then
        bd_LogRtMessagesInCy.InvalidIFConfiguration := TRUE;
        ClassState := -12;
        exit;
      end_if;
    end_while;
  end_if;

  if pActTimeSyncElement^.InfoBits.IsConnectionToMaster then
    // don't check for sync timeout if we're not the master
    b_CheckTimeSyncTimeout := FALSE;
  else
    b_CheckTimeSyncTimeout := TRUE;    
  end_if;
  
  // if it's a managing module and not the connection to the master, we give the token to a slave => remember it
  if pActTimeSyncElement^.InfoBits.IsManagingModule & (pActTimeSyncElement^.InfoBits.IsConnectionToMaster = FALSE) then
    b_SlaveSyncActive := TRUE;
  end_if;

  // tell the next module, to start the time synchronisation
  MyPara.uiCmd := CMD_SM_START_TIME_SYNC;
  pActTimeSyncElement^.pThis^.NewInst(#MyPara, #MyResult);

  LastTimeSyncThis := pActTimeSyncElement^.pThis;
  TimeSyncTimeStamp := ops.tAbsolute;
  
END_FUNCTION


FUNCTION SafetyManager::TellAllInterfaceManagingModules
  VAR_INPUT
    pPara 	: ^CmdStruct;
    pResult 	: ^results;
    OverrideSync 	: BOOL;(* := FALSE *)
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
  END_VAR
    
  if pFirstManagingInterfaceComponent then
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      // call the newinst with the given parameters
      if pHelpManaging^.InfoBits.IsSynchron | OverrideSync then
        pHelpManaging^.pThis^.NewInst(pPara, pResult);
      end_if;
      
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
  end_if;  

END_FUNCTION


FUNCTION SafetyManager::SafetyManager
  VAR_OUTPUT
    ret_code 	: ConfStates;
  END_VAR
  VAR
    sd_retval : DINT;
  END_VAR

  //get interface for hardwaretree functions
  if ( OS_CILGET(INTERFACE_HARDWARETREE, #pHardwareTree)) then
    pHardwareTree := NIL;
    
    // logging can be used here already because the constructor of the base has been called before to initialize the log-interface-pointer
    LogError("@0457 (SafetyManager::SafetyManager) Hardwaretree-Interface not found. A newer OS is required for Safety-Modules");
  end_if;
  
  pTempMemCy := To_StdLib.Malloc(size:=SIZE_TEMP_MEMORY);
  if pTempMemCy then
    ud_SizeTempMemCy := SIZE_TEMP_MEMORY;
  else
    LogError("@0458 (SafetyManager::SafetyManager) Allocate temporary Memory failed");
  end_if;

  // try the new interface first
  sd_retval := OS_CILGet(INTERFACE_SAFETY_DLL2, #pSafetyDll2)$DINT;

  if sd_retval then
    // the new safety interface is not available => try the old one
    sd_retval := OS_CILGet(INTERFACE_SAFETY_DLL, #pSafetyDll)$DINT;
    
    if sd_retval then
      // no dll available
    elsif pSafetyDll^.version >= 5 then
      // dll available that allows quit error
      QuitErrorAvailable := 1;
    end_if;
  else
    // dll available that allows quit error and multiple connections
    QuitErrorAvailable := 2;
  end_if;
  
  OS_CILGet( INTERFACE_VARANMANAGER, #pVaran );

  ret_code := C_OK;

END_FUNCTION


FUNCTION SafetyManager::AllManagingNodesSynchron
  VAR_OUTPUT
    AllReady 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
    MyPara 	  : CmdStruct;
    MyResult	: results;
    i               : UDINT;
    pActConnection  : ^t_UdpConnection;
  END_VAR

  // default value is that everything is ready. this method is also called if there are no managing modules.
  AllReady := TRUE;
  
  // if not all configs loaded
  if (ConfigNo < LocalCPUCfgNo) then
    // add our local configurations if there are any. the configuration contains the info if managing modules and/or reading udp connections are used.
    SendLocalCfgsToMaster();
  end_if;
   
  // check if any of the managing interface modules is not synchron
  if pFirstManagingInterfaceComponent then
    // check if any of the managing modules is optional (the optional flag is initialized with true and could lead to a problem here if they are not optional)
    CheckCPUConfigs(TRUE);
  
    MyPara.uiCmd := CMD_SM_SYNC_CHECK;

    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      // if any managing component isn't synchron => stop searching
      if pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult) = ERROR then
        if (pHelpManaging^.InfoBits.IsOptional = TRUE) then
          if ((SafetyDomainTimeout & ((ops.tAbsolute - SynchroTimeout) > SafetyDomainTimeout))) then
            // we give up waiting => timeout exceeded
            b_SyncTimeoutExceeded := TRUE;
            // if it's the last managing module, then it's our connection to the master module
            if (IsTimeSyncMaster = 0) & pHelpManaging^.InfoBits.IsConnectionToMaster then
              // we have to be our own master now
              IsTimeSyncMaster := 1;
            end_if;            
          else
            if (SafetyDomainTimeout = 0) then
              // optional part not available and no timeout set => potential configuration error 
              if bd_SingleLog.NoOptionalTimeoutSetVxC = FALSE then
                bd_SingleLog.NoOptionalTimeoutSetVxC := TRUE;
                LogError("@0954 (SafetyManager::AllManagingNodesSynchron) [SL] Optional domain (VBC/VMC) not available and no timeout set. Check client SafetyDomainTimeout.");              
              end_if;              
            end_if;

            // not everything is ready (waiting for timeout or no timeout set)            
            AllReady := FALSE;
          end_if;
        else
          // non-optional managing module missing => keep waiting
          AllReady := FALSE;
        end_if;

        // return; don't give up here => we have to check it for all domains, this one may be an optional interface frame domain
        pHelpManaging^.InfoBits.IsSynchron := FALSE;
      else
        pHelpManaging^.InfoBits.IsSynchron := TRUE;
      end_if;
      
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
  end_if;  
    
  // check if we have udp connections
  if sUdpVars.ProducerConnectionNo then
    // check all udp connections
    pActConnection := #sUdpVars.paConnectionsToProducers^[0];
    for i := 0 to (sUdpVars.ProducerConnectionNo-1) do      
      // if there is any connection where we haven't received anything
      if pActConnection^.ReceivedPackages = 0 then
        // and it's optional
        if pActConnection^.InfoBits.IsOptional then
          // if there is a timeout set for optional frames and it has been exceeded
          if ((SafetyDomainTimeout & ((ops.tAbsolute - SynchroTimeout) > SafetyDomainTimeout))) then
            // we give up waiting => timeout exceeded
            b_SyncTimeoutExceeded := TRUE;
          else
            if (SafetyDomainTimeout = 0) then
              // optional part not available and no timeout set => potential configuration error 
              if bd_SingleLog.NoOptionalTimeoutSetUDP = FALSE then
                bd_SingleLog.NoOptionalTimeoutSetUDP := TRUE;
                LogError("@0955 (SafetyManager::AllManagingNodesSynchron) [SL] Optional domain (UDP) not available and no timeout set. Check client SafetyDomainTimeout.");              
              end_if;              
            end_if;
            
            // not everything is ready (waiting for timeout or no timeout set)
            AllReady := FALSE;
          end_if;
          return;
        else
          // non-optional udp connection missing => keep waiting
          AllReady := FALSE;
        end_if;
      end_if;
      pActConnection += sizeof(t_UdpConnection);        
    end_for;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::GetMasterConnection
  VAR_OUTPUT
    pMasterConn : ^t_ManagingComponent;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
  END_VAR

  pMasterConn := NIL;

  if pFirstManagingInterfaceComponent then
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      if (pHelpManaging^.ModuleType = _VBC021Slave) | (pHelpManaging^.ModuleType = _VaranManager4VMC) | (pHelpManaging^.ModuleType = _GenericTimeSyncSlave) then
        pMasterConn := pHelpManaging;
        pMasterConn^.InfoBits.IsConnectionToMaster := TRUE;
        // if it's not the last in the list => we have to put it in the last place for easier timesync handling (last managing module to handle is the connection back to the master)
        if pMasterConn^.pNext then
          // if it's the first one
          if pMasterConn = pFirstManagingInterfaceComponent then
            pHelpManaging := pFirstManagingInterfaceComponent;
            while pHelpManaging^.pNext do
              pHelpManaging := pHelpManaging^.pNext;
            end_while;
            pFirstManagingInterfaceComponent := pFirstManagingInterfaceComponent^.pNext;
            pHelpManaging^.pNext := pMasterConn;
            pMasterConn^.pNext := NIL;
          else
            // if it's one in the middle
            pHelpManaging := pFirstManagingInterfaceComponent;
            while pHelpManaging^.pNext do
              if pHelpManaging^.pNext = pMasterConn then
                pHelpManaging^.pNext := pMasterConn^.pNext;
              end_if;

              pHelpManaging := pHelpManaging^.pNext;
            end_while;
            pHelpManaging^.pNext := pMasterConn;
            pMasterConn^.pNext := NIL;
          end_if;
        end_if;
        return;
      end_if;
      
      pHelpManaging := pHelpManaging^.pNext;
    end_while;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddConfiguration
  VAR_INPUT
    Length 	: UINT;
    pData 	: ^void;
    IsFSoEConfig 	: BOOL;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    pHelpCfg          : ^t_CfgListElem;
    pHelpModule       : ^t_ModuleListElem;
    pHelpPdo          : ^t_PdoListElem;
    pHelpSlave        : ^t_SlaveListElem;
    pHelpConnection   : ^t_FSoEConnectionDetails;
    pDataHelp         : ^void;
    i                 : UDINT;
    TmpPathLen        : UINT;
    TempCfgNo         : USINT;
    TempConnectionNo  : UINT;
    pHelpSafety       : ^t_SafetyComponent;
    pHelpManaging     : ^t_ManagingComponent;
    ModuleNo          : UDINT;
    UdpInfoBits       : t_UdpConInfoBits;
  END_VAR

(*
    Memory structure:
      4 Byte: SafetyNumber of CPU (is already there from initialisation in InitSafeModule of the safe CPU)
      1 Byte: Number of module configurations which are following
      for every module configuration (first is always the CPU itself)
        1 Byte: Info
        4 Byte: Interface Frame Source SafetyNumber (0 if it is no source for interface frames)
        4 Byte: Interface Frame CRC
        1 Byte: Number of destination inferface frame CRCs
        x Byte: Destination interface frame CRCs (number * sizeof(HDINT)
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
        1 Byte: PDO out length for this module
      1 Byte: Number of pdo configurations which are following
      for every PDO configuration
        1 Byte: PDO length
        1 Byte: Interface frame info
        4 Byte: Interface CRC
        2 Byte: Length of source hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of source
        2 Byte: Length of destination hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of destination       
      1 Byte: Number of slave configurations which are following
      for every slave configuration
        1 Byte: Info
        4 Byte: InterfaceCRC
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
      
      if IsFSoEConfig = TRUE
      2 Byte: Number of connection infos which are following
      for every FSoE connection info
        2 Byte: ConnectionID
        1 Byte: ConnectionState
        1 Byte: Flags // Actually, type is 4 bytes in size but for compatibility reasons it is still used as 1 byte. As a result, bits 9-32 are currently not used.
        4 Byte: Master safety number

only if feature available:        
        2 Byte: Transmission time in ms
        
        2 Byte: Offset where to read from master
        2 Byte: Size of data to read from master (and write to slave)
        2 Byte: Offset where to write to master
        2 Byte: Size of data to write to master (and read from slave)
        2 Byte: SlaveAddress
        2 Byte: Length of slave hardware path
        x Byte (depending on length in the 2 byte before): Slave hardware path
        2 Byte: Length of compressed slave hardware path
        x Byte (depending on length in the 2 byte before): Compressed slave hardware path
        2 Byte: Offset where to read from slave
        2 Byte: Offset where to write to slave
      
*)

  // check if the configuration is valid (pData <> NIL) & (Length must be at least 4 byte for SafetyNbr + 1 byte module cfg number + 1 byte pdo cfg number + 1 byte slave crc number)
  if pData & (Length >= 7)  then

    retcode := 0;
    
    // first we have to allocate some memory for the configuration
    if pConfigs = NIL then
      pConfigs$pVoid := To_StdLib.Malloc(size:=sizeof(t_CfgListElem));
      pHelpCfg := pConfigs;
    else
      pHelpCfg := pConfigs;
      while pHelpCfg^.pNext <> NIL do
        //if searched path is found, we already got this module in our list
        if pHelpCfg^.SafetyNbr = pData^$HDINT then
          // already in buffer => leave function
          return;
        else
          pHelpCfg := pHelpCfg^.pNext;
        end_if;
      end_while;

      //if searched path is found, we already got this module in our list
      if pHelpCfg^.SafetyNbr = pData^$HDINT then
        // already in buffer => leave function
        return;
      end_if;

      pHelpCfg^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_CfgListElem));
      pHelpCfg := pHelpCfg^.pNext;
    end_if;
    
    // if we have memory for the configuration => we add the data to the new element
    if pHelpCfg <> NIL then
      pDataHelp := pData;
      
      // set the next pointer to NIL
      pHelpCfg^.pNext := NIL;

      // safety number of the safety cpu
      pHelpCfg^.SafetyNbr := pDataHelp^$HDINT;
      pDataHelp += sizeof(HDINT);
      
      // number of module configurations which follow now
      TempCfgNo := pDataHelp^$USINT;
      pDataHelp += sizeof(USINT);

      pHelpCfg^.pFirstModuleCfg := NIL;
      
      // module configurations
      if TempCfgNo then
        for i := 0 to (TempCfgNo-1) do
          if pHelpCfg^.pFirstModuleCfg = NIL then
            pHelpCfg^.pFirstModuleCfg$pVoid := To_StdLib.Malloc(size:=sizeof(t_ModuleListElem));
            pHelpModule := pHelpCfg^.pFirstModuleCfg;
          else
            pHelpModule^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_ModuleListElem));
            pHelpModule := pHelpModule^.pNext;
          end_if;
          
          if pHelpModule then
            // initialize the pointers with NIL in case one of the mallocs fails
            pHelpModule^.ModuleInfo.pDestCRCs   := NIL;
            pHelpModule^.ModuleInfo.pPath       := NIL;
            pHelpModule^.ModuleInfo.pCompPath   := NIL;
            pHelpModule^.pNext                  := NIL;

            // info
            pHelpModule^.ModuleInfo.Info := pDataHelp^$BSINT;
            pDataHelp += sizeof(BSINT);
            
            // safety number of interface frame source
            pHelpModule^.ModuleInfo.SafetyNbr := pDataHelp^$HDINT;
            pDataHelp += sizeof(HDINT);

            // if it's the config owner => take the address service info to the info bits
            if pHelpCfg^.SafetyNbr = pHelpModule^.ModuleInfo.SafetyNbr then
              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                // check if we got a component with this number
                if pHelpSafety^.SafetyNbr = pHelpCfg^.SafetyNbr then
                  pHelpSafety^.InfoBits.IsAddressService := pHelpModule^.ModuleInfo.Info.IsAddressService;
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;
            end_if;

            // interface CRC of interface frame
            pHelpModule^.ModuleInfo.InterfaceCRC := pDataHelp^$HDINT;
            pDataHelp += sizeof(HDINT);
            
            if pHelpModule^.ModuleInfo.InterfaceCRC then
              // someone might connect to us via udp because we supply an interface frame
              sUdpVars.UdpInit.ProvidesIF := TRUE;
            end_if;

            // interface frame destination CRCs
            if pHelpModule^.ModuleInfo.Info.IsInterfaceDest then
              // number of CRCs
              TmpPathLen := pDataHelp^$USINT; // save some time with this local variable
              pHelpModule^.ModuleInfo.DestCRCNo := TmpPathLen$USINT;
              pDataHelp += sizeof(USINT);
              
              // allocate memory for the module path
              pHelpModule^.ModuleInfo.pDestCRCs$pVoid := To_StdLib.Malloc(size:=TmpPathLen * sizeof(HDINT));
              if pHelpModule^.ModuleInfo.pDestCRCs = NIL then
                ClassState := -1;
                retcode := -2;
                return;
              end_if;
              
              // save CRCs
              _memcpy(ptr1:=pHelpModule^.ModuleInfo.pDestCRCs, ptr2:=pDataHelp, cntr:=TmpPathLen * sizeof(HDINT));
              pDataHelp += TmpPathLen * sizeof(HDINT);
            else
              pHelpModule^.ModuleInfo.DestCRCNo := 0;
              pHelpModule^.ModuleInfo.pDestCRCs := NIL;
            end_if;
            
            // length of hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpModule^.ModuleInfo.PathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the module path
            pHelpModule^.ModuleInfo.pPath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpModule^.ModuleInfo.pPath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save hardwarepath
            _memcpy(ptr1:=pHelpModule^.ModuleInfo.pPath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;

            // length of hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpModule^.ModuleInfo.CompPathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the module path
            pHelpModule^.ModuleInfo.pCompPath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpModule^.ModuleInfo.pCompPath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save compressed hardwarepath
            _memcpy(ptr1:=pHelpModule^.ModuleInfo.pCompPath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;
            
            // PDO out length for this module (to assign the length to the pdo configurations)
            pHelpModule^.ModuleInfo.PdoOutLen := pDataHelp^$USINT;
            pDataHelp += sizeof(USINT);            
          else
            ClassState := -1;
            retcode := -2;
            return;
          end_if;          
        end_for;
      end_if;

      // number of Pdo configurations which follow now
      TempCfgNo := pDataHelp^$USINT;
      pDataHelp += sizeof(USINT);
      
      pHelpCfg^.pFirstPdoCfg := NIL;
      
      // Pdo configurations
      if TempCfgNo then
        for i := 0 to (TempCfgNo-1) do
          if pHelpCfg^.pFirstPdoCfg = NIL then
            pHelpCfg^.pFirstPdoCfg$pVoid := To_StdLib.Malloc(size:=sizeof(t_PdoListElem));
            pHelpPdo := pHelpCfg^.pFirstPdoCfg;
          else
            pHelpPdo^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_PdoListElem));
            pHelpPdo := pHelpPdo^.pNext;
          end_if;
          
          if pHelpPdo then
            // initialize the pointers with NIL in case one of the mallocs fails
            pHelpPdo^.PdoInfo.pSourcePath     := NIL;
            pHelpPdo^.PdoInfo.pDestPath       := NIL;
            pHelpPdo^.pNext                   := NIL;

            // pdo length
            pHelpPdo^.PdoInfo.PdoLen := pDataHelp^$USINT;
            pDataHelp += sizeof(USINT);
            
            // is it an interface pdo?
            pHelpPdo^.PdoInfo.Info := pDataHelp^$BSINT;
            pDataHelp += sizeof(BSINT);
            
            // if it is an interface frame, we have to copy the CRC too
            if pHelpPdo^.PdoInfo.Info.IsInterfacePDO then
              // interface CRC
              pHelpPdo^.PdoInfo.InterfaceCRC := pDataHelp^$HDINT;
            else
              pHelpPdo^.PdoInfo.InterfaceCRC := 0;
            end_if;
            pDataHelp += sizeof(HDINT);
            
            // length of source hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpPdo^.PdoInfo.SourcePathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the source module path
            pHelpPdo^.PdoInfo.pSourcePath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpPdo^.PdoInfo.pSourcePath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save source hardwarepath
            _memcpy(ptr1:=pHelpPdo^.PdoInfo.pSourcePath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;

            // check for an IP Address
            if pHelpPdo^.PdoInfo.Info.SrcPathIsCompressed & (pHelpPdo^.PdoInfo.pSourcePath^$USINT = INTERFACE_IP_MODE) then
              UdpInfoBits               := 0;
              UdpInfoBits.IsOptional    := pHelpPdo^.PdoInfo.Info.IsOptional;
              UdpInfoBits.OptionalValid := TRUE;

              // add the connection (we're consumer and we got the IP address of the producer)
              AddUdpConnection(IP:=(pHelpPdo^.PdoInfo.pSourcePath$^HDINT + 5)^, IFCRC:=(pHelpPdo^.PdoInfo.pSourcePath$^HDINT + 1)^, SafetyNumber:=pHelpCfg^.SafetyNbr, ProducerList:=TRUE, pInfoBits:=#UdpInfoBits);
            end_if;

            // length of destination hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpPdo^.PdoInfo.DestPathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the destination module path
            pHelpPdo^.PdoInfo.pDestPath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpPdo^.PdoInfo.pDestPath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save destination hardwarepath
            _memcpy(ptr1:=pHelpPdo^.PdoInfo.pDestPath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;
                        
            // check if a prior configuration also added this PDO (possible if a safe cpu gets PDOs from 2 different safe cpus)
            if DoublePDOEntry(pHelpPdo^.PdoInfo, pHelpCfg) then
              // use ID 255 (as kind of invalid) and mark the PDO as double entry (will not be distributed then)
              pHelpPdo^.PdoInfo.PDO_ID := 255;
              pHelpPdo^.PdoInfo.Info.DoublePDOEntry := TRUE;
            else
              // now we give it a priority and increase the number for the next pdo
              pHelpPdo^.PdoInfo.PDO_ID := ActPDO_ID;
              ActPDO_ID += 1;
            end_if;

          else
            ClassState := -1;
            retcode := -2;
            return;
          end_if;          
        end_for;
      end_if;

      // number of slave configurations which follow now
      TempCfgNo := pDataHelp^$USINT;
      pDataHelp += sizeof(USINT);

      pHelpCfg^.pFirstSlaveCfg:= NIL;
      
      // slave configurations
      if TempCfgNo then
        for i := 0 to (TempCfgNo-1) do
          if pHelpCfg^.pFirstSlaveCfg = NIL then
            pHelpCfg^.pFirstSlaveCfg$pVoid := To_StdLib.Malloc(size:=sizeof(t_SlaveListElem));
            pHelpSlave := pHelpCfg^.pFirstSlaveCfg;
          else
            pHelpSlave^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_SlaveListElem));
            pHelpSlave := pHelpSlave^.pNext;
          end_if;
          
          if pHelpSlave then
            // initialize the pointers with NIL in case one of the mallocs fails
            pHelpSlave^.SlaveInfo.pPath     := NIL;
            pHelpSlave^.SlaveInfo.pCompPath := NIL;
            pHelpSlave^.pNext               := NIL;

            // info
            pHelpSlave^.SlaveInfo.Info := pDataHelp^$BSINT;
            pDataHelp += sizeof(BSINT);
                       
            // InterfaceCRC
            pHelpSlave^.SlaveInfo.InterfaceCRC := pDataHelp^$HDINT;
            pDataHelp += sizeof(HDINT);

            // length of hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpSlave^.SlaveInfo.PathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the module path
            pHelpSlave^.SlaveInfo.pPath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpSlave^.SlaveInfo.pPath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save hardwarepath
            _memcpy(ptr1:=pHelpSlave^.SlaveInfo.pPath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;

            // length of hardwarepath
            TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
            pHelpSlave^.SlaveInfo.CompPathLen := TmpPathLen;
            pDataHelp += sizeof(UINT);
            
            // allocate memory for the module path
            pHelpSlave^.SlaveInfo.pCompPath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
            if pHelpSlave^.SlaveInfo.pCompPath = NIL then
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
            
            // save compressed hardwarepath
            _memcpy(ptr1:=pHelpSlave^.SlaveInfo.pCompPath, ptr2:=pDataHelp, cntr:=TmpPathLen);
            pDataHelp += TmpPathLen;            
          else
            ClassState := -1;
            retcode := -2;
            return;
          end_if;          
        end_for;
      end_if;

      pHelpCfg^.pFirstFSoEConnection := NIL;

      // check if it's a FSoE Config
      if IsFSoEConfig then
        // number of connection details which follow now
        TempConnectionNo := pDataHelp^$UINT;
        pDataHelp += sizeof(UINT);

        // if we got connections
        if TempConnectionNo then
          for i := 0 to (TempConnectionNo-1) do
            if pHelpCfg^.pFirstFSoEConnection = NIL then
              pHelpCfg^.pFirstFSoEConnection$pVoid := To_StdLib.Malloc(size:=sizeof(t_FSoEConnectionDetails));
              pHelpConnection := pHelpCfg^.pFirstFSoEConnection;
            else
              pHelpConnection^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_FSoEConnectionDetails));
              pHelpConnection := pHelpConnection^.pNext;
            end_if;
            
            // if we succeeded to allocate memory
            if pHelpConnection then
              // initialize the pointers with NIL in case one of the mallocs fails
              pHelpConnection^.pSlavePath       := NIL;
              pHelpConnection^.pSlavePathComp   := NIL;
              pHelpConnection^.pNext            := NIL;
              pHelpConnection^.pNextSorted      := NIL;
            
              // connection ID
              pHelpConnection^.ConnectionID := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);

              // connection state (use our own state internally)
              pHelpConnection^.InternalConnectionState := FCS_RESET;
              pHelpConnection^.ConnectionState := pDataHelp^$t_FSoEConnectionState;
                            
              pDataHelp += sizeof(USINT);

              // flags
              
              // Actually, type is 4 bytes in size but for compatibility reasons it is still used as 1 byte. As a result, bits 9-32 are currently not used.
              if bdFeatureEnable.FSoEFlags_4ByteSize then
                pHelpConnection^.Flags$t_FSoEFlags := pDataHelp^$t_FSoEFlags;
                pDataHelp += sizeof(t_FSoEFlags);
              else
                pHelpConnection^.Flags$USINT := pDataHelp^$USINT;
                pDataHelp += sizeof(USINT);
              end_if;

              // check if FlexCon functionality is used according to the flags in this connection
              if (pHelpConnection^.Flags AND SM_FLAGS_FLEXCON_MASK) = SM_FLAGS_FLEXCON_HOST then
                // if there are no generic dynamic FSoE devices registered (this is to prevent the FlexCon functionality from conflicting with the MultiModule used for the LogiMover)
                if GetModuleTypeNo(ModuleType:=_GenericDynamicFSoE) = 0 then
                  if sFlexCon.Role = _FlexConNode then
                    sFlexCon.Role := _InvalidConstellation;
                  elsif sFlexCon.Role <> _InvalidConstellation then
                    sFlexCon.Role := _FlexConHost;

                    pHelpSafety := pFirstSafetyComponent;
                    while pHelpSafety do
                      // check if we got an address service
                      if pHelpSafety^.InfoBits.IsAddressService then
                        // check if it's NIL or already our desired value
                        if (sFlexCon.pSafetyHost = NIL) | (sFlexCon.pSafetyHost = pHelpSafety) then
                          // remember the pointer for quick access later
                          sFlexCon.pSafetyHost := pHelpSafety;
                        else
                          LogError(e_msg:="@05DA (SafetyManager::AddConfiguration) Multiple FlexConHosts found! Check Safety Designer Projects of all connected Safe CPUs");
                          
                          sFlexCon.Role := _InvalidConstellation;
                        end_if;
                      end_if;
                      
                      pHelpSafety := pHelpSafety^.pNext;
                    end_while;                  
                  end_if;
                end_if;
              elsif (pHelpConnection^.Flags AND SM_FLAGS_FLEXCON_MASK) = SM_FLAGS_FLEXCON_NODE then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
                LogError(e_msg:="@DBG SafetyManager: Found FSOE Connection of FlexConNode");

                LogValue(pString:="Number of VBC021_Slave objects with safety memory: 0x{0}", Value:=GetModuleTypeNo(ModuleType:=_VBC021Slave));                
                LogValue(pString:="Number of VM objects with safety memory: 0x{0}", Value:=GetModuleTypeNo(ModuleType:=_VaranManager4VMC));                
#endif
                // if we detected a Node but the role has already been set to Host
                if sFlexCon.Role = _FlexConHost then
                  // it's an invalid constellation (can't be host and node at the same time)
                  sFlexCon.Role := _InvalidConstellation;
                elsif sFlexCon.Role <> _InvalidConstellation then
                
                  // check if there is only one possible connection to the host
                  ModuleNo := GetModuleTypeNo(ModuleType:=_VBC021Slave) + GetModuleTypeNo(ModuleType:=_VaranManager4VMC);
                  if ModuleNo = 1 then
                    sFlexCon.Role := _FlexConNode;
                    
                    // get the managing module connecting us with the host
                    pHelpManaging := pFirstManagingInterfaceComponent;
                    while pHelpManaging do
                      if (pHelpManaging^.ModuleType = _VBC021Slave) | (pHelpManaging^.ModuleType = _VaranManager4VMC) then
                        // remember the pointer for quick access later
                        sFlexCon.pManaging2Host := pHelpManaging;
                      end_if;
                      
                      pHelpManaging := pHelpManaging^.pNext;
                    end_while;                  
                  else
                    // invalid
                    sFlexCon.Role := _InvalidConstellation;
                                    
                    // check if there are no or too many vbc021 slave modules
                    if ModuleNo = 0 then
                      LogError(e_msg:="@05DB (SafetyManager::AddConfiguration) No VBC021_Slave with defined safety memory detected");
                    else
                      LogError(e_msg:="@05DD (SafetyManager::AddConfiguration) More than 1 VBC021_Slave detected => NOT ALLOWED!");
                    end_if;
                  end_if;
                end_if;
              end_if;

              // master safety number
              pHelpConnection^.OwnerSafetyNbr := pDataHelp^$HDINT;
              pDataHelp += sizeof(HDINT);

              // the feature is not available in older safety module classes, so we have to do this for compatibility reasons
              if bdFeatureEnable.TransmissionTimeInConfig then
                // transmission time in ms
                pHelpConnection^.TransmissionTime_ms := pDataHelp^$UINT;
                pDataHelp += sizeof(UINT);
              else
                pHelpConnection^.TransmissionTime_ms := 0;
              end_if;

              // offset where to read from master
              pHelpConnection^.MasterReadOffset := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);

              // size of data to read from master (and write to slave)
              pHelpConnection^.MasterReadSize := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);
              
              // offset where to write to master
              pHelpConnection^.MasterWriteOffset := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);

              // size of data to write to master (and read from slave)
              pHelpConnection^.MasterWriteSize := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);
              
              // slave address
              pHelpConnection^.SlaveAddress := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);

              // wait for slave addresses if any connection is not initialized
              if (pHelpConnection^.ConnectionState = FCS_NOT_INITIALIZED) & (pHelpConnection^.SlaveAddress = 0) then
                b_AllLocalSlaveAddressesValid := FALSE;
              end_if;

              // length of uncompressed slave hardwarepath
              TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
              pHelpConnection^.SlavePathLen := TmpPathLen;
              pDataHelp += sizeof(UINT);
              
              if pHelpConnection^.Flags.IsMaster then
                // allocate memory for the module path
                pHelpConnection^.pSlavePath$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
                if pHelpConnection^.pSlavePath = NIL then
                  ClassState := -1;
                  retcode := -2;
                  return;
                end_if;
                
                // save hardwarepath
                _memcpy(ptr1:=pHelpConnection^.pSlavePath, ptr2:=pDataHelp, cntr:=TmpPathLen);
                pDataHelp += TmpPathLen;           
              end_if;

              // length of compressed slave hardwarepath
              TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
              pHelpConnection^.SlavePathCompLen := TmpPathLen;
              pDataHelp += sizeof(UINT);

              if pHelpConnection^.Flags.IsMaster then
                // allocate memory for the module path
                pHelpConnection^.pSlavePathComp$pVoid := To_StdLib.Malloc(size:=TmpPathLen);
                if pHelpConnection^.pSlavePathComp = NIL then
                  ClassState := -1;
                  retcode := -2;
                  return;
                end_if;
                
                // save compressed hardwarepath
                _memcpy(ptr1:=pHelpConnection^.pSlavePathComp, ptr2:=pDataHelp, cntr:=TmpPathLen);
                pDataHelp += TmpPathLen;
                
                // if it's a udp connection, consider the transmission time
                if (pHelpConnection^.pSlavePathComp^ = INTERFACE_IP_MODE) & pHelpConnection^.TransmissionTime_ms then
                  if pHelpConnection^.TransmissionTime_ms < sUdpVars.LowestTransmissionTime_ms then
                    sUdpVars.LowestTransmissionTime_ms := pHelpConnection^.TransmissionTime_ms;
                  end_if;
                end_if;
              end_if;

              // offset where to read from master
              pHelpConnection^.SlaveReadOffset := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);
              
              // offset where to write to master
              pHelpConnection^.SlaveWriteOffset := pDataHelp^$UINT;
              pDataHelp += sizeof(UINT);
              
              // initialize log marker
              pHelpConnection^.bsLogError := 0;
            else
              ClassState := -1;
              retcode := -2;
              return;
            end_if;
          end_for;
        end_if;
      end_if;

      
      ConfigNo += 1;
      
      // check if the given size matches the memory structure
      if (pDataHelp$UDINT - pData$UDINT) <> Length then
        LogError("@0459 (SafetyManager::AddConfiguration) Invalid configuration data given (length doesn't match memory structure)");
        retcode := -3;
        return;
      end_if;
    else
      ClassState := -1;
      retcode := -2;
      return;
    end_if;
  else
    // invalid configuration
    LogError("@045A (SafetyManager::AddConfiguration) Found invalid safety cpu configuration: Pointer or size wrong");
    retcode := -1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::FreeCfgLists
  VAR
    pHelpCfg        : ^t_CfgListElem;
    pHelpModule     : ^t_ModuleListElem;
    pHelpPdo        : ^t_PdoListElem;
    pHelpConnection : ^t_FSoEConnectionDetails;
  END_VAR

  while pConfigs do
    pHelpCfg := pConfigs;
    pConfigs := pConfigs^.pNext;
    
    // while we got module cfgs 
    while pHelpCfg^.pFirstModuleCfg do
      pHelpModule := pHelpCfg^.pFirstModuleCfg;
      pHelpCfg^.pFirstModuleCfg := pHelpCfg^.pFirstModuleCfg^.pNext;
      
      if pHelpModule^.ModuleInfo.pDestCRCs then
        To_StdLib.Free(pHelpModule^.ModuleInfo.pDestCRCs);  
      end_if;

      To_StdLib.Free(pHelpModule^.ModuleInfo.pPath);
      To_StdLib.Free(pHelpModule^.ModuleInfo.pCompPath);
      To_StdLib.Free(pHelpModule);
    end_while;

    // while we got pdo cfgs
    while pHelpCfg^.pFirstPdoCfg do
      pHelpPdo := pHelpCfg^.pFirstPdoCfg;
      pHelpCfg^.pFirstPdoCfg := pHelpCfg^.pFirstPdoCfg^.pNext;
      
      To_StdLib.Free(pHelpPdo^.PdoInfo.pSourcePath);
      To_StdLib.Free(pHelpPdo^.PdoInfo.pDestPath);
      To_StdLib.Free(pHelpPdo);
    end_while;
    
    // while we got connection infos
    while pHelpCfg^.pFirstFSoEConnection do
      pHelpConnection := pHelpCfg^.pFirstFSoEConnection;
      pHelpCfg^.pFirstFSoEConnection := pHelpCfg^.pFirstFSoEConnection^.pNext;
      
      if pHelpConnection^.pSlavePath then
        To_StdLib.Free(mptr:=pHelpConnection^.pSlavePath);
      end_if;

      if pHelpConnection^.pSlavePathComp then
        To_StdLib.Free(mptr:=pHelpConnection^.pSlavePathComp);
      end_if;
      
      To_StdLib.Free(mptr:=pHelpConnection);
    end_while;
    
    To_StdLib.Free(pHelpCfg);
  end_while;
  pSortedConnections := NIL; // may have contained a pointer within the config (to the connection with the lowest interface CRC)
  ConfigNo := 0;

END_FUNCTION


FUNCTION SafetyManager::AddOwnCPUsToList
  VAR
    pHelpSafety : ^t_SafetyComponent;
  END_VAR

  // reset number of memorized safe cpu numbers
  SafeCPU_Nbr_No := 0;

  // check if any of the managing modules is not synchron
  if pFirstSafetyComponent then   
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety do
      // add all safety cpus to the parameters
      if IsCPUModule(pHelpSafety^.ModuleType) then
        if pHelpSafety^.SafetyNbr then
          aSafetyCPU_Nbr[SafeCPU_Nbr_No] := pHelpSafety^.SafetyNbr;
          SafeCPU_Nbr_No += 1;
        end_if;
      end_if;
      
      pHelpSafety := pHelpSafety^.pNext;
    end_while;
  end_if;  

END_FUNCTION


FUNCTION SafetyManager::AllCPUConfigsArrived
  VAR_OUTPUT
    AllReady 	: BOOL;
  END_VAR
  VAR
    pHelpCfg : ^t_CfgListElem;
    i : UDINT;
    Found : BOOL;
  END_VAR

  // check if there are any CPUs
  if SafeCPU_Nbr_No then
    // if we have already all configurations (according to the number) check if they are correct
    if ConfigNo = SafeCPU_Nbr_No then
      
      // normally we should be fine now
      AllReady := TRUE;
      
      // check all safety numbers stored
      for i := 0 to SafeCPU_Nbr_No-1 do
      
        Found := FALSE;
      
        // search all configurations
        pHelpCfg := pConfigs;
        while pHelpCfg do
          // if we found a match => stop searching for this one
          if pHelpCfg^.SafetyNbr = aSafetyCPU_Nbr[i] then
            Found := TRUE;
            exit;
          end_if;
          pHelpCfg := pHelpCfg^.pNext;
        end_while;
        
        // if this configuration hasn't been found, we may have a problem
        if Found = FALSE then
          AllReady := FALSE;
          LogError("@045B (SafetyManager::AllCPUConfigsArrived) Invalid CPU configuration found!");
          return;
        end_if;
      end_for;
    else
      AllReady := FALSE;
      
      if ConfigNo > SafeCPU_Nbr_No then
        LogError("@045C (SafetyManager::AllCPUConfigsArrived) ERROR! More configs available than safety CPUs");
      end_if;
    end_if;
  else
    AllReady := TRUE;
    LogError("@045D (SafetyManager::AllCPUConfigsArrived) No Safety CPU found in the system!");
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::AddLocalCPUCfg
  VAR_INPUT
    Length 	: UINT;
    pData 	: ^void;
    IsFSoECfg 	: BOOL;(* := FALSE *)
  END_VAR
  VAR
    pHelpLocalCfg   : ^t_LocalCfgListElem;
    EditOldCfg      : BOOL;
  END_VAR
  
  LastCfgReceiveTime := ops.tAbsolute;

  // if the a module wants to add the config which is already added => overwrite the old config
  EditOldCfg := FALSE;

  if pFirstLocalCPUCfg = NIL then
    pFirstLocalCPUCfg$pVoid := To_StdLib.Malloc(size:=sizeof(t_LocalCfgListElem));
    pHelpLocalCfg := pFirstLocalCPUCfg;
  else
    pHelpLocalCfg := pFirstLocalCPUCfg;
    
    if pHelpLocalCfg^.SafetyNbr = pData^$HDINT then
      EditOldCfg := TRUE;
    else
      while pHelpLocalCfg^.pNext <> NIL do
        pHelpLocalCfg := pHelpLocalCfg^.pNext;
        if pHelpLocalCfg^.SafetyNbr = pData^$HDINT then
          EditOldCfg := TRUE;
          exit;
        end_if;
      end_while;
      if EditOldCfg = FALSE then
        pHelpLocalCfg^.pNext$pVoid := To_StdLib.Malloc(size:=sizeof(t_LocalCfgListElem));

        pHelpLocalCfg := pHelpLocalCfg^.pNext;     
      end_if;
    end_if;
  end_if;
  
  if pHelpLocalCfg <> NIL then
    pHelpLocalCfg^.SafetyNbr          := pData^$HDINT;
    pHelpLocalCfg^.ConfigSize         := Length;
    pHelpLocalCfg^.pConfig            := pData;  // memory can be accessed always (never freed)
    pHelpLocalCfg^.ConfigSent2Master  := FALSE; 
    pHelpLocalCfg^.IsFSoECfg          := IsFSoECfg;
    
    if IsFSoECfg then
      sUdpVars.UdpInit.FSoEAvailable := TRUE;
    end_if;
    
    // only if we created a new element (incrementing has to be done last because of thread safety: bg increments the value and cyclic uses the list)
    if EditOldCfg = FALSE then
      pHelpLocalCfg^.pNext := NIL;
      LocalCPUCfgNo += 1;

      // new cpu config => reinitalize everything
      ReinitEverything();
    end_if;
  else
    ClassState := -1;
    LogError("@045E (SafetyManager::AddLocalCPUCfg) Failed to allocate Memory for the local CPU Configuration");
  end_if;  

END_FUNCTION


FUNCTION SafetyManager::SendLocalCfgsToMaster
  VAR
    pHelpLocalCfg : ^t_LocalCfgListElem;
    i : UDINT;
  END_VAR

  if LocalCPUCfgNo then
    pHelpLocalCfg := pFirstLocalCPUCfg;
    
    for i := 0 to (LocalCPUCfgNo-1) do
      if pHelpLocalCfg^.ConfigSize then
        // if we're the master, we keep the configuration of this cpu and share the essentials later (when we got all configurations) with all other domains (distribute the mapping)
        if AddConfiguration(pHelpLocalCfg^.ConfigSize, pHelpLocalCfg^.pConfig, pHelpLocalCfg^.IsFSoECfg) <> 0 then
          LogError("@045F (SafetyManager::SendLocalCfgsToMaster) Failed to add a safety cpu configuration to the local config-memory");
        end_if;
      end_if;
      
      pHelpLocalCfg := pHelpLocalCfg^.pNext;
    end_for;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::BroadCastActPDO
  VAR
    MyPara 	  : CmdStruct;
    PdoMemSize : UINT;
    pPdoMem    : ^USINT;
    pHelp      : ^USINT;
    TmpPathLen : UINT;
    SourceDest : t_SourceDest;
    SafetyNbr           : HDINT;
  END_VAR
  
  // put pdo (marked by pActualSendPDO) into linear memory, find out who needs it and broadcast it

(*
    Memory structure:
      1 Byte: Number of Domains involved
      x Byte: 1 Byte for every involved Domain number
      1 Byte: Number of Domains in source path
      x Byte: 1 Byte for every domain in source path
      1 Byte: Number of Domains in destination path
      x Byte: 1 Byte for every domain in destination path      
      1 Byte: ID of this PDO (starting at 0)
      1 Byte: Length of PDO
      1 Byte: Is it an interface frame
      4 Byte: Interface CRC
      2 Byte: Length of source hardware path
      x Byte (depending on length in the 2 byte before): Hardwarepath of source
      2 Byte: Length of destination hardware path
      x Byte (depending on length in the 2 byte before): Hardwarepath of destination
*)

  // if the pointer is NIL or it's an interface frame broadcast, don't distribute it, there are virtual PDOs to be distributed instead of the broadcasts (if necessary)
  if (pActualSendPDO = NIL) | pActualSendPDO^.PdoInfo.Info.IFBroadCast then
    return;
  end_if;


  // add up the sizes and allocate the memory
  PdoMemSize := sizeof(USINT) +                           // 1 byte number of domains involved
                1 +                                       // 1 byte for every involved domain
                sizeof(USINT) +                           // 1 byte number of domains in source path
                1 +                                       // 1 byte for every domain in source path
                sizeof(USINT) +                           // 1 byte number of domains in destination path
                1 +                                       // 1 byte for every domain in destination path                
                sizeof(USINT) +                           // 1 byte ID of this PDO (starting at 0)
                sizeof(USINT) +                           // 1 byte length of PDO
                sizeof(t_PdoCfgInfo) +                    // 1 byte pdo info (is it interface frame or optional...)
                sizeof(HDINT) +                           // 4 byte interface CRC
                sizeof(UINT) +                            // 2 byte length of source hardware path
                pActualSendPDO^.PdoInfo.SourcePathLen +   // x byte source hardware path
                sizeof(UINT) +                            // 2 byte length of destination hardware path
                pActualSendPDO^.PdoInfo.DestPathLen;      // x byte destination hardware path
                
  if ud_SizeTempMemCy >= PdoMemSize then
    pPdoMem := pTempMemCy$^USINT;
  else
    LogError("@0440 (SafetyManager::BroadCastActPDO) Temporary memory for cyclic task is too small!");
    LogValue("AvaliableMem: {0} / NeededMem: {1}", Value:=ud_SizeTempMemCy, Value2:=PdoMemSize);
    ClassState := -1;
    return;
  end_if;

  if pPdoMem then
    pHelp := pPdoMem;
    
    // number of domains involved
    pHelp^$USINT := 1;
    pHelp += sizeof(USINT);

    // domain number of this ack
    pHelp^$USINT := 0;
    pHelp += 1;

    // number of domains in source path
    pHelp^$USINT := 1;
    pHelp += sizeof(USINT);

    // domain numbers of source domains
    pHelp^$USINT := 0;
    pHelp += 1;
    
    // number of domains in dest path
    pHelp^$USINT := 1;
    pHelp += sizeof(USINT);

    // domain numbers of dest domains
    pHelp^$USINT := 0;
    pHelp += 1;

    pHelp^$USINT := pActualSendPDO^.PdoInfo.PDO_ID;
    pHelp += sizeof(USINT);
    
    pHelp^$USINT := pActualSendPDO^.PdoInfo.PdoLen;
    pHelp += sizeof(USINT);

    pHelp^$t_PdoCfgInfo := pActualSendPDO^.PdoInfo.Info;
    pHelp += sizeof(t_PdoCfgInfo);

    pHelp^$HDINT := pActualSendPDO^.PdoInfo.InterfaceCRC;
    pHelp += sizeof(HDINT);
    
    TmpPathLen := pActualSendPDO^.PdoInfo.SourcePathLen; // save some time with this local variable
    pHelp^$UINT := TmpPathLen;
    pHelp += sizeof(UINT);

    _memcpy(ptr1:=pHelp, ptr2:=pActualSendPDO^.PdoInfo.pSourcePath, cntr:=TmpPathLen);
    pHelp += TmpPathLen;

    TmpPathLen := pActualSendPDO^.PdoInfo.DestPathLen;
    pHelp^$UINT := TmpPathLen;
    pHelp += sizeof(UINT);

    _memcpy(ptr1:=pHelp, ptr2:=pActualSendPDO^.PdoInfo.pDestPath, cntr:=TmpPathLen);
          
    MyPara.uiCmd := CMD_SM_PDO_MAPPING_BROADCAST;
    MyPara.aPara[0] := PdoMemSize; // length of parameters
    MyPara.aPara[1] := pPdoMem$DINT;
    
    // check out if we have to work with this, and if so save the PDO info's
    if IsAffectedByPDO(#MyPara, #SourceDest, #SafetyNbr) then

    end_if;
  else
    ClassState := -1;
    LogError("@0462 (SafetyManager::BroadCastActPDO) Failed to allocate memory for the currently broadcasting PDO Info");
  end_if;

END_FUNCTION


FUNCTION SafetyManager::IsAffectedByPDO
  VAR_INPUT
    pPara 	: ^CmdStruct;
    pSourceDest 	: ^t_SourceDest;
    pSafetyNbr 	: ^HDINT;
  END_VAR
  VAR_OUTPUT
    IsAffected 	: BOOL;
  END_VAR
  VAR
    pHelp                 :^USINT;
    pFirstDomainAffected  : ^USINT;
    DomainsInside         : USINT;
    SourcePathLen         : UINT;
    pSourcePath           : ^USINT;
    DestPathLen           : UINT;
    pDestPath             : ^USINT;
    pHelpManaging         : ^t_ManagingComponent;
    pHelpSafety           : ^t_SafetyComponent;
    pSourceSafety         : ^t_SafetyComponent;
    pDestSafety           : ^t_SafetyComponent;
    MyPara 	              : CmdStruct;
    MyResult	            : results;
    PDO_ID                : USINT;
    Length                : USINT;
    SourceDomain          : USINT;
    DestDomain            : USINT;
    SourceDomainNo        : USINT;
    DestDomainNo          : USINT;
    aSourceDomains        : ARRAY [0..99] OF USINT;
    aDestDomains          : ARRAY [0..99] OF USINT;
    IsInterfaceFrame      : BOOL;
    InterfaceCRC          : HDINT;
    CopyCmdIndex          : INT;
    pPDOElement           : ^t_PDOElement;
    pRecyclePDO           : ^t_PDOElement;
    TempLen               : USINT;
    TempOffset            : UDINT;
    PDOInfo               : t_PdoCfgInfo;
  END_VAR

  IsAffected := FALSE;

  pSourceDest^ := SourceDest_None;
  pSafetyNbr^ := 0;

  // check the parameters
  pFirstDomainAffected := (pPara^.aPara[1])$^USINT;
  pHelp  := pFirstDomainAffected;
  
  DomainsInside := pHelp^$USINT;
  pHelp += sizeof(USINT);
  
  if DomainsInside then
    // skip the domain number
    pHelp += sizeof(USINT);

    IsAffected := TRUE;

(*
    Memory structure:
      1 Byte: Number of Domains involved
      x Byte: 1 Byte for every involved Domain number
      1 Byte: Number of Domains in source path
      x Byte: 1 Byte for every domain in source path
      1 Byte: Number of Domains in destination path
      x Byte: 1 Byte for every domain in destination path      
      1 Byte: ID of this PDO (starting at 0)
      1 Byte: Length of PDO
      1 Byte: PDO Info (Is it an interface frame, or optional...)
      4 Byte: Interface CRC
      2 Byte: Length of source hardware path
      x Byte (depending on length in the 2 byte before): Hardwarepath of source
      2 Byte: Length of destination hardware path
      x Byte (depending on length in the 2 byte before): Hardwarepath of destination
*)

    if IsAffected then
      // pHelp is now pointing to the Number of Domains in source path
    
      _memset(dest:=#aSourceDomains[0], usByte:=16#FF, cntr:=sizeof(aSourceDomains));
      _memset(dest:=#aDestDomains[0], usByte:=16#FF, cntr:=sizeof(aDestDomains));
    
      SourceDomainNo := pHelp^;
      pHelp += sizeof(USINT);

      _memcpy(ptr1:=#aSourceDomains[0], ptr2:=pHelp, cntr:=SourceDomainNo);
      pHelp += SourceDomainNo;

      DestDomainNo := pHelp^;
      pHelp += sizeof(USINT);

      _memcpy(ptr1:=#aDestDomains[0], ptr2:=pHelp, cntr:=DestDomainNo);
      pHelp += DestDomainNo;
    
      PDO_ID := pHelp^;
      pHelp += sizeof(USINT);
    
      Length := pHelp^;
      pHelp += sizeof(USINT);
      
      PDOInfo := pHelp^;
      IsInterfaceFrame := PDOInfo.IsInterfacePDO;
      pHelp += sizeof(t_PdoCfgInfo);
      
      InterfaceCRC := pHelp^$HDINT;
      pHelp += sizeof(HDINT);
      
      SourcePathLen := pHelp^$UINT;
      pHelp += sizeof(UINT);
      pSourcePath := pHelp;
      pHelp += SourcePathLen;
      DestPathLen := pHelp^$UINT;
      pHelp += sizeof(UINT);
      pDestPath := pHelp;
      
      SourceDomain := aSourceDomains[SourceDomainNo-1];
      DestDomain   := aDestDomains[DestDomainNo-1];
      
      pSourceSafety := NIL;
      pDestSafety   := NIL;
      
      // if we're the source or destination domain and there is a safety component in this system, check if it's the source or destination for this PDO
      if pFirstSafetyComponent then
        // check for source or dest module
        pHelpSafety := pFirstSafetyComponent;
        while pHelpSafety & ((pSourceSafety = NIL) | (pDestSafety = NIL)) do
          // we compare the last x byte (where x is the length of the local path including the domain)
          if (pSourceSafety = NIL) then
            // if the length matches it's in the same domain, otherwise the local search path must be shorter and there must be a domain escape sequence in the long path
            if pHelpSafety^.pPath & ((pSourcePath^ = pHelpSafety^.pPath^) | ((pSourcePath^ > pHelpSafety^.pPath^) & ((pSourcePath + 1 + pSourcePath^ - pHelpSafety^.pPath^)^ = HWT_ESCAPE_DOMAIN))) & 
               (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pSourcePath + 2 + pSourcePath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then

              pSourceSafety := pHelpSafety;
            end_if;
          end_if;
          
          if (pDestSafety = NIL) then
            // if the length matches it's in the same domain, otherwise the local search path must be shorter and there must be a domain escape sequence in the long path
            if pHelpSafety^.pPath & ((pDestPath^ = pHelpSafety^.pPath^) | ((pDestPath^ > pHelpSafety^.pPath^) & ((pDestPath + 1 + pDestPath^ - pHelpSafety^.pPath^)^ = HWT_ESCAPE_DOMAIN))) & 
               (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pDestPath + 2 + pDestPath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then

              pDestSafety := pHelpSafety;
              AddTimeSyncElement(pManaging:=NIL, pSafety:=pHelpSafety, IsInterfaceDomain := FALSE);
            end_if;
          end_if;
          
          pHelpSafety := pHelpSafety^.pNext;
        end_while;
      end_if;

      // use the pointer to the array element to save time
      pPDOElement := #aPDOElements[PDOElementNo];

      // here we have to check what to do with the PDO:
      // is the actual domain the source or the destination domain? or maybe both?
      if pSourceSafety & pDestSafety then
        pSourceDest^ := SourceDest_IsSourceAndDest;
        
        // only possible for interface frames
        if IsInterfaceFrame then
          // we have to tell the source, that there's a receiver for the PDO (also get the addresses)
          MyPara.uiCmd := CMD_SM_ADD_OUTPUT_PDO;
          MyPara.aPara[0] := 1;  // mark as interface frame PDO
          
          pSourceSafety^.pThis^.NewInst(#MyPara, #MyResult);

          // use a PDO element for automated removal of the input pdo in case of a new config
          pPDOElement^.PDO_ID                   := PDO_ID;
          pPDOElement^.Length                   := Length;
          pPDOElement^.ThisDomainsRole          := SourceDest_IsDest;
          pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
          pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
          pPDOElement^.SourceInfo.SafetyNbr     := pSourceSafety^.SafetyNbr;
          pPDOElement^.DestInfo.SafetyNbr       := pDestSafety^.SafetyNbr;
          pPDOElement^.pSafety                  := pDestSafety;
          pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
          pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
          PDOElementNo += 1;

          // then tell the destination about the PDO
          MyPara.uiCmd := CMD_SM_ADD_INPUT_PDO;
          MyPara.aPara[0] := pPDOElement^.SourceInfo.pPDOMem$DINT;  // we got the pointer to the local mem and the pointer to the DPRAM from the call of the source module
          MyPara.aPara[1] := pPDOElement^.SourceInfo.pPDOMemDPRAM$DINT;
          MyPara.aPara[2] := pSourceSafety^.SafetyNbr$DINT;
          MyPara.aPara[3] := Length;
          MyPara.aPara[4] := pActualSendPDO^.PdoInfo.Info.IsOptional; // only possible because this kind of PDOs is only available on Domain 0
          MyPara.aPara[5] := pPDOElement^.SourceInfo.HandlePDODPRAM$DINT; // Needed for Payload Support
          MyPara.aPara[6] := pPDOElement^.SourceInfo.OffsetPDODPRAM$DINT; // Needed for Payload Support
          MyPara.aPara[7] := 0;// target offset (not necessary here)
          MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
          MyPara.aPara[9] := TRUE;

          pDestSafety^.pThis^.NewInst(#MyPara, #MyResult);
        else
          LogError("@0463 (SafetyManager::IsAffectedByPDO) Error in PDO distribution: No InterfaceFrame but Source and Destination in this domain!");
        end_if;        
      elsif pSourceSafety & (pDestSafety = NIL) then
        pSourceDest^ := SourceDest_IsSource;
        
        // get safety number of the source
        MyPara.uiCmd := CMD_SM_GET_SAFETY_NUMBER;
        MyPara.aPara[0] := pSafetyNbr$DINT;
        pSourceSafety^.pThis^.NewInst(#MyPara, #MyResult);
        
        pPDOElement^.PDO_ID                 := PDO_ID;
        pPDOElement^.Length                 := Length;
        pPDOElement^.ThisDomainsRole        := SourceDest_IsSource;
        
        pPDOElement^.SourceInfo.SafetyNbr   := pSafetyNbr^;
        pPDOElement^.SourceInfo.DomainNr    := SourceDomain;
        pPDOElement^.SourceInfo.PathLen     := SourcePathLen;
        pPDOElement^.SourceInfo.pPath$pVoid := To_StdLib.Malloc(size:= SourcePathLen);
        if pPDOElement^.SourceInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.SourceInfo.pPath, ptr2:=pSourcePath, cntr:=SourcePathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        pPDOElement^.DestInfo.SafetyNbr     := 0;
        pPDOElement^.DestInfo.DomainNr      := DestDomain;
        pPDOElement^.DestInfo.PathLen       := DestPathLen;
        pPDOElement^.DestInfo.pPath$pVoid   := To_StdLib.Malloc(size:= DestPathLen);
        if pPDOElement^.DestInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.DestInfo.pPath, ptr2:=pDestPath, cntr:=DestPathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        pPDOElement^.pSafety              := pSourceSafety;

        // initialize with invalid;
        CopyCmdIndex := -1;
        
        // if we're the master, we may also route interfaceframes
        if IsInterfaceFrame then
          // search for the destination of this PDO
          pHelpManaging := pFirstManagingInterfaceComponent;
          while pHelpManaging do
            if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pDestPath, DestPathLen) = 0) then
              exit;
            else
              pHelpManaging := pHelpManaging^.pNext;
            end_if;
          end_while;
        
          pPDOElement^.pManagingDest := pHelpManaging;
        end_if;

        if pPDOElement^.pManagingDest then
        
          // if the module is marked as optional and the PDO is not optional => the module is also not optional
          if pPDOElement^.pManagingDest^.InfoBits.IsOptional & (PDOInfo.IsOptional = FALSE) then
            pPDOElement^.pManagingDest^.InfoBits.IsOptional  := FALSE;
          end_if;

          // check if we can recycle the memory of another DO (if this data is already transferred to this managing module
          pRecyclePDO := CheckPDORecycle(SourceDest_IsDest, pPDOElement);

          // only if there is enough space or we already have a memory
          if pRecyclePDO | ((pPDOElement^.pManagingDest^.PDOWriteMemUsed + pPDOElement^.Length) <= pPDOElement^.pManagingDest^.PDOWriteMemAvailable) then
            
            // if we can't use the copy command
            if b_SafetyOnLocalCDIAS then
              // remember the pointer to the memory to copy it manually in rt postscan method
              if pRecyclePDO then
                // this PDO is already in the memory of the managing node, we don't have to copy it again
                pPDOElement^.DestInfo.pPDOMem := NIL;//pRecyclePDO^.DestInfo.pPDOMem;
              else
                pPDOElement^.DestInfo.pPDOMem := pPDOElement^.pManagingDest^.pPDOWriteSpace + pPDOElement^.pManagingDest^.PDOWriteMemUsed;
              end_if;
            else
              if pRecyclePDO then
                // this PDO is already in the memory of the managing node, we don't have to copy it again
                pPDOElement^.DestInfo.pPDOMemDPRAM := NIL;//pRecyclePDO^.DestInfo.pPDOMemDPRAM;
              else
                pPDOElement^.DestInfo.pPDOMemDPRAM := pPDOElement^.pManagingDest^.pPDOWriteSpaceDPRAM + pPDOElement^.pManagingDest^.PDOWriteMemUsed;
              end_if;
              
              CopyCmdIndex := GetNewMovDO();
              if CopyCmdIndex < 0 then
                if pRecyclePDO then
                  // this PDO is already in the memory of the managing node, we don't have to copy it again
                  pPDOElement^.DestInfo.pPDOMem := NIL;//pRecyclePDO^.DestInfo.pPDOMem;
                else
                  pPDOElement^.DestInfo.pPDOMem := pPDOElement^.pManagingDest^.pPDOWriteSpace + pPDOElement^.pManagingDest^.PDOWriteMemUsed;
                end_if;
                LogError("@0465 (SafetyManager::IsAffectedByPDO) Failed to install copy command => PDO is beeing copied in RT");
              else
                aCopyCmds[CopyCmdIndex].pSettings^.ud_AddressDest := pPDOElement^.DestInfo.pPDOMemDPRAM$UDINT;
                aCopyCmds[CopyCmdIndex].pSettings^.ui_ByteCnt     := pPDOElement^.Length;
              end_if;
            end_if;
            
            // if we need new memory, tell the managing module
            if pRecyclePDO = NIL then
              pPDOElement^.pManagingDest^.PDOWriteMemUsed += pPDOElement^.Length;

              MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
              MyPara.aPara[0] := pPDOElement^.pManagingDest^.PDOWriteMemUsed$DINT;
              pPDOElement^.pManagingDest^.pThis^.NewInst(#MyPara, #MyResult);
            end_if;

            if IsInterfaceFrame then
              MyPara.uiCmd := CMD_SM_ADD_OUTPUT_PDO;
              MyPara.aPara[0] := 1;  // mark as interface frame PDO
              pSourceSafety^.pThis^.NewInst(#MyPara, #MyResult);

              pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
              pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
              pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
              pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
            end_if;
          else
            // we add the memory anyway to show the user how much he would need
            pPDOElement^.pManagingDest^.PDOWriteMemUsed += pPDOElement^.Length;
            
            MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
            MyPara.aPara[0] := pPDOElement^.pManagingDest^.PDOWriteMemUsed$DINT;
            pPDOElement^.pManagingDest^.pThis^.NewInst(#MyPara, #MyResult);

            LogError("@0466 (SafetyManager::IsAffectedByPDO) Not enough write memory to map all PDOs");
            return;
          end_if;
        else
          LogError("@0467 (SafetyManager::IsAffectedByPDO) Couldn't find destination managing module for PDO");
          return;
        end_if;

        pPDOElement^.CopyCmdIndex := CopyCmdIndex;

        PDOElementNo += 1;
      elsif pDestSafety & (pSourceSafety = NIL) then
        pSourceDest^ := SourceDest_IsDest;

        // get safety number of the destination
        MyPara.uiCmd := CMD_SM_GET_SAFETY_NUMBER;
        MyPara.aPara[0] := pSafetyNbr$DINT;
        pDestSafety^.pThis^.NewInst(#MyPara, #MyResult);

        pPDOElement^.PDO_ID                 := PDO_ID;
        pPDOElement^.Length                 := Length;
        pPDOElement^.ThisDomainsRole        := SourceDest_IsDest;
        
        pPDOElement^.SourceInfo.SafetyNbr   := 0;
        pPDOElement^.SourceInfo.DomainNr    := SourceDomain;
        pPDOElement^.SourceInfo.PathLen     := SourcePathLen;
        pPDOElement^.SourceInfo.pPath$pVoid := To_StdLib.Malloc(size:= SourcePathLen);
        if pPDOElement^.SourceInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.SourceInfo.pPath, ptr2:=pSourcePath, cntr:=SourcePathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        pPDOElement^.DestInfo.SafetyNbr     := pSafetyNbr^;
        pPDOElement^.DestInfo.DomainNr      := DestDomain;
        pPDOElement^.DestInfo.PathLen       := DestPathLen;
        pPDOElement^.DestInfo.pPath$pVoid   := To_StdLib.Malloc(size:= DestPathLen);
        if pPDOElement^.DestInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.DestInfo.pPath, ptr2:=pDestPath, cntr:=DestPathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        pPDOElement^.pSafety              := pDestSafety;

        // if we're the master, we may also route interfaceframes
        if IsInterfaceFrame then

          // search for the source of this PDO
          pHelpManaging := pFirstManagingInterfaceComponent;
          while pHelpManaging do
            if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pSourcePath, SourcePathLen) = 0) then
              exit;
            else
              pHelpManaging := pHelpManaging^.pNext;
            end_if;
          end_while;

          pPDOElement^.pManagingSource := pHelpManaging;
        end_if;
        
        if pPDOElement^.pManagingSource then

          // if the module is marked as optional and the PDO is not optional => the module is also not optional
          if pPDOElement^.pManagingSource^.InfoBits.IsOptional & (PDOInfo.IsOptional = FALSE) then
            pPDOElement^.pManagingSource^.InfoBits.IsOptional  := FALSE;
          end_if;
          
          // check if we can recycle the memory of another DO (if this data is already transferred to this managing module
          pRecyclePDO := CheckPDORecycle(SourceDest_IsSource, pPDOElement);
        
          // only if there is enough space or we already have a memory
          if pRecyclePDO | ((pPDOElement^.pManagingSource^.PDOReadMemUsed + pPDOElement^.Length) <= pPDOElement^.pManagingSource^.PDOReadMemAvailable) then
            if IsInterfaceFrame then
              pPDOElement^.SourceInfo.SafetyNbr   := (GetIFInfoByCRC(InterfaceCRC, #TempOffset, #TempLen))$HDINT;
              if TempLen = Length then
                // get the place in the memory of the source managing module
                pPDOElement^.SourceInfo.pPDOMem       := pPDOElement^.pManagingSource^.pPDOReadSpace + TempOffset;
                pPDOElement^.SourceInfo.pPDOMemDPRAM  := pPDOElement^.pManagingSource^.pPDOReadSpaceDPRAM + TempOffset;
                
                pPDOElement^.SourceInfo.HandlePDODPRAM := pPDOElement^.pManagingSource^.ud_HandlePDOReadSpaceDPRAM;
                pPDOElement^.SourceInfo.OffsetPDODPRAM := TempOffset;
              
                // if we need new memory, tell the managing module
                if (TempOffset + Length) > pPDOElement^.pManagingSource^.PDOReadMemUsed then
                  pPDOElement^.pManagingSource^.PDOReadMemUsed := TempOffset + Length;
                  
                  MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
                  MyPara.aPara[0] := pPDOElement^.pManagingSource^.PDOReadMemUsed$DINT;
                  pPDOElement^.pManagingSource^.pThis^.NewInst(#MyPara, #MyResult);
                end_if;

                MyPara.uiCmd := CMD_SM_ADD_INPUT_PDO;
                MyPara.aPara[0] := pPDOElement^.SourceInfo.pPDOMem$DINT;
                MyPara.aPara[1] := pPDOElement^.SourceInfo.pPDOMemDPRAM$DINT;
                MyPara.aPara[2] := pPDOElement^.SourceInfo.SafetyNbr$DINT;
                MyPara.aPara[3] := Length;
                MyPara.aPara[4] := PDOInfo.IsOptional;
                MyPara.aPara[5] := pPDOElement^.SourceInfo.HandlePDODPRAM$DINT; // Needed for Payload Support
                MyPara.aPara[6] := pPDOElement^.SourceInfo.OffsetPDODPRAM$DINT; // Needed for Payload Support
                MyPara.aPara[7] := 0;// target offset (not necessary here)
                MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                MyPara.aPara[9] := TRUE;

                pDestSafety^.pThis^.NewInst(#MyPara, #MyResult);
              else
                LogError("@0469 (SafetyManager::IsAffectedByPDO) Length of interface frames doesn't match the local configuration");
              end_if;
            
            else
              if pRecyclePDO then
                pPDOElement^.SourceInfo.pPDOMem       := pRecyclePDO^.SourceInfo.pPDOMem;
                pPDOElement^.SourceInfo.pPDOMemDPRAM  := pRecyclePDO^.SourceInfo.pPDOMemDPRAM;
              else
                pPDOElement^.SourceInfo.pPDOMem       := pPDOElement^.pManagingSource^.pPDOReadSpace + pPDOElement^.pManagingSource^.PDOReadMemUsed;
                pPDOElement^.SourceInfo.pPDOMemDPRAM  := pPDOElement^.pManagingSource^.pPDOReadSpaceDPRAM + pPDOElement^.pManagingSource^.PDOReadMemUsed;
              
                // if we need new memory, tell the managing module
                pPDOElement^.pManagingSource^.PDOReadMemUsed += pPDOElement^.Length;

                MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
                MyPara.aPara[0] := pPDOElement^.pManagingSource^.PDOReadMemUsed$DINT;
                pPDOElement^.pManagingSource^.pThis^.NewInst(#MyPara, #MyResult);
              end_if;
            end_if;
          else
            // we add the memory anyway to show the user how much he would need
            pPDOElement^.pManagingSource^.PDOReadMemUsed += pPDOElement^.Length;

            MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
            MyPara.aPara[0] := pPDOElement^.pManagingSource^.PDOReadMemUsed$DINT;
            pPDOElement^.pManagingSource^.pThis^.NewInst(#MyPara, #MyResult);

            LogError("@046B (SafetyManager::IsAffectedByPDO) Not enough read memory to map all PDOs");
            return;
          end_if;
        else

          LogError("@046C (SafetyManager::IsAffectedByPDO) Couldn't find source managing module for PDO");
          return;
        end_if;

        PDOElementNo += 1;
      else
        pSourceDest^ := SourceDest_IsForwarder;
        
        // if source and destination are not in this domain, is it a bifurcation or a linear forwarding?
        pPDOElement^.PDO_ID                 := PDO_ID;
        pPDOElement^.Length                 := Length;
        pPDOElement^.ThisDomainsRole        := SourceDest_IsForwarder;
        
        pPDOElement^.SourceInfo.DomainNr    := SourceDomain;
        pPDOElement^.SourceInfo.PathLen     := SourcePathLen;
        pPDOElement^.SourceInfo.pPath$pVoid := To_StdLib.Malloc(size:= SourcePathLen);
        if pPDOElement^.SourceInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.SourceInfo.pPath, ptr2:=pSourcePath, cntr:=SourcePathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        pPDOElement^.DestInfo.DomainNr      := DestDomain;
        pPDOElement^.DestInfo.PathLen       := DestPathLen;
        pPDOElement^.DestInfo.pPath$pVoid   := To_StdLib.Malloc(size:= DestPathLen);
        if pPDOElement^.DestInfo.pPath then
          _memcpy(ptr1:=pPDOElement^.DestInfo.pPath, ptr2:=pDestPath, cntr:=DestPathLen);
        else
          LogError("@0464 (SafetyManager::IsAffectedByPDO) Failed to allocate memory for PDO path info");
          ClassState := -1;
          return;
        end_if;

        // initialize with invalid;
        CopyCmdIndex := -1;

        // if we're the master, we may also route interfaceframes
        if IsInterfaceFrame then

          // search for the source of this PDO
          pHelpManaging := pFirstManagingInterfaceComponent;
          while pHelpManaging do
            if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pSourcePath, SourcePathLen) = 0) then
              exit;
            else
              pHelpManaging := pHelpManaging^.pNext;
            end_if;
          end_while;

          pPDOElement^.pManagingSource := pHelpManaging;          
        end_if;

        if pPDOElement^.pManagingSource then
        
          // if the module is marked as optional and the PDO is not optional => the module is also not optional
          if pPDOElement^.pManagingSource^.InfoBits.IsOptional & (PDOInfo.IsOptional = FALSE) then
            pPDOElement^.pManagingSource^.InfoBits.IsOptional  := FALSE;
          end_if;

          // check if we can recycle the memory of another DO (if this data is already transferred to this managing module
          pRecyclePDO := CheckPDORecycle(SourceDest_IsSource, pPDOElement);

          if pRecyclePDO | ((pPDOElement^.pManagingSource^.PDOReadMemUsed + pPDOElement^.Length) <= pPDOElement^.pManagingSource^.PDOReadMemAvailable) then
            if pRecyclePDO then
              pPDOElement^.SourceInfo.pPDOMem       := pRecyclePDO^.SourceInfo.pPDOMem;
              pPDOElement^.SourceInfo.pPDOMemDPRAM  := pRecyclePDO^.SourceInfo.pPDOMemDPRAM;
            else
              pPDOElement^.SourceInfo.pPDOMem       := pPDOElement^.pManagingSource^.pPDOReadSpace + pPDOElement^.pManagingSource^.PDOReadMemUsed;
              pPDOElement^.SourceInfo.pPDOMemDPRAM  := pPDOElement^.pManagingSource^.pPDOReadSpaceDPRAM + pPDOElement^.pManagingSource^.PDOReadMemUsed;
              pPDOElement^.pManagingSource^.PDOReadMemUsed += pPDOElement^.Length;

              MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
              MyPara.aPara[0] := pPDOElement^.pManagingSource^.PDOReadMemUsed$DINT;
              pPDOElement^.pManagingSource^.pThis^.NewInst(#MyPara, #MyResult);
            end_if;
          else
            // we add the memory anyway to show the user how much he would need
            pPDOElement^.pManagingSource^.PDOReadMemUsed += pPDOElement^.Length;

            MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
            MyPara.aPara[0] := pPDOElement^.pManagingSource^.PDOReadMemUsed$DINT;
            pPDOElement^.pManagingSource^.pThis^.NewInst(#MyPara, #MyResult);

            LogError("@046B (SafetyManager::IsAffectedByPDO) Not enough read memory to map all PDOs");
            return;
          end_if;
        else
          LogError("@046C (SafetyManager::IsAffectedByPDO) Couldn't find source managing module for PDO");
          return;
        end_if;

        // if we're the master, we may also route interfaceframes
        if IsInterfaceFrame then
          // search for the destination of this PDO
          pHelpManaging := pFirstManagingInterfaceComponent;
          while pHelpManaging do
            if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pDestPath, DestPathLen) = 0) then
              exit;
            else
              pHelpManaging := pHelpManaging^.pNext;
            end_if;
          end_while;

          pPDOElement^.pManagingDest := pHelpManaging;
        end_if;

        if pPDOElement^.pManagingDest then

          // if the module is marked as optional and the PDO is not optional => the module is also not optional
          if pPDOElement^.pManagingDest^.InfoBits.IsOptional & (PDOInfo.IsOptional = FALSE) then
            pPDOElement^.pManagingDest^.InfoBits.IsOptional  := FALSE;
          end_if;

          if pPDOElement^.pManagingDest = pPDOElement^.pManagingSource then
            LogError("@0470 (SafetyManager::IsAffectedByPDO) In PDO Forwarding a domain can't be the source and the destination!");
          end_if;
          
          // check if we can recycle the memory of another DO (if this data is already transferred to this managing module
          pRecyclePDO := CheckPDORecycle(SourceDest_IsDest, pPDOElement);
        
          if pRecyclePDO | ((pPDOElement^.pManagingDest^.PDOWriteMemUsed + pPDOElement^.Length) <= pPDOElement^.pManagingDest^.PDOWriteMemAvailable) then

            // if we can't use the copy command
            if b_SafetyOnLocalCDIAS then
              // remember the pointer to the memory to copy it manually in rt postscan method
              if pRecyclePDO then
                // this PDO is already in the memory of the managing node, we don't have to copy it again
                pPDOElement^.DestInfo.pPDOMem := NIL;//pRecyclePDO^.SourceInfo.pPDOMem;
              else
                pPDOElement^.DestInfo.pPDOMem := pPDOElement^.pManagingDest^.pPDOWriteSpace + pPDOElement^.pManagingDest^.PDOWriteMemUsed;
              end_if;
            else
              if pRecyclePDO then
                // this PDO is already in the memory of the managing node, we don't have to copy it again
                pPDOElement^.DestInfo.pPDOMemDPRAM := NIL;//pRecyclePDO^.DestInfo.pPDOMemDPRAM;
              else
                pPDOElement^.DestInfo.pPDOMemDPRAM := pPDOElement^.pManagingDest^.pPDOWriteSpaceDPRAM + pPDOElement^.pManagingDest^.PDOWriteMemUsed;
              end_if;
              
              CopyCmdIndex := GetNewMovDO();
              if CopyCmdIndex < 0 then
                pPDOElement^.DestInfo.pPDOMem := pPDOElement^.pManagingDest^.pPDOWriteSpace + pPDOElement^.pManagingDest^.PDOWriteMemUsed;              
                LogError("@0465 (SafetyManager::IsAffectedByPDO) Failed to install copy command => PDO is beeing copied in RT");
              else
                aCopyCmds[CopyCmdIndex].pSettings^.ud_AddressDest := pPDOElement^.DestInfo.pPDOMemDPRAM$UDINT;
                aCopyCmds[CopyCmdIndex].pSettings^.ui_ByteCnt     := pPDOElement^.Length;
              end_if;
            end_if;

            // if we need new memory, tell the managing module
            if pRecyclePDO = NIL then
              pPDOElement^.pManagingDest^.PDOWriteMemUsed += pPDOElement^.Length;
              
              MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
              MyPara.aPara[0] := pPDOElement^.pManagingDest^.PDOWriteMemUsed$DINT;
              pPDOElement^.pManagingDest^.pThis^.NewInst(#MyPara, #MyResult);
            end_if;
          else
            // we add the memory anyway to show the user how much he would need
            pPDOElement^.pManagingDest^.PDOWriteMemUsed += pPDOElement^.Length;
            
            MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
            MyPara.aPara[0] := pPDOElement^.pManagingDest^.PDOWriteMemUsed$DINT;
            pPDOElement^.pManagingDest^.pThis^.NewInst(#MyPara, #MyResult);

            LogError("@0466 (SafetyManager::IsAffectedByPDO) Not enough write memory to map all PDOs");
            return;
          end_if;
        else
          LogError("@0467 (SafetyManager::IsAffectedByPDO) Couldn't find destination managing module for PDO");
          return;
        end_if;

        pPDOElement^.CopyCmdIndex := CopyCmdIndex;

        PDOElementNo += 1;
      end_if;
      
    end_if;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::GetDestManagingOfDO
  VAR_INPUT
    pDO 	: ^SafetyCDIAS_Base::SDOHeader;
  END_VAR
  VAR_OUTPUT
    pDestManaging 	: ^t_ManagingComponent;
  END_VAR
  VAR
    pHelpManaging 	          : ^t_ManagingComponent;
    pAddDynSlaveRsp           : ^t_AddDynSlaveRsp;
    pRemDynSlaveRsp           : ^t_RemDynSlaveRsp;
    pHelpConfig               : ^t_CfgListElem;
    pHelpConnection           : ^t_FSoEConnectionDetails;
    MyPara                    : CmdStruct;
    MyResult                  : results;
    i                         : USINT;
    b_ConnectionAlreadyExists : BOOL;
  END_VAR

  pDestManaging := NIL;

  // maybe it's one of the interface domains
  pHelpManaging := pFirstManagingInterfaceComponent;
  while pHelpManaging do
    if (pHelpManaging^.FlexCon.SafetyNumber = pDO^.DstAdr) then
      if (pDO^.StdHeader.Typ.Response) & ((pDO^.StdHeader.Cmd = SM_SDO_COMMAND_ADD_DYN_SLAVE) | (pDO^.StdHeader.Cmd = SM_SDO_COMMAND_REMOVE_DYN_SLAVE)) then

        // if the initialisation of the class is not finished or failed
        if (sFlexCon.Role <> _NoFlexCon) & (ClassState <> 1) then
          // don't forward add or remove dyn slave requests. otherwise the order in the managing module is likely to be wrong if flexcon is used in combination with interface frames.
          return;
        end_if;

        if (pDO^.StdHeader.Cmd = SM_SDO_COMMAND_ADD_DYN_SLAVE) then
          pAddDynSlaveRsp := pDO$^t_AddDynSlaveRsp;
          
          // if it was successful and we have a safety host module pointer => try to install the PDO
          if (pAddDynSlaveRsp^.Retcode = 0) & sFlexCon.pSafetyHost then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
            LogError("@DBG AddDynSlave Response detected in SafetyManager of Host (succeeded)");
#endif          
            
            // install the access to the next free memory on the VBC

            // we're the host => search for the FSoE connection with the host flags
            pHelpConfig := pConfigs;
            while pHelpConfig do
              pHelpConnection := pHelpConfig^.pFirstFSoEConnection;
              while pHelpConnection do
                if ((pHelpConnection^.Flags AND SM_FLAGS_FLEXCON_MASK) = SM_FLAGS_FLEXCON_HOST) &   // check if it's the right kind of connection
//                   (pHelpConnection^.InternalConnectionState =  FCS_RESET)               &          // check if it's still free
                   (pHelpConnection^.MasterReadOffset  = pAddDynSlaveRsp^.SafeOutOffset) &          // check if offsets and lengths match
                   (pHelpConnection^.MasterWriteOffset = pAddDynSlaveRsp^.SafeInOffset)  &
                   (pHelpConnection^.MasterReadSize    = pAddDynSlaveRsp^.SafeOutLength) &
                   (pHelpConnection^.MasterWriteSize   = pAddDynSlaveRsp^.SafeInLength) then
                   
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
                  LogValue(pString:="@DBG HOST: Found FSoE Connection of FlexConHost to install the cyclic PDO Routing between 0x{0} local and 0x{1} at Node", Value:=pHelpConnection^.OwnerSafetyNbr, Value2:=pHelpManaging^.FlexCon.SafetyNumber);
#endif

                  // remember if we already had this connection before
                  b_ConnectionAlreadyExists := pHelpManaging^.FlexCon.pCopyCmd <> NIL;

                  if b_ConnectionAlreadyExists = FALSE then
                    pHelpManaging^.FlexCon.WriteOffset:= pHelpManaging^.PDOWriteMemUsed$UINT;
                    pHelpManaging^.FlexCon.WriteLen   := pHelpConnection^.MasterReadSize;
                  end_if;

                  // GET THE DATA FROM THE SCP TO THE VBC
  
                  MyPara.uiCmd := CMD_SM_ADD_OUTPUT_PDO;
//                      MyPara.aPara[0] := 1;  not used // mark as interface frame PDO
                  sFlexCon.pSafetyHost^.pThis^.NewInst(#MyPara, #MyResult);

  //                  pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
  //                  pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
  //                  pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
  //                  pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
                  
                  // check if we already have a copy pointer
                  if pHelpManaging^.FlexCon.pCopyCmd then
                    pHelpManaging^.FlexCon.pCopyCmd^.pSource  := MyResult.aData[0]$^USINT + pHelpConnection^.MasterReadOffset;
                    pHelpManaging^.FlexCon.pCopyCmd^.pDest    := pHelpManaging^.pPDOWriteSpace + pHelpManaging^.FlexCon.WriteOffset;
                    pHelpManaging^.FlexCon.pCopyCmd^.Size     := pHelpManaging^.FlexCon.WriteLen;
                  else
                    // create a new
                    if FSoECopyCmdNo < MAX_FSOE_CONNECTIONS then
                      aFSoECopyCmds[FSoECopyCmdNo].pSource  := MyResult.aData[0]$^USINT + pHelpConnection^.MasterReadOffset;
                      aFSoECopyCmds[FSoECopyCmdNo].pDest    := pHelpManaging^.pPDOWriteSpace + pHelpManaging^.FlexCon.WriteOffset;
                      aFSoECopyCmds[FSoECopyCmdNo].Size     := pHelpManaging^.FlexCon.WriteLen;
                      pHelpManaging^.FlexCon.pCopyCmd := #aFSoECopyCmds[FSoECopyCmdNo];
                      FSoECopyCmdNo                         += 1;                        
                    else
                      LogValue("@0493 (SafetyManager::GetDestManagingOfDO) Maximum limit of allowed FSoE connections exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                    end_if;
                  end_if;

                  if b_ConnectionAlreadyExists = FALSE then
                    pHelpManaging^.PDOWriteMemUsed += pHelpManaging^.FlexCon.WriteLen;
                    MyPara.uiCmd    := CMD_SM_PDO_MEM_USAGE_WR;
                    MyPara.aPara[0] := pHelpManaging^.PDOWriteMemUsed$DINT;
                    pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult);
                    
                    pHelpManaging^.FlexCon.ReadOffset := pHelpManaging^.PDOReadMemUsed$UINT;
                    pHelpManaging^.FlexCon.ReadLen    := pHelpConnection^.MasterWriteSize;
                  end_if;
                  

                  // HOW TO GET DATA FROM THE VBC TO THE SCP
                  
                  MyPara.uiCmd := CMD_SM_ADD_INPUT_PDO;
                  MyPara.aPara[0] := (pHelpManaging^.pPDOReadSpace + pHelpManaging^.FlexCon.ReadOffset)$DINT;
                  MyPara.aPara[1] := 0;
                  MyPara.aPara[2] := pHelpManaging^.FlexCon.SafetyNumber$DINT;
                  MyPara.aPara[3] := pHelpManaging^.FlexCon.ReadLen;
                  MyPara.aPara[4] := 0;
                  MyPara.aPara[5] := 0;
                  MyPara.aPara[6] := 0;
                  MyPara.aPara[7] := pHelpConnection^.MasterWriteOffset;
                  MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                  MyPara.aPara[9] := FALSE; // no interface frame => needed to reuse the same input PDO at the module

                  if sFlexCon.pSafetyHost^.pThis^.NewInst(#MyPara, #MyResult) <> READY then
                    LogError(e_msg:="@05DE (SafetyManager::GetDestManagingOfDO) Failed to add FlexCon PDO to safe CPU PDO input buffer (Host side)");
                  end_if;

                  // check if we already have a input pdo pointer
                  if pHelpManaging^.FlexCon.pInputPdo then
                    pHelpManaging^.FlexCon.pInputPdo^.pSafety := sFlexCon.pSafetyHost;
                    pHelpManaging^.FlexCon.pInputPdo^.pMem    := MyPara.aPara[0];
                  else
                    if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
                      aFSoEInputPDOs[FSoEInputPDONo].pSafety    := sFlexCon.pSafetyHost;
                      aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
                      pHelpManaging^.FlexCon.pInputPdo          := #aFSoEInputPDOs[FSoEInputPDONo];
                      FSoEInputPDONo += 1;
                    else
                      LogValue("@0490 (SafetyManager::GetDestManagingOfDO) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                    end_if;
                  end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::GetDestManagingOfDO) Mng => Modul: Wr Offset Modul: 0x{0}, Rd Offset Mng: 0x{1}, Größe: 0x{2}", Value:=pHelpConnection^.MasterWriteOffset, Value2:=pHelpManaging^.FlexCon.ReadOffset, Value3:=pHelpManaging^.FlexCon.ReadLen);
                  LogValue("@DBG (SafetyManager::GetDestManagingOfDO) Modul => Mng: Rd Offset Modul: 0x{0}, Wr Offset Mng: 0x{1}, Größe: 0x{2}", Value:=pHelpConnection^.MasterReadOffset, Value2:=pHelpManaging^.FlexCon.WriteOffset, Value3:=pHelpManaging^.FlexCon.WriteLen);
#endif

                  if b_ConnectionAlreadyExists = FALSE then
                    pHelpManaging^.PDOReadMemUsed += pHelpManaging^.FlexCon.ReadLen;

                    MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
                    MyPara.aPara[0] := pHelpManaging^.PDOReadMemUsed$DINT;
                    pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult);
                  end_if;

                  // mark the connection as established
//                  pHelpConnection^.InternalConnectionState := FCS_CONNECTION;

                  // remember the connection if it's connected
                  pHelpManaging^.FlexCon.pFSoEConnection := pHelpConnection;

                  // we got our connection, stop cycling through the rest of the configurations
                  pDestManaging := pHelpManaging;
                  return;
                end_if;
              
                pHelpConnection := pHelpConnection^.pNext;
              end_while;
              pHelpConfig := pHelpConfig^.pNext;
            end_while;
          else
            LogValue(pString:="@05DF (SafetyManager::GetDestManagingOfDO) AddDynSlave Response detected in SafetyManager of Host (failed with retcode 0x{0})", Value:=pAddDynSlaveRsp^.Retcode);
          end_if;
        else
          pRemDynSlaveRsp := pDO$^t_RemDynSlaveRsp;

          if pRemDynSlaveRsp^.Retcode = 0 then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
            LogError("@DBG RemoveDynSlave Response detected in SafetyManager of Host (succeeded)");
#endif
            if pHelpManaging^.FlexCon.pFSoEConnection then
              // remove the InputPDO from the Safety CPU
              if pHelpManaging^.FlexCon.pInputPdo & pHelpManaging^.FlexCon.pInputPdo^.pMem & pHelpManaging^.FlexCon.pInputPdo^.pSafety then
                // unregister this PDO
                MyPara.uiCmd := CMD_SM_REMOVE_INPUT_PDO;
                MyPara.aPara[0] := pHelpManaging^.FlexCon.pInputPdo^.pMem;
                MyPara.aPara[1] := 0;
                pHelpManaging^.FlexCon.pInputPdo^.pSafety^.pThis^.NewInst(#MyPara, #MyResult);              
              end_if;

//              pHelpManaging^.FlexCon.pFSoEConnection^.InternalConnectionState := FCS_RESET;
              pHelpManaging^.FlexCon.pFSoEConnection := NIL;
            end_if;
          else
            LogValue(pString:="@05E0 (SafetyManager::GetDestManagingOfDO) RemoveDynSlave Response detected in SafetyManager of Host (failed with retcode 0x{0})", Value:=pRemDynSlaveRsp^.Retcode);
          end_if;
        end_if;
      end_if;

      pDestManaging := pHelpManaging;
      return;
    end_if;
  
    if pHelpManaging^.ModuleNo then
      for i := 0 to (pHelpManaging^.ModuleNo-1) do
        if pHelpManaging^.ModuleInfo[i].SafetyNbr = pDO^.DstAdr then
          pDestManaging := pHelpManaging;
          return;
        end_if;
      end_for;
    end_if;
  
    pHelpManaging := pHelpManaging^.pNext;
  end_while;

END_FUNCTION


FUNCTION SafetyManager::GetDestSafetyOfDO
  VAR_INPUT
    pDO 	: ^SafetyCDIAS_Base::SDOHeader;
  END_VAR
  VAR_OUTPUT
    pDestSafety 	: ^t_SafetyComponent;
  END_VAR
  VAR
    pHelpSafety 	            : ^t_SafetyComponent;
    pAddDynSlaveRsp           : ^t_AddDynSlaveRsp;
    pRemDynSlaveRsp           : ^t_RemDynSlaveRsp;
    pHelpConfig               : ^t_CfgListElem;
    pHelpConnection           : ^t_FSoEConnectionDetails;
    MyPara                    : CmdStruct;
    MyResult                  : results;
    b_ConnectionAlreadyExists : BOOL;
  END_VAR

  pDestSafety := NIL;

  // check if it's a SDO addressed via safety number
  if (pDO^.StdHeader.Typ AND SSDO_MASK_TYPE) = FRM_TYPECODE_SSDO_MOD_TP then
    // we can't find it via Topology path
    LogError("@043C (SafetyManager::GetDestSafetyOfDO) Misdirected SDO via Topology path. Addressee can't be found here");
    return;
  end_if;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
    if pHelpSafety^.SafetyNbr = pDO^.DstAdr then
     
      // if it's a response to AddDynSlave or RemoveDynSlave request
      if (pDO^.StdHeader.Typ.Response) & ((pDO^.StdHeader.Cmd = SM_SDO_COMMAND_ADD_DYN_SLAVE) | (pDO^.StdHeader.Cmd = SM_SDO_COMMAND_REMOVE_DYN_SLAVE)) then

        // if the initialisation of the class is not finished or failed
        if (sFlexCon.Role <> _NoFlexCon) & (ClassState <> 1) then
          // don't forward add or remove dyn slave requests. otherwise the order in the managing module is likely to be wrong if flexcon is used in combination with interface frames.
          return;
        end_if;
      
        // if it's an AddDynSlave response
        if (pDO^.StdHeader.Cmd = SM_SDO_COMMAND_ADD_DYN_SLAVE) then
          pAddDynSlaveRsp := pDO$^t_AddDynSlaveRsp;
          
          // if it was successful and we have a safety host managing module pointer as well as a safety number connected with it => try to install the PDO
          if (pAddDynSlaveRsp^.Retcode = 0) & sFlexCon.pManaging2Host & sFlexCon.pManaging2Host^.FlexCon.SafetyNumber then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
            LogError("@DBG AddDynSlave Response detected in SafetyManager of Node (succeeded)");
#endif

            // install the access to the next free memory on the VBC

            // we're the node => search for the FSoE connection with the node flags
            pHelpConfig := pConfigs;
            while pHelpConfig do
              pHelpConnection := pHelpConfig^.pFirstFSoEConnection;
              while pHelpConnection do
                if ((pHelpConnection^.Flags AND SM_FLAGS_FLEXCON_MASK) = SM_FLAGS_FLEXCON_NODE) & // check if it's the right kind of connection
                    (pHelpConnection^.MasterWriteSize   = pAddDynSlaveRsp^.SafeOutLength) &
                    (pHelpConnection^.MasterReadSize    = pAddDynSlaveRsp^.SafeInLength) then

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
                  LogValue(pString:="@DBG NODE: Found FSoE Connection of FlexConNode to install the cyclic PDO Routing between 0x{0} local and 0x{1} at Host", Value:=pHelpConnection^.OwnerSafetyNbr, Value2:=sFlexCon.pManaging2Host^.FlexCon.SafetyNumber);
#endif

                  // remember if we already had this connection before
                  b_ConnectionAlreadyExists := sFlexCon.pManaging2Host^.FlexCon.pCopyCmd <> NIL;

                  // if we just created this connection
                  if b_ConnectionAlreadyExists = FALSE then
                    // remember offset and length of the write access
                    sFlexCon.pManaging2Host^.FlexCon.WriteOffset:= sFlexCon.pManaging2Host^.PDOWriteMemUsed$UINT;
                    sFlexCon.pManaging2Host^.FlexCon.WriteLen   := pAddDynSlaveRsp^.SafeOutLength;
                  end_if;

                  // GET THE DATA FROM THE SCP TO THE VBC
  
                  MyPara.uiCmd := CMD_SM_ADD_OUTPUT_PDO;
//                      MyPara.aPara[0] := 1;  not used // mark as interface frame PDO
                  pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

  //                  pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
  //                  pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
  //                  pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
  //                  pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support

                  // check if we already have a copy pointer
                  if sFlexCon.pManaging2Host^.FlexCon.pCopyCmd then
                    sFlexCon.pManaging2Host^.FlexCon.pCopyCmd^.pSource  := MyResult.aData[0]$^USINT + pHelpConnection^.SlaveReadOffset;
                    sFlexCon.pManaging2Host^.FlexCon.pCopyCmd^.pDest    := sFlexCon.pManaging2Host^.pPDOWriteSpace + sFlexCon.pManaging2Host^.FlexCon.WriteOffset;
                    sFlexCon.pManaging2Host^.FlexCon.pCopyCmd^.Size     := sFlexCon.pManaging2Host^.FlexCon.WriteLen;
                  else
                    if FSoECopyCmdNo < MAX_FSOE_CONNECTIONS then
                      aFSoECopyCmds[FSoECopyCmdNo].pSource  := MyResult.aData[0]$^USINT + pHelpConnection^.SlaveReadOffset;
                      aFSoECopyCmds[FSoECopyCmdNo].pDest    := sFlexCon.pManaging2Host^.pPDOWriteSpace + sFlexCon.pManaging2Host^.FlexCon.WriteOffset;
                      aFSoECopyCmds[FSoECopyCmdNo].Size     := sFlexCon.pManaging2Host^.FlexCon.WriteLen;
                      sFlexCon.pManaging2Host^.FlexCon.pCopyCmd := #aFSoECopyCmds[FSoECopyCmdNo];
                      FSoECopyCmdNo                         += 1;
                    else
                      LogValue("@0493 (SafetyManager::GetDestSafetyOfDO) Maximum limit of allowed FSoE connections exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                    end_if;
                  end_if;

                  // if we just created this connection
                  if b_ConnectionAlreadyExists = FALSE then
                    // increase the used write memory on the managing module
                    sFlexCon.pManaging2Host^.PDOWriteMemUsed += sFlexCon.pManaging2Host^.FlexCon.WriteLen;

                    MyPara.uiCmd    := CMD_SM_PDO_MEM_USAGE_WR;
                    MyPara.aPara[0] := sFlexCon.pManaging2Host^.PDOWriteMemUsed$DINT;
                    sFlexCon.pManaging2Host^.pThis^.NewInst(#MyPara, #MyResult);
                    
                    // remember offset and length of the read access
                    sFlexCon.pManaging2Host^.FlexCon.ReadOffset:= sFlexCon.pManaging2Host^.PDOReadMemUsed$UINT;
                    sFlexCon.pManaging2Host^.FlexCon.ReadLen   := pHelpConnection^.MasterReadSize;
                  end_if;

  

                  // HOW TO GET DATA FROM THE VBC TO THE SCP
                  
                  MyPara.uiCmd := CMD_SM_ADD_INPUT_PDO;
                  MyPara.aPara[0] := (sFlexCon.pManaging2Host^.pPDOReadSpace + sFlexCon.pManaging2Host^.FlexCon.ReadOffset)$DINT;
                  MyPara.aPara[1] := 0;
                  MyPara.aPara[2] := sFlexCon.pManaging2Host^.FlexCon.SafetyNumber$DINT;
                  MyPara.aPara[3] := sFlexCon.pManaging2Host^.FlexCon.ReadLen;
                  MyPara.aPara[4] := 0;
                  MyPara.aPara[5] := 0;
                  MyPara.aPara[6] := 0;
                  MyPara.aPara[7] := pHelpConnection^.SlaveWriteOffset;
                  MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                  MyPara.aPara[9] := FALSE; // no interface frame => needed to reuse the same input PDO at the module

                  if pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult) <> READY then
                    LogError(e_msg:="@05E1 (SafetyManager::GetDestManagingOfDO) Failed to add FlexCon PDO to safe CPU PDO input buffer (Node side)");
                  end_if;

                  // check if we already have a input pdo pointer
                  if sFlexCon.pManaging2Host^.FlexCon.pInputPdo then
                    sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pSafety := pHelpSafety;
                    sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pMem    := MyPara.aPara[0];
                  else
                    if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
                      aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pHelpSafety;
                      aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
                      sFlexCon.pManaging2Host^.FlexCon.pInputPdo := #aFSoEInputPDOs[FSoEInputPDONo];
                      FSoEInputPDONo += 1;
                    else
                      LogValue("@0490 (SafetyManager::GetDestSafetyOfDO) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                    end_if;
                  end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::GetDestSafetyOfDO) Mng => Modul: Wr Offset Modul: 0x{0}, Rd Offset Mng: 0x{1}, Größe: 0x{2}", Value:=pHelpConnection^.SlaveWriteOffset, Value2:=sFlexCon.pManaging2Host^.FlexCon.ReadOffset, Value3:=sFlexCon.pManaging2Host^.FlexCon.ReadLen);
                  LogValue("@DBG (SafetyManager::GetDestSafetyOfDO) Modul => Mng: Rd Offset Modul: 0x{0}, Wr Offset Mng: 0x{1}, Größe: 0x{2}", Value:=pHelpConnection^.SlaveReadOffset, Value2:=sFlexCon.pManaging2Host^.FlexCon.WriteOffset, Value3:=sFlexCon.pManaging2Host^.FlexCon.WriteLen);
#endif
                  
                  // if we just created this connection
                  if b_ConnectionAlreadyExists = FALSE then
                    // increase the used read memory on the managing module
                    sFlexCon.pManaging2Host^.PDOReadMemUsed += sFlexCon.pManaging2Host^.FlexCon.ReadLen;

                    MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
                    MyPara.aPara[0] := sFlexCon.pManaging2Host^.PDOReadMemUsed$DINT;
                    sFlexCon.pManaging2Host^.pThis^.NewInst(#MyPara, #MyResult);
                  end_if;

                  // remember the connection if it's connected
                  sFlexCon.pManaging2Host^.FlexCon.pFSoEConnection := pHelpConnection;

                  // we got our connection, stop cycling through the rest of the configurations
                  pDestSafety := pHelpSafety;
                  return;                    
                end_if;
              
                pHelpConnection := pHelpConnection^.pNext;
              end_while;
              pHelpConfig := pHelpConfig^.pNext;
            end_while;
          else
            //only log error if retcode is not zero (AddDynSlave is also used for the runner and there is no Flexcon).
            if pAddDynSlaveRsp^.Retcode <> 0 then
              LogValue(pString:="@05E2 (SafetyManager::GetDestSafetyOfDO) AddDynSlave Response detected in SafetyManager of Node (failed with retcode 0x{0})", Value:=pAddDynSlaveRsp^.Retcode);
            end_if;
          end_if;
        else
          // it's a RemoveDynSlave response
          
          pRemDynSlaveRsp := pDO$^t_RemDynSlaveRsp;

          if pRemDynSlaveRsp^.Retcode = 0 then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
            LogError("@DBG RemoveDynSlave Response detected in SafetyManager of Node (succeeded)");
#endif

            if sFlexCon.pManaging2Host & sFlexCon.pManaging2Host^.FlexCon.pFSoEConnection then
              // remove the InputPDO from the Safety CPU
              if sFlexCon.pManaging2Host^.FlexCon.pInputPdo & sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pMem & sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pSafety then
                // unregister this PDO
                MyPara.uiCmd := CMD_SM_REMOVE_INPUT_PDO;
                MyPara.aPara[0] := sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pMem;
                MyPara.aPara[1] := 0;
                sFlexCon.pManaging2Host^.FlexCon.pInputPdo^.pSafety^.pThis^.NewInst(#MyPara, #MyResult);              
              end_if;

//              sFlexCon.pManaging2Host^.FlexCon.pFSoEConnection^.InternalConnectionState := FCS_RESET;
              sFlexCon.pManaging2Host^.FlexCon.pFSoEConnection := NIL;
            end_if;

          else
            LogValue(pString:="@05E3 (SafetyManager::GetDestSafetyOfDO) RemoveDynSlave Response detected in SafetyManager of Node (failed with retcode 0x{0})", Value:=pRemDynSlaveRsp^.Retcode);
          end_if;
        end_if;  
      end_if;

      pDestSafety := pHelpSafety;
      return;
    end_if;
  
    pHelpSafety := pHelpSafety^.pNext;
  end_while;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL SafetyManager::RtWork
  VAR_INPUT
    EAX 	: UDINT;
  END_VAR
  VAR_OUTPUT
    state (EAX) 	: UDINT;
  END_VAR
  VAR
    i                 : UDINT;
    pPDOElement       : ^t_PDOElement;
    pFSoEElement      : ^t_FSoECopyCmd;
    AllCopyCmdsReady  : BOOL;
  END_VAR

  if EAX = CALL_OPTION_RT_PRESCAN then
    // prescan

//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StartTimeMeasurement(VARANTIME_RT);
#endif
//=================================================================================================

    // copy commands for udp connections (receiving via udp)
    if sUdpVars.ReceiveCopyCmdNo then
      // we may have some udp send data, so we better protect the copy mechanism
      UdpReceiveData.SectionStart();

      pFSoEElement := #sUdpVars.aReceiveCopyCmds[0];
      for i := 1 to sUdpVars.ReceiveCopyCmdNo do
        _memcpy(ptr1:=pFSoEElement^.pDest, ptr2:=pFSoEElement^.pSource, cntr:=pFSoEElement^.Size);
        pFSoEElement += sizeof(t_FSoECopyCmd);
      end_for;

      UdpReceiveData.SectionStop();
    end_if;

    case InitSSW of
//**********************************************************************************************************************************************************
      _Wait4CopyCmdStart:
        // start all used copy commands
        AllCopyCmdsReady := TRUE;
        if CopyCmdsUsed then
          for i := 0 to (CopyCmdsUsed-1) do
            if aCopyCmds[i].b_Ready4Action then
              aCopyCmds[i].pHandle^.ucCtrlStat := VARAN_DO_CONT_RUN;
            else
              AllCopyCmdsReady := FALSE;
              bd_LogRtMessagesInCy.UnkownCopySource := TRUE;
            end_if;
          end_for;        
        end_if;
        
        if AllCopyCmdsReady then
          InitSSW := _FinishInitialisation;
        end_if;

//**********************************************************************************************************************************************************
      _InitFinished:
        // if we're master or we're actually time synchronizing
        if b_NewSyncMode & b_CheckTimeSyncTimeout then
          if (ops.tAbsolute - TimeSyncTimeStamp) > TIMESYNC_TIMEOUT then
            // increment timeout counter => message is shown in cyclic after x consecutive 
            SyncTimeoutCounter += 1;
            b_CheckTimeSyncTimeout := FALSE; // => show Trace Message only once per error
            // restart the synchronisation (maybe the other domain has been disconnected)
            b_StartTimeSync := TRUE;
          end_if;
        end_if;

//**********************************************************************************************************************************************************
    end_case;


    // marker is set in cyclic priority (except on timeout), but time synchronisation has to be done in rt priority => start it from here
    if b_StartTimeSync then
      b_StartTimeSync := FALSE;
      
      StartTimeSync();
    end_if;

//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StopTimeMeasurement(VARANTIME_RT);
#endif
//=================================================================================================

  else
    // postscan
    
//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StartTimeMeasurement(VARANTIME_RTPOSTSCAN);
#endif
//=================================================================================================

    // copy data from our memory to the memory of the next domain
    if PDOElementNo then
      pPDOElement := #aPDOElements[0];
      for i := 1 to PDOElementNo do       
        if (pPDOElement^.ThisDomainsRole = SourceDest_IsSource) |
           (pPDOElement^.ThisDomainsRole = SourceDest_IsForwarder) then
          if pPDOElement^.DestInfo.pPDOMem & pPDOElement^.SourceInfo.pPDOMem & pPDOElement^.Length then
            _memcpy(ptr1:=pPDOElement^.DestInfo.pPDOMem, ptr2:=pPDOElement^.SourceInfo.pPDOMem, cntr:=pPDOElement^.Length);          
          end_if;
        end_if;
        pPDOElement += sizeof(t_PDOElement);
      end_for;
    elsif FSoECopyCmdNo then
      pFSoEElement := #aFSoECopyCmds[0];
      for i := 1 to FSoECopyCmdNo do
        _memcpy(ptr1:=pFSoEElement^.pDest, ptr2:=pFSoEElement^.pSource, cntr:=pFSoEElement^.Size);
        pFSoEElement += sizeof(t_FSoECopyCmd);
      end_for;
    end_if;

    // copy commands for udp connections (sending via udp)
    if sUdpVars.SendCopyCmdNo then
      // we may have some udp send data, so we better protect the copy mechanism
      UdpSendData.SectionStart();

      pFSoEElement := #sUdpVars.aSendCopyCmds[0];
      for i := 1 to sUdpVars.SendCopyCmdNo do
        _memcpy(ptr1:=pFSoEElement^.pDest, ptr2:=pFSoEElement^.pSource, cntr:=pFSoEElement^.Size);
        pFSoEElement += sizeof(t_FSoECopyCmd);
      end_for;

      UdpSendData.SectionStop();
    end_if;


//=================================================================================================
#ifdef VM_CLASS_TIME_MEASUREMENT
  StopTimeMeasurement(VARANTIME_RTPOSTSCAN);
#endif
//=================================================================================================

  end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
  if InitSSW <> OldInitSSW then
#ifdef SAFETY_DEBUG_TRACING_ACTIVATED
    LogValue("@DBG (SafetyManager::RtWork) InitSSW changed from {0} to {1}", Value:=OldInitSSW, Value2:=InitSSW);
#endif
    LogBuf[ActLogIndex].NewState := InitSSW;
    LogBuf[ActLogIndex].OldState := OldInitSSW;
    LogBuf[ActLogIndex].Command := 0;
    LogBuf[ActLogIndex].TimeSinceLastLog_us := OS_READMICROSEC();
    LogBuf[ActLogIndex].TimeSinceLastLog_us -= TimeStampLastLog_us;
    TimeStampLastLog_us := OS_READMICROSEC();
    if ActLogIndex = (SM_LogBufferSize - 1) then
      ActLogIndex := 0;
    else
      ActLogIndex += 1;
    end_if;
  end_if;
  OldInitSSW := InitSSW;
#endif

  state := READY;

END_FUNCTION


FUNCTION SafetyManager::CheckTopSDO
  VAR_INPUT
    pPara 	: ^CmdStruct;
  END_VAR
  VAR
    pDO 	            : ^SafetyCDIAS_Base::SDOHeader;
    pCompDestPath     : ^USINT;
    pHelpConfig       : ^t_CfgListElem;
    pHelpSlave        : ^t_SlaveListElem;
    pHelpModule       : ^t_ModuleListElem;
    pHelpPdo          : ^t_PdoListElem;
    pHelpManaging     : ^t_ManagingComponent;
    pHelpSafety       : ^t_SafetyComponent;
    pHelpConnection   : ^t_FSoEConnectionDetails;
    MyPara            : CmdStruct;
    MyResult          : results;
    pTempMem          : ^USINT;
    pHelp             : ^USINT;
    TmpLen            : UINT;
    DomainChangeFound : BOOL;
    i                 : UDINT;
    DestDomain        : USINT;
    InterfaceCRC      : HDINT;
    TargetSafetyNbr   : HDINT;
    pActConnection    : ^t_UdpConnection;
    IPAddr            : HDINT;
  END_VAR

  // get a pointer to the compressed destination path
  pDO := (pPara^.aPara[1])$^SafetyCDIAS_Base::SDOHeader;
  pCompDestPath := (#pDO^.DstAdr)$^USINT;

  // if we have interfaceframes, check them first
  if pFirstManagingInterfaceComponent & (pCompDestPath^ = INTERFACE_CRC_MODE) then
    // after the mode byte we find the interface CRC
    InterfaceCRC := ((pCompDestPath + 1)$^HDINT)^;

    // if we have local connections and the source is not local => check the local connections for a receiver
    if pSortedConnections & (pPara^.aPara[2] <> this$DINT) then
      // we have to search for a receiver
      pHelpConnection := pSortedConnections;

      while pHelpConnection do
        if pHelpConnection^.SlavePathCompLen & pHelpConnection^.pSlavePathComp then
          if (pHelpConnection^.pSlavePathComp$^HDINT + 1)^ = InterfaceCRC then
            // we found the interface CRC, now we have to make a connection via the SafetyNumber between the actual config and the module handles
            pHelpSafety := pFirstSafetyComponent;
            while pHelpSafety do
              // we compare the safety number
              if pHelpSafety^.SafetyNbr = pHelpConnection^.OwnerSafetyNbr then
                // change the command, because now we forward it to the receiver
                pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            

                return;
              end_if;
              
              pHelpSafety := pHelpSafety^.pNext;
            end_while;
          end_if;
        end_if;
      
        pHelpConnection := pHelpConnection^.pNext;
      end_while;
    end_if;

    // if there are FSoE Connection
    if IFFSoENo then
      // if this system is the sender
      if (pPara^.aPara[2] = this$DINT) then
        // check the FSoE Connections
        for i := 0 to (IFFSoENo-1) do
          if aIFFSoEInfos[i].FromOtherSide.InterfaceCRC = InterfaceCRC then
            if aIFFSoEInfos[i].pManaging & aIFFSoEInfos[i].pManaging^.InfoBits.IsSynchron then
              aIFFSoEInfos[i].pManaging^.pThis^.NewInst(pPara, #MyResult);
            end_if;
            
            // we're done here
            return;
          end_if;
        end_for;
        
        // check if it's a local interface frame
        pHelpSafety := NIL;
        if LocalIFNo then
          for i := 0 to (LocalIFNo-1) do
            if aLocalIFs[i].InterfaceCRC = InterfaceCRC then
              // found it
              pHelpSafety := aLocalIFs[i].pSafetyModule;
              exit;
            end_if;
          end_for;
        end_if;
        
        if pHelpSafety then
          // change the command, because now we forward it to the receiver
          pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

          pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            
          
          return;
        else
          LogError("@043E (SafetyManager::CheckTopSDO) Couldn't find target of SDO via TOP (Interface Frame in FSoE mode)");
          LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=InterfaceCRC);
        end_if;
      else
        // we have to search for a receiver
        pHelpConfig := pConfigs;
        while pHelpConfig do
          pHelpConnection := pHelpConfig^.pFirstFSoEConnection;
          while pHelpConnection do
            if pHelpConnection^.SlavePathCompLen & pHelpConnection^.pSlavePathComp then
              if (pHelpConnection^.pSlavePathComp$^HDINT + 1)^ = InterfaceCRC then
                // we found the interface CRC, now we have to make a connection via the SafetyNumber between the actual config and the module handles
                pHelpSafety := pFirstSafetyComponent;
                while pHelpSafety do
                  // we compare the safety number
                  if pHelpSafety^.SafetyNbr = pHelpConfig^.SafetyNbr then
                    // change the command, because now we forward it to the receiver
                    pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                    pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            

                    return;
                  end_if;
                  
                  pHelpSafety := pHelpSafety^.pNext;
                end_while;
              end_if;
            end_if;
          
            pHelpConnection := pHelpConnection^.pNext;
          end_while;
          pHelpConfig := pHelpConfig^.pNext;
        end_while;
      end_if;
    else
      if b_GotFSoEModules then
        // search in the module lists
        pHelpConfig := pConfigs;
        TargetSafetyNbr := 0;
        while pHelpConfig do
          pHelpModule := pHelpConfig^.pFirstModuleCfg;
          while pHelpModule do
            if pHelpModule^.ModuleInfo.InterfaceCRC = InterfaceCRC then
              TargetSafetyNbr := pHelpModule^.ModuleInfo.SafetyNbr;
            end_if;
            
            // if we found our target => forward the SDO to it
            if TargetSafetyNbr then
              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                // we compare the safety number
                if pHelpSafety^.SafetyNbr = TargetSafetyNbr then
                  // change the command, because now we forward it to the receiver
                  pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                  pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            

                  return;
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;
            end_if;
          
            pHelpModule := pHelpModule^.pNext;
          end_while;
          
          pHelpConfig := pHelpConfig^.pNext;
        end_while;
      else
  // check if we have an interface frame with this CRC
        pHelpConfig := pConfigs;
        while pHelpConfig do
          pHelpPdo := pHelpConfig^.pFirstPdoCfg;
          while pHelpPdo do
            // don't look at IFBroatCast PDOs, they are not distributed
            if (pHelpPdo^.PdoInfo.InterfaceCRC = InterfaceCRC) then
              if (pHelpPdo^.PdoInfo.Info.IFBroadCast = FALSE) then

                // if we found the CRC, forward it to the source of the PDO (the destination asks for the safety number)

                // check the source

                // if the master itself is the caller, it's from this system (check if it's for another system)
                if (pPara^.aPara[2] = this$DINT) then
                  pHelpManaging := pFirstManagingInterfaceComponent;
                  while pHelpManaging do
                    if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pHelpPdo^.PdoInfo.pSourcePath, pHelpPdo^.PdoInfo.SourcePathLen) = 0) then
                      exit;
                    else
                      pHelpManaging := pHelpManaging^.pNext;
                    end_if;
                  end_while;

                  if pHelpManaging then
                    if pHelpManaging^.InfoBits.IsSynchron then
                      pHelpManaging^.pThis^.NewInst(pPara, #MyResult);
                    end_if;
                  else
                    // no managing module found => continue search in our special safety module list
                    pHelpSafety := NIL;
                    if LocalIFNo then
                      for i := 0 to (LocalIFNo-1) do
                        if aLocalIFs[i].InterfaceCRC = InterfaceCRC then
                          // found it
                          pHelpSafety := aLocalIFs[i].pSafetyModule;
                          exit;
                        end_if;
                      end_for;
                    end_if;
                    
                    if pHelpSafety then
                      // change the command, because now we forward it to the receiver
                      pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                      pHelpSafety^.pThis^.NewInst(pPara, #MyResult);
                    else
                      // check the slave modules if there is one with the interface frame we need
                      pHelpConfig := pConfigs;
                      while pHelpConfig do
                        pHelpSlave := pHelpConfig^.pFirstSlaveCfg;
                        while pHelpSlave do

                          if (pHelpSlave^.SlaveInfo.Info.IsInterface) & (pHelpSlave^.SlaveInfo.InterfaceCRC = InterfaceCRC) then
                            // we have to add some additional information to the message

                            (*
                              Memory structure:
                                1 Byte destination domain
                                1 Byte IsInterfaceFrame (0/1)
                                2 Byte length of uncompressed path
                                x Byte uncompressed path
                                x Byte SDO message
                            *)
                            
                            MyPara.uiCmd := CMD_SM_TOP_SDO_TO_MODULE;
                            MyPara.aPara[0] := sizeof(USINT) + sizeof(BOOL) + sizeof(UINT) + pHelpSlave^.SlaveInfo.PathLen + pPara^.aPara[0];
                            
                            if ud_SizeTempMemCy >= MyPara.aPara[0]$UDINT then
                              pTempMem := pTempMemCy$^USINT;
                            else
                              LogError("@0440 (SafetyManager::CheckTopSDO) Temporary memory for cyclic task is too small!");
                              LogValue("AvaliableMem: {0} / NeededMem: {1}", Value:=ud_SizeTempMemCy, Value2:=MyPara.aPara[0]$UDINT);
                              return;
                            end_if;

                            TmpLen := pHelpSlave^.SlaveInfo.PathLen; // save some time with this local variable
                            DomainChangeFound := FALSE;
                            pHelp := pHelpSlave^.SlaveInfo.pPath + TmpLen - 2; // pointer on penultimate byte of path
                            // search from the end for the last domain change
                            for i := (TmpLen-2) to 3 by -1 do
                              if pHelp^ = HWT_ESCAPE_DOMAIN then
                                DomainChangeFound := TRUE;
                                DestDomain := (pHelp + 1)^;
                                exit;
                              end_if;
                              
                              pHelp -= sizeof(USINT);
                            end_for;

                            // if there was no domainchange
                            if DomainChangeFound = FALSE then
                              // the master is the Dest
                              DestDomain := (pHelpSlave^.SlaveInfo.pPath + 2)^;
                            end_if;

                            pHelp := pTempMem;
                            
                            // destination domain
                            pHelp^ := DestDomain;
                            pHelp += sizeof(USINT);

                            // is it for an interface frame?
                            pHelp^$BOOL := TRUE;
                            pHelp += sizeof(BOOL);

                            // length of uncompressed path
                            pHelp^$UINT := TmpLen;
                            pHelp += sizeof(UINT);

                            // uncompressed path
                            _memcpy(ptr1:=pHelp, ptr2:=pHelpSlave^.SlaveInfo.pPath, cntr:=TmpLen);
                            pHelp += TmpLen;
                            
                            // SDO message
                            _memcpy(ptr1:=pHelp, ptr2:=pDO, cntr:=pPara^.aPara[0]$UDINT);
                            
                            MyPara.aPara[1] := pTempMem$DINT;
                            
                            NewInst(#MyPara, #MyResult);

                            // path can exist only once in a system
                            return;
                          end_if;
                          
                          pHelpSlave := pHelpSlave^.pNext;
                        end_while;
                        
                        pHelpConfig := pHelpConfig^.pNext;
                      end_while;

                      LogError("@0441 (SafetyManager::CheckTopSDO) Couldn't find target of SDO via TOP (Interface Frame)");
                      LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=InterfaceCRC);
                    end_if;
                  end_if;
                else
                  // we got it from another system => check our modules
                  pHelpSafety := NIL;
                  if LocalIFNo then
                    for i := 0 to (LocalIFNo-1) do
                      if aLocalIFs[i].InterfaceCRC = InterfaceCRC then
                        // found it
                        pHelpSafety := aLocalIFs[i].pSafetyModule;
                        exit;
                      end_if;
                    end_for;
                  end_if;
                  
                  if pHelpSafety then
                    // change the command, because now we forward it to the receiver
                    pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                    pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            
                  else
                    LogError("@0441 (SafetyManager::CheckTopSDO) Couldn't find target of SDO via TOP (Interface Frame) received by another PLC");
                    LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=InterfaceCRC);
                  end_if;
                end_if;
                // path can exist only once in a system
                return;
              elsif (pHelpPdo^.PdoInfo.Info.VirtualPDOsCreated = FALSE) then
                // it's a broadcast, the IF-CRC matches and we didn't create virtual PDOs so far
                pHelpSafety := pFirstSafetyComponent;
                while pHelpSafety do
                  if (pHelpPdo^.PdoInfo.pSourcePath^ = pHelpSafety^.pPath^) & 
                     (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pHelpPdo^.PdoInfo.pSourcePath + 2 + pHelpPdo^.PdoInfo.pSourcePath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then

                    // change the command, because now we forward it to the receiver
                    pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                    pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            

                    return;
                  else
                    pHelpSafety := pHelpSafety^.pNext;
                  end_if;
                end_while;
              end_if;
            end_if;
          
            pHelpPdo := pHelpPdo^.pNext;
          end_while;
        
          pHelpConfig := pHelpConfig^.pNext;
        end_while;    
      end_if;
    end_if;

    // check if there is any interface frame domain optional and not available
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      if (pHelpManaging^.InfoBits.IsSynchron = 0) & (pHelpManaging^.InfoBits.IsOptional = 1) then
        // found one => no error here
        return;
      end_if;
    
      pHelpManaging := pHelpManaging^.pNext;
    end_while;

    // no optional interface domains missing => must be an error
    LogError("@0468 (SafetyManager::CheckTopSDO) Couldn't find InterfaceCRC. Check SafetyDesigner projects, maybe the interface variables do not match.");
    LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=InterfaceCRC);
    ClassState := -8;
    
  // check if it's a local interface frame and we have no interface frame domains
  elsif (pCompDestPath^ = INTERFACE_CRC_MODE) then
    // after the mode byte we find the interface CRC
    InterfaceCRC := ((pCompDestPath + 1)$^HDINT)^;

    // search for the CRC
    pHelpSafety := NIL;
    if LocalIFNo then
      for i := 0 to (LocalIFNo-1) do
        if aLocalIFs[i].InterfaceCRC = InterfaceCRC then
          // found it
          pHelpSafety := aLocalIFs[i].pSafetyModule;
          exit;
        end_if;
      end_for;
    end_if;

    if pHelpSafety then
      // change the command, because now we forward it to the receiver
      pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

      pHelpSafety^.pThis^.NewInst(pPara, #MyResult);
    else
      // check the slave modules if there is one with the interface frame we need
      TargetSafetyNbr := 0;
      pHelpConfig := pConfigs;
      while pHelpConfig do
        pHelpSlave := pHelpConfig^.pFirstSlaveCfg;
        while pHelpSlave do

          if (pHelpSlave^.SlaveInfo.Info.IsInterface) & (pHelpSlave^.SlaveInfo.InterfaceCRC = InterfaceCRC) then
            // we have to add some additional information to the message

            (*
              Memory structure:
                1 Byte destination domain
                1 Byte IsInterfaceFrame (0/1)
                2 Byte length of uncompressed path
                x Byte uncompressed path
                x Byte SDO message
            *)
            
            MyPara.uiCmd := CMD_SM_TOP_SDO_TO_MODULE;
            MyPara.aPara[0] := sizeof(USINT) + sizeof(BOOL) + sizeof(UINT) + pHelpSlave^.SlaveInfo.PathLen + pPara^.aPara[0];
            
            if ud_SizeTempMemCy >= MyPara.aPara[0]$UDINT then
              pTempMem := pTempMemCy$^USINT;
            else
              LogError("@0440 (SafetyManager::CheckTopSDO) Temporary memory for cyclic task is too small!");
              LogValue("AvaliableMem: {0} / NeededMem: {1}", Value:=ud_SizeTempMemCy, Value2:=MyPara.aPara[0]$UDINT);
              return;
            end_if;

            TmpLen := pHelpSlave^.SlaveInfo.PathLen; // save some time with this local variable
            DomainChangeFound := FALSE;
            pHelp := pHelpSlave^.SlaveInfo.pPath + TmpLen - 2; // pointer on penultimate byte of path
            // search from the end for the last domain change
            for i := (TmpLen-2) to 3 by -1 do
              if pHelp^ = HWT_ESCAPE_DOMAIN then
                DomainChangeFound := TRUE;
                DestDomain := (pHelp + 1)^;
                exit;
              end_if;
              
              pHelp -= sizeof(USINT);
            end_for;

            // if there was no domainchange
            if DomainChangeFound = FALSE then
              // the master is the Dest
              DestDomain := (pHelpSlave^.SlaveInfo.pPath + 2)^;
            end_if;

            pHelp := pTempMem;
            
            // destination domain
            pHelp^ := DestDomain;
            pHelp += sizeof(USINT);

            // is it for an interface frame?
            pHelp^$BOOL := TRUE;
            pHelp += sizeof(BOOL);

            // length of uncompressed path
            pHelp^$UINT := TmpLen;
            pHelp += sizeof(UINT);

            // uncompressed path
            _memcpy(ptr1:=pHelp, ptr2:=pHelpSlave^.SlaveInfo.pPath, cntr:=TmpLen);
            pHelp += TmpLen;
            
            // SDO message
            _memcpy(ptr1:=pHelp, ptr2:=pDO, cntr:=pPara^.aPara[0]$UDINT);
            
            MyPara.aPara[1] := pTempMem$DINT;
            
            NewInst(#MyPara, #MyResult);

            // path can exist only once in a system
            return;
          end_if;        
          
          pHelpSlave := pHelpSlave^.pNext;
        end_while;
        
        pHelpModule := pHelpConfig^.pFirstModuleCfg;
        while pHelpModule do
          if pHelpModule^.ModuleInfo.InterfaceCRC = InterfaceCRC then
            // if we found our target => forward the SDO to it
            TargetSafetyNbr := pHelpModule^.ModuleInfo.SafetyNbr;

            pHelpSafety := pFirstSafetyComponent;
            while pHelpSafety do
              // we compare the safety number
              if pHelpSafety^.SafetyNbr = TargetSafetyNbr then
                // change the command, because now we forward it to the receiver
                pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                pHelpSafety^.pThis^.NewInst(pPara, #MyResult);            

                return;
              end_if;
              
              pHelpSafety := pHelpSafety^.pNext;
            end_while;
          end_if;
          
          pHelpModule := pHelpModule^.pNext;
        end_while;       
        
        pHelpConfig := pHelpConfig^.pNext;
      end_while;

      LogError("@046D (SafetyManager::CheckTopSDO) Couldn't find target of SDO via TOP (Local Interface Frame)");
      LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=InterfaceCRC);
    end_if;
  elsif (pCompDestPath^ = INTERFACE_IP_MODE) then
  
    IPAddr := ((pCompDestPath + 5)$^HDINT)^;
    // after the mode byte we find the interface CRC
    InterfaceCRC := ((pCompDestPath + 1)$^HDINT)^;
    
    // Check Valid IP address
    if IPAddr AND 16#00FFFFFF then

      if sUdpVars.ProducerConnectionNo then
     
      // check udp producer list (if available)
        pActConnection := #sUdpVars.paConnectionsToProducers^[0];
        for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
          
          if pActConnection^.IFCRC = InterfaceCRC then
            // got it => forward the SDO
            TmpLen := pPara^.aPara[0]$UINT;
            
            if TmpLen <= UDP_MAX_SDO_SIZE then
              // check for unsent SDO first
              if pActConnection^.ActSendData.SdoLength = 0 then
                UdpSendData.SectionStart();
                
                _memcpy(ptr1:=#pActConnection^.ActSendData.SdoData, ptr2:=pPara^.aPara[1]$pVoid, cntr:=TmpLen);
                pActConnection^.ActSendData.SdoLength     := TmpLen$USINT;

                UdpSendData.SectionStop();
              else
                sUdpVars.SdoAlreadyUsedCnt += 1;
              end_if;
            else
              LogError("@046E (SafetyManager::CheckTopSDO) SDO size exceeds the allowed size (Compressed Topology Path: UDP)");
            end_if;

            return;
          end_if;

          pActConnection += sizeof(t_UdpConnection);
        end_for;
      end_if;

      LogError("@146F (SafetyManager::CheckTopSDO) Couldn't find destination of SDO. (Compressed Topology Path: UDP)");
      LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=((pCompDestPath + 1)$^HDINT)^);
    else
      
      // if source is in this station => search for managing components to forward
      pHelpSafety := pFirstSafetyComponent;
      
      while pHelpSafety do
        
        if pHelpSafety^.SafetyNbr = pDO^.SrcAdr then
          exit;
        end_if;
        
        pHelpSafety := pHelpSafety^.pNext;
      end_while;
      
      if pHelpSafety then
        
        // We are soure, send the data
        pHelpManaging := pFirstManagingInterfaceComponent;
        
        while pHelpManaging do

          if pHelpManaging^.pModulePath & pHelpManaging^.pModulePath^ = 3 then
            
            if ( (pHelpManaging^.pModulePath + 3)^ = HWT_ESCAPE_SAFETY_CUSTOM) & 
                 (pHelpManaging^.pModulePath + 4)^ = (IPAddr shr 24)$USINT   then
              
              if pHelpManaging^.InfoBits.IsSynchron then
                pHelpManaging^.pThis^.NewInst(pPara, #MyResult);
              end_if;
              
              return;
            end_if;
            
          end_if;
          
          pHelpManaging := pHelpManaging^.pNext;
        end_while;
        
      else // source not here => search for destination with IF CRC

        // check if we have a module with this path
        pHelpConfig := pConfigs;
        while pHelpConfig do

          pHelpModule := pHelpConfig^.pFirstModuleCfg;
          while pHelpModule do
            if pHelpModule^.ModuleInfo.InterfaceCRC = InterfaceCRC then
              // if we found our target => forward the SDO to it
              TargetSafetyNbr := pHelpModule^.ModuleInfo.SafetyNbr;

              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                // we compare the safety number
                if pHelpSafety^.SafetyNbr = TargetSafetyNbr then
                  // change the command, because now we forward it to the receiver
                  pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;

                  pHelpSafety^.pThis^.NewInst(pPara, #MyResult);

                  return;
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;
            end_if;
            
            pHelpModule := pHelpModule^.pNext;
          end_while;
          
          pHelpConfig := pHelpConfig^.pNext;
        end_while;
      
      end_if;
      
      LogValue("@0471 (SafetyManager::CheckTopSDO) Couldn't find destination of SDO. (Custom connection no: {0})", Value:=(IPAddr shr 24) );
      LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=((pCompDestPath + 1)$^HDINT)^);
      
    end_if;
  else
    // check if we have a module with this path
    pHelpConfig := pConfigs;
    while pHelpConfig do
      pHelpModule := pHelpConfig^.pFirstModuleCfg;
      while pHelpModule do
        if (To_StdLib.MemCmp(pCompDestPath, pHelpModule^.ModuleInfo.pCompPath, pHelpModule^.ModuleInfo.CompPathLen) = 0) then       
          // we have to add some additional information to the message
          
          (*
            Memory structure:
              1 Byte destination domain
              1 Byte IsInterfaceFrame (0/1)
              2 Byte length of uncompressed path
              x Byte uncompressed path
              x Byte SDO message
          *)
          
          TmpLen := pHelpModule^.ModuleInfo.PathLen; // save some time with this local variable
          MyPara.uiCmd := CMD_SM_TOP_SDO_TO_MODULE;
          MyPara.aPara[0] := sizeof(USINT) + sizeof(BOOL) + sizeof(UINT) + TmpLen + pPara^.aPara[0];
          if ud_SizeTempMemCy >= MyPara.aPara[0]$UDINT then
            pTempMem := pTempMemCy$^USINT;
          else
            LogError("@0440 (SafetyManager::CheckTopSDO) Temporary memory for cyclic task is too small!");
            LogValue("AvaliableMem: {0} / NeededMem: {1}", Value:=ud_SizeTempMemCy, Value2:=MyPara.aPara[0]$UDINT);
            return;
          end_if;

          DomainChangeFound := FALSE;
          pHelp := pHelpModule^.ModuleInfo.pPath + TmpLen - 2; // pointer on penultimate byte of path
          // search from the end for the last domain change
          for i := (TmpLen-2) to 3 by -1 do
            if pHelp^ = HWT_ESCAPE_DOMAIN then
              DomainChangeFound := TRUE;
              DestDomain := (pHelp + 1)^;
              exit;
            end_if;
            
            pHelp -= sizeof(USINT);
          end_for;

          // if there was no domainchange
          if DomainChangeFound = FALSE then
            // the master is the Dest
            DestDomain := (pHelpModule^.ModuleInfo.pPath + 2)^;
          end_if;

          pHelp := pTempMem;
          
          // destination domain
          pHelp^ := DestDomain;
          pHelp += sizeof(USINT);

          // is it for an interface frame?
          pHelp^$BOOL := FALSE;
          pHelp += sizeof(BOOL);

          // length of uncompressed path
          pHelp^$UINT := TmpLen;
          pHelp += sizeof(UINT);

          // uncompressed path
          _memcpy(ptr1:=pHelp, ptr2:=pHelpModule^.ModuleInfo.pPath, cntr:=TmpLen);
          pHelp += TmpLen;
          
          // SDO message
          _memcpy(ptr1:=pHelp, ptr2:=pDO, cntr:=pPara^.aPara[0]$UDINT);
          
          MyPara.aPara[1] := pTempMem$DINT;
          
          NewInst(#MyPara, #MyResult);
          
          // path can exist only once in a system
          return;
        end_if;
      
        pHelpModule := pHelpModule^.pNext;
      end_while;

      if pHelpConfig^.SafetyNbr = pDO^.SrcAdr then
        pHelpSlave := pHelpConfig^.pFirstSlaveCfg;
        while pHelpSlave do
          if (To_StdLib.MemCmp(pCompDestPath, pHelpSlave^.SlaveInfo.pCompPath, pHelpSlave^.SlaveInfo.CompPathLen) = 0) then
            // we have to add some additional information to the message
            
            (*
              Memory structure:
                1 Byte destination domain
                1 Byte IsInterfaceFrame (0/1)
                2 Byte length of uncompressed path
                x Byte uncompressed path
                x Byte SDO message
            *)
            
            TmpLen := pHelpSlave^.SlaveInfo.PathLen; // save some time with this local variable
            MyPara.uiCmd := CMD_SM_TOP_SDO_TO_MODULE;
            MyPara.aPara[0] := sizeof(USINT) + sizeof(BOOL) + sizeof(UINT) + TmpLen + pPara^.aPara[0];
            if ud_SizeTempMemCy >= MyPara.aPara[0]$UDINT then
              pTempMem := pTempMemCy$^USINT;
            else
              LogError("@0440 (SafetyManager::CheckTopSDO) Temporary memory for cyclic task is too small!");
              LogValue("AvaliableMem: {0} / NeededMem: {1}", Value:=ud_SizeTempMemCy, Value2:=MyPara.aPara[0]$UDINT);
              return;
            end_if;

            DomainChangeFound := FALSE;
            pHelp := pHelpSlave^.SlaveInfo.pPath + TmpLen - 2; // pointer on penultimate byte of path
            // search from the end for the last domain change
            for i := (TmpLen-2) to 3 by -1 do
              if pHelp^ = HWT_ESCAPE_DOMAIN then
                DomainChangeFound := TRUE;
                DestDomain := (pHelp + 1)^;
                exit;
              end_if;
              
              pHelp -= sizeof(USINT);
            end_for;

            // if there was no domainchange
            if DomainChangeFound = FALSE then
              // the master is the Dest
              DestDomain := (pHelpSlave^.SlaveInfo.pPath + 2)^;
            end_if;

            pHelp := pTempMem;
            
            // destination domain
            pHelp^ := DestDomain;
            pHelp += sizeof(USINT);

            // is it for an interface frame?
            pHelp^$BOOL := FALSE;
            pHelp += sizeof(BOOL);

            // length of uncompressed path
            pHelp^$UINT := TmpLen;
            pHelp += sizeof(UINT);

            // uncompressed path
            _memcpy(ptr1:=pHelp, ptr2:=pHelpSlave^.SlaveInfo.pPath, cntr:=TmpLen);
            pHelp += TmpLen;
            
            // SDO message
            _memcpy(ptr1:=pHelp, ptr2:=pDO, cntr:=pPara^.aPara[0]$UDINT);
            
            MyPara.aPara[1] := pTempMem$DINT;
            
            NewInst(#MyPara, #MyResult);
            
            // path can exist only once in a system
            return;
          end_if;
          
          pHelpSlave := pHelpSlave^.pNext;
        end_while;
      end_if;
    
      pHelpConfig := pHelpConfig^.pNext;
    end_while;    

    LogError("@0472 (SafetyManager::CheckTopSDO) Couldn't find destination of SDO. (Compressed Topology Path)");
    LogValue("SrcAddr: {0} / InterfaceCRC: {1}", Value:=pDO^.SrcAdr, Value2:=((pCompDestPath + 1)$^HDINT)^);

  end_if;

END_FUNCTION


FUNCTION SafetyManager::TopSDODest
  VAR_INPUT
    pPara 	: ^CmdStruct;
    pResult 	: ^results;
  END_VAR
  VAR_OUTPUT
    IsDestDomain 	: BOOL;
  END_VAR
  VAR
    pHelpUSINT : ^USINT;
    PathLength : UINT;
    pPath : ^USINT;
    pHelpSafety : ^t_SafetyComponent;
  END_VAR

  IsDestDomain := FALSE;
(*
  Memory structure:
    1 Byte destination domain
    1 Byte IsInterfaceFrame (0/1)
    2 Byte length of uncompressed path
    x Byte uncompressed path
    x Byte SDO message
*)
  
  // check size (at least 1 byte for the destination domain + 2 byte for the path length + the header of the SDO message)
  if pPara^.aPara[0] > (sizeof(USINT) + sizeof(PathLength) + sizeof(SafetyCDIAS_Base::SDOHeader.StdHeader)) then
    pHelpUSINT := (pPara^.aPara[1])$^USINT;
    
    // domain
    pHelpUSINT += sizeof(USINT);
    
    // is it an interface frame info?
    pHelpUSINT += sizeof(BOOL);

    // path length
    PathLength := (pHelpUSINT$^UINT)^;
    pHelpUSINT += sizeof(UINT);
    
    // path
    pPath := pHelpUSINT;
    pHelpUSINT += PathLength;
    
    // pHelpUSINT points now to the SDO message
    
    // now we search the logged in modules for one with a matching topology path
    if pFirstSafetyComponent then

      pHelpSafety := pFirstSafetyComponent;
      while pHelpSafety do
        // we compare the last x byte (where x is the length of the local path including the domain) => update: no more transfer between different domains, 
        //                                                                                                      therefore the length has to match before we compare the end of the path
        if pHelpSafety^.pPath & (pHelpSafety^.pPath^ = pPath^) & (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pPath + 2 + pPath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then
          pPara^.uiCmd := CMD_SM_SDO_TO_MODULE;
          pPara^.aPara[0] := pPara^.aPara[0] - (sizeof(USINT) + sizeof(PathLength) + PathLength);
          pPara^.aPara[1] := pHelpUSINT$DINT;
          
          pHelpSafety^.pThis^.NewInst(pPara, pResult);
          
          IsDestDomain := TRUE;
          return;
        end_if;
        
        pHelpSafety := pHelpSafety^.pNext;
      end_while;
    end_if;
  end_if;  

END_FUNCTION


FUNCTION SafetyManager::PDOLengthCorrection
  VAR
    pHelpConfig : ^t_CfgListElem;
    pHelpPDO    : ^t_PdoListElem;
    pHelpModule : ^t_ModuleListElem;
    pHelpSearchConfig : ^t_CfgListElem;
    FoundIt : BOOL;
    GotFirst : BOOL;
  END_VAR

  // with FSoE we use connections instead of the PDOs (PDOs are only remembered to check the optional state)
  if b_GotFSoEModules then
    return;
  end_if;


  GotFirst := FALSE;

  // check if there is a PDO with a zero length
  pHelpConfig := pConfigs;
  while pHelpConfig do
    pHelpPDO := pHelpConfig^.pFirstPdoCfg;
    while pHelpPDO do
      // remember the first PDO found as our act (to start from)
      if GotFirst = FALSE then
        GotFirst := TRUE;
        
        pActualSendCfg := pHelpConfig;
        pActualSendPDO := pHelpPDO;
      end_if;
    
      // if there is a PDO with no length
      if pHelpPDO^.PdoInfo.PdoLen = 0 then
        FoundIt := FALSE;
        pHelpSearchConfig := pConfigs;
        // search for the source module, because we use the length of it
        while pHelpSearchConfig & (FoundIt = FALSE) do
          pHelpModule := pHelpSearchConfig^.pFirstModuleCfg;
          while pHelpModule & (FoundIt = FALSE) do
            if pHelpModule^.ModuleInfo.PathLen = pHelpPDO^.PdoInfo.SourcePathLen then
              if (To_StdLib.MemCmp(pHelpModule^.ModuleInfo.pPath, pHelpPDO^.PdoInfo.pSourcePath, pHelpModule^.ModuleInfo.PathLen) = 0) then
                pHelpPDO^.PdoInfo.PdoLen := pHelpModule^.ModuleInfo.PdoOutLen;
                FoundIt := TRUE;
              end_if;
            end_if;
          
            pHelpModule := pHelpModule^.pNext;
          end_while;

          pHelpSearchConfig := pHelpSearchConfig^.pNext;
        end_while;      
      end_if;
    
      pHelpPDO := pHelpPDO^.pNext;
    end_while;
    
    pHelpConfig := pHelpConfig^.pNext;
  end_while;

END_FUNCTION


FUNCTION SafetyManager::DoublePDOEntry
  VAR_INPUT
    NewPDO 	: t_PdoCfg;
    pNewCfg 	: ^t_CfgListElem;
  END_VAR
  VAR_OUTPUT
    IsDoubleEntry 	: BOOL;
  END_VAR
  VAR
    pHelpCfg : ^t_CfgListElem;
    pHelpPdo : ^t_PdoListElem;
  END_VAR

  IsDoubleEntry := FALSE;

  // if it's our first config, there is no chance for a double PDO => check only if not first config
  if pNewCfg <> pConfigs then
    // otherwise: search all prior configs for the same PDO (source and destination path match)    
    pHelpCfg := pConfigs;
    while pHelpCfg <> pNewCfg do
      pHelpPdo := pHelpCfg^.pFirstPdoCfg;
      while pHelpPdo do
        if (pHelpPdo^.PdoInfo.SourcePathLen = NewPDO.SourcePathLen) & (pHelpPdo^.PdoInfo.DestPathLen = NewPDO.DestPathLen) & 
           (To_StdLib.MemCmp(pHelpPdo^.PdoInfo.pSourcePath, NewPDO.pSourcePath, NewPDO.SourcePathLen) = 0) & 
           (To_StdLib.MemCmp(pHelpPdo^.PdoInfo.pDestPath, NewPDO.pDestPath, NewPDO.DestPathLen) = 0) then
          // we found the same PDO in a prior config
          IsDoubleEntry := TRUE;
          return;
        end_if;
      
        pHelpPdo := pHelpPdo^.pNext;
      end_while;

      pHelpCfg := pHelpCfg^.pNext;
    end_while;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::StoreInterfaceRoutingList
	VAR_INPUT
		pPara 	: ^CmdStruct;
		pResult 	: ^results;
		pHelpManaging 	: ^t_ManagingComponent;
		IsFSoE 	: BOOL;
	END_VAR
	VAR_OUTPUT
		IFDataChanged 	: BOOL;
	END_VAR
  VAR
    EntryNo       : USINT;
    i             : UDINT;
    pHelp         : ^t_InterfaceFrameInfo;
    ActOffset     : UDINT;
    pHelpFSoE     : ^t_IFConnectionInfo;
    j             : UDINT;
    GenerateNo    : USINT;
    OldFSoENo     : DINT;
    OldInterfaceFrameNo : DINT;
    TooManyTotal  : BOOL;
    TooManyPerMng : BOOL;
    TempModuleInfo: ARRAY [0..MAX_MODULES_IN_OTHER_DOMAINS-1] OF t_ModuleInfo;
    GotTempInfo   : BOOL;
  END_VAR

  // initialize error flags
  TooManyTotal  := FALSE;
  TooManyPerMng := FALSE;
  
  // initialize return value with true. it has to be checked and set to false if it's the same
  IFDataChanged := TRUE;

  GotTempInfo := FALSE; // Default, used for comparing the data
  
  EntryNo := 0;

  if IsFSoE then  // FSoE Interface Frames

    // if we already have different data from this management component => delete the old stuff first
    if IFFSoENo then
      OldFSoENo := IFFSoENo$DINT;
      
      // remove the existing stuff of this managing component
      for i$DINT := (OldFSoENo-1) to 0 by -1 do
        // check if it's from the same managing component
        if aIFFSoEInfos[i].pManaging^.pThis$DINT = pPara^.aPara[2] then
          // remove it

          // move the following blocks over this one (if it's not the last)
          if i$UDINT < IFFSoENo-1 then
            for j := i to (IFFSoENo-2) do
              aIFFSoEInfos[j] := aIFFSoEInfos[j+1];
            end_for;
          end_if;
          IFFSoENo -= 1;
        end_if;
      end_for;        
    end_if;

    if pHelpManaging then
      // if we found the sender of this list in our list of interfaceframe components, remember the safety numbers
      if pHelpManaging^.pThis$DINT = pPara^.aPara[2] then
        
        EntryNo := ((pPara^.aPara[0] - sizeof(t_IFInfoFSoE.IsResponse)) / sizeof(t_IFConnectionInfo))$USINT;
        
        // if there has been no IF info before and we are in the middle of the initialisation => it's the first time and it didn't change (the data will be considered in the normal initialisation, no need to restart it)
        if (pHelpManaging^.InfoBits.GotIFInfo = FALSE) & (InitSSW < _Wait4InterfaceConfigs) then
          IFDataChanged := FALSE;
        
        // if the entry number is different, we definitely got new data. don't even remember the old configuration here
        elsif EntryNo <> pHelpManaging^.ModuleNo then
          
        elsif EntryNo & (EntryNo <= MAX_MODULES_IN_OTHER_DOMAINS) then
          _memcpy(ptr1:=#TempModuleInfo[0], ptr2:=#pHelpManaging^.ModuleInfo[0], cntr:=EntryNo * sizeof(t_ModuleInfo));
          GotTempInfo := TRUE;
        elsif EntryNo = 0 then
          // if it's not the first time and we reive a package with 0 entries (again, because it's the same as the old number of entries) => no change
          IFDataChanged := FALSE;
        end_if;
                
        if EntryNo then
          // set the help pointer to the first entry of the interface frame info array
          pHelpFSoE := pPara^.aPara[1]$^t_IFConnectionInfo + sizeof(BOOL);
          for i := 0 to (EntryNo-1) do              
            if i < MAX_MODULES_IN_OTHER_DOMAINS then
              pHelpManaging^.ModuleInfo[i].SafetyNbr := pHelpFSoE^.SafetyNbr;
              
              if pHelpFSoE^.Flags.IsMaster then
                pHelpManaging^.ModuleInfo[i].InfoBits.IsFSoEMaster := TRUE;
                pHelpManaging^.ModuleInfo[i].InfoBits.IsFSoESlave  := FALSE;
              else
                pHelpManaging^.ModuleInfo[i].InfoBits.IsFSoEMaster := FALSE;
                pHelpManaging^.ModuleInfo[i].InfoBits.IsFSoESlave  := TRUE;
              end_if;
            else
              TooManyPerMng := TRUE;
            end_if;
          
            if IFFSoENo < MAX_MODULES_FSOE_IF_INFOS then
              // we save the whole info for PDO/SDO routing
              aIFFSoEInfos[IFFSoENo].FromOtherSide := pHelpFSoE^;
              aIFFSoEInfos[IFFSoENo].FromOtherSide.FollowUps := 0; // only relevant for the transfer to this domain
              aIFFSoEInfos[IFFSoENo].pManaging := pHelpManaging;

              IFFSoENo += 1;
              
              if pHelpFSoE^.FollowUps then
                GenerateNo := pHelpFSoE^.FollowUps;
                for j := 1 to GenerateNo do               
                  if IFFSoENo < MAX_MODULES_FSOE_IF_INFOS then

                    // we save the whole info for PDO/SDO routing
                    aIFFSoEInfos[IFFSoENo].FromOtherSide := pHelpFSoE^;
                    aIFFSoEInfos[IFFSoENo].FromOtherSide.FollowUps := 0; // only relevant for the transfer to this domain
                    aIFFSoEInfos[IFFSoENo].pManaging := pHelpManaging;

                    IFFSoENo += 1;
                  else
                    TooManyTotal := TRUE;
                    exit;
                  end_if;
                end_for;
              end_if;
            else
              TooManyTotal := TRUE;
            end_if;
            
            pHelpFSoE += sizeof(t_IFConnectionInfo);
          end_for;

          if TooManyTotal | TooManyPerMng then
            LogError("@0473 (SafetyManager::StoreInterfaceRoutingList) Invalid size of configuration (FSoE)");
          end_if;          
        end_if;
        
        pHelpManaging^.ModuleNo := EntryNo;
        // compare the memory before and after storing the data
        if GotTempInfo & (_memcmp(ptr1:=#TempModuleInfo[0], ptr2:=#pHelpManaging^.ModuleInfo[0], cntr:=EntryNo * sizeof(t_ModuleInfo)) = 0) then
          // same data => no new data received
          IFDataChanged := FALSE;
        end_if;

        pHelpManaging^.InfoBits.GotIFInfo := TRUE;
      end_if;
    end_if;    
  else  // "Old" Interface Frames (SCP011)
    ActOffset := 0;
    
    // if we already have different data from this management component => delete the old stuff first
    if InterfaceFrameNo then
      OldInterfaceFrameNo := InterfaceFrameNo$DINT;
      
      // remove the existing stuff of this managing component
      for i$DINT := (OldInterfaceFrameNo-1) to 0 by -1 do
        // check if it's from the same managing component
        if aInterfaceFrameInfos[i].pThisManaging$DINT = pPara^.aPara[2] then
          // remove it

          // move the following blocks over this one (if it's not the last)
          if i$UDINT < InterfaceFrameNo-1 then
            for j := i to (InterfaceFrameNo-2) do
              aInterfaceFrameInfos[j] := aInterfaceFrameInfos[j+1];
            end_for;
          end_if;
          InterfaceFrameNo -= 1;
        end_if;
      end_for;        
    end_if;
    
    if pHelpManaging then
      // if we found the sender of this list in our list of interfaceframe components, remember the safety numbers
      if pHelpManaging^.pThis$DINT = pPara^.aPara[2] then
        EntryNo := ((pPara^.aPara[0] - sizeof(t_IFInfo.IsResponse)) / sizeof(t_InterfaceFrameInfo))$USINT;
        
        // if there has been no IF info before and we are in the middle of the initialisation => it's the first time and it didn't change (the data will be considered in the normal initialisation, no need to restart it)
        if (pHelpManaging^.InfoBits.GotIFInfo = FALSE) & (InitSSW < _Wait4InterfaceConfigs) then
          IFDataChanged := FALSE;
        
        // if the entry number is different, we definitely got new data. don't even remember the old configuration here
        elsif EntryNo <> pHelpManaging^.ModuleNo then
          
        elsif EntryNo & (EntryNo <= MAX_MODULES_IN_OTHER_DOMAINS) then
          _memcpy(ptr1:=#TempModuleInfo[0], ptr2:=#pHelpManaging^.ModuleInfo[0], cntr:=EntryNo * sizeof(t_ModuleInfo));
          GotTempInfo := TRUE;
        elsif EntryNo = 0 then
          // if it's not the first time and we reive a package with 0 entries (again, because it's the same as the old number of entries) => no change
          IFDataChanged := FALSE;
        end_if;
        
        if EntryNo then
          // set the help pointer to the first entry of the interface frame info array
          pHelp := pPara^.aPara[1]$^t_InterfaceFrameInfo + sizeof(BOOL);
          for i := 0 to (EntryNo-1) do
            if i < MAX_MODULES_IN_OTHER_DOMAINS then
              // we save the module info's with the domain interface module for TDO/SDO routing
              pHelpManaging^.ModuleInfo[i] := pHelp^.ModuleInfo;
            else
              TooManyPerMng := TRUE;
            end_if;
            
            if InterfaceFrameNo < MAX_MODULES_IN_OTHER_DOMAINS then
              // we also save the whole info for PDO routing
              aInterfaceFrameInfos[InterfaceFrameNo].Info := pHelp^;
              
              // only for IFs where the communication partner is the source
              if aInterfaceFrameInfos[InterfaceFrameNo].Info.IFPdoLen then
                aInterfaceFrameInfos[InterfaceFrameNo].Info.OffsetInDomainModule := ActOffset;
                ActOffset += aInterfaceFrameInfos[InterfaceFrameNo].Info.IFPdoLen;
              end_if;
              
              aInterfaceFrameInfos[InterfaceFrameNo].pThisManaging$DINT := pPara^.aPara[2];

              InterfaceFrameNo += 1;
            else
              TooManyTotal := TRUE;
            end_if;
            
            pHelp += sizeof(t_InterfaceFrameInfo);
          end_for;

          if TooManyTotal | TooManyPerMng then
            LogError("@0475 (SafetyManager::StoreInterfaceRoutingList) Invalid size of configuration");
          end_if;          
        end_if;
        
        pHelpManaging^.ModuleNo := EntryNo;
        // compare the memory before and after storing the data
        if GotTempInfo & (_memcmp(ptr1:=#TempModuleInfo[0], ptr2:=#pHelpManaging^.ModuleInfo[0], cntr:=EntryNo * sizeof(t_ModuleInfo)) = 0) then
          // same data => no new data received
          IFDataChanged := FALSE;
        end_if;
        
        pHelpManaging^.InfoBits.GotIFInfo := TRUE;
      end_if;
    end_if;
  end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
  if EntryNo then
    if IFDataChanged = FALSE then
      HWC_LogPrintf(pThis:=this, format:="@DBG (SafetyManager::StoreInterfaceRoutingList) No Data Change");
    elsif GotTempInfo = FALSE then
      HWC_LogPrintf(pThis:=this, format:="@DBG (SafetyManager::StoreInterfaceRoutingList) Data Change, no Compare");
    else
      HWC_LogPrintf(pThis:=this, format:="@DBG (SafetyManager::StoreInterfaceRoutingList) Data Change, Compare:");
      for i := 0 to (EntryNo-1) do
        HWC_LogPrintf(pThis:=this, format:="@DBG (SafetyManager::StoreInterfaceRoutingList) Old: %x; %V1; %V1; %V1; %V1 ", #TempModuleInfo[i].SafetyNbr, #TempModuleInfo[i].InfoBits, #TempModuleInfo[i].Reserved[0], #TempModuleInfo[i].Reserved[1], #TempModuleInfo[i].Reserved[2]  );
        HWC_LogPrintf(pThis:=this, format:="@DBG (SafetyManager::StoreInterfaceRoutingList) New: %x; %V1; %V1; %V1; %V1 ", #pHelpManaging^.ModuleInfo[i].SafetyNbr, #pHelpManaging^.ModuleInfo[i].InfoBits, #pHelpManaging^.ModuleInfo[i].Reserved[0], #pHelpManaging^.ModuleInfo[i].Reserved[1], #TempModuleInfo[i].Reserved[2]  );
      end_for;
    end_if; 
  end_if;
#endif 

END_FUNCTION


FUNCTION SafetyManager::AllInterfaceCfgsArrived
  VAR_OUTPUT
    AllReady 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
  END_VAR
  
  AllReady := TRUE;
  
  // check if we have the safety numbers for every interface managing module in the system
  pHelpManaging := pFirstManagingInterfaceComponent;
  while pHelpManaging do
    if pHelpManaging^.InfoBits.IsSynchron & (pHelpManaging^.InfoBits.GotIFInfo = FALSE) then
      AllReady := FALSE;
      return;
    end_if;
  
    pHelpManaging := pHelpManaging^.pNext;
  end_while;

END_FUNCTION


FUNCTION SafetyManager::SendInterfaceConfigs
  VAR_INPUT
    DestinationThis 	: pVirtualBase;(* := NIL *)
    IsResponse 	: BOOL;(* := FALSE *)
    IsFSoE 	: BOOL;
  END_VAR
  VAR
    pHelpCfg                  : ^t_CfgListElem;
    pHelpModule               : ^t_ModuleListElem;
    pHelpPdo                  : ^t_PdoListElem;
    pHelpConnection           : ^t_FSoEConnectionDetails;
    MyPara                    : CmdStruct;
    MyResult                  : results;
    TmpIFInfo                 : t_IFInfo;
    ModuleInfoNo              : USINT;
    i                         : UDINT;
    pActCRC                   : ^HDINT;
    TmpIFConn                 : t_IFInfoFSoESend;
    pLastReferenceConnection  : ^t_IFConnectionInfo;
  END_VAR

  // collect the safety numbers of this system
  ModuleInfoNo := 0;

  if IsFSoE then
    //reset memory => now 0 is an invalid safety number (= unused element of the array)
    _memset(dest:=#TmpIFConn, usByte:=0, cntr:=sizeof(t_IFInfoFSoESend));
    TmpIFConn.IsResponse := IsResponse;  
    
    pLastReferenceConnection  := NIL;
    
    // if not all configs available, create and send the available data
    if (LocalCPUCfgNo <> LocalCPUNo) then
      // check FSoE Connections in the available configs
      pHelpCfg := pConfigs;
      while pHelpCfg do
        pHelpConnection := pHelpCfg^.pFirstFSoEConnection;
        while pHelpConnection do
          // only interface connections are relevant
          if pHelpConnection^.Flags.IsInterfaceConnection then
            if pLastReferenceConnection & (pHelpConnection^.SlaveAddress = pLastReferenceConnection^.SlaveAddress) &
               (pHelpConnection^.Flags.IsMaster = pLastReferenceConnection^.Flags.IsMaster) then
              pLastReferenceConnection^.FollowUps += 1;
            else
              // add the modules to the info message
              TmpIFConn.aConnectionInfos[ModuleInfoNo].Flags            := pHelpConnection^.Flags;
              TmpIFConn.aConnectionInfos[ModuleInfoNo].SlaveAddress     := pHelpConnection^.SlaveAddress;
              TmpIFConn.aConnectionInfos[ModuleInfoNo].SafetyNbr        := pHelpConnection^.OwnerSafetyNbr;
              TmpIFConn.aConnectionInfos[ModuleInfoNo].MasterReadSize   := pHelpConnection^.MasterReadSize;
              TmpIFConn.aConnectionInfos[ModuleInfoNo].MasterWriteSize  := pHelpConnection^.MasterWriteSize;
              TmpIFConn.aConnectionInfos[ModuleInfoNo].FollowUps        := 0;
                     
              // some data are only available at the master
              if pHelpConnection^.Flags.IsMaster then
                // no follow ups for master connections
                pLastReferenceConnection                                := NIL;
                
                TmpIFConn.aConnectionInfos[ModuleInfoNo].ConnectionID   := pHelpConnection^.ConnectionID;
              else
                // update this value for the next connections with the same settings to save a lot of memory
                pLastReferenceConnection                                := #TmpIFConn.aConnectionInfos[ModuleInfoNo];

                TmpIFConn.aConnectionInfos[ModuleInfoNo].ConnectionID   := 0;
              end_if;
                     
              if pHelpConnection^.SlavePathCompLen & pHelpConnection^.pSlavePathComp then
                TmpIFConn.aConnectionInfos[ModuleInfoNo].InterfaceCRC := (pHelpConnection^.pSlavePathComp$^HDINT + 1)^;
              else
                TmpIFConn.aConnectionInfos[ModuleInfoNo].InterfaceCRC := 0; 
              end_if;        
              
              ModuleInfoNo += 1;
            end_if;
          end_if;
          
          pHelpConnection := pHelpConnection^.pNext;
        end_while;
        
        pHelpCfg := pHelpCfg^.pNext;
      end_while;
    else
      // check FSoE Connections in the sorted list
      pHelpConnection := pSortedConnections;
      while pHelpConnection do
        // only interface connections are relevant
        if pHelpConnection^.Flags.IsInterfaceConnection then
          if pLastReferenceConnection & (pHelpConnection^.SlaveAddress = pLastReferenceConnection^.SlaveAddress) &
             (pHelpConnection^.Flags.IsMaster = pLastReferenceConnection^.Flags.IsMaster) then
            pLastReferenceConnection^.FollowUps += 1;
          else
            // add the modules to the info message
            TmpIFConn.aConnectionInfos[ModuleInfoNo].Flags            := pHelpConnection^.Flags;
            TmpIFConn.aConnectionInfos[ModuleInfoNo].SlaveAddress     := pHelpConnection^.SlaveAddress;
            TmpIFConn.aConnectionInfos[ModuleInfoNo].SafetyNbr        := pHelpConnection^.OwnerSafetyNbr;
            TmpIFConn.aConnectionInfos[ModuleInfoNo].MasterReadSize   := pHelpConnection^.MasterReadSize;
            TmpIFConn.aConnectionInfos[ModuleInfoNo].MasterWriteSize  := pHelpConnection^.MasterWriteSize;
            TmpIFConn.aConnectionInfos[ModuleInfoNo].FollowUps        := 0;
                   
            // some data are only available at the master
            if pHelpConnection^.Flags.IsMaster then
              // no follow ups for master connections
              pLastReferenceConnection                                := NIL;
              
              TmpIFConn.aConnectionInfos[ModuleInfoNo].ConnectionID   := pHelpConnection^.ConnectionID;
            else
              // update this value for the next connections with the same settings to save a lot of memory
              pLastReferenceConnection                                := #TmpIFConn.aConnectionInfos[ModuleInfoNo];

              TmpIFConn.aConnectionInfos[ModuleInfoNo].ConnectionID   := 0;
            end_if;
                   
            if pHelpConnection^.SlavePathCompLen & pHelpConnection^.pSlavePathComp then
              TmpIFConn.aConnectionInfos[ModuleInfoNo].InterfaceCRC := (pHelpConnection^.pSlavePathComp$^HDINT + 1)^;
            else
              TmpIFConn.aConnectionInfos[ModuleInfoNo].InterfaceCRC := 0; 
            end_if;        
            
            ModuleInfoNo += 1;
          end_if;
        end_if;
        
        pHelpConnection := pHelpConnection^.pNextSorted;
      end_while;   
    end_if; 

    CheckLocalSlaveAddresses();

    if b_AllLocalSlaveAddressesValid then
      MyPara.uiCmd := CMD_SM_IF_MODULE_INFO_FSOE_OK;
    else
      MyPara.uiCmd := CMD_SM_IF_MODULE_INFO_FSOE;
    end_if;

    MyPara.aPara[0] := ModuleInfoNo * sizeof(t_IFConnectionInfo) + sizeof(t_IFInfoFSoE.IsResponse);
    MyPara.aPara[1] := (#TmpIFConn)$DINT;
  else
    // browse through the configs
    TmpIFInfo.IsResponse := IsResponse;
    
    pHelpCfg := pConfigs;
    while pHelpCfg do
      pHelpPdo := pHelpCfg^.pFirstPdoCfg;
      while pHelpPdo do
        // if it's a interface broadcast
        if pHelpPdo^.PdoInfo.Info.IFBroadCast then
          // search for the source module
          pHelpModule := pHelpCfg^.pFirstModuleCfg;
          while pHelpModule do
            // if it's an interface source and the interface CRC matches the one of our PDO => Bingo! But to be sure we check also the path length and the path itself
            if pHelpModule^.ModuleInfo.Info.IsInterfaceSource & (pHelpModule^.ModuleInfo.InterfaceCRC = pHelpPdo^.PdoInfo.InterfaceCRC) & 
              (pHelpModule^.ModuleInfo.PathLen = pHelpPdo^.PdoInfo.SourcePathLen) & 
              (To_StdLib.MemCmp(pHelpModule^.ModuleInfo.pPath, pHelpPdo^.PdoInfo.pSourcePath, pHelpPdo^.PdoInfo.SourcePathLen) = 0) then

              TmpIFInfo.aModuleInfos[ModuleInfoNo].InterfaceCRC          := pHelpModule^.ModuleInfo.InterfaceCRC;
              TmpIFInfo.aModuleInfos[ModuleInfoNo].IFPdoLen              := pHelpPdo^.PdoInfo.PdoLen;
              TmpIFInfo.aModuleInfos[ModuleInfoNo].OffsetInDomainModule  := 0; // the correct offset is calculated in the receiver
              TmpIFInfo.aModuleInfos[ModuleInfoNo].ModuleInfo.SafetyNbr  := pHelpModule^.ModuleInfo.SafetyNbr;
              TmpIFInfo.aModuleInfos[ModuleInfoNo].ModuleInfo.InfoBits$UDINT := 0;  // Reset Infobits and reserved bytes
              
              ModuleInfoNo += 1;
            end_if;

            pHelpModule := pHelpModule^.pNext;
          end_while;        
        end_if;

        pHelpPdo := pHelpPdo^.pNext;
      end_while;

      // check the modules of the actual config if there are interface frame destinations (needed for unidirectional communication, otherwise the SDO routing would fail)
      pHelpModule := pHelpCfg^.pFirstModuleCfg;
      while pHelpModule do
        // if it's a interface destination
        if pHelpModule^.ModuleInfo.Info.IsInterfaceDest then
          pActCRC := pHelpModule^.ModuleInfo.pDestCRCs;
          for i := 0 to (pHelpModule^.ModuleInfo.DestCRCNo-1) do
            TmpIFInfo.aModuleInfos[ModuleInfoNo].InterfaceCRC          := pActCRC^;
            TmpIFInfo.aModuleInfos[ModuleInfoNo].IFPdoLen              := 0; // we don't know the length here, we're only the receiver
            TmpIFInfo.aModuleInfos[ModuleInfoNo].OffsetInDomainModule  := 0; // the correct offset is calculated in the receiver
            TmpIFInfo.aModuleInfos[ModuleInfoNo].ModuleInfo.SafetyNbr  := pHelpModule^.ModuleInfo.SafetyNbr;
            TmpIFInfo.aModuleInfos[ModuleInfoNo].ModuleInfo.InfoBits$UDINT := 0;  // Reset Infobits and reserved bytes

            pActCRC += sizeof(HDINT);
            ModuleInfoNo += 1;
          end_for;
        end_if;         

        pHelpModule := pHelpModule^.pNext;
      end_while;     

      pHelpCfg := pHelpCfg^.pNext;
    end_while;
    
    MyPara.uiCmd := CMD_SM_IF_MODULE_INFO;
    MyPara.aPara[0] := ModuleInfoNo * sizeof(t_InterfaceFrameInfo) + sizeof(TmpIFInfo.IsResponse);
    MyPara.aPara[1] := (#TmpIFInfo)$DINT;
  end_if;
  
  
  // if we have only 1 destination => sent it to this destination
  if DestinationThis then
    DestinationThis^.NewInst(#MyPara, #MyResult);
  else
    // otherwise send our safety numbers to every interface managing module in the system
    TellAllInterfaceManagingModules(#MyPara, #MyResult);
  end_if;

END_FUNCTION


FUNCTION SafetyManager::GetIFInfoByCRC
  VAR_INPUT
    InterfaceCRC 	: HDINT;
    pMemOffset 	: ^UDINT;
    pLength 	: ^USINT;
  END_VAR
  VAR_OUTPUT
    SafetyNbr 	: HDINT;
  END_VAR
  VAR
    i : UDINT;
  END_VAR

  SafetyNbr := 0;

  if InterfaceFrameNo then
    for i := 0 to (InterfaceFrameNo-1) do
      // if we found the interface CRC and it's a interface frame we receive (the sender adds the length)
      if (aInterfaceFrameInfos[i].Info.InterfaceCRC = InterfaceCRC) & aInterfaceFrameInfos[i].Info.IFPdoLen then
        SafetyNbr := aInterfaceFrameInfos[i].Info.ModuleInfo.SafetyNbr;
        // check if pointers are valid
        if pMemOffset & pLength then
          pMemOffset^ := aInterfaceFrameInfos[i].Info.OffsetInDomainModule;
          pLength^    := aInterfaceFrameInfos[i].Info.IFPdoLen;
        end_if;
        return;
      end_if;
    end_for;
    
    if SafetyNbr = 0 then
      LogError("@0476 (SafetyManager::GetIFInfoByCRC) Safetynumber of Interface-Frame source couldn't be found in the list");
    end_if;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::SetTimeout

  ud_InitTimeOut := ops.tAbsolute;

END_FUNCTION


FUNCTION SafetyManager::CheckTimeout

  // if the step took too long
  if (OPS.tAbsolute - ud_InitTimeOut) > T_INIT_TIMEOUT then     
    // show the possible error in the log/trace and restart the time for timeout
    case InitSSW of
//**********************************************************************************************************************************************************
      _FirstWait4SynchronSubdomains:
        LogError("@0477 (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting first time for subdomains to get synchron!");
//**********************************************************************************************************************************************************
      _Wait4FSoEConnectionsLocal:
        LogError("@0478 (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for the local FSoE connections to get initialized!");
//**********************************************************************************************************************************************************
      _Wait4SynchronSubdomains:
        LogError("@0479 (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for subdomains to get synchron!");
//**********************************************************************************************************************************************************
//      _Wait4ConfigFlush:
//        LogError("@ZZZZ Timeout in Initialisation while waiting for the slave domains to send their safety numbers of safety cpus and if they can handle the new time sync method!");        
//**********************************************************************************************************************************************************
      _Wait4CPUConfigs:
        LogError("@047A (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for all available cpu configurations!");        
//**********************************************************************************************************************************************************
      _Wait4InterfaceConfigs:
        LogError("@047B (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for module info needed for interface frame communication!");
//**********************************************************************************************************************************************************
      _Wait4FSoEConnectionsExternal:
        LogError("@047C (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for the external FSoE connections to get initialized!");
//**********************************************************************************************************************************************************
      _Wait4UnsafeConfigAck:
        LogError("@047D (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for an unsafe variable configuration acknowledgment!");
//**********************************************************************************************************************************************************
      _Wait4PDOAcks:
        LogError("@047F (SafetyManager::CheckTimeout) Timeout in Initialisation while waiting for a pdo configuration acknowledgment!");
//**********************************************************************************************************************************************************
    end_case;
    
    // this error gets quitted if initialisation finishes
    ClassState := -11;
    
    ud_InitTimeOut := ops.tAbsolute;
  end_if; 

END_FUNCTION


FUNCTION SafetyManager::AllNonOptionalIFDomainsSynchron
  VAR_OUTPUT
    EverythingSynchron 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
    pHelpConfig : ^t_CfgListElem;
    pHelpPdo : ^t_PdoListElem;
    TempCfg : t_PdoCfg;
    MyPara 	  : CmdStruct;
    MyResult	: results;
  END_VAR
  
  EverythingSynchron := TRUE;

  MyPara.uiCmd := CMD_SM_SYNC_CHECK;

  // check for all interface frame PDOs in the configurations if the corresponding domain managing module is synchron
  pHelpConfig := pConfigs;
  while pHelpConfig do
    pHelpPdo := pHelpConfig^.pFirstPdoCfg;
    while pHelpPdo do
      TempCfg := pHelpPdo^.PdoInfo;
      // if it's an interface  PDO, no double entry and not optional => it has to be synchron!
      if (TempCfg.Info.IsInterfacePDO = TRUE) & (TempCfg.Info.DoublePDOEntry = FALSE) & (TempCfg.Info.IsOptional = FALSE) then
        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          // we found the corresponding domain managing module
          if (pHelpManaging^.InfoBits.IsOptional = FALSE) &
            ((To_StdLib.MemCmp(pHelpManaging^.pModulePath, TempCfg.pSourcePath, TempCfg.SourcePathLen) = 0) |
             (To_StdLib.MemCmp(pHelpManaging^.pModulePath, TempCfg.pDestPath, TempCfg.DestPathLen) = 0)) then

            // if any of those is not synchron, stop searching
            if pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult) = ERROR then
              EverythingSynchron := FALSE;
              return;
            end_if;

            exit;
          end_if;
          
          pHelpManaging := pHelpManaging^.pNext;
        end_while;  
      end_if;
      
      pHelpPdo := pHelpPdo^.pNext;
    end_while;

    pHelpConfig := pHelpConfig^.pNext;
  end_while;

END_FUNCTION


FUNCTION VIRTUAL SafetyManager::GetThisViaServerName
  VAR_INPUT
    pObjectDotServer 	: ^CHAR;
  END_VAR
  VAR_OUTPUT
    ThisPtr 	: pVoid;
  END_VAR
  VAR
    ChannelMode : CHMODE;
  END_VAR
  
  ThisPtr := NIL;

  if pObjectDotServer = NIL then
    return;
  end_if;
  
  // get thispointer to server via name
  ChannelMode := _LookUpEmbed_C(pObjectDotServer, #ThisPtr, NIL);

  // check if it's a server
  case ChannelMode of
    _CH_CMD,
    _CH_SVR:
    
  else
    ThisPtr := NIL;
    LogError("@0480 (SafetyManager::GetThisViaServerName) Couldn't get thispointer of server:");
    LogError(e_msg:=pObjectDotServer);
  end_case;

END_FUNCTION


FUNCTION SafetyManager::GetNewMovDO
  VAR_OUTPUT
    Index 	: INT;
  END_VAR
//  VAR
//  	sd_retval : DINT;
//  END_VAR
  
  Index := -1;

  if CopyCmdsUsed < MAX_PDO_COPY_CMDS then
    // check if there is one available but not used
    if CopyCmdsUsed < CopyCmdsAvailable then
      Index := CopyCmdsUsed;
      CopyCmdsUsed += 1;      
    else
      // we have to create a new one

(*
// connection to varan manager required
      // add data object to copy our data
      sd_retval := pVaranManager^.AddMovDO(ud_dol_type:=VARAN_DOL_ISO
                                         , p_ud_handle:=(#aCopyCmds[CopyCmdsAvailable].pHandle)$^UDINT
                                         , p_ud_MovSettings:=( #aCopyCmds[CopyCmdsAvailable].pSettings)$^UDINT
                                         , ud_type := VM_PRIORITY_2
                                         );

      //look for an error
      if ( sd_retval <> 0 ) then
        ClassState := -9;
        LogError("@ZZZZ (SafetyManager::GetNewMovDO) Failed to add a new copy command to the VaranManager!");
        return;
      end_if;
*)
      aCopyCmds[CopyCmdsAvailable].b_Ready4Action := FALSE;
      
      Index := CopyCmdsAvailable;
      
      CopyCmdsAvailable += 1;
      CopyCmdsUsed += 1;
    end_if;
  else
    LogError("@0481 (SafetyManager::GetNewMovDO) Maximum of possible PDO copy commands reached!");
  end_if;

END_FUNCTION


FUNCTION SafetyManager::CheckCPUConfigs
	VAR_INPUT
		OnlyOptionalCheck 	: BOOL;(* := FALSE *)
	END_VAR
	VAR_OUTPUT
		ConfigsOK 	: BOOL;
	END_VAR
  VAR
    pHelpManaging   : ^t_ManagingComponent;
    pSourceManaging : ^t_ManagingComponent;
    pHelpConfig     : ^t_CfgListElem;
    pHelpPdo        : ^t_PdoListElem;
    pLastPdo        : ^t_PdoListElem;
    IsSource        : DINT;
    PDOCount : UDINT;
  END_VAR
  
  // before we go into the usual check routine, we have to ensure that we have enough virtual PDOs for writing interface frames (config holds a broadcast and we have to look for possible target modules)
  CreateVirtualPDOs();

  // initialize with true (normal case is that everything is ok => now we check for errors)
  ConfigsOK := TRUE;
  
  PDOCount := 0;
  
  // check if all non-optional modules are available and all corresponding domain managing module are synchron (if necessary)
  pHelpConfig := pConfigs;
  while pHelpConfig do
    pLastPdo := NIL;
    pHelpPdo := pHelpConfig^.pFirstPdoCfg;
    while pHelpPdo do
      // if it's an interface frame and no broadcast frame
      if pHelpPdo^.PdoInfo.Info.IsInterfacePDO then
        if (pHelpPdo^.PdoInfo.Info.IFBroadCast = FALSE) then

          // check if we have already added the domain interface to the IF List
          IsSource := -1; // -1 = not found, 0 = destination, 1 = source
          pSourceManaging := NIL;
          pHelpManaging := pFirstManagingInterfaceComponent;
          while pHelpManaging do
            // if source or destination of the PDO matches our managing module path => it's an interface frame to/from this domain
            if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pHelpPdo^.PdoInfo.pSourcePath, pHelpPdo^.PdoInfo.SourcePathLen) = 0) then
              pSourceManaging := pHelpManaging;
              IsSource := 1;
              exit;
            elsif (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pHelpPdo^.PdoInfo.pDestPath, pHelpPdo^.PdoInfo.DestPathLen) = 0) then
              IsSource := 0;
              exit;
            end_if;
            
            pHelpManaging := pHelpManaging^.pNext;
          end_while;

          // if we got a source managing module and it isn't synchron
          if pSourceManaging then
            // if this pdo is not optional => the source module also isn't
            if pHelpPdo^.PdoInfo.Info.IsOptional = FALSE then
              pSourceManaging^.InfoBits.IsOptional := FALSE;
            end_if;
            
            if (pSourceManaging^.InfoBits.IsSynchron = FALSE) then
              if pHelpPdo^.PdoInfo.Info.IsOptional then
                // ignore it
  //              LogError("Info: Optional interface frame can't be received, because source domain is not connected or not synchron");
                
                // remove the PDO from the list (if not the receiver module would wait endless for the PDO and we can't send the set bus ready to quit errors of local PDOs)
                if pLastPdo then
                  // if we had already a PDO before in this configuration we have to skip the removed PDO with the next pointer
                  pLastPdo^.pNext := pHelpPdo^.pNext;
                  
                  To_StdLib.Free(pHelpPdo^.PdoInfo.pSourcePath);
                  To_StdLib.Free(pHelpPdo^.PdoInfo.pDestPath);
                  To_StdLib.Free(pHelpPdo);
                  
                  pHelpPdo := pLastPdo;
                else
                  // if it was the first PDO, we have to correct the first pointer
                  pHelpConfig^.pFirstPdoCfg := pHelpPdo^.pNext;

                  To_StdLib.Free(pHelpPdo^.PdoInfo.pSourcePath);
                  To_StdLib.Free(pHelpPdo^.PdoInfo.pDestPath);
                  To_StdLib.Free(pHelpPdo);

                  pHelpPdo := pHelpConfig^.pFirstPdoCfg;
                end_if;
                
                if pHelpPdo = NIL then
                  exit;
                end_if;
              elsif OnlyOptionalCheck = FALSE then
                // non-optional interface frame domain missing => error
                LogError("@0482 (SafetyManager::CheckCPUConfigs) ERROR! Non-Optional interface frame can't be received, because domain is not connected");
                ConfigsOK := FALSE;
                ClassState := -10;
              end_if;
            end_if;
          end_if;
        end_if;
      end_if;
    
      PDOCount += 1;
      
      pLastPdo := pHelpPdo;
      pHelpPdo := pHelpPdo^.pNext;
    end_while;
  
    pHelpConfig := pHelpConfig^.pNext;
  end_while;
  
  // if now PDOs found => reset the act ID to show that there are none
  if PDOCount = 0 then
    ActPDO_ID := 0;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::RemoveLocalCPUCfg
  VAR_INPUT
    SafetyNumber 	: HDINT;
  END_VAR
  VAR
    pHelpCfg    : ^t_LocalCfgListElem;
    i : UDINT;
  END_VAR

  if LocalCPUCfgNo then
    pHelpCfg := pFirstLocalCPUCfg;
    for i := 0 to (LocalCPUCfgNo-1) do
      if pHelpCfg^.SafetyNbr = SafetyNumber then
        // found it => deactivate it
        pHelpCfg^.ConfigSize := 0;
        pHelpCfg^.ConfigSent2Master := FALSE;
        exit;
      end_if;
    
      pHelpCfg := pHelpCfg^.pNext;
    end_for;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::ClearRoutingInformations
  VAR_OUTPUT
    finished 	: BOOL;
  END_VAR
  VAR
    MyPara        : CmdStruct;
    MyResult      : results;
    pHelpManaging : ^t_ManagingComponent;
    i             : UDINT;
    pHelpCfg      : ^t_LocalCfgListElem;
    pPDOElement   : ^t_PDOElement;
    pInputPDO     : ^t_InputPDOs;
    retcode       : DINT;
    pActConnection: ^t_UdpConnection;
    pUdpNo        : ^UDINT;
    paUdpCons     : ^t_aUdpConnections;
  END_VAR

  finished := FALSE;
  
  if sUdpVars.ProducerConnectionNo then
    pUdpNo    := #sUdpVars.ProducerConnectionNo;
    paUdpCons := sUdpVars.paConnectionsToProducers;
  elsif sUdpVars.ConsumerConnectionNo then
    pUdpNo    := #sUdpVars.ConsumerConnectionNo;
    paUdpCons := sUdpVars.paConnectionsToConsumers;
  else
    // no UDP connections => proceed to last step
    sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_FinishEverythingElse;
  end_if;

  case sUdpVars.ClearRoutingInfoSSW of
//**********************************************************************************************************************************************************
    t_ClearRoutingInfoSSW::_PrepareUdpDisconnect:
      // reset all send markers, because we will need them in the next step
      pActConnection := #paUdpCons^[0];
      for i := 0 to (pUdpNo^-1) do
        pActConnection^.WaitForResponse      := FALSE;
        pActConnection^.bSequenceCntReceived := FALSE; //reset flag to allow sequence counter for new udp communication
        pActConnection^.ActReceiveData.usHeaderVersion := 0;
        pActConnection^.ActSendData.usHeaderVersion    := 0;
        pActConnection^.RequestRetries                 := 0;

        pActConnection += sizeof(t_UdpConnection);
      end_for;

      sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_SendDisconnect;

//**********************************************************************************************************************************************************
    t_ClearRoutingInfoSSW::_SendDisconnect:
     
      // if it's the source of the reinit => don't send him the info, just remove from the list
      if (paUdpCons^[pUdpNo^-1].IP = 0) | (paUdpCons^[pUdpNo^-1].IP = sUdpVars.ReInitSenderIP) then
        pUdpNo^ -= 1;
      else
        // disconnect all UDP connections (1 by 1 or we might get a problem with the net load)
        sUdpVars.pTempSendBufferCy^.Header.Command       := t_UdpCommand::UdpCmd_ReInitialize;
        sUdpVars.pTempSendBufferCy^.Header.State         := t_UdpPacketHeaderV2.State.IsFromConsumer OR t_UdpPacketHeaderV2.State.IsFromProducer; // invalid state for normal data

        paUdpCons^[pUdpNo^-1].InfoBits.Disconnecting  := TRUE;
        paUdpCons^[pUdpNo^-1].WaitForResponse         := TRUE; 
        paUdpCons^[pUdpNo^-1].TLastRequest            := ops.tAbsolute;

        // only send command and state
        retcode := SendUDPData(sUdpVars.pTempSendBufferCy, sizeof(t_UdpPacketHeaderV2.Command) + sizeof(t_UdpPacketHeaderV2.State), paUdpCons^[pUdpNo^-1].IP); 

        if retcode < 0 then
          sUdpVars.LastSendError := retcode;
          sUdpVars.SendErrorCnt += 1;
        end_if;
        
        sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_Wait4DisconnectConfirm;
      end_if;
   
//**********************************************************************************************************************************************************
    t_ClearRoutingInfoSSW::_Wait4DisconnectConfirm:

      // check for the confirmation     
      if paUdpCons^[pUdpNo^-1].InfoBits.Disconnecting = FALSE then

        // reset the IP address of all others with the same IP to not resend if there are multiple connections to this IP
        if sUdpVars.ConsumerConnectionNo then
          pActConnection := #sUdpVars.paConnectionsToConsumers^[0];
          for i := 0 to (sUdpVars.ConsumerConnectionNo-1) do
            // check if it's the same IP
            if (pActConnection^.IP = paUdpCons^[pUdpNo^-1].IP) then
              // reset the IP
              pActConnection^.IP := 0;
            end_if;

            pActConnection += sizeof(t_UdpConnection);
          end_for;
        end_if;
        
        if sUdpVars.ProducerConnectionNo then
          pActConnection := #sUdpVars.paConnectionsToProducers^[0];
          for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
            // check if it's the same IP
            if (pActConnection^.IP = paUdpCons^[pUdpNo^-1].IP) then
              // reset the IP
              pActConnection^.IP := 0;
            end_if;

            pActConnection += sizeof(t_UdpConnection);
          end_for;
        end_if;
      
        // this one is out fine => proceed with the next
        pUdpNo^ -= 1;
        sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_SendDisconnect;
                    
      // look if we actual wait for a response to send retries and check if we didn't receive data in time
      elsif (paUdpCons^[pUdpNo^-1].WaitForResponse = TRUE) & ((ops.tAbsolute - paUdpCons^[pUdpNo^-1].TLastRequest) > (sUdpVars.CalculatedRequestTimeout * 2)) then                              
                              
        if paUdpCons^[pUdpNo^-1].RequestRetries >= (UDP_MAX_RETRIES * 2) then
          
          // we had enough retries, just forget about this one and proceed with the next
          pUdpNo^ -= 1;
          sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_SendDisconnect;
        else

          //send retry to actual cpu
          paUdpCons^[pUdpNo^-1].RequestRetries += 1;
          sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_SendDisconnect;
        end_if;
        
      end_if;

      // producer finished but still some consumer connections to go
      if (pUdpNo^ = 0) then
        if sUdpVars.ConsumerConnectionNo then
          // go back to the initialisation and do it for the consumer connections now
          sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_PrepareUdpDisconnect;
        else
          // all connections disconnected => finish the rest
          sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_FinishEverythingElse;
        end_if;
      end_if;

//**********************************************************************************************************************************************************
    t_ClearRoutingInfoSSW::_FinishEverythingElse:

      // we're flushin our stuff here

      // forget who sent us the reinitialize
      sUdpVars.ReInitSenderIP := 0;

      // set ID to zero for the next mapping assignment
      ActPDO_ID := 0;

      // keep the interface info from other systems to not rely on them sending the info again (updated automatically per managing component if necessary)
//      IFFSoENo := 0;
//      InterfaceFrameNo := 0;
      
      // reset local interface frameno
      LocalIFNo := 0;

      // clear configurations of safety-cpus
      FreeCfgLists();

      // clear number of copy DOs used
      CopyCmdsUsed := 0; 
      
      // reset flexible constellation informations
      _memset(dest:=#sFlexCon, usByte:=0, cntr:=sizeof(t_FlexCon));
            
      // reset memory usage for isochronous transfer (the values are set when distributing the PDOs)
      MyPara.aPara[0] := 0;
      pHelpManaging := pFirstManagingInterfaceComponent;
      while pHelpManaging do
        pHelpManaging^.PDOReadMemUsed := 0;
        pHelpManaging^.PDOWriteMemUsed := 0;
        
        MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
        pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult);
        
        MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
        pHelpManaging^.pThis^.NewInst(#MyPara, #MyResult);

        pHelpManaging^.InfoBits.HasBeenFlushed      := FALSE;
        pHelpManaging^.InfoBits.NewTimeSyncPossible := FALSE;
        pHelpManaging^.InfoBits.SubDomainsSynchron  := FALSE;
//        pHelpManaging^.InfoBits.GotIFInfo           := FALSE; // if we already got IF info from this managing module, it stays valid until we receive something else
        pHelpManaging^.InfoBits.LocalConfigReady    := FALSE;
        pHelpManaging^.InfoBits.IsOptional          := TRUE;  // initialize with true, later in the init process (CheckCPUConfigs) it's checked and corrected if necessary
        // synchron bit is already true at this time (or the timeout has expired)

        pHelpManaging^.DomainNr := 16#FF;
        
// if we already got IF info from this managing module, it stays valid until we receive something else
//        pHelpManaging^.ModuleNo := 0;
//        _memset(dest:=#pHelpManaging^.ModuleInfo, usByte:=16#00, cntr:=sizeof(pHelpManaging^.ModuleInfo));

        // delete flexcon information
        _memset(dest:=#pHelpManaging^.FlexCon, usByte:=0, cntr:=sizeof(t_ManagingFlexCon));

        pHelpManaging := pHelpManaging^.pNext;
      end_while;
      
      // all local cpu configs have to be sent again on reconnect
      if LocalCPUCfgNo then
        pHelpCfg := pFirstLocalCPUCfg;
        for i := 0 to (LocalCPUCfgNo-1) do
          pHelpCfg^.ConfigSent2Master := FALSE;
        
          pHelpCfg := pHelpCfg^.pNext;
        end_for;
      end_if;

      if PDOElementNo then
      
        // at first we have to check if there are any safety modules out there with a pointer to the PDO List (if they use a VMC/VBC as source)
        pPDOElement := #aPDOElements[0];
        for i := 0 to (PDOElementNo-1) do
          if (pPDOElement^.ThisDomainsRole = SourceDest_IsDest) & (pPDOElement^.SourceInfo.pPDOMem) & (pPDOElement^.pSafety) then
            // unregister this PDO
            MyPara.uiCmd := CMD_SM_REMOVE_INPUT_PDO;
            MyPara.aPara[0] := pPDOElement^.SourceInfo.pPDOMem$DINT;
            MyPara.aPara[1] := pPDOElement^.SourceInfo.pPDOMemDPRAM$DINT;
            pPDOElement^.pSafety^.pThis^.NewInst(#MyPara, #MyResult);
          end_if;

          // free the memory of the path infos and reset the rest of pdo element info
          if pPDOElement^.SourceInfo.pPath then
            To_StdLib.Free(pPDOElement^.SourceInfo.pPath);
          end_if;
          if pPDOElement^.DestInfo.pPath then
            To_StdLib.Free(pPDOElement^.DestInfo.pPath);
          end_if;
          
          pPDOElement += sizeof(t_PDOElement);
        end_for;

        _memset(dest:=#aPDOElements, usByte:=0, cntr:=PDOElementNo * sizeof(t_PDOElement));
        PDOElementNo := 0;
      end_if;
      
      if FSoECopyCmdNo then
        _memset(dest:=#aFSoECopyCmds, usByte:=0, cntr:=FSoECopyCmdNo * sizeof(t_FSoECopyCmd));
        FSoECopyCmdNo := 0;
      end_if;
      
      if FSoEInputPDONo then
        pInputPDO := #aFSoEInputPDOs[0];
        for i := 0 to (FSoEInputPDONo-1) do
          // unregister this PDO
          MyPara.uiCmd := CMD_SM_REMOVE_INPUT_PDO;
          MyPara.aPara[0] := pInputPDO^.pMem;
          MyPara.aPara[1] := 0;
          pInputPDO^.pSafety^.pThis^.NewInst(#MyPara, #MyResult);

          pInputPDO += sizeof(t_InputPDOs);
        end_for;
        
        FSoEInputPDONo := 0;
      end_if;

      // reset other udp settings that are not directly connected to the udp connections
      sUdpVars.UdpInit.ProvidesIF            := 0; // no need to install callback twice and FSoE is still available if it was before
      sUdpVars.LowestTransmissionTime_ms  := UDP_DEFAULT_TRANSMISSION_TIME;
      sUdpVars.ConnectionsInitialized     := FALSE;
      
      // set scheduler to default until a connection is established
      SetUdpSchedulerGrid();

      if sUdpVars.SendCopyCmdNo then
        _memset(dest:=#sUdpVars.aSendCopyCmds, usByte:=0, cntr:=sUdpVars.SendCopyCmdNo * sizeof(t_FSoECopyCmd));
        sUdpVars.SendCopyCmdNo := 0;
      end_if;
     
      if sUdpVars.ReceiveCopyCmdNo then
        _memset(dest:=#sUdpVars.aReceiveCopyCmds, usByte:=0, cntr:=sUdpVars.ReceiveCopyCmdNo * sizeof(t_FSoECopyCmd));
        sUdpVars.ReceiveCopyCmdNo := 0;
      end_if;
      
      // reset step
      sUdpVars.ClearRoutingInfoSSW := t_ClearRoutingInfoSSW::_PrepareUdpDisconnect;

      finished := TRUE;
    
  end_case;

END_FUNCTION


FUNCTION SafetyManager::CheckPDORecycle
  VAR_INPUT
    SourceDest 	: t_SourceDest;
    pNewPDO 	: ^t_PDOElement;
  END_VAR
  VAR_OUTPUT
    pRecyclePDO 	: ^t_PDOElement;
  END_VAR
  VAR
    i : UDINT;
    pHelpInfo : ^t_PDOModuleInfo;
    pSearchInfo : ^t_PDOModuleInfo;
    ppHelpManaging : ^t_pManagingComponent;
    pSearchManaging : ^t_ManagingComponent;
  END_VAR

  // initialize with invalid
  pRecyclePDO := NIL;

  // if we have no pdoelements yet, quit
  if PDOElementNo = 0 then
    return;
  end_if;

  // we search for a PDO with the same source (then it contains the same data)
  pHelpInfo := #aPDOElements[0].SourceInfo;
  pSearchInfo := #pNewPDO^.SourceInfo;
  
  case SourceDest of
    SourceDest_IsSource:
      ppHelpManaging := #aPDOElements[0].pManagingSource;
      pSearchManaging := pNewPDO^.pManagingSource;
    SourceDest_IsDest:
      ppHelpManaging := #aPDOElements[0].pManagingDest;
      pSearchManaging := pNewPDO^.pManagingDest;
  else
    return;
  end_case;

  // check if we already have the path info of this source/destination
  for i := 0 to (PDOElementNo-1) do
    // if managing node, pathlength and path matches
    if (ppHelpManaging^ = pSearchManaging) & (pHelpInfo^.PathLen = pSearchInfo^.PathLen) & (To_StdLib.MemCmp(pHelpInfo^.pPath, pSearchInfo^.pPath, pHelpInfo^.PathLen) = 0) then
      // we found our PDO to recycle (PDO is already tranferred over this managing node
      pRecyclePDO := #aPDOElements[i];
      return;
    end_if;
    
    pHelpInfo       += sizeof(t_PDOElement);
    ppHelpManaging  += sizeof(t_PDOElement);
  end_for;

END_FUNCTION


FUNCTION SafetyManager::AddManagingInfo
  VAR_INPUT
    pPara 	: ^CmdStruct;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
  END_VAR

(*
  Parameter description:
  0..pointer to write PDOs in VARAN manager DPRAM
  1..pointer to read PDOs in VARAN manager DPRAM
  2..thispointer of calling class
  3..pointer to write PDOs in internal memory
  4..pointer to read PDOs in internal memory
  5..maximum available space for write PDOs in bytes
  6..maximum available space for read PDOs in bytes
*)

  pHelpManaging := pFirstManagingInterfaceComponent;
  while pHelpManaging do
    if pHelpManaging^.pThis = pPara^.aPara[2]$pVirtualBase then
      exit;
    end_if;

    pHelpManaging := pHelpManaging^.pNext;
  end_while;    
  
  if pHelpManaging then
    pHelpManaging^.pPDOWriteSpaceDPRAM  := pPara^.aPara[0]$^USINT;
    pHelpManaging^.pPDOReadSpaceDPRAM   := pPara^.aPara[1]$^USINT;
    
    pHelpManaging^.pPDOWriteSpace       := pPara^.aPara[3]$^USINT;
    pHelpManaging^.pPDOReadSpace        := pPara^.aPara[4]$^USINT;

    pHelpManaging^.PDOWriteMemAvailable := pPara^.aPara[5]$UDINT;
    pHelpManaging^.PDOReadMemAvailable  := pPara^.aPara[6]$UDINT;
    
    pHelpManaging^.ud_HandlePDOWriteSpaceDPRAM  := pPara^.aPara[7]$UDINT;
    pHelpManaging^.ud_HandlePDOReadSpaceDPRAM   := pPara^.aPara[8]$UDINT;
    
  end_if;

END_FUNCTION


FUNCTION SafetyManager::CheckModuleUsage
  VAR_INPUT
    Length 	: UINT;
    pData 	: ^void;
    IsFSoEConfig 	: BOOL;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    pDataHelp : ^void;
    i : UDINT;
    TmpPathLen : UINT;
    TempCfgNo : USINT;
    ModuleCfgInfo : t_ModuleCfgInfo;
    PdoCfgInfo : t_PdoCfgInfo;
    TempConnectionNo : UINT;
  END_VAR
  
(*
    Memory structure:
      4 Byte: SafetyNumber of CPU (is already there from initialisation in InitSafeModule of the safe CPU))
      1 Byte: Number of module configurations which are following
      for every module configuration (first is always the CPU itself)
        1 Byte: Info
        4 Byte: Interface Frame Source SafetyNumber (0 if it is no source for interface frames)
        4 Byte: Interface Frame CRC
        1 Byte: Number of destination inferface frame CRCs
        x Byte: Destination interface frame CRCs (number * sizeof(HDINT)
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
        1 Byte: PDO out length for this module
      1 Byte: Number of pdo configurations which are following
      for every PDO configuration
        1 Byte: PDO length
        1 Byte: Interface frame info
        4 Byte: Interface CRC
        2 Byte: Length of source hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of source
        2 Byte: Length of destination hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of destination       
      1 Byte: Number of slave configurations which are following
      for every slave configuration
        1 Byte: Info
        4 Byte: InterfaceCRC
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
      
      if IsFSoEConfig = TRUE
      2 Byte: Number of connection infos which are following
      for every FSoE connection info
        2 Byte: ConnectionID
        1 Byte: ConnectionState
        1 Byte: Flags
        4 Byte: Master safety number

only if feature available:        
        2 Byte: Transmission time in ms
        
        2 Byte: Offset where to read from master
        2 Byte: Size of data to read from master (and write to slave)
        2 Byte: Offset where to write to master
        2 Byte: Size of data to write to master (and read from slave)
        2 Byte: SlaveAddress
        2 Byte: Length of slave hardware path
        x Byte (depending on length in the 2 byte before): Slave hardware path
        2 Byte: Length of compressed slave hardware path
        x Byte (depending on length in the 2 byte before): Compressed slave hardware path
        2 Byte: Offset where to read from slave
        2 Byte: Offset where to write to slave
*)

  // check if the configuration is valid (pData <> NIL) & (Length must be at least 4 byte for SafetyNbr + 1 byte module cfg number + 1 byte pdo cfg number + 1 byte slave cfc number)
  if pData & (Length >= 7)  then

    retcode := 0;
    
    pDataHelp := pData;
    
    // skip safety number of the safety cpu
    pDataHelp += sizeof(HDINT);
    
    // number of module configurations which follow now
    TempCfgNo := pDataHelp^$USINT;
    
    // skip number of module configurations
    pDataHelp += sizeof(USINT);
    
    // module configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        // info
        ModuleCfgInfo := pDataHelp^$t_ModuleCfgInfo;
        
        // skip info
        pDataHelp += sizeof(BSINT);
        
        // skip safety number of interface frame source
        pDataHelp += sizeof(HDINT);

        // skip interface CRC of interface frame
        pDataHelp += sizeof(HDINT);
        
        // interface frame destination CRCs
        if ModuleCfgInfo.IsInterfaceDest then
          // number of CRCs
          TmpPathLen := pDataHelp^$USINT;

          // skip number of CRCs
          pDataHelp += sizeof(USINT);
          
          // skip interface destination CRCs
          pDataHelp += TmpPathLen * sizeof(HDINT);
        end_if;
        
        // length of hardware path
        TmpPathLen := pDataHelp^$UINT;

        // skip length of hardware path
        pDataHelp += sizeof(UINT);

        SetModuleUsed(PathLen:=TmpPathLen, pPath:=pDataHelp$^USINT, IgnoreOptional:=FALSE, IsOptional:=ModuleCfgInfo.IsOptional, IsInterfaceDomain := FALSE);

        // skip hardware path
        pDataHelp += TmpPathLen;

        // length of compressed hardware path
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable

        // skip length of compressed hardware path
        pDataHelp += sizeof(UINT);

        // skip compressed hardware path
        pDataHelp += TmpPathLen;
        
        // skip pdo out length of this module
        pDataHelp += sizeof(USINT);
      end_for;
    end_if;

    // number of Pdo configurations which follow now
    TempCfgNo := pDataHelp^$USINT;
    
    // skip number of pdo configurations
    pDataHelp += sizeof(USINT);
        
    // Pdo configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        // skip pdo length
        pDataHelp += sizeof(USINT);
        
        // is it an interface pdo?
        PdoCfgInfo := pDataHelp^$t_PdoCfgInfo;
        
        // skip info
        pDataHelp += sizeof(BSINT);
        
        // skip interface CRC
        pDataHelp += sizeof(HDINT);
        
        // length of source hardware path
        TmpPathLen := pDataHelp^$UINT;

        // skip length of source hardware path
        pDataHelp += sizeof(UINT);
        
        SetModuleUsed(PathLen:=TmpPathLen, pPath:=pDataHelp$^USINT, IgnoreOptional:=FALSE, IsOptional:=PdoCfgInfo.IsOptional, IsInterfaceDomain := PdoCfgInfo.IsInterfacePDO);
        
        // skip source hardware path
        pDataHelp += TmpPathLen;

        // length of destination hardware path
        TmpPathLen := pDataHelp^$UINT;

        // skip length of destination hardware path
        pDataHelp += sizeof(UINT);

        // a destination module doesn't influence the optional property=> ignore the optional setting
        SetModuleUsed(PathLen:=TmpPathLen, pPath:=pDataHelp$^USINT, IgnoreOptional:=TRUE, IsOptional:=FALSE, IsInterfaceDomain := PdoCfgInfo.IsInterfacePDO);

        // skip destination hardware path
        pDataHelp += TmpPathLen;
      end_for;
    end_if;

    // number of slave configurations which follow now
    TempCfgNo := pDataHelp^$USINT;

    // skip number of slave configurations
    pDataHelp += sizeof(USINT);
   
    // slave configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        // skip info
        pDataHelp += sizeof(BSINT);
                   
        // skip InterfaceCRC
        pDataHelp += sizeof(HDINT);

        // length of hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        
        // skip length of hardwarepath
        pDataHelp += sizeof(UINT);
        
        // skip hardwarepath
        pDataHelp += TmpPathLen;

        // length of compressed hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable

        // skip length of compressed hardwarepath
        pDataHelp += sizeof(UINT);
               
        // skip compressed hardwarepath
        pDataHelp += TmpPathLen;
      end_for;
    end_if;
    
    // check if it's a FSoE Config
    if IsFSoEConfig then
      // number of connection details which follow now
      TempConnectionNo := pDataHelp^$UINT;
      pDataHelp += sizeof(UINT);

      // if we got connections
      if TempConnectionNo then
        for i := 0 to (TempConnectionNo-1) do
          // connection ID
          pDataHelp += sizeof(UINT);

          // connection state
          pDataHelp += sizeof(USINT);

          // flags
          // Actually, type is 4 bytes in size but for compatibility reasons it is still used as 1 byte. As a result, bits 9-32 are currently not used.
          pDataHelp += sizeof(t_FSoEFlags);

          // master safety number
          pDataHelp += sizeof(HDINT);

          if bdFeatureEnable.TransmissionTimeInConfig then
            // transmission time in ms
            pDataHelp += sizeof(UINT);
          end_if;

          // offset where to read from master
          pDataHelp += sizeof(UINT);

          // size of data to read from master (and write to slave)
          pDataHelp += sizeof(UINT);
          
          // offset where to write to master
          pDataHelp += sizeof(UINT);

          // size of data to write to master (and read from slave)
          pDataHelp += sizeof(UINT);
          
          // slave address
          pDataHelp += sizeof(UINT);
          
          // length of uncompressed slave hardwarepath
          TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
          pDataHelp += sizeof(UINT);
          
          // uncompressed slave hardwarepath
          pDataHelp += TmpPathLen;           

          // length of compressed slave hardwarepath
          TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
          pDataHelp += sizeof(UINT);

          // compressed slave hardwarepath
          pDataHelp += TmpPathLen;
          
          // offset where to read from master
          pDataHelp += sizeof(UINT);
          
          // offset where to write to master
          pDataHelp += sizeof(UINT);          
        end_for;
      end_if;
    end_if;
    
    // check if the given size matches the memory structure
    if (pDataHelp$UDINT - pData$UDINT) <> Length then
      LogError("@459 (SafetyManager::CheckModuleUsage) Invalid configuration data given (length doesn't match memory structure)");
      retcode := -3;
      return;
    end_if;
  else   
    // invalid configuration
    LogError("@045A (SafetyManager::CheckModuleUsage) Found invalid safety cpu configuration: Pointer or size wrong");
    retcode := -1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::SetModuleUsed
  VAR_INPUT
    PathLen 	: UINT;
    pPath 	: ^USINT;
    IgnoreOptional 	: BOOL;
    IsOptional 	: BOOL;
    IsInterfaceDomain 	: BOOL;
  END_VAR
  VAR
    pHelpManaging : ^t_ManagingComponent;
    pHelpSafety   : ^t_SafetyComponent;
    i             : UDINT;
    pActConnection: ^t_UdpConnection;
  END_VAR

  // search for the hardware path
  
  // only possible for UDP connections
  if sUdpVars.ProducerConnectionNo & (pPath^ = INTERFACE_IP_MODE) & (PathLen = UDP_COMP_PATH_LENGTH) then
    pActConnection := #sUdpVars.paConnectionsToProducers^[0];
    for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
      if (pActConnection^.IFCRC = (pPath$^HDINT + 1)^) & (pActConnection^.IP = (pPath$^HDINT + 5)^) then
        // update optional info
        pActConnection^.InfoBits.IsOptional     := IsOptional;
        pActConnection^.InfoBits.OptionalValid  := TRUE;
        
        return;
      end_if;

      pActConnection += sizeof(t_UdpConnection);
    end_for;

  else
    
    // start with the safety components
    pHelpSafety := pFirstSafetyComponent;
    while pHelpSafety & pHelpSafety^.pPath do
      // we have 1 byte path length on the safety component but we got a 2 byte path length from the configuration
      // furthermore the pPath string could be over more domains and we have only our local path => check if length is bigger and if there is a domain escape right before the domain number
      if ((pPath^ = pHelpSafety^.pPath^) | ((pPath^ > pHelpSafety^.pPath^) & ((pPath + 1 + pPath^ - pHelpSafety^.pPath^)^ = HWT_ESCAPE_DOMAIN))) & 
         (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pPath + 2 + pPath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then
        // only set IsOptional to false (has been initialized with true)
        if (IsOptional = FALSE) & (IgnoreOptional = FALSE) then
          pHelpSafety^.InfoBits.IsOptional := FALSE;
        end_if;
        return;
      end_if;
     
      pHelpSafety := pHelpSafety^.pNext;
    end_while;

    // if not found, continue with the managing interface components
    pHelpManaging := pFirstManagingInterfaceComponent;
    while pHelpManaging do
      if (To_StdLib.MemCmp(pHelpManaging^.pModulePath, pPath, PathLen) = 0) then
        // only set IsOptional to false (has been initialized with true)
        if (IsOptional = FALSE) & (IgnoreOptional = FALSE) then
          pHelpManaging^.InfoBits.IsOptional := FALSE;
        end_if;
        return;
      end_if;

      pHelpManaging := pHelpManaging^.pNext;
    end_while;
  
  end_if;

END_FUNCTION


FUNCTION SafetyManager::CreateTimeSyncList
  VAR
    pHelp : ^t_TimeSyncElement;
    pHelpSafety   : ^t_SafetyComponent;
    pHelpManaging : ^t_ManagingComponent;
  END_VAR

  // first we stop time syncing (if active) to prevent crashing due tue a running time synchronisation
  pActTimeSyncElement := NIL;

  // if there is an old list, free it first
  if pFirstTimeSyncElement then
    // open the ring (set the next pointer of the last element to NIL)
    pFirstTimeSyncElement^.pPrev^.pNext := NIL;
    
    // remove the elements
    while pFirstTimeSyncElement do
      pHelp := pFirstTimeSyncElement;
      pFirstTimeSyncElement := pFirstTimeSyncElement^.pNext;
    
      To_StdLib.Free(mptr:=pHelp);
    end_while;
  end_if;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
    // if it's a module with not only inputs (then there's nothing to synchronize with) => trigger it for time synchronisation
    if (pHelpSafety^.InfoBits.OnlyInputs = FALSE) & (IsFSoEModule(pHelpSafety^.ModuleType) = FALSE) then
      AddTimeSyncElement(pManaging:=NIL, pSafety:=pHelpSafety, IsInterfaceDomain := FALSE);
    end_if;

    pHelpSafety := pHelpSafety^.pNext;
  end_while;

  // interface domains will not get our token, they got their own
  pHelpManaging := pFirstManagingInterfaceComponent;
  while pHelpManaging do
    AddTimeSyncElement(pManaging:=pHelpManaging, pSafety:=NIL, IsInterfaceDomain := TRUE);

    pHelpManaging := pHelpManaging^.pNext;
  end_while;  

END_FUNCTION


FUNCTION SafetyManager::AddTimeSyncElement
  VAR_INPUT
    pManaging 	: ^t_ManagingComponent;
    pSafety 	: ^t_SafetyComponent;
    IsInterfaceDomain 	: BOOL;
  END_VAR
  VAR
    pLast : ^t_TimeSyncElement;
    pHelp : ^t_TimeSyncElement;
  END_VAR

  // first we have to allocate some memory for the new element
  if pFirstTimeSyncElement = NIL then
    pFirstTimeSyncElement$pVoid := To_StdLib.Malloc(size:=sizeof(t_TimeSyncElement));
    pLast := pHelp := pFirstTimeSyncElement;
  else
    // check if we got this already
    pHelp := pFirstTimeSyncElement;
    repeat
      if pSafety & (pHelp^.pSafety = pSafety) then
        // already in list => leave
        return;
      end_if;

      if pManaging & (pHelp^.pManaging = pManaging) then
        // already in list => leave
        return;
      end_if;
      
      pHelp := pHelp^.pNext;
    until (pHelp = pFirstTimeSyncElement) end_repeat;
  
    pHelp$pVoid := To_StdLib.Malloc(size:=sizeof(t_TimeSyncElement));
    pLast := pFirstTimeSyncElement^.pPrev;
  end_if;

  if pHelp then
    // close the double linked ring with the new elements
    pFirstTimeSyncElement^.pPrev := pHelp;
    pHelp^.pNext := pFirstTimeSyncElement;
    
    pHelp^.pPrev := pLast;
    pLast^.pNext := pHelp;
  
    pHelp^.InfoBits := 0;
    pHelp^.InfoBits.IsInterfaceDomain := IsInterfaceDomain;
    
    pHelp^.pManaging := pManaging;
    pHelp^.pSafety   := pSafety;
    
    // managing modules
    if (pManaging <> NIL) then
      pHelp^.InfoBits.IsManagingModule := TRUE;
      pHelp^.pThis := pManaging^.pThis;
      
      // if it's the master connection
      if pManaging^.InfoBits.IsConnectionToMaster then
        // remember it
        pHelp^.InfoBits.IsConnectionToMaster := TRUE;
        
        // and make it the end of the list
        pFirstTimeSyncElement := pHelp^.pNext;
        
        // remember that we got a master connection
        pMasterTimeSyncElement := pHelp;
        
      elsif pMasterTimeSyncElement then
        // if we're not master and there is already a master time sync element in the list, we have to make it the last element
        pFirstTimeSyncElement := pMasterTimeSyncElement^.pNext;
      end_if;
    else
      // safety modules
      pHelp^.InfoBits.IsManagingModule := FALSE;
      pHelp^.pThis := pSafety^.pThis;
      
    // insert new safety modules at the beginning (managing modules at the end)
      pFirstTimeSyncElement := pHelp;
    end_if;
        

  else
    ClassState := -1;
    LogError("@0485 (SafetyManager::AddTimeSyncElement) Failed to allocate memory for a new Element");
  end_if;

END_FUNCTION


FUNCTION SafetyManager::CreateVirtualPDOs
  VAR
    pHelpManaging   : ^t_ManagingComponent;
    pHelpConfig     : ^t_CfgListElem;
    pHelpPdo        : ^t_PdoListElem;
    TempPDO         : t_PdoCfg;
    pSearchConfig   : ^t_CfgListElem;
    pSearchPdo      : ^t_PdoListElem;
    b_UsedAsDest    : BOOL;
  END_VAR

  pHelpConfig := pConfigs;
  while pHelpConfig do
    pHelpPdo := pHelpConfig^.pFirstPdoCfg;
    while pHelpPdo do
      // check if it's a broadcast
      if (pHelpPdo^.PdoInfo.Info.IFBroadCast) & (pHelpPdo^.PdoInfo.Info.VirtualPDOsCreated = FALSE) then
        // found one => create virtual PDOs from it (add them to the same configuration)

        // prepare new virtual PDO
        TempPDO := pHelpPdo^.PdoInfo;

        TempPDO.Info.IFBroadCast := FALSE;
        TempPDO.Info.VirtualPDO := TRUE;

        // add a new PDO for every interface managing module
        pHelpManaging := pFirstManagingInterfaceComponent;
        while pHelpManaging do
          // change destination path to the actual module
          TempPDO.DestPathLen := pHelpManaging^.pModulePath^ + 2;
          TempPDO.pDestPath := pHelpManaging^.pModulePath;
          
          if AddVirtualPDOToCfg(pCfg:=pHelpConfig, pNewPDO:=#TempPDO) then
            LogError("@0486 (SafetyManager::CreateVirtualPDOs) Failed to allocate memory for a new virtual PDO");
          end_if;
        
          pHelpManaging := pHelpManaging^.pNext;
        end_while;

        // search in all configs (if there are other) for a pdo with this interface CRC as source
        pSearchConfig := pConfigs;
        while pSearchConfig do
          pSearchPdo := pSearchConfig^.pFirstPdoCfg;
          while pSearchPdo do
            // we must not manipulate our help pdo
            if pSearchPdo <> pHelpPdo then
              b_UsedAsDest := FALSE;
              
              // check if the path is already used as destination
              if (pSearchPdo^.PdoInfo.DestPathLen = pHelpPdo^.PdoInfo.SourcePathLen) & 
                 (To_StdLib.MemCmp(pSearchPdo^.PdoInfo.pDestPath, pHelpPdo^.PdoInfo.pSourcePath, pHelpPdo^.PdoInfo.SourcePathLen) = 0) then
                b_UsedAsDest := TRUE;
              end_if;

              // check if the interface CRC appears in any PDO (only if the path isn't already used as destination path)
              if (pSearchPdo^.PdoInfo.InterfaceCRC = pHelpPdo^.PdoInfo.InterfaceCRC) & (b_UsedAsDest = FALSE) then
                // now take the source path of the broadcast PDO and use it as source path for this PDO
                To_StdLib.Free(mptr:=pSearchPdo^.PdoInfo.pSourcePath);
                
                pSearchPdo^.PdoInfo.pSourcePath$pVoid := To_StdLib.Malloc(size:=pHelpPdo^.PdoInfo.SourcePathLen);
                
                // save source hardwarepath
                if pSearchPdo^.PdoInfo.pSourcePath then
                  _memcpy(ptr1:=pSearchPdo^.PdoInfo.pSourcePath, ptr2:=pHelpPdo^.PdoInfo.pSourcePath, cntr:=pHelpPdo^.PdoInfo.SourcePathLen);
                  pSearchPdo^.PdoInfo.SourcePathLen := pHelpPdo^.PdoInfo.SourcePathLen;
                  
                  // source and destination of this PDO are both local safe cpu modules which communicate with each other via interface frames => store them in extra list for routing SDOs and TDOs
                  AddToLocalIFs(#pSearchPdo^.PdoInfo);
                else
                  LogError("@0487 (SafetyManager::CreateVirtualPDOs) Failed to allocate memory for a source path");
                  pSearchPdo^.PdoInfo.SourcePathLen := 0;
                  ClassState := -1;
                end_if;
              end_if;              
            end_if;
          
            pSearchPdo := pSearchPdo^.pNext;
          end_while;
        
          pSearchConfig := pSearchConfig^.pNext;
        end_while;
        pHelpPdo^.PdoInfo.Info.VirtualPDOsCreated := TRUE;
      end_if;
    
      pHelpPdo := pHelpPdo^.pNext;
    end_while;
  
    pHelpConfig := pHelpConfig^.pNext;
  end_while;
  
END_FUNCTION


FUNCTION SafetyManager::AddVirtualPDOToCfg
  VAR_INPUT
    pCfg 	: ^t_CfgListElem;
    pNewPDO 	: ^t_PdoCfg;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    pHelpPdo        : ^t_PdoListElem;
    TempLen         : UINT;
  END_VAR

  retcode := 0;

  if pCfg^.pFirstPdoCfg = NIL then
    // first PDO in this cfg (should not be possible, because the template for this PDO is also in the same config)
    pCfg^.pFirstPdoCfg$pVoid := To_StdLib.Malloc(size:=sizeof(t_PdoListElem));
    pHelpPdo := pCfg^.pFirstPdoCfg;
  else
    // search a place for the new PDO
    pHelpPdo := pCfg^.pFirstPdoCfg;
    while pHelpPdo^.pNext do
      
      pHelpPdo := pHelpPdo^.pNext;
    end_while;
    
    pHelpPdo^.pNext$pvoid := To_StdLib.Malloc(size:=sizeof(t_PdoListElem));
    pHelpPdo := pHelpPdo^.pNext;
  end_if;
  
  if pHelpPdo then
    // init the values
    pHelpPdo^.PdoInfo.PdoLen        := pNewPDO^.PdoLen;
    pHelpPdo^.PdoInfo.Info          := pNewPDO^.Info;
    pHelpPdo^.PdoInfo.InterfaceCRC  := pNewPDO^.InterfaceCRC;
    
    // length of source hardwarepath
    TempLen := pNewPDO^.SourcePathLen; // save some time with this local variable
    pHelpPdo^.PdoInfo.SourcePathLen := TempLen;
    
    // allocate memory for the source module path
    pHelpPdo^.PdoInfo.pSourcePath$pVoid := To_StdLib.Malloc(size:=TempLen);
    if pHelpPdo^.PdoInfo.pSourcePath = NIL then
      ClassState := -1;
      retcode := -2;
      return;
    end_if;
    
    // save source hardwarepath
    _memcpy(ptr1:=pHelpPdo^.PdoInfo.pSourcePath, ptr2:=pNewPDO^.pSourcePath, cntr:=TempLen);

    // length of destination hardwarepath
    TempLen := pNewPDO^.DestPathLen; // save some time with this local variable
    pHelpPdo^.PdoInfo.DestPathLen := TempLen;
    
    // allocate memory for the destination module path
    pHelpPdo^.PdoInfo.pDestPath$pVoid := To_StdLib.Malloc(size:=TempLen);
    if pHelpPdo^.PdoInfo.pDestPath = NIL then
      ClassState := -1;
      retcode := -2;
      return;
    end_if;
    
    // save destination hardwarepath
    _memcpy(ptr1:=pHelpPdo^.PdoInfo.pDestPath, ptr2:=pNewPDO^.pDestPath, cntr:=TempLen);               
    
    // now we give it an ID and increase the number for the next pdo
    pHelpPdo^.PdoInfo.PDO_ID := ActPDO_ID;
    ActPDO_ID += 1;
    
    pHelpPdo^.pNext                  := NIL;
  else
    ClassState := -1;
    retcode := -2;
    return;
  end_if;
            
END_FUNCTION


FUNCTION SafetyManager::AddToLocalIFs
  VAR_INPUT
    pHelpPDO 	: ^t_PdoCfg;
  END_VAR
  VAR
    AddSource : BOOL;
    i : USINT;
    pHelpSafety : ^t_SafetyComponent;
  END_VAR

  // check if there is still room for new modules
  if LocalIFNo < MAX_INTERNAL_IF_MODULES then
    
    AddSource := TRUE;

    // if there is a list, check if any of our modules are already in the list
    if LocalIFNo then
      for i := 0 to (LocalIFNo-1) do
        // check if crc matches
        if (aLocalIFs[i].InterfaceCRC = pHelpPDO^.InterfaceCRC) then
          AddSource := FALSE;
          exit;
        end_if;
      end_for;
    end_if;

    // now we know how many we have to add to the list. if there are modules to add and there is enough space for all of them => do it
    if AddSource then
      // search it in the local modules
      pHelpSafety := pFirstSafetyComponent;
      while pHelpSafety do
        // if the path matches, add the module to the list

        // source module
        if (pHelpSafety^.PathLen = (pHelpPDO^.SourcePathLen - 1)) & 
        // we compare the last x byte (where x is the length of the local path including the domain) because we got different length info sizes (1 byte vs. 2 byte)
           (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pHelpPDO^.pSourcePath + 2 + pHelpPDO^.pSourcePath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then
          aLocalIFs[LocalIFNo].InterfaceCRC  := pHelpPDO^.InterfaceCRC;
          aLocalIFs[LocalIFNo].pSafetyModule := pHelpSafety;
          LocalIFNo += 1;
          exit;
        end_if;
      
        pHelpSafety := pHelpSafety^.pNext;
      end_while;
    end_if;
  end_if;

END_FUNCTION

#pragma warning(disable:74)
#pragma warning(disable:73)
FUNCTION SafetyManager::StartTimeMeasurement
  VAR_INPUT
    usMode (EAX) 	: DINT;
  END_VAR
#ifdef VM_CLASS_TIME_MEASUREMENT
  VAR
    tmpTime : ^t_TimeMeasurementVaran;
  END_VAR

  case usMode of
  
    //-----------------------------------------------------------
    
    VARANTIME_RT:
      tmpTime := #s_TimeRt;
    
    //-----------------------------------------------------------
    
    VARANTIME_RTPOSTSCAN:
      tmpTime := #s_TimeRtPostScan;
    
    //-----------------------------------------------------------
    
    VARANTIME_CY:
      tmpTime := #s_TimeCy;
    
    //-----------------------------------------------------------
    
  end_case;
  
  if ( tmpTime^.us_Activate ) then
//    Save_Flg();
//    CLI();
    tmpTime^.ud_Timemark := OS_READMICROSEC();
  end_if;

#endif

END_FUNCTION


FUNCTION SafetyManager::StopTimeMeasurement
  VAR_INPUT
    usMode (EAX) 	: DINT;
  END_VAR
#ifdef VM_CLASS_TIME_MEASUREMENT
  VAR
    tmpTime : ^t_TimeMeasurementVaran;
  END_VAR

  case usMode of
  
    //-----------------------------------------------------------
    
    VARANTIME_RT:
      tmpTime := #s_TimeRt;
    
    //-----------------------------------------------------------
    
    VARANTIME_RTPOSTSCAN:
      tmpTime := #s_TimeRtPostScan;
    
    //-----------------------------------------------------------
    
    VARANTIME_CY:
      tmpTime := #s_TimeCy;
    
    //-----------------------------------------------------------
    
  end_case;

  if ( tmpTime^.us_Activate ) then

    //get actual time
    tmpTime^.ud_ActualTime := OS_READMICROSEC();
    tmpTime^.ud_ActualTime -= tmpTime^.ud_Timemark;

    //look for maximum time
    if ( tmpTime^.ud_ActualTime > tmpTime^.ud_MaximalTime ) then
      tmpTime^.ud_MaximalTime := tmpTime^.ud_ActualTime;
    end_if;

    //look for minimum time
    if ( tmpTime^.ud_ActualTime < tmpTime^.ud_MinimalTime ) then
      tmpTime^.ud_MinimalTime := tmpTime^.ud_ActualTime;
    end_if;

    //look for average time (100000 cycles)
    if ( tmpTime^.ud_AverageCounter = 100000 ) then

      tmpTime^.ud_AverageTime 	:= tmpTime^.ud_AverageSum / tmpTime^.ud_AverageCounter;
      tmpTime^.ud_AverageSum 	:= 0;
      tmpTime^.ud_AverageCounter := 0;

    else
      tmpTime^.ud_AverageSum += ( tmpTime^.ud_ActualTime / 100 );
      tmpTime^.ud_AverageCounter += 1;
    end_if;

//		STI();
//    Restore_Flg();
  end_if;
  
#endif

END_FUNCTION // Varan_Base::StopTimeMeasurement
#pragma warning(default:74)
#pragma warning(default:73)


FUNCTION SafetyManager::AnyIFDomainSynchron
  VAR_OUTPUT
    AnythingSynchron 	: BOOL;
  END_VAR
  VAR
    pHelpManaging   : ^t_ManagingComponent;
  END_VAR

  AnythingSynchron := FALSE;
  
  pHelpManaging := pFirstManagingInterfaceComponent;
  while pHelpManaging do
    if pHelpManaging^.InfoBits.IsSynchron then
      AnythingSynchron := TRUE;
      return;
    end_if;
    
    pHelpManaging := pHelpManaging^.pNext;
  end_while;

END_FUNCTION


FUNCTION SafetyManager::LocalIFsAvailable
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  VAR
    pHelpCfg : ^t_CfgListElem;
    pHelpPdo : ^t_PdoListElem;
    pHelpModule : ^t_ModuleListElem;
  END_VAR

  retcode := FALSE;
  
  // add all local configs to our structure
  SendLocalCfgsToMaster();
  
  // check the local configs for interface frames
  pHelpCfg := pConfigs;
  while pHelpCfg do
    pHelpPdo := pHelpCfg^.pFirstPdoCfg;
    while pHelpPdo do
      if pHelpPdo^.PdoInfo.Info.IsInterfacePDO | pHelpPdo^.PdoInfo.Info.IFBroadCast then
        retcode := TRUE;
        return;
      end_if;
      
      pHelpPdo := pHelpPdo^.pNext;
    end_while;

    pHelpModule := pHelpCfg^.pFirstModuleCfg;
    while pHelpModule do
      if pHelpModule^.ModuleInfo.Info.IsInterfaceSource | pHelpModule^.ModuleInfo.Info.IsInterfaceDest then
        retcode := TRUE;
        return;
      end_if;
    
      pHelpModule := pHelpModule^.pNext;
    end_while;
  
    pHelpCfg := pHelpCfg^.pNext;
  end_while;

END_FUNCTION


FUNCTION SafetyManager::IsCPUModule
  VAR_INPUT
    ModuleType : t_e_ModuleType;
  END_VAR
  VAR_OUTPUT
    retcode : BOOL;
  END_VAR

  case ModuleType of
    _CSCP01x,
    _SCP010,
    _SCP011,
    _SCP111,
    _SCP211,
    _SDD1300,
    _SDD1400,
    _SDD1500,
    _SDD1600:
      retcode := TRUE;
  else
    retcode := FALSE;
  end_case; 

END_FUNCTION


FUNCTION SafetyManager::IsFSoEModule
  VAR_INPUT
    ModuleType 	: t_e_ModuleType;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR

  case ModuleType of
    _SCP111,
    _SCP211,
    _SDD1300,
    _SDD1400,
    _SDD1500,
    _SDD1600,
    _ModuleWithoutConfig:
      retcode := TRUE;
  else
    retcode := FALSE;
  end_case; 

END_FUNCTION


FUNCTION SafetyManager::CreateFSoEPDOs
  VAR
    pHelpConfigManager      : ^t_CfgListElem;
    pHelpConnectionManager  : ^t_FSoEConnectionDetails;
    pHelpConfigSlave        : ^t_CfgListElem;
    pHelpConnectionSlave    : ^t_FSoEConnectionDetails;
    SlavesFoundNo           : INT;
    pLastFoundSlave         : ^t_FSoEConnectionDetails; 
    pLastFoundSlaveCfg      : ^t_CfgListElem;
    SortConnectionList      : BOOL;
    ConnectionsAvailable    : UINT;
    pConnectionPointerList  : pVoid;
    ActIndex                : UINT;
    pActConnection          : ^t_UdpConnection;    
    i                       : UDINT;
    MandatoryUdpConMissing  : BOOL;
  END_VAR

//*****************************************************************************
//** SEARCH FOR MASTER / SLAVE PAIRS VIA SLAVE ADDRESS                       **
//*****************************************************************************

  MandatoryUdpConMissing  := FALSE;
  SortConnectionList      := FALSE;
  ConnectionsAvailable    := 0;

  // check all configs for manager connections (those contain the info about the slaves)
  pHelpConfigManager := pConfigs;
  while pHelpConfigManager do
  
    pHelpConnectionManager := pHelpConfigManager^.pFirstFSoEConnection;
    while pHelpConnectionManager do
      
      ConnectionsAvailable += 1;
      
      if pHelpConnectionManager^.Flags.FSoESlaveHasNoConfig & pHelpConnectionManager^.Flags.IsMaster then

        if pHelpConnectionManager^.InternalConnectionState = FCS_RESET then
          ShareSingleConnectionInfos(pHelpConnectionManager, pHelpConfigManager^.pFirstModuleCfg);
        end_if;

      // if we got a master connection => search for a match of the slaveaddress in the slave configurations
      elsif pHelpConnectionManager^.Flags.IsMaster then
        SlavesFoundNo := 0;
        
        if (pHelpConnectionManager^.SlavePathCompLen = UDP_COMP_PATH_LENGTH) & (pHelpConnectionManager^.pSlavePathComp^ = INTERFACE_IP_MODE) then
          // UDP Connection where we are consumer => check for it in the list of producers
          if sUdpVars.ProducerConnectionNo then
            pActConnection := #sUdpVars.paConnectionsToProducers^[0];
            for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
              if (pActConnection^.IFCRC = (pHelpConnectionManager^.pSlavePathComp$^HDINT + 1)^) & (pActConnection^.IP = (pHelpConnectionManager^.pSlavePathComp$^HDINT + 5)^) then
                // check if it has to be there but isn't
                if (pActConnection^.InfoBits.IsOptional = FALSE) & (pActConnection^.InfoBits.OptionalValid = TRUE) & (pActConnection^.InfoBits.PdosInitialized = FALSE) then
                  // remember that there is something missing
                  MandatoryUdpConMissing := TRUE;                  
                end_if;
                exit;
              end_if;

              pActConnection += sizeof(t_UdpConnection);
            end_for;
          end_if;
        else
          pHelpConfigSlave := pConfigs;
          while pHelpConfigSlave do
            
            pHelpConnectionSlave := pHelpConfigSlave^.pFirstFSoEConnection;
            while pHelpConnectionSlave do
              if pHelpConnectionSlave^.Flags.IsMaster = FALSE then
                if (pHelpConnectionManager^.SlaveAddress) &
                   (pHelpConnectionManager^.SlaveAddress = pHelpConnectionSlave^.SlaveAddress) then
                  // check if it's an internal interface frame
                  if pHelpConnectionManager^.Flags.IsInterfaceConnection & pHelpConnectionSlave^.Flags.IsInterfaceConnection then
                    // check if the connection is still available
                    if pHelpConnectionSlave^.InternalConnectionState = FCS_RESET then
                      pLastFoundSlave     := pHelpConnectionSlave;
                      pLastFoundSlaveCfg  := pHelpConfigSlave;

                      // mark as internal interface frame
                      SlavesFoundNo := -1;
                      
                      // exit both loops
                      pHelpConfigSlave := NIL;
                      exit;
                    end_if;
                  else
                    SlavesFoundNo       += 1;
                    pLastFoundSlave     := pHelpConnectionSlave;
                    pLastFoundSlaveCfg  := pHelpConfigSlave;
                  end_if;
                end_if;
              end_if;
              
              pHelpConnectionSlave := pHelpConnectionSlave^.pNext;
            end_while;
          
            if pHelpConfigSlave then
              pHelpConfigSlave := pHelpConfigSlave^.pNext;
            end_if;
          end_while;
          
          if SlavesFoundNo = 1 then
            // perfect, since the slave address is meant to be unique
            ShareConnectionInfos(pHelpConnectionManager, pHelpConfigManager, pLastFoundSlave, pLastFoundSlaveCfg);
            
          elsif SlavesFoundNo = 0 then
            // we have to search for the address in other systems which could be connected via interfaceframe
            if pFirstManagingInterfaceComponent = NIL then
              if (pHelpConnectionManager^.SlaveAddress <> 0) & (pHelpConnectionManager^.bsLogError.SlaveForMasterNotFound = FALSE) then
                LogValue("@0489 (SafetyManager::CreateFSoEPDOs) Could not find Slave for Master connection with SlaveAddress 0x{0}", Value:=pHelpConnectionManager^.SlaveAddress);
                pHelpConnectionManager^.bsLogError.SlaveForMasterNotFound := TRUE; //Only log error one time              
              end_if;
            else
              SortConnectionList := TRUE;
            end_if;
          elsif SlavesFoundNo = -1 then
            // internal interface frame => add it to the list for SDO routing
            AddInternalIFFSoE(pHelpConnectionManager, pHelpConfigManager, pLastFoundSlave, pLastFoundSlaveCfg);
          else
            if pHelpConnectionManager^.bsLogError.MoreThan1SlaveFound = FALSE then
              LogValue("@048A (SafetyManager::CreateFSoEPDOs) Found more than 1 Slave for Master connection with SlaveAddress 0x{0}! Slave Address must be unique!", Value:=pHelpConnectionManager^.SlaveAddress);
              pHelpConnectionManager^.bsLogError.MoreThan1SlaveFound := TRUE; //Only log error one time              
            end_if;
          end_if;
        end_if;
      else
        if pHelpConnectionManager^.Flags.IsInterfaceConnection then
          if pFirstManagingInterfaceComponent then
            SortConnectionList := TRUE;
          end_if;
          
          // check if we can get an interface crc from the config
          if pHelpConfigManager^.pFirstModuleCfg & (pHelpConfigManager^.pFirstModuleCfg^.ModuleInfo.SafetyNbr = pHelpConnectionManager^.OwnerSafetyNbr) then
            // store the interface CRC (to send it to the other domain later)
            if pHelpConnectionManager^.pSlavePathComp = NIL then
              pHelpConnectionManager^.pSlavePathComp$pVoid := To_StdLib.Malloc(size:=8);
              
              if pHelpConnectionManager^.pSlavePathComp then
                // generate a compressed path (IF CRC)
                _memset(dest:=pHelpConnectionManager^.pSlavePathComp, usByte:=16#FF, cntr:=8);
                pHelpConnectionManager^.pSlavePathComp^$USINT := 3;
                _memcpy(ptr1:=pHelpConnectionManager^.pSlavePathComp + 1, ptr2:=#pHelpConfigManager^.pFirstModuleCfg^.ModuleInfo.InterfaceCRC, cntr:=sizeof(HDINT));
                pHelpConnectionManager^.SlavePathCompLen := 8;
              else
                LogError("@048B (SafetyManager::CreateFSoEPDOs) Failed to allocate memory for an interface CRC!");
              end_if;
            end_if;
          end_if;
        end_if;
      end_if;

      pHelpConnectionManager := pHelpConnectionManager^.pNext;
    end_while;
    
    pHelpConfigManager := pHelpConfigManager^.pNext;
  end_while;

  if MandatoryUdpConMissing then
    // don't set b_AllLocalSlaveAddressesValid to TRUE, we need to keep waiting
    return;
  end_if;

  // check if we got all slave addresses of local configs
  CheckLocalSlaveAddresses();

  // reset pointer
  pSortedConnections := NIL;

  // there are interface connections => sort the list by slave address to ensure correct order in memory
  if SortConnectionList | (ConnectionsAvailable & (b_AllLocalSlaveAddressesValid = FALSE)) then
    // first we need a pointer list of connection (not linked) with 4 byte entries
    pConnectionPointerList := To_StdLib.Malloc(size:=ConnectionsAvailable * sizeof(pVoid));
    
    if pConnectionPointerList = NIL then
      LogError("@048C (SafetyManager::CreateFSoEPDOs) Failed to allocate memory for sorting the FSoE connections!");
      return;
    end_if;

    // store the pointers of all connections into the array
    ActIndex := 0;
    pHelpConfigManager := pConfigs;
    while pHelpConfigManager do
    
      pHelpConnectionManager := pHelpConfigManager^.pFirstFSoEConnection;
      while pHelpConnectionManager do
        (pConnectionPointerList + ActIndex * sizeof(pVoid))^$^t_FSoEConnectionDetails := pHelpConnectionManager;
        
        ActIndex                += 1;
        pHelpConnectionManager  := pHelpConnectionManager^.pNext;
      end_while;
      
      pHelpConfigManager := pHelpConfigManager^.pNext;
    end_while;
    
    // now that we got our linear memory to sort => do it
    MySort(src:=pConnectionPointerList$^USINT, no:=ActIndex, itemsize:=sizeof(pVoid), sortfunct:=#ConnectionSort());
    
    // create the new sorted linked list
    pSortedConnections := pConnectionPointerList^$^t_FSoEConnectionDetails;
    pHelpConnectionManager := pSortedConnections;
    
    // we already have the first connection at the pSortedConnections;
    for ActIndex := 1 to ConnectionsAvailable-1 do
      pHelpConnectionManager^.pNextSorted := (pConnectionPointerList + ActIndex * sizeof(pVoid))^$^t_FSoEConnectionDetails;
      pHelpConnectionManager := pHelpConnectionManager^.pNextSorted;
    end_for;
    
    To_StdLib.Free(mptr:=pConnectionPointerList);
    // now we can access the connection in order of the slave address via pSortedConnections
  end_if;

END_FUNCTION


FUNCTION SafetyManager::ShareConnectionInfos
  VAR_INPUT
    pMasterConnection 	: ^t_FSoEConnectionDetails;
    pMasterConfig 	: ^t_CfgListElem;
    pSlaveConnection 	: ^t_FSoEConnectionDetails;
    pSlaveConfig 	: ^t_CfgListElem;
  END_VAR
  VAR
    pHelpSafety   : ^t_SafetyComponent;
    pMasterSafety : ^t_SafetyComponent;
    pSlaveSafety  : ^t_SafetyComponent;
    MyPara 	      : CmdStruct;
    MyResult	    : results;
    pPDOReadData  : pVoid;
    i             : UDINT;
    IsInList      : BOOL;
  END_VAR

//*****************************************************************************
//** TELL THE MODULES WHAT DATA TO COPY IN REALTIME                          **
//*****************************************************************************

  pMasterSafety := NIL;
  pSlaveSafety  := NIL;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
  
    if pHelpSafety^.SafetyNbr & (pHelpSafety^.SafetyNbr = pMasterConfig^.SafetyNbr) then
      pMasterSafety := pHelpSafety;
      exit;
    end_if;

    pHelpSafety := pHelpSafety^.pNext;
  end_while;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
  
    if pHelpSafety^.SafetyNbr & (pHelpSafety^.SafetyNbr = pSlaveConfig^.SafetyNbr) then
      pSlaveSafety := pHelpSafety;
      exit;
    end_if;

    pHelpSafety := pHelpSafety^.pNext;
  end_while;

  // if we found the safety modules who are keeping the configs => tell them what to copy
  if pMasterSafety & pSlaveSafety then
    // check for matching hardware paths
    if (pMasterConnection^.pSlavePath^ <> pSlaveSafety^.pPath^) |
       (To_StdLib.MemCmp(pSlaveSafety^.pPath + 1, pMasterConnection^.pSlavePath + 2, pSlaveSafety^.pPath^) <> 0) then
      LogError("@048D (SafetyManager::ShareConnectionInfos) Error! The hardware path of the master connection doesn't match the hw path");
      LogValue(" of the slave module with the corresponding slave address (Slave Address 0x{0} and ConnectionID 0x{1})!", Value:=pMasterConnection^.SlaveAddress, Value2:=pMasterConnection^.ConnectionID);
      return;
    end_if;

    // check for equal sizes
    if (pMasterConnection^.MasterReadSize <> pSlaveConnection^.MasterReadSize) |
       (pMasterConnection^.MasterWriteSize <> pSlaveConnection^.MasterWriteSize) then
      LogError("@048E (SafetyManager::ShareConnectionInfos) Error! The sizes of master and slave connection");
      LogValue(" do not match for Slave Address 0x{0} and ConnectionID 0x{1}!", Value:=pMasterConnection^.SlaveAddress, Value2:=pMasterConnection^.ConnectionID);
      return;
    end_if;
    
    // we start with the safe outputs: get the data pointer from the master and give it to the slave
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@048F (SafetyManager::ShareConnectionInfos) Invalid return value when asking the master safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;

    // check if it's in the list
    IsInList := FALSE;
    if FSoEInputPDONo then
      for i := 0 to (FSoEInputPDONo-1) do
        // check if the memory is already in the list for this slave safety module
        if ((aFSoEInputPDOs[i].pSafety = pSlaveSafety) & (aFSoEInputPDOs[i].pMem = pPDOReadData$DINT + pMasterConnection^.MasterReadOffset)) then
          // if we found it => skip adding the PDO to the module
          IsInList := TRUE;
        end_if;
      end_for;
    end_if;
    
    if IsInList = FALSE then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
      LogValue("@DBG (SafetyManager::ShareConnectionInfos) ConnectionID: {0}, SlaveAddress: {1}", Value:=pMasterConnection^.ConnectionID, Value2:=pMasterConnection^.SlaveAddress);
      LogValue("@DBG (SafetyManager::ShareConnectionInfos) MasterSafetyNbr: {0}, SlaveSafetyNbr: {1}", Value:=pMasterConnection^.OwnerSafetyNbr, Value2:=pSlaveConnection^.OwnerSafetyNbr);
      LogValue("@DBG (SafetyManager::ShareConnectionInfos) MasterReadOffset: {0}, MasterReadSize: {1}, SlaveWriteOffset: {2}", Value:=pMasterConnection^.MasterReadOffset, Value2:=pMasterConnection^.MasterReadSize, Value3 := pSlaveConnection^.SlaveWriteOffset);
#endif

      MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
      MyPara.aPara[0] := pPDOReadData$DINT + pMasterConnection^.MasterReadOffset;
      MyPara.aPara[2] := pMasterConnection^.OwnerSafetyNbr$DINT;
      MyPara.aPara[3] := pMasterConnection^.MasterReadSize;
      MyPara.aPara[4] := pMasterSafety^.InfoBits.IsOptional;
      MyPara.aPara[7] := pSlaveConnection^.SlaveWriteOffset;
      MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
      MyPara.aPara[9] := pMasterConnection^.Flags.IsInterfaceConnection;
      pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

      if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
        aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pSlaveSafety;
        aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
        FSoEInputPDONo += 1;
      else
        LogValue("@0490 (SafetyManager::ShareConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
      end_if;    
    end_if;

    // safe inputs: get the data pointer from the slave and give it to the master
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@0491 (SafetyManager::ShareConnectionInfos) Invalid return value when asking the slave safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;


    // check if it's in the list
    IsInList := FALSE;
    if FSoEInputPDONo then
      for i := 0 to (FSoEInputPDONo-1) do
        // check if the memory is already in the list for this slave safety module
        if ((aFSoEInputPDOs[i].pSafety = pMasterSafety)  & (aFSoEInputPDOs[i].pMem = pPDOReadData$DINT + pSlaveConnection^.SlaveReadOffset)) then
          // if we found it => skip adding the PDO to the module
          IsInList := TRUE;
        end_if;
      end_for;
    end_if;

    if IsInList = FALSE then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
      LogValue("@DBG (SafetyManager::ShareConnectionInfos) SlaveReadOffset: {0}, SlaveReadSize: {1}, MasterWriteOffset: {2}", Value:=pSlaveConnection^.SlaveReadOffset, Value2:=pMasterConnection^.MasterWriteSize, Value3 := pMasterConnection^.MasterWriteOffset);
#endif

      MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
      MyPara.aPara[0] := pPDOReadData$DINT + pSlaveConnection^.SlaveReadOffset;
      MyPara.aPara[2] := pSlaveConnection^.OwnerSafetyNbr$DINT;
      MyPara.aPara[3] := pMasterConnection^.MasterWriteSize;
      MyPara.aPara[4] := pSlaveSafety^.InfoBits.IsOptional;
      MyPara.aPara[7] := pMasterConnection^.MasterWriteOffset;
      MyPara.aPara[8] := 0;
      MyPara.aPara[9] := 0;
      pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);
      
      if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
        aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pMasterSafety;
        aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
        FSoEInputPDONo += 1;
      else
        LogValue("@0490 (SafetyManager::ShareConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
      end_if;
    end_if;
  end_if;

END_FUNCTION

FUNCTION MyXSort VAR_INPUT p1:^void; p2:^void; END_VAR VAR_OUTPUT retcode : dint; END_VAR;

FUNCTION SafetyManager::MySort
  VAR_INPUT
    src 	: ^USINT;
    no 	: UDINT;
    itemsize 	: UDINT;
    sortfunct 	: ^void;
  END_VAR
  VAR_OUTPUT
    retcode 	: BOOL;
  END_VAR
  var
    tmp, hp : ^DINT;
    mx, nx, wx, parent, child, max, i : dint;
    spuf : array[0..127] of dint;
  end_var

  retcode := false;
  
  // work faster with the pointer list (no memcopy used)
  if itemsize = 4 then    
    nx      := to_dint(no);
    mx      := (nx + 6) / 8;
    src     := src;
    tmp     := #spuf[0];
    retcode := true;

    while(1) do
      if(mx) then
        mx     -= 1;
        parent := mx;
        tmp^ := (src$^DINT + (parent$udint*4))^;
      else
        nx -= 1;
        if(nx) then
          hp := src + (nx$udint*4);
          tmp^ := hp^;
          hp^ := (src$^DINT)^;
          parent := 0;
        else
          exit;
        end_if;
      end_if;

      child := parent * 8 + 1;
      while(child < nx) do
        wx := nx - child;
        if(wx > 8) then
          wx := 8;
        end_if;
        
        max := 0;
        if(wx > 1) then
          for i:=1 to wx-1 do 
            if(sortfunct$MyXSort(src+(to_udint(child+i)*4), src+to_udint(child+max)*4) > 0) then
              max := i;
            end_if;
          end_for;
        end_if;

        child += max;

        if(sortfunct$MyXSort(src+(child$udint*4), tmp) <= 0) then
          exit;
        end_if;
        
        (src$^DINT+(parent$udint*4))^ := (src$^DINT+(child$udint*4))^;
        parent := child;
        child  := parent * 8 + 1;
      end_while;

      (src$^DINT+(parent$udint*4))^ := tmp^;
    end_while;
  else
    if(itemsize <= sizeof(spuf)) then
    
      nx      := to_dint(no);
      mx      := (nx + 6) / 8;
      src     := src;
      tmp     := #spuf[0];
      retcode := true;

      while(1) do
        if(mx) then
          mx     -= 1;
          parent := mx;
          _memcpy(tmp, src + (parent$udint*itemsize), itemsize);
        else
          nx -= 1;
          if(nx) then
            hp := src + (nx$udint*itemsize);
            _memcpy(tmp, hp, itemsize);
            _memmove(hp, src, itemsize);
            parent := 0;
          else
            exit;
          end_if;
        end_if;

        child := parent * 8 + 1;
        while(child < nx) do
          wx := nx - child;
          if(wx > 8) then
            wx := 8;
          end_if;
          
          max := 0;
          if(wx > 1) then
            for i:=1 to wx-1 do 
              if(sortfunct$MyXSort(src+(to_udint(child+i)*itemsize), src+to_udint(child+max)*itemsize) > 0) then
                max := i;
              end_if;
            end_for;
          end_if;

          child += max;

          if(sortfunct$MyXSort(src+(child$udint*itemsize), tmp) <= 0) then
            exit;
          end_if;
          
          _memcpy(src+(parent$udint*itemsize), src+(child$udint*itemsize), itemsize);
          parent := child;
          child  := parent * 8 + 1;
        end_while;

        _memcpy(src+(parent$udint*itemsize), tmp, itemsize);
      end_while;
    end_if;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::ConnectionSort
  VAR_INPUT
    pCon1 	: ^pVoid;
    pCon2 	: ^pVoid;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    Var1 : HDINT;
    Var2 : HDINT;
    pHelp : ^HDINT;
    Uint1 : UINT;
    Uint2 : UINT;
  END_VAR

  // dint MyXSort(void *p1, void *p2)
  //   if(a>b) retcode = 1;
  //   if(a=b) retcode = 0;
  //   if(a<b) retcode = -1
  
  pHelp := (pCon1^$^t_FSoEConnectionDetails^.pSlavePathComp)$^HDINT;
  if pHelp then
    if pHelp^$USINT = INTERFACE_CRC_MODE then
      Var1 := (pHelp + 1)^;
    else
      // no interface frame => sort to the end
      Var1 := 16#FFFFFFFF;
    end_if;
  else
    // no compressed path available => sort to the end
    Var1 := 16#FFFFFFFF;
  end_if;

  pHelp := (pCon2^$^t_FSoEConnectionDetails^.pSlavePathComp)$^HDINT;
  if pHelp then
    if pHelp^$USINT = INTERFACE_CRC_MODE then
      Var2 := (pHelp + 1)^;
    else
      // no interface frame => sort to the end
      Var2 := 16#FFFFFFFF;
    end_if;
  else
    // no compressed path available => sort to the end
    Var2 := 16#FFFFFFFF;
  end_if;

  // we want all connections to be sorted by the Interface CRC
  if Var1 > Var2 then
    retcode := 1;
  elsif Var1 < Var2 then
    retcode := -1;
  else
    // not possible with unique Interface CRCs, but a slave can provide between 1 and 150 connections with the same settings (only different offsets)
    // therefore it can only be detected as an error if the sources differ and they are not interface masters
    if pCon1^$^t_FSoEConnectionDetails^.OwnerSafetyNbr <> pCon2^$^t_FSoEConnectionDetails^.OwnerSafetyNbr then
      retcode := 0; 
    else
      // otherwise sort by offset
      Uint1 := pCon1^$^t_FSoEConnectionDetails^.SlaveReadOffset;
      Uint2 := pCon2^$^t_FSoEConnectionDetails^.SlaveReadOffset;
      
      if Uint1 > Uint2 then
        retcode := 1;
      elsif Uint1 < Uint2 then
        retcode := -1;
      else
        retcode := 0;
      end_if;
    end_if;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::FsoEInfoSort
	VAR_INPUT
		pInfo1 	: ^pVoid;
		pInfo2 	: ^pVoid;
	END_VAR
	VAR_OUTPUT
		retcode 	: DINT;
	END_VAR
  VAR
    Var1 : HDINT;
    Var2 : HDINT;
  END_VAR

  // dint MyXSort(void *p1, void *p2)
  //   if(a>b) retcode = 1;
  //   if(a=b) retcode = 0;
  //   if(a<b) retcode = -1
  
  // get the interface CRCs from both infos
  Var1 := pInfo1^$t_FSoEConnInfo.FromOtherSide.InterfaceCRC;
  Var2 := pInfo2^$t_FSoEConnInfo.FromOtherSide.InterfaceCRC;

  // now compare them to get all infos sorted by the Interface CRC
  if Var1 > Var2 then
    retcode := 1;
  elsif Var1 < Var2 then
    retcode := -1;
  else
    // equal => don't sort
    retcode := 0;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::FinalizePDOMapping
  VAR
    pHelpConnection   : ^t_FSoEConnectionDetails;
    pHelpSafety       : ^t_SafetyComponent;
    i                 : UDINT;
    ModuleReadOffset  : UINT;
    ModuleReadSize    : UINT;
    ModuleWriteOffset : UINT;
    ModuleWriteSize   : UINT;
    MngReadOffset     : UINT;
    MngReadSize       : UINT;
    MngWriteOffset    : UINT;
    MngWriteSize      : UINT;
    MyPara            : CmdStruct;
    MyResult          : results;
    bIsCustomConnValid: BOOL;
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
    LogStr            : ARRAY [0..255] OF CHAR;
#endif
  END_VAR
   
  // check the FSoE Infos from the other side
  if IFFSoENo then
    // if there is more than 1 entry
    if IFFSoENo > 1 then
      // sort the entries by interface CRC just like the pSortedConnections, to ensure ascending interface CRCs in the VBC/VMC memory mapping
      MySort(src:=(#aIFFSoEInfos[0])$^USINT, no:=IFFSoENo, itemsize:=sizeof(t_FSoEConnInfo), sortfunct:=#FsoEInfoSort());      
    end_if;

    for i := 0 to (IFFSoENo-1) do
      // search for a local module to handle this
      pHelpConnection := pSortedConnections;      
      while pHelpConnection do        
        bIsCustomConnValid := CheckCustomConnectionValid(pHelpConnection,#aIFFSoEInfos[i]);
        if (bIsCustomConnValid                    | 
           ((pHelpConnection^.SlaveAddress)  &
           (pHelpConnection^.SlaveAddress    =  aIFFSoEInfos[i].FromOtherSide.SlaveAddress)))  &
           (pHelpConnection^.Flags.IsMaster  <> aIFFSoEInfos[i].FromOtherSide.Flags.IsMaster)  &
           (pHelpConnection^.MasterReadSize  =  aIFFSoEInfos[i].FromOtherSide.MasterReadSize)  &
           (pHelpConnection^.MasterWriteSize =  aIFFSoEInfos[i].FromOtherSide.MasterWriteSize) &
           (pHelpConnection^.InternalConnectionState =  FCS_RESET) then
        
          // now we check the interface CRC
          if pHelpConnection^.pSlavePathComp & pHelpConnection^.SlavePathCompLen then
            if aIFFSoEInfos[i].FromOtherSide.InterfaceCRC = (pHelpConnection^.pSlavePathComp$^HDINT + 1)^ then
              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                if pHelpSafety^.SafetyNbr = pHelpConnection^.OwnerSafetyNbr then
                  exit;
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;

              if pHelpSafety then
              
                if pHelpConnection^.Flags.IsMaster then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::FinalizePDOMapping) <This side is Master> ConnectionID: 0x{0}, SlaveAddress: 0x{1}, InterfaceCRC: 0x{2}", Value:=pHelpConnection^.ConnectionID, Value2:=pHelpConnection^.SlaveAddress, Value3 := aIFFSoEInfos[i].FromOtherSide.InterfaceCRC);
#endif
                  
                  // the module is master and the managing module slave
                  ModuleReadOffset  := pHelpConnection^.MasterReadOffset;
                  ModuleReadSize    := pHelpConnection^.MasterReadSize;
                  ModuleWriteOffset := pHelpConnection^.MasterWriteOffset;
                  ModuleWriteSize   := pHelpConnection^.MasterWriteSize;
                  MngReadOffset     := 0; // offsets in managing modules are always dynamic
                  MngReadSize       := pHelpConnection^.MasterWriteSize;
                  MngWriteOffset    := 0; // offsets in managing modules are always dynamic
                  MngWriteSize      := pHelpConnection^.MasterReadSize;
                else
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::FinalizePDOMapping) <This side is Slave> ConnectionID: 0x{0}, SlaveAddress: 0x{1}, InterfaceCRC: 0x{2}", Value:=aIFFSoEInfos[i].FromOtherSide.ConnectionID, Value2:=aIFFSoEInfos[i].FromOtherSide.SlaveAddress, Value3 := aIFFSoEInfos[i].FromOtherSide.InterfaceCRC);
#endif

                  // the module is slave and the managing module master
                  ModuleReadOffset  := pHelpConnection^.SlaveReadOffset;
                  ModuleReadSize    := pHelpConnection^.MasterWriteSize;
                  ModuleWriteOffset := pHelpConnection^.SlaveWriteOffset;
                  ModuleWriteSize   := pHelpConnection^.MasterReadSize;
                  MngReadOffset     := 0; // offsets in managing modules are always dynamic
                  MngReadSize       := pHelpConnection^.MasterReadSize;
                  MngWriteOffset    := 0; // offsets in managing modules are always dynamic
                  MngWriteSize      := pHelpConnection^.MasterWriteSize;
                end_if;


                MyPara.uiCmd := CMD_SM_ADD_OUTPUT_PDO;
                MyPara.aPara[0] := 1;  // mark as interface frame PDO
                pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

//                  pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
//                  pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
//                  pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
//                  pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
                
                if FSoECopyCmdNo < MAX_FSOE_CONNECTIONS then
                  aFSoECopyCmds[FSoECopyCmdNo].pSource  := MyResult.aData[0]$^USINT + ModuleReadOffset;
                  aFSoECopyCmds[FSoECopyCmdNo].pDest    := aIFFSoEInfos[i].pManaging^.pPDOWriteSpace + aIFFSoEInfos[i].pManaging^.PDOWriteMemUsed;
                  aFSoECopyCmds[FSoECopyCmdNo].Size     := ModuleReadSize;
                  FSoECopyCmdNo                         += 1;
                else
                  LogValue("@0493 (SafetyManager::FinalizePDOMapping) Maximum limit of allowed FSoE connections exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                end_if;


#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                _strcpy(dest:=#LogStr[0], src:="@DBG (SafetyManager::FinalizePDOMapping) Modul (");
                _strcat(dest:=#LogStr[0], src:=#pHelpSafety^.ObjName[0]);
                _strcat(dest:=#LogStr[0], src:=") => Mng (");
                _strcat(dest:=#LogStr[0], src:=#aIFFSoEInfos[i].pManaging^.ObjName[0]);
                _strcat(dest:=#LogStr[0], src:="): Rd Offset Modul: 0x{0}, Wr Offset Mng: 0x{1}, Größe: 0x{2}");
                LogValue(#LogStr[0], Value:=ModuleReadOffset, Value2:=aIFFSoEInfos[i].pManaging^.PDOWriteMemUsed, Value3:=ModuleReadSize);
#endif
                
                aIFFSoEInfos[i].pManaging^.PDOWriteMemUsed += MngWriteSize;

                MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_WR;
                MyPara.aPara[0] := aIFFSoEInfos[i].pManaging^.PDOWriteMemUsed$DINT;
                aIFFSoEInfos[i].pManaging^.pThis^.NewInst(#MyPara, #MyResult);

                MyPara.uiCmd := CMD_SM_ADD_INPUT_PDO;
                MyPara.aPara[0] := (aIFFSoEInfos[i].pManaging^.pPDOReadSpace + aIFFSoEInfos[i].pManaging^.PDOReadMemUsed)$DINT;
                MyPara.aPara[1] := 0;
                MyPara.aPara[2] := aIFFSoEInfos[i].FromOtherSide.SafetyNbr$DINT;
                MyPara.aPara[3] := ModuleWriteSize;
                MyPara.aPara[4] := 0;
                MyPara.aPara[5] := 0;
                MyPara.aPara[6] := 0;
                MyPara.aPara[7] := ModuleWriteOffset;
                MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                MyPara.aPara[9] := TRUE;

                pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

                if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
                  aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pHelpSafety;
                  aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
                  FSoEInputPDONo += 1;
                else
                  LogValue("@0490 (SafetyManager::FinalizePDOMapping) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                _strcpy(dest:=#LogStr[0], src:="@DBG (SafetyManager::FinalizePDOMapping) Mng (");
                _strcat(dest:=#LogStr[0], src:=#aIFFSoEInfos[i].pManaging^.ObjName[0]);
                _strcat(dest:=#LogStr[0], src:=") => Modul (");
                _strcat(dest:=#LogStr[0], src:=#pHelpSafety^.ObjName[0]);
                _strcat(dest:=#LogStr[0], src:="): Wr Offset Modul: 0x{0}, Rd Offset Mng: 0x{1}, Größe: 0x{2}");
                LogValue(#LogStr[0], Value:=ModuleWriteOffset, Value2:=aIFFSoEInfos[i].pManaging^.PDOReadMemUsed, Value3:=ModuleWriteSize);
#endif

                aIFFSoEInfos[i].pManaging^.PDOReadMemUsed += MngReadSize;

                MyPara.uiCmd := CMD_SM_PDO_MEM_USAGE_RD;
                MyPara.aPara[0] := aIFFSoEInfos[i].pManaging^.PDOReadMemUsed$DINT;
                aIFFSoEInfos[i].pManaging^.pThis^.NewInst(#MyPara, #MyResult);

                // mark the connection as established
                pHelpConnection^.InternalConnectionState := FCS_CONNECTION;
                               
                // we found something for this connection => continue with the next
                exit;
              else
                LogValue("@0495 (SafetyManager::FinalizePDOMapping) Could not find safety module with safety number 0x{0} for FSoE interface connection", Value:=pHelpConnection^.OwnerSafetyNbr);
              end_if;
            end_if;
          end_if;
        end_if;
        pHelpConnection := pHelpConnection^.pNextSorted;
      end_while;
    end_for; 
  end_if;

END_FUNCTION


FUNCTION SafetyManager::AddInternalIFFSoE
  VAR_INPUT
    pMasterConnection 	: ^t_FSoEConnectionDetails;
    pMasterConfig 	: ^t_CfgListElem;
    pSlaveConnection 	: ^t_FSoEConnectionDetails;
    pSlaveConfig 	: ^t_CfgListElem;
  END_VAR
  VAR
    i             : USINT;
    InterfaceCRC  : HDINT;
    pHelpSafety   : ^t_SafetyComponent;
    pMasterSafety : ^t_SafetyComponent;
    pSlaveSafety  : ^t_SafetyComponent;
    MyPara 	      : CmdStruct;
    MyResult	    : results;
    pPDOReadData  : pVoid;
    AddToList     : BOOL;
  END_VAR

  if pMasterConnection^.pSlavePathComp then
    InterfaceCRC := (pMasterConnection^.pSlavePathComp$^HDINT + 1)^;
  else
    // no interface CRC available
    return;
  end_if;
  
  AddToList := TRUE;
  if LocalIFNo then
    // check if the interfaceCRCs are already registered
    for i := 0 to (LocalIFNo-1) do
      if (aLocalIFs[i].InterfaceCRC = InterfaceCRC) & (aLocalIFs[i].MasterSafetyNbr = pMasterConfig^.SafetyNbr) then
        // already in the list => leave to prevent adding a second time
        AddToList := FALSE;
        return;
      end_if;
    end_for;
  end_if;

  pSlaveSafety := NIL;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
  
    if pHelpSafety^.SafetyNbr = pSlaveConfig^.SafetyNbr then
      pSlaveSafety := pHelpSafety;
      exit;
    end_if;

    pHelpSafety := pHelpSafety^.pNext;
  end_while;

  if pSlaveSafety & AddToList then
    if LocalIFNo < MAX_INTERNAL_IF_MODULES then
      aLocalIFs[LocalIFNo].InterfaceCRC     := InterfaceCRC;
      aLocalIFs[LocalIFNo].pSafetyModule    := pSlaveSafety;
       aLocalIFs[LocalIFNo].MasterSafetyNbr := pMasterConfig^.SafetyNbr;
      LocalIFNo                           += 1;
    else
      LogValue("@0496 (SafetyManager::ShareConnectionInfos) Maximum limit of allowed local FSoE connections exceeded (0x{0})", Value:=MAX_INTERNAL_IF_MODULES);
    end_if;
  end_if;


//*****************************************************************************
//** TELL THE MODULES WHAT DATA TO COPY IN REALTIME                          **
//*****************************************************************************

  pMasterSafety  := NIL;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
  
    if pHelpSafety^.SafetyNbr = pMasterConfig^.SafetyNbr then
      pMasterSafety := pHelpSafety;
      exit;
    end_if;

    pHelpSafety := pHelpSafety^.pNext;
  end_while;

  // if we found the safety modules who are keeping the configs => tell them what to copy
  if pMasterSafety & pSlaveSafety then
    // check for equal sizes
    if (pMasterConnection^.MasterReadSize <> pSlaveConnection^.MasterReadSize) |
       (pMasterConnection^.MasterWriteSize <> pSlaveConnection^.MasterWriteSize) then
      LogValue("@048E (SafetyManager::ShareConnectionInfos) Error! The sizes of master and slave connection do not match for Slave Address 0x{0} and ConnectionID 0x{1}!", Value:=pMasterConnection^.SlaveAddress, Value2:=pMasterConnection^.ConnectionID);
      return;
    end_if;
    
    // we start with the safe outputs: get the data pointer from the master and give it to the slave
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@048F (SafetyManager::ShareConnectionInfos) Invalid return value when asking the master safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
    LogValue("@DBG (SafetyManager::AddInternalIFFSoE) ConnectionID: {0}, SlaveAddress: {1}, InterfaceCRC: {2}", Value:=pMasterConnection^.ConnectionID, Value2:=pMasterConnection^.SlaveAddress, Value3 := InterfaceCRC);
    LogValue("@DBG (SafetyManager::AddInternalIFFSoE) MasterSafetyNbr: {0}, SlaveSafetyNbr: {1}", Value:=pMasterConnection^.OwnerSafetyNbr, Value2:=pSlaveConnection^.OwnerSafetyNbr);
    LogValue("@DBG (SafetyManager::AddInternalIFFSoE) MasterReadOffset: {0}, MasterReadSize: {1}, SlaveWriteOffset: {2}", Value:=pMasterConnection^.MasterReadOffset, Value2:=pMasterConnection^.MasterReadSize, Value3 := pSlaveConnection^.SlaveWriteOffset);
#endif

    MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
    MyPara.aPara[0] := pPDOReadData$DINT + pMasterConnection^.MasterReadOffset;
    MyPara.aPara[2] := pMasterConnection^.OwnerSafetyNbr$DINT;
    MyPara.aPara[3] := pMasterConnection^.MasterReadSize;
    MyPara.aPara[4] := pMasterSafety^.InfoBits.IsOptional;
    MyPara.aPara[7] := pSlaveConnection^.SlaveWriteOffset;
    MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
    MyPara.aPara[9] := TRUE;
    pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
      aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pSlaveSafety;
      aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
      FSoEInputPDONo += 1;
    else
      LogValue("@0490 (SafetyManager::AddInternalIFFSoE) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
    end_if;

    // safe inputs: get the data pointer from the slave and give it to the master
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@0491 (SafetyManager::AddInternalIFFSoE) Invalid return value when asking the slave safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
    LogValue("@DBG (SafetyManager::AddInternalIFFSoE) SlaveReadOffset: {0}, SlaveReadSize: {1}, MasterWriteOffset: {2}", Value:=pSlaveConnection^.SlaveReadOffset, Value2:=pMasterConnection^.MasterWriteSize, Value3 := pMasterConnection^.MasterWriteOffset);
#endif

    MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
    MyPara.aPara[0] := pPDOReadData$DINT + pSlaveConnection^.SlaveReadOffset;
    MyPara.aPara[2] := pSlaveConnection^.OwnerSafetyNbr$DINT;
    MyPara.aPara[3] := pMasterConnection^.MasterWriteSize;
    MyPara.aPara[4] := pSlaveSafety^.InfoBits.IsOptional;
    MyPara.aPara[7] := pMasterConnection^.MasterWriteOffset;
    MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
    MyPara.aPara[9] := TRUE;
    pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
      aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pMasterSafety;
      aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
      FSoEInputPDONo += 1;
    else
      LogValue("@0490 (SafetyManager::AddInternalIFFSoE) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
    end_if;
    
    pSlaveConnection^.InternalConnectionState   := FCS_CONNECTION;
    
    // for easier debugging we take the values to the manager-connection as well (only for debugging to have the correct values in the connections)
    pMasterConnection^.InternalConnectionState  := FCS_CONNECTION;
    pMasterConnection^.SlaveReadOffset          := pSlaveConnection^.SlaveReadOffset;
    pMasterConnection^.SlaveWriteOffset         := pSlaveConnection^.SlaveWriteOffset;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::CheckOptional
  VAR
    pHelpConfig     : ^t_CfgListElem;
    pHelpConfig2    : ^t_CfgListElem;
    pHelpSlaveCfg   : ^t_SlaveListElem;
    pHelpModuleCfg  : ^t_ModuleListElem;
    FoundIt         : BOOL;
  END_VAR

  // check all available configs
  pHelpConfig := pConfigs;
  while pHelpConfig do
  
    // search in module configs
    pHelpModuleCfg := pHelpConfig^.pFirstModuleCfg;
    while pHelpModuleCfg do
      if pHelpModuleCfg^.ModuleInfo.Info.IsOptional = FALSE then
        // if the slave config is not optional => check if we got the config
        FoundIt := FALSE;
        pHelpConfig2 := pConfigs;
        while pHelpConfig2 & (FoundIt = FALSE) do
          if (pHelpConfig2^.pFirstModuleCfg^.ModuleInfo.PathLen = pHelpModuleCfg^.ModuleInfo.PathLen) & 
             (_memcmp(ptr1:=pHelpConfig2^.pFirstModuleCfg^.ModuleInfo.pPath, ptr2:=pHelpModuleCfg^.ModuleInfo.pPath, cntr:=pHelpModuleCfg^.ModuleInfo.PathLen) = 0) then
            
            FoundIt := TRUE;
            exit;
          end_if;          
                  
          pHelpConfig2 := pHelpConfig2^.pNext;
        end_while;
        
        if FoundIt = FALSE then
          // non-optional config not available
          return;
        end_if;
      end_if;
    
      pHelpModuleCfg := pHelpModuleCfg^.pNext;      
    end_while;
    
    // search in slave configs
    pHelpSlaveCfg := pHelpConfig^.pFirstSlaveCfg;
    while pHelpSlaveCfg do
      if pHelpSlaveCfg^.SlaveInfo.Info.IsOptional = FALSE then
        // if the slave config is not optional => check if we got the config
        FoundIt := FALSE;
        pHelpConfig2 := pConfigs;
        while pHelpConfig2 do
          if pHelpConfig2^.pFirstModuleCfg & (pHelpConfig2^.pFirstModuleCfg^.ModuleInfo.PathLen = pHelpSlaveCfg^.SlaveInfo.PathLen) & 
             (_memcmp(ptr1:=pHelpConfig2^.pFirstModuleCfg^.ModuleInfo.pPath, ptr2:=pHelpSlaveCfg^.SlaveInfo.pPath, cntr:=pHelpSlaveCfg^.SlaveInfo.PathLen) = 0) then
            
            FoundIt := TRUE;
            exit;
          end_if;          
          
          pHelpConfig2 := pHelpConfig2^.pNext;
        end_while;
        
        if FoundIt = FALSE then
          // non-optional config not available
          return;
        end_if;
      end_if;
    
      pHelpSlaveCfg := pHelpSlaveCfg^.pNext;      
    end_while;
  
    pHelpConfig := pHelpConfig^.pNext;
  end_while;
  
  // if nothing non-optional is missing => we're good to go (if something would be missing the method would have been left at this position)
  b_MandatoryStuffAvailable := TRUE;
  
END_FUNCTION


FUNCTION SafetyManager::UpdateConnectionInLocalCfg
  VAR_INPUT
    SafetyNbr 	: HDINT;
    ConnectionNo 	: UDINT;
    pFirstConnection 	: ^t_FSoEConnectionDetails;
  END_VAR
  VAR
    pHelpCfg          : ^t_LocalCfgListElem;
    pHelpConnection   : ^t_FSoEConnectionDetails;
    pDataHelp         : ^void;
    i                 : UDINT;
    TmpPathLen        : UINT;
    TempCfgNo         : USINT;
    TempConnectionNo  : UINT;
    TempModuleInfo    : t_ModuleCfgInfo;
  END_VAR

(*
    Memory structure:
      4 Byte: SafetyNumber of CPU (is already there from initialisation in InitSafeModule of the safe CPU)
      1 Byte: Number of module configurations which are following
      for every module configuration (first is always the CPU itself)
        1 Byte: Info
        4 Byte: Interface Frame Source SafetyNumber (0 if it is no source for interface frames)
        4 Byte: Interface Frame CRC
        1 Byte: Number of destination inferface frame CRCs
        x Byte: Destination interface frame CRCs (number * sizeof(HDINT)
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
        1 Byte: PDO out length for this module
      1 Byte: Number of pdo configurations which are following
      for every PDO configuration
        1 Byte: PDO length
        1 Byte: Interface frame info
        4 Byte: Interface CRC
        2 Byte: Length of source hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of source
        2 Byte: Length of destination hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath of destination       
      1 Byte: Number of slave configurations which are following
      for every slave configuration
        1 Byte: Info
        4 Byte: InterfaceCRC
        2 Byte: Length of hardware path
        x Byte (depending on length in the 2 byte before): Hardwarepath
        2 Byte: Length of compressed hardware path
        x Byte (depending on length in the 2 byte before): Compressed Hardwarepath
      
      if IsFSoEConfig = TRUE
      2 Byte: Number of connection infos which are following
      for every FSoE connection info
        2 Byte: ConnectionID
        1 Byte: ConnectionState
        1 Byte: Flags // Actually, type is 4 bytes in size but for compatibility reasons it is still used as 1 byte. As a result, bits 9-32 are currently not used.
        4 Byte: Master safety number

only if feature available:        
        2 Byte: Transmission time in ms
        
        2 Byte: Offset where to read from master
        2 Byte: Size of data to read from master (and write to slave)
        2 Byte: Offset where to write to master
        2 Byte: Size of data to write to master (and read from slave)
        2 Byte: SlaveAddress
        2 Byte: Length of slave hardware path
        x Byte (depending on length in the 2 byte before): Slave hardware path
        2 Byte: Length of compressed slave hardware path
        x Byte (depending on length in the 2 byte before): Compressed slave hardware path
        2 Byte: Offset where to read from slave
        2 Byte: Offset where to write to slave
      
*)

  // find the local config via safety number
  pHelpCfg := pFirstLocalCPUCfg;
  while pHelpCfg do
    if pHelpCfg^.SafetyNbr = SafetyNbr then
      // found it
      exit;
    end_if;
    
    pHelpCfg := pHelpCfg^.pNext;
  end_while;

  // found a config
  if pHelpCfg & pHelpCfg^.IsFSoECfg then

    // skip everything in the config to get to the FSoE connections that have to be updated
    
    pDataHelp := pHelpCfg^.pConfig;
    
    // safety number of the safety cpu
    pDataHelp += sizeof(HDINT);
    
    // number of module configurations which follow now
    TempCfgNo := pDataHelp^$USINT;
    pDataHelp += sizeof(USINT);
    
    // module configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        
        // info
        TempModuleInfo := pDataHelp^$BSINT;
        pDataHelp += sizeof(BSINT);
        
        // safety number of interface frame source
        pDataHelp += sizeof(HDINT);

        // interface CRC of interface frame
        pDataHelp += sizeof(HDINT);
        
        // interface frame destination CRCs
        if TempModuleInfo.IsInterfaceDest then
          // number of CRCs
          TmpPathLen := pDataHelp^$USINT; // save some time with this local variable
          pDataHelp += sizeof(USINT);
          
          // CRCs
          pDataHelp += TmpPathLen * sizeof(HDINT);
        end_if;
        
        // length of hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
        
        // hardwarepath
        pDataHelp += TmpPathLen;

        // length of hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
              
        // compressed hardwarepath
        pDataHelp += TmpPathLen;
        
        // PDO out length for this module (to assign the length to the pdo configurations)
        pDataHelp += sizeof(USINT);            
      end_for;
    end_if;

    // number of Pdo configurations which follow now
    TempCfgNo := pDataHelp^$USINT;
    pDataHelp += sizeof(USINT);
    
    // Pdo configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        // pdo length
        pDataHelp += sizeof(USINT);
        
        // PDO Info bits
        pDataHelp += sizeof(BSINT);

        // Interface CRC        
        pDataHelp += sizeof(HDINT);
        
        // length of source hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
               
        // source hardwarepath
        pDataHelp += TmpPathLen;

        // length of destination hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
        
        // destination hardwarepath
        pDataHelp += TmpPathLen;
      end_for;
    end_if;

    // number of slave configurations which follow now
    TempCfgNo := pDataHelp^$USINT;
    pDataHelp += sizeof(USINT);
    
    // slave configurations
    if TempCfgNo then
      for i := 0 to (TempCfgNo-1) do
        // info
        pDataHelp += sizeof(BSINT);
                   
        // InterfaceCRC
        pDataHelp += sizeof(HDINT);

        // length of hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
               
        // hardwarepath
        pDataHelp += TmpPathLen;

        // length of hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
        
        // compressed hardwarepath
        pDataHelp += TmpPathLen;            
      end_for;
    end_if;

    // number of connection details which follow now
    TempConnectionNo := pDataHelp^$UINT;
    pDataHelp += sizeof(UINT);

    // if we got connections and they match the update info
    if TempConnectionNo & (TempConnectionNo = ConnectionNo) then     
      pHelpConnection := pFirstConnection;

      for i := 0 to (TempConnectionNo-1) do

        // connection ID
        pDataHelp += sizeof(UINT);

        // update connection state
        pDataHelp^$t_FSoEConnectionState := pHelpConnection^.ConnectionState;
                      
        pDataHelp += sizeof(USINT);

        // flags
        
        // Actually, type is 4 bytes in size but for compatibility reasons it is still used as 1 byte. As a result, bits 9-32 are currently not used.
        if bdFeatureEnable.FSoEFlags_4ByteSize then
          pDataHelp += sizeof(t_FSoEFlags);
        else
          pDataHelp += sizeof(USINT);
        end_if;

        // master safety number
        pDataHelp += sizeof(HDINT);

        // the feature is not available in older safety module classes, so we have to do this for compatibility reasons
        if bdFeatureEnable.TransmissionTimeInConfig then
          // transmission time in ms
          pDataHelp += sizeof(UINT);
        end_if;

        // offset where to read from master
        pDataHelp += sizeof(UINT);

        // size of data to read from master (and write to slave)
        pDataHelp += sizeof(UINT);
        
        // offset where to write to master
        pDataHelp += sizeof(UINT);

        // size of data to write to master (and read from slave)
        pDataHelp += sizeof(UINT);

        // update slave address
        pDataHelp^$UINT := pHelpConnection^.SlaveAddress;
        pDataHelp += sizeof(UINT);

        // length of uncompressed slave hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);
        
        if pHelpConnection^.Flags.IsMaster then          
          // save hardwarepath
          pDataHelp += TmpPathLen;           
        end_if;

        // length of compressed slave hardwarepath
        TmpPathLen := pDataHelp^$UINT; // save some time with this local variable
        pDataHelp += sizeof(UINT);

        if pHelpConnection^.Flags.IsMaster then          
          // compressed hardwarepath
          pDataHelp += TmpPathLen;
        end_if;

        // offset where to read from master
        pDataHelp += sizeof(UINT);
        
        // offset where to write to master
        pDataHelp += sizeof(UINT);
        
        pHelpConnection += sizeof(t_FSoEConnectionDetails);
      end_for;
    else
      // no connections in config or not matching the update info
      LogValue(pString:="@0956 (SafetyManager::UpdateConnectionInLocalCfg) No connections in config or not matching the update info at safety configuration of module with SafetyNbr 0x{0}", Value:=SafetyNbr);
    end_if;
  else
    // couldn't find configuration of that safety module
    LogValue(pString:="@0957 (SafetyManager::UpdateConnectionInLocalCfg) Couldn't find safety configuration of module with SafetyNbr 0x{0}", Value:=SafetyNbr);
  end_if;

END_FUNCTION


FUNCTION SafetyManager::FSoEConInitialized
  VAR_INPUT
    SafetyNbr 	: HDINT;
    ConnectionNo 	: UDINT;
    pFirstConnection 	: ^t_FSoEConnectionDetails;
  END_VAR
  VAR
    i               : UDINT;
    pActConnection  : ^t_FSoEConnectionDetails;   
    pActConnLocal   : ^t_FSoEConnectionDetails;   
    pHelpConfig     : ^t_CfgListElem;
  END_VAR

  // search for the configuration of the safety component
  pHelpConfig := pConfigs;
  while pHelpConfig do
    if pHelpConfig^.SafetyNbr = SafetyNbr then
      // update the info in local config (linear memory), so it is still up to date after a new initialisation (ClearRoutingInfos)
      UpdateConnectionInLocalCfg(SafetyNbr, ConnectionNo, pFirstConnection);
    
      // found the module => now update the connection infos
      pActConnLocal  := pHelpConfig^.pFirstFSoEConnection;
      pActConnection := pFirstConnection;
      for i := 0 to (ConnectionNo-1) do
        // no valid local connection => leave
        if pActConnLocal = NIL then
          return;
        end_if;
        
        if pActConnLocal^.ConnectionState = FCS_NOT_INITIALIZED then
        
          // update slave address first because we are in background here and the data is also accessed in cyclic (connection state is checked first in cyclic)
          pActConnLocal^.SlaveAddress := pActConnection^.SlaveAddress;
          
          // now update the connection state
          pActConnLocal^.ConnectionState := pActConnection^.ConnectionState;
        end_if;
        
        pActConnLocal  := pActConnLocal^.pNext;
        pActConnection += sizeof(t_FSoEConnectionDetails);
      end_for;
      
      exit;
    end_if;
  
    pHelpConfig := pHelpConfig^.pNext;
  end_while;

  CheckLocalSlaveAddresses();

  if InitSSW = _InitFinished then
    // maybe we have some new informations here
    CreateFSoEPDOs();
  end_if;

END_FUNCTION


FUNCTION SafetyManager::CheckLocalSlaveAddresses
  VAR
    pActConnection  : ^t_FSoEConnectionDetails;   
    pHelpConfig     : ^t_CfgListElem;
  END_VAR

  // check if we got all slave addresses
  pHelpConfig := pConfigs;
  while pHelpConfig do
    pActConnection := pHelpConfig^.pFirstFSoEConnection;
    while pActConnection do
#ifdef SM_CHECK_CON_STATE_AND_SLAVE_ADDRESS    
      if (pActConnection^.ConnectionState = FCS_NOT_INITIALIZED) & (pActConnection^.SlaveAddress = 0) then     
#else
      if (pActConnection^.ConnectionState = FCS_NOT_INITIALIZED) then     
#endif
        // as soon as we find a not initialized connection => leave
        return;
      end_if;
      
      pActConnection := pActConnection^.pNext;
    end_while;
  
    pHelpConfig := pHelpConfig^.pNext;
  end_while;

  // if we got this far, we got all slave addresses
  b_AllLocalSlaveAddressesValid := TRUE;

END_FUNCTION


FUNCTION SafetyManager::AddUdpConnection
	VAR_INPUT
		IP 	: HDINT;
		IFCRC 	: HDINT;
		SafetyNumber 	: HDINT;
		ProducerList 	: BOOL;
		ppConnection 	: ^t_pUdpConnection;(* := NIL *)
		pInfoBits 	: ^t_UdpConInfoBits;(* := NIL *)
	END_VAR
	VAR_OUTPUT
		success 	: BOOL;
	END_VAR
  VAR
    i : UDINT;
    pActConnection : ^t_UdpConnection;
  END_VAR

  success := FALSE;

  // if we're consumer we add the connection to the producer list, because it's our producer
  if ProducerList then
    if sUdpVars.paConnectionsToProducers = NIL then
      bd_LogRtMessagesInCy.UdpNoMem4Connections := TRUE;
      return;
    end_if;
  
    // check if it already exists
    if sUdpVars.ProducerConnectionNo then
      pActConnection := #sUdpVars.paConnectionsToProducers^[0];
      for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
        // we only add a new connection if it doesn't exist yet
        if (pActConnection^.IP = IP) & (pActConnection^.IFCRC = IFCRC) then
          // already existing
          LogValue("@0499 (SafetyManager::AddUdpConnection) Found 2 equal producer connections (IP: 0x{0}, IF CRC: 0x{1}) which is not possible to handle", Value:=IP, Value2:=IFCRC);
          return;
        end_if;
        pActConnection += sizeof(t_UdpConnection);
      end_for;
    end_if;
    
    pActConnection := #sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo];
    
    pActConnection^.IP                             := IP;
    pActConnection^.IFCRC                          := IFCRC;
    pActConnection^.SafetyNbr                      := SafetyNumber;

    // reset some elements
    pActConnection^.SafetyNbrOtherSide             := 0;
    if pInfoBits then
      pActConnection^.InfoBits                     := pInfoBits^;    
    else
      pActConnection^.InfoBits                     := 0;    
    end_if;
    pActConnection^.WaitForResponse                := FALSE;
    pActConnection^.SendResponse                   := FALSE;
    pActConnection^.bSequenceCntReceived           := FALSE; //reset flag to allow sequence counter for new udp communication
    pActConnection^.ActReceiveData.usHeaderVersion := 0;
    pActConnection^.ActSendData.usHeaderVersion    := 0;
    pActConnection^.SentPackages                   := 0;
    pActConnection^.ReceivedPackages               := 0;
    pActConnection^.MaxResponseTime                := 0;
    pActConnection^.ActSendData.PdoLength          := 0;
    pActConnection^.ActSendData.SdoLength          := 0;
    pActConnection^.ActReceiveData.PdoLength       := 0;
    pActConnection^.ActReceiveData.SdoLength       := 0;  
    sUdpVars.ProducerConnectionNo                  += 1;
    success                                        := TRUE;
    
    if ppConnection then
      ppConnection^ := pActConnection;
    end_if;
  else
    if sUdpVars.paConnectionsToConsumers = NIL then
      bd_LogRtMessagesInCy.UdpNoMem4Connections   := TRUE;
      return;
    end_if;

    // check if it already exists
    if sUdpVars.ConsumerConnectionNo then
      pActConnection := #sUdpVars.paConnectionsToConsumers^[0];
      for i := 0 to (sUdpVars.ConsumerConnectionNo-1) do
        // we only add a new connection if it doesn't exist yet
        if (pActConnection^.IP = IP) & (pActConnection^.IFCRC = IFCRC) then
          // already existing
          LogValue("@049A (SafetyManager::AddUdpConnection) Found 2 equal consumer connections (IP: 0x{0}, IF CRC: 0x{1}) which is not possible to handle", Value:=IP, Value2:=IFCRC);
          return;
        end_if;
        pActConnection += sizeof(t_UdpConnection);
      end_for;
    end_if;
    
    pActConnection := #sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo];
    
    pActConnection^.IP                            := IP;
    pActConnection^.IFCRC                         := IFCRC;
    pActConnection^.SafetyNbr                     := SafetyNumber;

    // reset some elements
    pActConnection^.SafetyNbrOtherSide              := 0;
    pActConnection^.InfoBits                        := 0;
    pActConnection^.WaitForResponse                 := FALSE;
    pActConnection^.SendResponse                    := FALSE;
    pActConnection^.bSequenceCntReceived            := FALSE; //reset flag to allow sequence counter for new udp communication
    pActConnection^.ActReceiveData.usHeaderVersion  := 0;
    pActConnection^.ActSendData.usHeaderVersion     := 0;
    pActConnection^.SentPackages                    := 0;
    pActConnection^.ReceivedPackages                := 0;
    pActConnection^.MaxResponseTime                 := 0;
    pActConnection^.ActSendData.PdoLength           := 0;
    pActConnection^.ActSendData.SdoLength           := 0;
    pActConnection^.ActReceiveData.PdoLength        := 0;
    pActConnection^.ActReceiveData.SdoLength        := 0;

    sUdpVars.ConsumerConnectionNo                   += 1;
    success                                         := TRUE;
    
    if ppConnection then
      ppConnection^ := pActConnection;
    end_if;
  end_if;

  // whenever new connections are added, they need to be initialized
  sUdpVars.ConnectionsInitialized := FALSE;

END_FUNCTION


FUNCTION SafetyManager::UdpRequestScheduler
  VAR
    pActRequestEntry  : ^t_UdpConnection;
    pActRetryEntry    : ^t_UdpConnection;
    tmpretcode        : DINT;
    i                 : UDINT;
  END_VAR

  //point to actual request-entry
  pActRequestEntry := #sUdpVars.paConnectionsToProducers^[sUdpVars.RequestScheduler.ActRequestedIndex];

  //send a request every   
  if ((sUdpVars.RequestScheduler.CyCnt MOD sUdpVars.CalculatedRequestTime) = 0) then
    //calculate actual time diff from last request to this
    sUdpVars.ActTimeRequestScheduler := ops.tAbsolute - sUdpVars.LastRequestTimeScheduler;
    sUdpVars.LastRequestTimeScheduler := ops.tAbsolute;
        
    //set maximal time diff of request scheduler call.    
    if sUdpVars.MaxTimeRequestScheduler < sUdpVars.ActTimeRequestScheduler then
      sUdpVars.MaxTimeRequestScheduler := sUdpVars.ActTimeRequestScheduler;
      
      if sUdpVars.bSetFirstMaxRequestSchedulerTime = FALSE then      
        //set initial value for time measure of maximum of request scheduler
        sUdpVars.bSetFirstMaxRequestSchedulerTime := TRUE;
        sUdpVars.MaxTimeRequestScheduler          := 0;
      end_if;
    end_if;

    // search through the list if something has to be sent (check also for changes in the data)
    for i := 0 to sUdpVars.ProducerConnectionNo-1 do
      
      //only send a request if not waiting for a response and the IP is valid (on cleaning out the connections some may be set to 0)
      if (pActRequestEntry^.WaitForResponse = FALSE) & (pActRequestEntry^.IP <> 0) then

        //send request to mover of actual entry
        tmpretcode := SendUdpRequest(pActRequestEntry);
        if tmpretcode >= 0 then
          
          //if sending was successful we are waiting for a response
          pActRequestEntry^.WaitForResponse   := TRUE;
          pActRequestEntry^.TLastRequest      := ops.tAbsolute;
          pActRequestEntry^.RequestRetries    := 0;
        
        else 
          //unable to send request
          if bd_SingleLog.SendUdpReqFailed = FALSE then
            bd_SingleLog.SendUdpReqFailed := TRUE;
            LogError("@049B (SafetyManager::UdpRequestScheduler) [SL] Failed to send udp request");
          end_if;
        end_if;
      end_if;
      
      //increase actual index
      if sUdpVars.RequestScheduler.ActRequestedIndex < (sUdpVars.ProducerConnectionNo-1) then
        sUdpVars.RequestScheduler.ActRequestedIndex += 1;
      else
        // check the time of the cycle
        UdpSetTimeMeasurement();
        
        //when reaching last entry -> reset index
        sUdpVars.RequestScheduler.ActRequestedIndex := 0;
      end_if;

      pActRequestEntry := #sUdpVars.paConnectionsToProducers^[sUdpVars.RequestScheduler.ActRequestedIndex];
    end_for;

  // between the normal requests we do the retries
  elsif ((sUdpVars.RequestScheduler.CyCnt MOD UDP_RETRY_TIME) = 0) then
    
    pActRetryEntry := #sUdpVars.paConnectionsToProducers^[0];
    
    // check if we have to send a retry
    for i := 0 to sUdpVars.ProducerConnectionNo-1 do
      
      // look if we actual wait for a response to send retries and check if we didn't receive data in time
      if (pActRetryEntry^.WaitForResponse = TRUE) & ((ops.tAbsolute - pActRetryEntry^.TLastRequest) > sUdpVars.CalculatedRequestTimeout) then                              
        //check for connection timeout
        if (ops.tAbsolute-pActRetryEntry^.TLastValidRequest) > (UDP_DEFAULT_TRANSMISSION_TIME*2) then
          //reset header version if connection timeout is reached.
          pActRetryEntry^.ActReceiveData.usHeaderVersion := 0;
          pActRetryEntry^.ActSendData.usHeaderVersion    := 0;
          pActRetryEntry^.TLastValidRequest := ops.tAbsolute;          
        end_if;
        
        if pActRetryEntry^.RequestRetries >= UDP_MAX_RETRIES then
        
          pActRetryEntry^.MaxRetryCnt     += 1;
          pActRetryEntry^.WaitForResponse := FALSE;
          
          //check for new header version with extended sequence number (4Byte)
          if pActRetryEntry^.ActSendData.usHeaderVersion >= UDP_HEADER_VERSION_V2 then
            // we did not get our response => increase the sequence number for the next attempt
            pActRetryEntry^.ActSendData.udSequenceNumber += 1;
          else
            // we did not get our response => increase the sequence number for the next attempt
            pActRetryEntry^.ActSendData.usSequenceNumber += 1;
            pActRetryEntry^.ActSendData.udSequenceNumber := pActRetryEntry^.ActSendData.usSequenceNumber;
          end_if;

        // only if the IP is valid
        elsif (pActRetryEntry^.IP <> 0) then

          //send retry to actual cpu
          tmpretcode := SendUdpRequest(pActRetryEntry);
          if tmpretcode >= 0 then
            pActRetryEntry^.TLastRequest   := ops.tAbsolute;
            pActRetryEntry^.RequestRetries += 1;
            exit;
          else
            //unable to send retry
            if bd_SingleLog.SendUdpReqRetryFailed = FALSE then
              bd_SingleLog.SendUdpReqRetryFailed := TRUE;
              LogError("@049D (SafetyManager::UdpTimeScheduler) [SL] Failed to send udp retry");
            end_if;
          end_if;
        end_if;
        
      end_if;
      
      pActRetryEntry += sizeof(t_UdpConnection);
      
    end_for;
  
  end_if;

  sUdpVars.RequestScheduler.CyCnt += 1;

END_FUNCTION


FUNCTION SafetyManager::UdpSetTimeMeasurement

  if sUdpVars.RequestScheduler.LastTime then
    sUdpVars.RequestScheduler.ActRuntime := ops.tAbsolute - sUdpVars.RequestScheduler.LastTime;
    if sUdpVars.RequestScheduler.ActRuntime > sUdpVars.RequestScheduler.MaxRuntime then
      sUdpVars.RequestScheduler.MaxRuntime := sUdpVars.RequestScheduler.ActRuntime;
    end_if;

    if sUdpVars.RequestScheduler.ActRuntime < sUdpVars.RequestScheduler.MinRuntime then
      sUdpVars.RequestScheduler.MinRuntime := sUdpVars.RequestScheduler.ActRuntime;
    end_if;
  else
    sUdpVars.RequestScheduler.MinRuntime := 9999;
  end_if;           
  sUdpVars.RequestScheduler.LastTime := ops.tAbsolute;

END_FUNCTION


FUNCTION SafetyManager::SendUdpRequest
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    udActSendLength : UDINT;
  END_VAR
  
  sUdpVars.pTempSendBufferCy^.Header.Command   := t_UdpCommand::UdpCmd_DataReq;
  // add supported header version to state (is set if we got response from other side)
  sUdpVars.pTempSendBufferCy^.Header.State     := t_UdpPacketHeaderV2.State.IsFromConsumer + pConnection^.ActSendData.usHeaderVersion;
  sUdpVars.pTempSendBufferCy^.Header.IFCRC     := pConnection^.IFCRC;
  sUdpVars.pTempSendBufferCy^.Header.SafetyNbr := pConnection^.SafetyNbr;
  
  UdpSendData.SectionStart();
  
  //check for new header version with extended sequence number (4Byte)
  if pConnection^.ActSendData.usHeaderVersion >= UDP_HEADER_VERSION_V2 then
  
    sUdpVars.pTempSendBufferCy^.Header.SequenceNumber := pConnection^.ActSendData.udSequenceNumber;
    
    udActSendLength := sizeof(t_UdpPacketHeaderV2);
    
    if pConnection^.ActSendData.PdoLength then
      sUdpVars.pTempSendBufferCy^.Header.PdoDataLength  := pConnection^.ActSendData.PdoLength;      
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^.Data, ptr2:=#pConnection^.ActSendData.PdoData, cntr:=pConnection^.ActSendData.PdoLength);
      udActSendLength += pConnection^.ActSendData.PdoLength;
    else
      sUdpVars.pTempSendBufferCy^.Header.PdoDataLength  := 0;
    end_if;

    if pConnection^.ActSendData.SdoLength then
      sUdpVars.pTempSendBufferCy^.Header.SdoDataLength  := pConnection^.ActSendData.SdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^.Data + pConnection^.ActSendData.PdoLength, ptr2:=#pConnection^.ActSendData.SdoData, cntr:=pConnection^.ActSendData.SdoLength);
      udActSendLength += pConnection^.ActSendData.SdoLength;
    else
      sUdpVars.pTempSendBufferCy^.Header.SdoDataLength  := 0;
    end_if;
    
  else
    //use old headerversion with 1byte Sequence Counter  
    sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SequenceNumber := pConnection^.ActSendData.usSequenceNumber;
    
    udActSendLength := sizeof(t_UdpPacketHeaderV1);
    
    if pConnection^.ActSendData.PdoLength then
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.PdoDataLength  := pConnection^.ActSendData.PdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Data, ptr2:=#pConnection^.ActSendData.PdoData, cntr:=pConnection^.ActSendData.PdoLength);
      udActSendLength += pConnection^.ActSendData.PdoLength;
    else
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.PdoDataLength  := 0;
    end_if;

    if pConnection^.ActSendData.SdoLength then
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SdoDataLength  := pConnection^.ActSendData.SdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Data + pConnection^.ActSendData.PdoLength, ptr2:=#pConnection^.ActSendData.SdoData, cntr:=pConnection^.ActSendData.SdoLength);
      udActSendLength += pConnection^.ActSendData.SdoLength;
    else
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SdoDataLength  := 0;
    end_if;      
  
  end_if;
  
  UdpSendData.SectionStop(); 
  
  retcode := SendUDPData(sUdpVars.pTempSendBufferCy, udActSendLength, pConnection^.IP); 

  
  sUdpVars.SendCnt += 1;
  
  if retcode < 0 then
    // error at sending
    sUdpVars.LastSendError := retcode;
    sUdpVars.SendErrorCnt += 1;
  else
    // only send this once
    pConnection^.ActSendData.SdoLength := 0;
    
    pConnection^.SentPackages += 1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::ReceiveUdpData
  VAR_INPUT
    pData 	: ^void;
    udLength 	: UDINT;
    udIPAddr 	: HDINT;
    udPort 	: UDINT;
  END_VAR
  VAR
    i               : UDINT;
    FoundConnection : BOOL;
    pPacketHeader   : ^t_UdpPacketHeaderV2;
    pActConnection  : ^t_UdpConnection;
    pHelpCfg        : ^t_CfgListElem;
    pHelpModule     : ^t_ModuleListElem;
    retcode         : DINT;
  END_VAR

  pPacketHeader := pData$^t_UdpPacketHeaderV2;

  // only take the data if we're not clearing the routing infos at the very moment
  if InitSSW > _ClearRoutingInfos then 
    
    // increase receive counter
    sUdpVars.ReceiveCnt += 1;
    
    // check for longest time between data packages
    if sUdpVars.LastReceivedTimeStamp then
      sUdpVars.ActTimeBetweenReceiveData := ops.tAbsolute - sUdpVars.LastReceivedTimeStamp;
      if sUdpVars.ActTimeBetweenReceiveData > sUdpVars.MaxTimeBetweenReceiveData then
        sUdpVars.MaxTimeBetweenReceiveData := sUdpVars.ActTimeBetweenReceiveData;
      end_if;
    end_if;
    sUdpVars.LastReceivedTimeStamp := ops.tAbsolute;
  
    
    if (pPacketHeader^.State AND (t_UdpFrameState.IsFromConsumer OR t_UdpFrameState.IsFromProducer)) = t_UdpFrameState.IsFromConsumer then
      // it's a request from a consumer => check our list of consumers
      
      // the only valid command here is data request
      if pPacketHeader^.Command = t_UdpCommand::UdpCmd_DataReq then
        FoundConnection := FALSE;
        if sUdpVars.ConsumerConnectionNo then
          pActConnection := #sUdpVars.paConnectionsToConsumers^[0];
          // check if it's one of our existing connections
          for i := 0 to sUdpVars.ConsumerConnectionNo-1 do
            if (pActConnection^.IP = udIPAddr) & (pActConnection^.SafetyNbrOtherSide = pPacketHeader^.SafetyNbr) then
              // check if it's the same IF CRC
              if (pActConnection^.IFCRC = pPacketHeader^.IFCRC) then
                FoundConnection := TRUE;
                exit;
              end_if;
            end_if;
            
            pActConnection += sizeof(t_UdpConnection);
          end_for;
        end_if;
        
        if FoundConnection then
          //check for new header version with extended sequence number (4Byte)
          if pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK >= UDP_HEADER_VERSION_V2 then            
            // pActConnection points to the right connection
            HandleUdpRequestV2(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV2, udLength:=udLength);
          else
            // pActConnection points to the right connection
            HandleUdpRequest(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV1, udLength:=udLength);
          end_if;  
          //set received header version
          pActConnection^.ActReceiveData.usHeaderVersion := (pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK);                  
          //we can set the headerversion from receive data also for the send data
          pActConnection^.ActSendData.usHeaderVersion    := pActConnection^.ActReceiveData.usHeaderVersion;
          //if we got header version info with version higher our own supported header version set the own supported version
          if pActConnection^.ActSendData.usHeaderVersion > UDP_HEADER_ACTVERSION then
            pActConnection^.ActSendData.usHeaderVersion := UDP_HEADER_ACTVERSION;
          end_if;
        else
          // search for the safety number of the producer
          pHelpCfg := pConfigs;
          while pHelpCfg do
            pHelpModule := pHelpCfg^.pFirstModuleCfg;
            while pHelpModule do
              if pHelpModule^.ModuleInfo.InterfaceCRC = pPacketHeader^.IFCRC then
                // add the connection (we're producer and we the consumer wants something from us)
                if AddUdpConnection(IP:=udIPAddr, IFCRC:=pPacketHeader^.IFCRC, SafetyNumber:=pHelpCfg^.SafetyNbr, ProducerList:=FALSE, #pActConnection) then
                  //check for new header version with extended sequence number (4Byte)
                  if pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK >= UDP_HEADER_VERSION_V2 then            
                    // pActConnection points to the right connection
                    HandleUdpRequestV2(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV2, udLength:=udLength);
                  else
                    // pActConnection points to the right connection
                    HandleUdpRequest(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV1, udLength:=udLength);
                  end_if;
                  //set received header version
                  pActConnection^.ActReceiveData.usHeaderVersion := (pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK);                  
                  //we can set the headerversion from receive data also for the send data
                  pActConnection^.ActSendData.usHeaderVersion    := pActConnection^.ActReceiveData.usHeaderVersion;
                  //if we got header version info with version higher our own supported header version set the own supported version
                  if pActConnection^.ActSendData.usHeaderVersion > UDP_HEADER_ACTVERSION then
                    pActConnection^.ActSendData.usHeaderVersion := UDP_HEADER_ACTVERSION;
                  end_if;
                end_if;
                return;
              end_if;

              pHelpModule := pHelpModule^.pNext;
            end_while;     

            pHelpCfg := pHelpCfg^.pNext;
          end_while;
          sUdpVars.UnhandledReceivePacketCnt += 1;
        end_if;

      else
        // invalid command
        bd_LogRtMessagesInCy.UdpReceivedInvalidCommand := TRUE;
        sUdpVars.UnhandledReceivePacketCnt += 1;
      end_if;  
    elsif (pPacketHeader^.State AND (t_UdpFrameState.IsFromConsumer OR t_UdpFrameState.IsFromProducer)) = t_UdpFrameState.IsFromProducer then
      // it's a response from a producer => check our list of producers

      // the only valid command here is data response
      if pPacketHeader^.Command = t_UdpCommand::UdpCmd_DataRsp then
        
        if sUdpVars.ProducerConnectionNo then
          FoundConnection := FALSE;
          pActConnection := #sUdpVars.paConnectionsToProducers^[0];
          // check if it's one of our existing connections
          for i := 0 to sUdpVars.ProducerConnectionNo-1 do
            if (pActConnection^.IP = udIPAddr) & (pActConnection^.IFCRC = pPacketHeader^.IFCRC) then
              FoundConnection := TRUE;
              exit;
            end_if;
            
            pActConnection += sizeof(t_UdpConnection);
          end_for;

          if FoundConnection then
            //check for new version of udp header
            if pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK >= UDP_HEADER_VERSION_V2 then              
              // pActConnection points to the right connection
              HandleUdpResponseV2(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV2, udLength:=udLength);
            else
              // pActConnection points to the right connection
              HandleUdpResponse(pConnection:=pActConnection, pPacket:=pData$^t_UdpPacketV1, udLength:=udLength);
            end_if;
            pActConnection^.ActReceiveData.usHeaderVersion := pData^$t_UdpPacketV2.Header.State and UDP_HEADER_VERSION_MASK;                 
            //we can set the headerversion from receive data also for the send data
            pActConnection^.ActSendData.usHeaderVersion    := pActConnection^.ActReceiveData.usHeaderVersion;
            //if we got header version info with version higher our own supported header version set the own supported version
            if pActConnection^.ActSendData.usHeaderVersion > UDP_HEADER_ACTVERSION then
              pActConnection^.ActSendData.usHeaderVersion := UDP_HEADER_ACTVERSION;
            end_if;
          else
            // ignore it, maybe the safety project changed
            sUdpVars.UnhandledReceivePacketCnt += 1;
          end_if;

        else
          // ignore it, maybe the safety project changed
          sUdpVars.UnhandledReceivePacketCnt += 1;
        end_if;
        
      else
        // invalid command
        bd_LogRtMessagesInCy.UdpReceivedInvalidCommand := TRUE;
        sUdpVars.UnhandledReceivePacketCnt += 1;
      end_if;  
    else
      // the only valid command here is the reinitialize or the confirmation for reinitialize
      if pPacketHeader^.Command = t_UdpCommand::UdpCmd_ReInitialize then
        // confirm the command and reinitialize
        sUdpVars.pTempSendBufferIRQ^.Header.Command            := t_UdpCommand::UdpCmd_ReInitConfirm;
        sUdpVars.pTempSendBufferIRQ^.Header.State              := t_UdpPacketHeaderV2.State.IsFromConsumer OR t_UdpPacketHeaderV2.State.IsFromProducer; // invalid state for normal data

        // only send command and state
        retcode := SendUDPData(sUdpVars.pTempSendBufferIRQ, sizeof(t_UdpPacketHeaderV2.Command) + sizeof(t_UdpPacketHeaderV2.State), udIPAddr); 

        if retcode < 0 then
          sUdpVars.LastSendError := retcode;
          sUdpVars.SendErrorCnt += 1;
        else
          // remember the sender
          sUdpVars.ReInitSenderIP := udIPAddr;
          
          // reset module sync no (no more modules synchron)
          ModuleSyncNo := 0;

          // if sending the answer succeeded => reinitialize ourself
          ClassState := 0;

          // wait until we're synchron again
          InitSSW := _ClearRoutingInfos;

        end_if;
        
        b_AllLocalSlaveAddressesValid := FALSE;

      elsif pPacketHeader^.Command = t_UdpCommand::UdpCmd_ReInitConfirm then

        // we got this, finished with disconnecting this one
        if sUdpVars.ProducerConnectionNo then
          sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].WaitForResponse        := FALSE;
          sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].InfoBits.Disconnecting := FALSE;
        elsif sUdpVars.ConsumerConnectionNo then
          sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].WaitForResponse        := FALSE;
          sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].InfoBits.Disconnecting := FALSE;
          sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].bSequenceCntReceived   := FALSE; //reset flag to allow sequence counter for new udp communication
          sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].ActReceiveData.usHeaderVersion := 0;
          sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].ActSendData.usHeaderVersion    := 0;
        end_if;
      else
        // invalid state/command combination
        bd_LogRtMessagesInCy.UdpReceivedInvalidState := TRUE;
        sUdpVars.UnhandledReceivePacketCnt += 1;
      end_if;
    end_if;
  
  elsif pPacketHeader^.Command = t_UdpCommand::UdpCmd_ReInitialize then
    // before the clear config phase is finished, the only thing allowed is to answer on reinitialize requests (but without reinitializing again) or take the reinitconfirm packets
  
    // confirm the command and reinitialize
    sUdpVars.pTempSendBufferIRQ^.Header.Command            := t_UdpCommand::UdpCmd_ReInitConfirm;
    sUdpVars.pTempSendBufferIRQ^.Header.State              := t_UdpPacketHeaderV2.State.IsFromConsumer OR t_UdpPacketHeaderV2.State.IsFromProducer; // invalid state for normal data

    // only send command and state
    retcode := SendUDPData(sUdpVars.pTempSendBufferIRQ, sizeof(t_UdpPacketHeaderV2.Command) + sizeof(t_UdpPacketHeaderV2.State), udIPAddr); 

    if retcode < 0 then
      sUdpVars.LastSendError := retcode;
      sUdpVars.SendErrorCnt += 1;
    end_if;

    b_AllLocalSlaveAddressesValid := FALSE;

  elsif pPacketHeader^.Command = t_UdpCommand::UdpCmd_ReInitConfirm then

    // we got this, finished with disconnecting this one
    if sUdpVars.ProducerConnectionNo then
      sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].WaitForResponse        := FALSE;
      sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].InfoBits.Disconnecting := FALSE;
      sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].ActReceiveData.usHeaderVersion := 0;
      sUdpVars.paConnectionsToProducers^[sUdpVars.ProducerConnectionNo-1].ActSendData.usHeaderVersion    := 0;
    elsif sUdpVars.ConsumerConnectionNo then
      sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].WaitForResponse        := FALSE;
      sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].InfoBits.Disconnecting := FALSE;
      sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].bSequenceCntReceived   := FALSE; //reset flag to allow sequence counter for new udp communication
      sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].ActReceiveData.usHeaderVersion := 0;
      sUdpVars.paConnectionsToConsumers^[sUdpVars.ConsumerConnectionNo-1].ActSendData.usHeaderVersion    := 0;
    end_if;

  end_if;

END_FUNCTION


FUNCTION SafetyManager::HandleUdpRequest
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
    pPacket 	: ^t_UdpPacketV1;
    udLength 	: UDINT;
  END_VAR
  VAR
    SequenceDifference : SINT;
  END_VAR

  if udLength >= (sizeof(pPacket^.Header)) then
  
    pConnection^.ReceivedPackages += 1;
    
    //Allow the sequence counter if it is a new udp communication.
    if pConnection^.bSequenceCntReceived = FALSE then 
      //In order for the packet to be forwarded to safety, the sequencing counter must be reduced.
      pConnection^.ActReceiveData.usSequenceNumber:= pPacket^.Header.SequenceNumber-1;
      pConnection^.bSequenceCntReceived := TRUE;
    end_if;
    
    SequenceDifference$SINT := (pPacket^.Header.SequenceNumber$SINT - pConnection^.ActReceiveData.usSequenceNumber$SINT);

     
    // Only packages with a new or identical sequence counter are allowed.
    if (SequenceDifference >= 0) & (SequenceDifference < 127) then
      //set flag and timestamp for last request
      if pConnection^.WaitForResponse = TRUE then
        pConnection^.WaitForResponse := FALSE;
        pConnection^.LastResponseTime := ops.tAbsolute - pConnection^.TLastRequest;
        if pConnection^.LastResponseTime > pConnection^.MaxResponseTime then
          pConnection^.MaxResponseTime := pConnection^.LastResponseTime;
        end_if;
      end_if;
      
      //only forward data to safety if it is a new package
      if SequenceDifference > 0 then
      
        UdpReceiveData.SectionStart();    
        // The data is only copied if more than 0 bytes are available in the package and the size is smaller then max size.
        if pPacket^.Header.PdoDataLength & pPacket^.Header.PdoDataLength <= UDP_MAX_PDO_SIZE then
          pConnection^.ActReceiveData.PdoLength := pPacket^.Header.PdoDataLength;
          _memcpy(ptr1:=#pConnection^.ActReceiveData.PdoData, ptr2:=#pPacket^.Data, cntr:=pConnection^.ActReceiveData.PdoLength);
           
          //check for communication ok of fsoe connection (CMD 0x64 = Connection)
          if pConnection^.ActReceiveData.PdoData[0] = 16#64 & pConnection^.bs_SingleLog.OldHeaderVersionOfResponseIF = FALSE then
            LogError("@04E1 (SafetyManager::HandleUdpRequest) [SL] The header version of the request interface doesn't support 4 byte sequence number!");
            LogValue("@04E1 (SafetyManager::HandleUdpRequest) [SL] IFCRC: 0x{0}, SafetyNbr: 0x{1}, IPAddress: 0x{2}", pConnection^.IFCRC, pConnection^.SafetyNbrOtherSide, pConnection^.IP);
            pConnection^.bs_SingleLog.OldHeaderVersionOfResponseIF := TRUE;
          end_if;
        else
          pConnection^.ActReceiveData.PdoLength := 0;
        end_if;

        if pPacket^.Header.SdoDataLength & (pConnection^.ActReceiveData.SdoLength = 0)
        & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) then
          pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
          _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);
        end_if;
        UdpReceiveData.SectionStop();
      
      end_if;
      
      // save safety number of other side
      pConnection^.SafetyNbrOtherSide             := pPacket^.Header.SafetyNbr;    
      pConnection^.ActReceiveData.usSequenceNumber:= pPacket^.Header.SequenceNumber;
      pConnection^.ActSendData.usSequenceNumber   := pConnection^.ActReceiveData.usSequenceNumber;
      //set sequence number of v2 header
      pConnection^.ActSendData.udSequenceNumber    := pConnection^.ActReceiveData.usSequenceNumber;
      pConnection^.ActReceiveData.udSequenceNumber := pConnection^.ActReceiveData.usSequenceNumber;
      
      //if the callback time is smaller than min request time and sequence diff = 0 its an old request -> don't send the response again
      if (ops.tAbsolute - pConnection^.TLastRequest) > UDP_MIN_REQUEST_TIME | SequenceDifference > 0 then
        pConnection^.SendResponse := TRUE;    
      end_if;

    else
      //it's an old package
      sUdpVars.WrongSequenceInRequestCnt += 1;
      //if the last valid request is greater then maximal FSoE Watchdog Time we can reset the sequence Counter from this connection
      if (ops.tAbsolute - pConnection^.TLastRequest) > (UDP_DEFAULT_TRANSMISSION_TIME*2) then
        pConnection^.bSequenceCntReceived := FALSE;
//        pConnection^.TLastRequest := ops.tAbsolute;
      end_if;
    end_if;
  else
    sUdpVars.WrongReceiveUDPLength += 1;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::HandleUdpRequestV2
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
    pPacket 	: ^t_UdpPacketV2;
    udLength 	: UDINT;
  END_VAR
  VAR
    SequenceDifference : DINT;
  END_VAR
  
  if udLength >= (sizeof(pPacket^.Header)) then
  
    pConnection^.ReceivedPackages += 1;
    
    //Allow the sequence counter if it is a new udp communication.
    if pConnection^.bSequenceCntReceived = FALSE then 
      //In order for the packet to be forwarded to safety, the sequencing counter must be reduced.
      pConnection^.ActReceiveData.udSequenceNumber:= pPacket^.Header.SequenceNumber-1;
      pConnection^.bSequenceCntReceived := TRUE;
    end_if;
    
    SequenceDifference := pPacket^.Header.SequenceNumber$DINT - pConnection^.ActReceiveData.udSequenceNumber$DINT;
     
    // Only packages with a new or identical sequence counter are allowed.
    if (SequenceDifference >= 0) & (SequenceDifference < 127) then
      //set flag and timestamp for last request
      if pConnection^.WaitForResponse = TRUE then
        pConnection^.WaitForResponse := FALSE;
        pConnection^.LastResponseTime := ops.tAbsolute - pConnection^.TLastRequest;
        if pConnection^.LastResponseTime > pConnection^.MaxResponseTime then
          pConnection^.MaxResponseTime := pConnection^.LastResponseTime;
        end_if;
      end_if;
      
      //only forward data to safety if it is a new package
      if SequenceDifference > 0 then
      
        UdpReceiveData.SectionStart();    
        // The data is only copied if more than 0 bytes are available in the package and the size is smaller then max size.
        if pPacket^.Header.PdoDataLength & pPacket^.Header.PdoDataLength <= UDP_MAX_PDO_SIZE then
          pConnection^.ActReceiveData.PdoLength := pPacket^.Header.PdoDataLength;
          _memcpy(ptr1:=#pConnection^.ActReceiveData.PdoData, ptr2:=#pPacket^.Data, cntr:=pConnection^.ActReceiveData.PdoLength);
        else
          pConnection^.ActReceiveData.PdoLength := 0;
        end_if;

        if pPacket^.Header.SdoDataLength & (pConnection^.ActReceiveData.SdoLength = 0) 
        & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) then
          pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
          _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);    
        end_if;
        UdpReceiveData.SectionStop();
      
      end_if;
      
      // save safety number of other side
      pConnection^.SafetyNbrOtherSide             := pPacket^.Header.SafetyNbr;    
      pConnection^.ActReceiveData.udSequenceNumber:= pPacket^.Header.SequenceNumber;
      pConnection^.ActSendData.udSequenceNumber   := pConnection^.ActReceiveData.udSequenceNumber;
      
      //if the callback time is smaller than min request time and sequence diff = 0 its an old request -> don't send the response again
      if (ops.tAbsolute - pConnection^.TLastRequest) > UDP_MIN_REQUEST_TIME | SequenceDifference > 0 then
        pConnection^.SendResponse := TRUE;    
      end_if;

    else
      //it's an old package
      sUdpVars.WrongSequenceInRequestCnt += 1;
      //if the last valid request is greater then maximal FSoE Watchdog Time we can reset the sequence Counter from this connection
      if (ops.tAbsolute - pConnection^.TLastRequest) > (UDP_DEFAULT_TRANSMISSION_TIME*2) then
        pConnection^.bSequenceCntReceived := FALSE;
//        pConnection^.TLastRequest := ops.tAbsolute;
      end_if;
    end_if;
  else
    sUdpVars.WrongReceiveUDPLength += 1;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::HandleUdpResponse
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
    pPacket 	: ^t_UdpPacketV1;
    udLength 	: UDINT;
  END_VAR
  VAR
    SequenceDifference : SINT;
  END_VAR
  
  //check length of udp data -> length must be more then header size
  if udLength >= (sizeof(pPacket^.Header)) then
    pConnection^.ReceivedPackages += 1;

    pConnection^.ActReceiveData.usSequenceNumber := pPacket^.Header.SequenceNumber;
    pConnection^.ActReceiveData.udSequenceNumber := pConnection^.ActReceiveData.usSequenceNumber;
    
    // Only packages with identical sequence counter are allowed.
    if pConnection^.ActReceiveData.usSequenceNumber = pConnection^.ActSendData.usSequenceNumber then
      // we got our response => increase the sequence number
      pConnection^.ActSendData.usSequenceNumber += 1;
      pConnection^.ActSendData.udSequenceNumber := pConnection^.ActSendData.usSequenceNumber;

      if pConnection^.WaitForResponse = TRUE then
        pConnection^.WaitForResponse := FALSE;
        pConnection^.LastResponseTime := ops.tAbsolute - pConnection^.TLastRequest;
        if pConnection^.LastResponseTime > pConnection^.MaxResponseTime then
          pConnection^.MaxResponseTime := pConnection^.LastResponseTime;
        end_if;      
        pConnection^.TLastValidRequest := ops.tAbsolute;
      else
        sUdpVars.ReceivedUnexpectedResponse += 1;
        return;
      end_if;

      UdpReceiveData.SectionStart();
      
      // The data is only copied if more than 0 bytes are available in the package and the size is smaller then max size.
      if pPacket^.Header.PdoDataLength & pPacket^.Header.PdoDataLength <= UDP_MAX_PDO_SIZE then
        pConnection^.ActReceiveData.PdoLength := pPacket^.Header.PdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.PdoData, ptr2:=#pPacket^.Data, cntr:=pConnection^.ActReceiveData.PdoLength);
        
        //check for communication ok of fsoe connection (CMD 0x64 = Connection)
        if pConnection^.ActReceiveData.PdoData[0] = 16#64 & pConnection^.bs_SingleLog.OldHeaderVersionOfResponseIF = FALSE then
          LogError("@04E0 (SafetyManager::HandleUdpResponse) The header version of the response interface doesn't support 4 byte sequence number!");          
          LogValue("@04E1 (SafetyManager::HandleUdpResponse) [SL] IFCRC: 0x{0}, SafetyNbr: 0x{1}, IPAddress: 0x{2}", pConnection^.IFCRC, pConnection^.SafetyNbrOtherSide, pConnection^.IP);
          pConnection^.bs_SingleLog.OldHeaderVersionOfResponseIF := TRUE;
        end_if;  
      else
        pConnection^.ActReceiveData.PdoLength := 0;
      end_if;           
        
      if (pPacket^.Header.SdoDataLength) & (pConnection^.ActReceiveData.SdoLength = 0) 
      & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) then
        pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);
      end_if;
      
      UdpReceiveData.SectionStop();
    else
      //sequence counter is not the same
      sUdpVars.WrongSequenceInResponseCnt += 1;
      
      //check the sequence difference, it could be a retry from other side which contains sdo data
      SequenceDifference := pConnection^.ActReceiveData.udSequenceNumber$SINT - pConnection^.ActSendData.udSequenceNumber$SINT;
      
      UdpReceiveData.SectionStart();
      //if Sequence difference is -1 it could be a response retry with sdo data
      if (pPacket^.Header.SdoDataLength) & (pConnection^.ActReceiveData.SdoLength = 0) 
      & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) & (SequenceDifference = -1) then
        pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);
      end_if;      
      UdpReceiveData.SectionStop();
    end_if;
    
    // save safety number of other side
    pConnection^.SafetyNbrOtherSide       := pPacket^.Header.SafetyNbr;
  else
    sUdpVars.WrongReceiveUDPLength += 1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::HandleUdpResponseV2
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
    pPacket 	: ^t_UdpPacketV2;
    udLength 	: UDINT;
  END_VAR
  VAR
    SequenceDifference : DINT;
  END_VAR
  
  //check length of udp data -> length must be more then header size
  if udLength >= (sizeof(pPacket^.Header)) then
  
    pConnection^.ReceivedPackages += 1;
    pConnection^.ActReceiveData.udSequenceNumber := pPacket^.Header.SequenceNumber;
    
    // Only packages with identical sequence counter are allowed.
    if pConnection^.ActReceiveData.udSequenceNumber = pConnection^.ActSendData.udSequenceNumber then
      // we got our response => increase the sequence number
      pConnection^.ActSendData.udSequenceNumber += 1;

      if pConnection^.WaitForResponse = TRUE then
        pConnection^.WaitForResponse := FALSE;
        pConnection^.LastResponseTime := ops.tAbsolute - pConnection^.TLastRequest;
        if pConnection^.LastResponseTime > pConnection^.MaxResponseTime then
          pConnection^.MaxResponseTime := pConnection^.LastResponseTime;
        end_if;
        pConnection^.TLastValidRequest := ops.tAbsolute;
      else
        sUdpVars.ReceivedUnexpectedResponse += 1;
        return;
      end_if;

      UdpReceiveData.SectionStart();
      
      // The data is only copied if more than 0 bytes are available in the package and the size is smaller then max size.
      if pPacket^.Header.PdoDataLength & pPacket^.Header.PdoDataLength <= UDP_MAX_PDO_SIZE then
        pConnection^.ActReceiveData.PdoLength := pPacket^.Header.PdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.PdoData, ptr2:=#pPacket^.Data, cntr:=pConnection^.ActReceiveData.PdoLength);
      else
        pConnection^.ActReceiveData.PdoLength := 0;
      end_if;
        
      if (pPacket^.Header.SdoDataLength) & (pConnection^.ActReceiveData.SdoLength = 0) 
      & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) then
        pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);
      end_if;
      
      UdpReceiveData.SectionStop();
    else
      //sequence counter is not the same
      sUdpVars.WrongSequenceInResponseCnt += 1;
      
      //check the sequence difference, it could be a retry from other side which contains sdo data
      SequenceDifference := pConnection^.ActReceiveData.udSequenceNumber$DINT - pConnection^.ActSendData.udSequenceNumber$DINT;
      
      UdpReceiveData.SectionStart();
      //if Sequence difference is -1 it could be a response retry with sdo data
      if (pPacket^.Header.SdoDataLength) & (pConnection^.ActReceiveData.SdoLength = 0) 
      & (pPacket^.Header.SdoDataLength <= UDP_MAX_SDO_SIZE) & (SequenceDifference = -1) then
        pConnection^.ActReceiveData.SdoLength := pPacket^.Header.SdoDataLength;
        _memcpy(ptr1:=#pConnection^.ActReceiveData.SdoData, ptr2:=#pPacket^.Data + pConnection^.ActReceiveData.PdoLength, cntr:=pConnection^.ActReceiveData.SdoLength);
      end_if;
      UdpReceiveData.SectionStop();
    end_if;
    
    // save safety number of other side
    pConnection^.SafetyNbrOtherSide       := pPacket^.Header.SafetyNbr;
  else
    sUdpVars.WrongReceiveUDPLength += 1;
  end_if;


END_FUNCTION



FUNCTION SafetyManager::ForwardReceivedUdpSDOs
  VAR_INPUT
    paConnections 	: ^t_aUdpConnections;
    ConnectionNo 	: UDINT;
  END_VAR
  VAR
    i               : UDINT;
    pActConnection  : ^t_UdpConnection;
    pHelpCfg        : ^t_CfgListElem;
    pHelpModule     : ^t_ModuleListElem;
    InterfaceCRC    : HDINT;
    SafetyNumber    : HDINT;
    MyPara          : CmdStruct;
    MyResult        : results;
    pDO 	          : ^SafetyCDIAS_Base::SDOHeader;
    pCompDestPath   : ^USINT;
    pHelpSafety     : ^t_SafetyComponent;
    ResponseValue   : BOOL;
    FoundTarget     : BOOL;
  END_VAR

  ResponseValue := paConnections = sUdpVars.paConnectionsToProducers;

  pActConnection := #paConnections^[0];
  for i := 0 to (ConnectionNo-1) do
  
    // check if there is a SDO to forward
    if pActConnection^.ActReceiveData.SdoLength then
      
      FoundTarget := FALSE;

      UdpReceiveData.SectionStart();
      
      // get a pointer to the compressed destination path
      pDO := (#pActConnection^.ActReceiveData.SdoData)$^SafetyCDIAS_Base::SDOHeader;
      if (pDO^.StdHeader.Typ.Response = ResponseValue) then
        pCompDestPath := (#pDO^.DstAdr)$^USINT;
        
        // check if it's addressed by topology path and it's an IP
        if ((pDO^.StdHeader.Typ AND SSDO_MASK_TYPE) = FRM_TYPECODE_SSDO_MOD_TP) & (pCompDestPath^ = INTERFACE_IP_MODE) then
          // after the mode byte we find the interface CRC
          InterfaceCRC := ((pCompDestPath + 1)$^HDINT)^;
              
          // search all configs, all moduleconfigs

          pHelpCfg := pConfigs;
          while pHelpCfg & pActConnection^.ActReceiveData.SdoLength do
            pHelpModule := pHelpCfg^.pFirstModuleCfg;
            while pHelpModule & pActConnection^.ActReceiveData.SdoLength do
              if pHelpModule^.ModuleInfo.InterfaceCRC = InterfaceCRC then
                // we found the interface CRC, now we have to make a connection via the SafetyNumber between the actual module config and the module handles
                pHelpSafety := pFirstSafetyComponent;
                while pHelpSafety & pActConnection^.ActReceiveData.SdoLength do
                  // we compare the safety number
                  if pHelpSafety^.SafetyNbr = pHelpModule^.ModuleInfo.SafetyNbr then

                    MyPara.uiCmd := CMD_SM_SDO_TO_MODULE;
                    MyPara.aPara[0] := pActConnection^.ActReceiveData.SdoLength;
                    MyPara.aPara[1] := (#pActConnection^.ActReceiveData.SdoData)$DINT;
                    MyPara.aPara[2] := this$DINT;

                    pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);  

                    FoundTarget := TRUE;
                  end_if;
                  
                  pHelpSafety := pHelpSafety^.pNext;
                end_while;
              end_if;

              pHelpModule := pHelpModule^.pNext;
            end_while;     

            pHelpCfg := pHelpCfg^.pNext;
          end_while;
        else
          SafetyNumber := pDO^.DstAdr;
        
          // search by safety number and forward to internal module
          pHelpSafety := pFirstSafetyComponent;
          while pHelpSafety & pActConnection^.ActReceiveData.SdoLength do
            // we compare the safety number
            if pHelpSafety^.SafetyNbr = SafetyNumber then            
              MyPara.uiCmd := CMD_SM_SDO_TO_MODULE;
              MyPara.aPara[0] := pActConnection^.ActReceiveData.SdoLength;
              MyPara.aPara[1] := (#pActConnection^.ActReceiveData.SdoData)$DINT;
              MyPara.aPara[2] := this$DINT;

              pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);  

              FoundTarget := TRUE;
            end_if;
            
            pHelpSafety := pHelpSafety^.pNext;
          end_while;
        end_if;
        
        // reset length if the response bit matches (if we found it, we already forwarded it. if not, we're not going to find it next time)
        pActConnection^.ActReceiveData.SdoLength := 0;
      else
        // check which list it is
        if ResponseValue then
          sUdpVars.InvalidResponseSettingAtFowardingProducerList += 1;
        else
          sUdpVars.InvalidResponseSettingAtFowardingConsumerList += 1;
        end_if;
      end_if;
      
      UdpReceiveData.SectionStop();
      
    end_if;

    pActConnection += sizeof(t_UdpConnection);
  end_for;

END_FUNCTION


FUNCTION SafetyManager::ForwardSDO2UdpBySafetyNbr
  VAR_INPUT
    SDOLength 	: UINT;
    pSDO 	: ^USINT;
  END_VAR
  VAR_OUTPUT
    FoundTarged 	: BOOL;
  END_VAR
  VAR
    i               : UDINT;
    pActConnection  : ^t_UdpConnection;
    TargetSafetyNbr : HDINT;
    pDO 	          : ^SafetyCDIAS_Base::SDOHeader;
  END_VAR

  FoundTarged := FALSE;

  // get a pointer to the compressed destination path
  pDO := pSDO$^SafetyCDIAS_Base::SDOHeader;
  TargetSafetyNbr := pDO^.DstAdr;
  
  if sUdpVars.ConsumerConnectionNo then
    pActConnection := #sUdpVars.paConnectionsToConsumers^[0];
    for i := 0 to (sUdpVars.ConsumerConnectionNo-1) do
      // the safety number has to match and since it's a message to a consumer it has to be a response
      if (pActConnection^.SafetyNbrOtherSide = TargetSafetyNbr) & (pDO^.StdHeader.Typ.Response = TRUE) then
        // got it => forward the SDO       
        if SDOLength <= UDP_MAX_SDO_SIZE then
          // check for unsent SDO first
          if pActConnection^.ActSendData.SdoLength = 0 then
            UdpSendData.SectionStart();

            _memcpy(ptr1:=#pActConnection^.ActSendData.SdoData, ptr2:=pSDO, cntr:=SDOLength);
            pActConnection^.ActSendData.SdoLength     := SDOLength$USINT;

            UdpSendData.SectionStop();
          else
            sUdpVars.SdoAlreadyUsedCnt += 1;
          end_if;
        else
          LogError("@046E (SafetyManager::ForwardSDO2UdpBySafetyNbr) SDO size exceeds the allowed size (Compressed Topology Path: UDP)");
        end_if;
      
        FoundTarged := TRUE;
        return;
      end_if;

      pActConnection += sizeof(t_UdpConnection);
    end_for;
  end_if;
  
  if sUdpVars.ProducerConnectionNo then
    pActConnection := #sUdpVars.paConnectionsToProducers^[0];
    for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
      // the safety number has to match and since it's a message to a producer it has to be a request
      if (pActConnection^.SafetyNbrOtherSide = TargetSafetyNbr) & (pDO^.StdHeader.Typ.Response = FALSE) then
        // got it => forward the SDO        
        if SDOLength <= UDP_MAX_SDO_SIZE then
          // check for unsent SDO first
          if pActConnection^.ActSendData.SdoLength = 0 then
            UdpSendData.SectionStart();

            _memcpy(ptr1:=#pActConnection^.ActSendData.SdoData, ptr2:=pSDO, cntr:=SDOLength);
            pActConnection^.ActSendData.SdoLength     := SDOLength$USINT;

            UdpSendData.SectionStop();
          else
            sUdpVars.SdoAlreadyUsedCnt += 1;
          end_if;
        else
          LogError("@046E (SafetyManager::ForwardSDO2Udp) SDO size exceeds the allowed size (Compressed Topology Path: UDP)");
        end_if;
      
        FoundTarged := TRUE;
        return;
      end_if;

      pActConnection += sizeof(t_UdpConnection);
    end_for;
  end_if;
  
END_FUNCTION


FUNCTION SafetyManager::ShareUdpConnectionInfos
  VAR
    i                 : UDINT;
    pUdpConnection    : ^t_UdpConnection;
    AnythingMissing   : BOOL;
    ActInitialized    : BOOL;
    FoundConnection   : BOOL;
    pHelpSafety       : ^t_SafetyComponent;
    pHelpCfg          : ^t_CfgListElem;
    pFSoEConnection   : ^t_FSoEConnectionDetails;
    MyPara            : CmdStruct;
    MyResult          : results;
    retcode           : iprStates;

    ModuleReadOffset  : UINT;
    ModuleReadSize    : UINT;
    ModuleWriteOffset : UINT;
    ModuleWriteSize   : UINT;
    MngReadOffset     : UINT;
    MngReadSize       : UINT;
    MngWriteOffset    : UINT;
    MngWriteSize      : UINT;
  END_VAR
  
  AnythingMissing := FALSE;
  
  if sUdpVars.ConsumerConnectionNo then
    pUdpConnection := #sUdpVars.paConnectionsToConsumers^[0];
    for i := 0 to (sUdpVars.ConsumerConnectionNo-1) do
      // check if we received something so far at this connection
      if (pUdpConnection^.ReceivedPackages & (pUdpConnection^.InfoBits.PdosInitialized = FALSE)) then
        FoundConnection := ActInitialized := FALSE;

        // check FSoE Connections in the available configs if there is a match at the interface CRC
        pHelpCfg := pConfigs;
        while pHelpCfg & (FoundConnection = FALSE) do
          pFSoEConnection := pHelpCfg^.pFirstFSoEConnection;
          while pFSoEConnection & (FoundConnection = FALSE) do
            // only interface slave connections which are not used yet are relevant
            if pFSoEConnection^.Flags.IsInterfaceConnection & (pFSoEConnection^.Flags.IsMaster = FALSE) & (pFSoEConnection^.InternalConnectionState = FCS_RESET) &
               pFSoEConnection^.pSlavePathComp & ((pFSoEConnection^.pSlavePathComp$^HDINT + 1)^ = pUdpConnection^.IFCRC) then

              // found the connection
              FoundConnection := TRUE;
              
              // now try to initialize it

              pHelpSafety := pFirstSafetyComponent;
              while pHelpSafety do
                if pHelpSafety^.SafetyNbr = pFSoEConnection^.OwnerSafetyNbr then
                  exit;
                end_if;
                
                pHelpSafety := pHelpSafety^.pNext;
              end_while;

              if pHelpSafety then
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) <This side is Slave> ConnectionID: 0x{0}, SlaveAddress: 0x{1}, IFCRC: 0x{2}", Value:=pFSoEConnection^.ConnectionID, Value2:=pFSoEConnection^.SlaveAddress, Value3 := (pFSoEConnection^.pSlavePathComp$^HDINT + 1)^);
                LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) <This side is Slave> UdpInfo: SNr: 0x{0}, SNrOtherSide: 0x{1}, IFCRC: 0x{2}", Value:=pUdpConnection^.SafetyNbr, Value2:=pUdpConnection^.SafetyNbrOtherSide, Value3 := pUdpConnection^.IFCRC);
#endif

                // the module is slave and the managing module master
                ModuleReadOffset  := pFSoEConnection^.SlaveReadOffset;
                ModuleReadSize    := pFSoEConnection^.MasterWriteSize;
                ModuleWriteOffset := pFSoEConnection^.SlaveWriteOffset;
                ModuleWriteSize   := pFSoEConnection^.MasterReadSize;
                MngReadOffset     := 0; // offsets in managing modules are always dynamic
                MngReadSize       := pFSoEConnection^.MasterReadSize;
                MngWriteOffset    := 0; // offsets in managing modules are always dynamic
                MngWriteSize      := pFSoEConnection^.MasterWriteSize;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) Modul => Udp: Rd Offset Modul: 0x{0}, IP: 0x{1}, Größe: 0x{2}", Value:=ModuleReadOffset, Value2:=pUdpConnection^.IP, Value3:=ModuleReadSize);
#endif
                
                MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
                MyPara.aPara[0] := (#pUdpConnection^.ThreadSafePdoReceiveBuffer)$DINT;
                MyPara.aPara[1] := 0;
                MyPara.aPara[2] := pUdpConnection^.SafetyNbrOtherSide$DINT;
                MyPara.aPara[3] := ModuleWriteSize;
                MyPara.aPara[4] := 0;
                MyPara.aPara[5] := 0;
                MyPara.aPara[6] := 0;
                MyPara.aPara[7] := ModuleWriteOffset;
                MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                MyPara.aPara[9] := TRUE;

                retcode := pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

                if retcode = ERROR then
                  LogError("@0848 (SafetyManager::ShareUdpConnectionInfos) Failed to add input PDO for a connection to a Consumer");
                  
                  // mark the connection to not try again later
                  pFSoEConnection^.InternalConnectionState := FCS_NOT_INITIALIZED;
                  
                  return;
                elsif retcode = BUSY then
                  // try again later (skip further initialisation steps)
                  return;
                end_if;

                if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
                  aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pHelpSafety;
                  aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
                  FSoEInputPDONo                            += 1;

                  pUdpConnection^.ActReceiveData.PdoLength  := ModuleWriteSize;
                else
                  LogValue("@0490 (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                  exit;
                end_if;

                if sUdpVars.ReceiveCopyCmdNo < MAX_UDP_IF_ALLOWED then
                  sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].pSource      := (#pUdpConnection^.ActReceiveData.PdoData)$^USINT;
                  sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].pDest        := MyPara.aPara[0]$^USINT;
                  sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].Size         := ModuleWriteSize;
                  sUdpVars.ReceiveCopyCmdNo                                            += 1;
                else
                  LogValue("@049F (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed UDP receive copy commands exceeded (0x{0})", Value:=MAX_UDP_IF_ALLOWED);
                  exit;
                end_if;



                MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
                MyPara.aPara[0] := 1;  // mark as interface frame PDO
                pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

//                  pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
//                  pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
//                  pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
//                  pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
                
                if sUdpVars.SendCopyCmdNo < MAX_UDP_IF_ALLOWED then
                  sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].pSource      := MyResult.aData[0]$^USINT + ModuleReadOffset;
                  sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].pDest        := #pUdpConnection^.ActSendData.PdoData;
                  sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].Size         := ModuleReadSize;
                  sUdpVars.SendCopyCmdNo                                         += 1;

                  pUdpConnection^.ActSendData.PdoLength           := ModuleReadSize;
                else
                  LogValue("@049E (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed UDP send copy commands exceeded (0x{0})", Value:=MAX_UDP_IF_ALLOWED);
                  exit;
                end_if;
                
#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
#pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) Udp => Modul: Wr Offset Modul: 0x{0}, IP: 0x{1}, Größe: 0x{2}", Value:=ModuleWriteOffset, Value2:=pUdpConnection^.IP, Value3:=ModuleWriteSize);
#endif

                // mark the FSoE connection as established
                pFSoEConnection^.InternalConnectionState  := FCS_CONNECTION;

                // mark the udp connection as established
                pUdpConnection^.InfoBits.PdosInitialized  := TRUE;

                // connection initialized
                ActInitialized                            := TRUE;

                // check if we have all slave addresses now
                CheckLocalSlaveAddresses();
              else
                LogValue("@495 (SafetyManager::ShareUdpConnectionInfos) Could not find safety module with safety number 0x{0} for FSoE interface connection", Value:=pFSoEConnection^.OwnerSafetyNbr);
              end_if;
            end_if;
            
            pFSoEConnection := pFSoEConnection^.pNext;
          end_while;
          
          if pHelpCfg then
            pHelpCfg := pHelpCfg^.pNext;
          end_if;
        end_while;
        
        if ActInitialized = FALSE then
          AnythingMissing := TRUE;
        end_if;
      elsif (pUdpConnection^.ReceivedPackages = 0) then
        AnythingMissing := TRUE;
      end_if;

      pUdpConnection += sizeof(t_UdpConnection);
    end_for;
  end_if;
  
  if sUdpVars.ProducerConnectionNo then
    pUdpConnection := #sUdpVars.paConnectionsToProducers^[0];
    for i := 0 to (sUdpVars.ProducerConnectionNo-1) do
      // check if we received something so far at this connection
      if (pUdpConnection^.ReceivedPackages & (pUdpConnection^.InfoBits.PdosInitialized = FALSE)) then
        FoundConnection := ActInitialized := FALSE;

        // check FSoE Connections in the available configs if there is a match at the interface CRC
        pHelpCfg := pConfigs;
        while pHelpCfg & (FoundConnection = FALSE) do
          pFSoEConnection := pHelpCfg^.pFirstFSoEConnection;
          while pFSoEConnection & (FoundConnection = FALSE) do
            // only interface master connections which are not used are relevant
            if pFSoEConnection^.Flags.IsInterfaceConnection & pFSoEConnection^.Flags.IsMaster & (pFSoEConnection^.InternalConnectionState = FCS_RESET) then
              // check if it's our connection
              if ( pFSoEConnection^.pSlavePathComp) & 
                 ( pFSoEConnection^.pSlavePathComp^             = INTERFACE_IP_MODE) & 
                 ((pFSoEConnection^.pSlavePathComp$^HDINT + 1)^ = pUdpConnection^.IFCRC) & 
                 ((pFSoEConnection^.pSlavePathComp$^HDINT + 5)^ = pUdpConnection^.IP) then

                // found the connection
                FoundConnection := TRUE;
                
                // now try to initialize it

                pHelpSafety := pFirstSafetyComponent;
                while pHelpSafety do
                  if pHelpSafety^.SafetyNbr = pFSoEConnection^.OwnerSafetyNbr then
                    exit;
                  end_if;
                  
                  pHelpSafety := pHelpSafety^.pNext;
                end_while;

                if pHelpSafety then
  #ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) <This side is Master> ConnectionID: 0x{0}, SlaveAddress: 0x{1}, InterfaceCRC: 0x{2}", Value:=pFSoEConnection^.ConnectionID, Value2:=pFSoEConnection^.SlaveAddress, Value3 := pUdpConnection^.IFCRC);
#endif
                  
                  // the module is master and the managing module slave
                  ModuleReadOffset  := pFSoEConnection^.MasterReadOffset;
                  ModuleReadSize    := pFSoEConnection^.MasterReadSize;
                  ModuleWriteOffset := pFSoEConnection^.MasterWriteOffset;
                  ModuleWriteSize   := pFSoEConnection^.MasterWriteSize;
                  MngReadOffset     := 0; // offsets in managing modules are always dynamic
                  MngReadSize       := pFSoEConnection^.MasterWriteSize;
                  MngWriteOffset    := 0; // offsets in managing modules are always dynamic
                  MngWriteSize      := pFSoEConnection^.MasterReadSize;

  #ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) Modul => Udp: Rd Offset Modul: 0x{0}, IP: 0x{1}, Größe: 0x{2}", Value:=ModuleReadOffset, Value2:=pUdpConnection^.IP, Value3:=ModuleReadSize);
  #endif
                  
                  MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
                  MyPara.aPara[0] := (#pUdpConnection^.ThreadSafePdoReceiveBuffer)$DINT;
                  MyPara.aPara[1] := 0;
                  MyPara.aPara[2] := pUdpConnection^.SafetyNbrOtherSide$DINT;
                  MyPara.aPara[3] := ModuleWriteSize;
                  MyPara.aPara[4] := 0;
                  MyPara.aPara[5] := 0;
                  MyPara.aPara[6] := 0;
                  MyPara.aPara[7] := ModuleWriteOffset;
                  MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
                  MyPara.aPara[9] := TRUE;

                  retcode := pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

                  if retcode = ERROR then
                    LogError("@0849 (SafetyManager::ShareUdpConnectionInfos) Failed to add input PDO for a connection to a Producer");
                    
                    // mark the connection to not try again later
                    pFSoEConnection^.InternalConnectionState := FCS_NOT_INITIALIZED;
                    
                    return;
                  elsif retcode = BUSY then
                    // try again later (skip further initialisation steps)
                    return;
                  end_if;

                  if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
                    aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pHelpSafety;
                    aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
                    FSoEInputPDONo += 1;
                    
                    pUdpConnection^.ActReceiveData.PdoLength  := ModuleWriteSize;
                  else
                    LogValue("@0490 (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
                    exit;
                  end_if;

                  if sUdpVars.ReceiveCopyCmdNo < MAX_UDP_IF_ALLOWED then
                    sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].pSource  := (#pUdpConnection^.ActReceiveData.PdoData)$^USINT;
                    sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].pDest    := MyPara.aPara[0]$^USINT;
                    sUdpVars.aReceiveCopyCmds[sUdpVars.ReceiveCopyCmdNo].Size     := ModuleWriteSize;
                    sUdpVars.ReceiveCopyCmdNo                                        += 1;
                  else
                    LogValue("@049F (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed UDP receive copy commands exceeded (0x{0})", Value:=MAX_UDP_IF_ALLOWED);
                    exit;
                  end_if;



                  MyPara.uiCmd      := CMD_SM_ADD_OUTPUT_PDO;
                  MyPara.aPara[0]   := 1;  // mark as interface frame PDO
                  pHelpSafety^.pThis^.NewInst(#MyPara, #MyResult);

  //                  pPDOElement^.SourceInfo.pPDOMem       := MyResult.aData[0]$^USINT;
  //                  pPDOElement^.SourceInfo.pPDOMemDPRAM  := MyResult.aData[4]$^USINT;
  //                  pPDOElement^.SourceInfo.HandlePDODPRAM:= MyResult.aData[8]$UDINT;   // Needed for Payload Support
  //                  pPDOElement^.SourceInfo.OffsetPDODPRAM:= MyResult.aData[12]$UDINT;  // Needed for Payload Support
                  
                  if sUdpVars.SendCopyCmdNo < MAX_UDP_IF_ALLOWED then
                    sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].pSource      := MyResult.aData[0]$^USINT + ModuleReadOffset;
                    sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].pDest        := #pUdpConnection^.ActSendData.PdoData;
                    sUdpVars.aSendCopyCmds[sUdpVars.SendCopyCmdNo].Size         := ModuleReadSize;
                    sUdpVars.SendCopyCmdNo                                         += 1;

                    pUdpConnection^.ActSendData.PdoLength                             := ModuleReadSize;
                  else
                    LogValue("@049E (SafetyManager::ShareUdpConnectionInfos) Maximum limit of allowed UDP send copy commands exceeded (0x{0})", Value:=MAX_UDP_IF_ALLOWED);
                    exit;
                  end_if;
                  
  #ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
                  LogValue("@DBG (SafetyManager::ShareUdpConnectionInfos) Udp => Modul: Wr Offset Modul: 0x{0}, IP: 0x{1}, Größe: 0x{2}", Value:=ModuleWriteOffset, Value2:=pUdpConnection^.IP, Value3:=ModuleWriteSize);
  #endif

                  // mark the FSoE connection as established
                  pFSoEConnection^.InternalConnectionState  := FCS_CONNECTION;
                  
                  // mark the udp connection as established
                  pUdpConnection^.InfoBits.PdosInitialized  := TRUE;
                                 
                  // connection initialized
                  ActInitialized                            := TRUE;

                  // check if we have all slave addresses now
                  CheckLocalSlaveAddresses();
                else
                  LogValue("@0495 (SafetyManager::ShareUdpConnectionInfos) Could not find safety module with safety number 0x{0} for FSoE interface connection", Value:=pFSoEConnection^.OwnerSafetyNbr);
                end_if;

              end_if;

            end_if;
            
            pFSoEConnection := pFSoEConnection^.pNext;
          end_while;
          
          pHelpCfg := pHelpCfg^.pNext;
       end_while;
        
        if ActInitialized = FALSE then
          AnythingMissing := TRUE;
        end_if;
      elsif (pUdpConnection^.ReceivedPackages = 0) then
        AnythingMissing := TRUE;
      end_if;

      pUdpConnection += sizeof(t_UdpConnection);
    end_for;
  end_if;

  // if all available udp connections are initialized => set the marker for not coming here any more
  if AnythingMissing = FALSE then 
    sUdpVars.ConnectionsInitialized := TRUE;

    // update scheduler grid
    SetUdpSchedulerGrid();
  end_if;

END_FUNCTION


FUNCTION SafetyManager::SetUdpSchedulerGrid
  VAR
    Temp            : UDINT;
    bGetRequestTimeout : BOOL;
  END_VAR
  
  bGetRequestTimeout := FALSE;
  
  // we can only calculate if there are producer connections => otherwise the scheduler is not active
  if sUdpVars.ProducerConnectionNo then

    // the timeout is calculated so we could fit 4 timed out transmissions into the watchdog time (watchdog = 2*transmission time)
    Temp := sUdpVars.LowestTransmissionTime_ms * 2 / 4;

    // if the result is smaller than the minimum timeout => set it to the minimum
    if Temp < UDP_MIN_REQUEST_TIMEOUT then
      Temp := UDP_MIN_REQUEST_TIMEOUT;
    end_if;

    sUdpVars.CalculatedRequestTimeout := Temp;
    bGetRequestTimeout := TRUE;
  else
    // activate the default settings (min settings)
    sUdpVars.CalculatedRequestTimeout := UDP_MIN_REQUEST_TIMEOUT;
  end_if;
  
  // use default value for request time
  sUdpVars.CalculatedRequestTime    := UDP_MIN_REQUEST_TIME;
    
  //check for advanced request time from user
  if sUdpVars.udAdvancedRequestCycle >= UDP_MIN_REQUEST_TIME
  & sUdpVars.udAdvancedRequestCycle <= (sUdpVars.CalculatedRequestTimeout/2)  then
    sUdpVars.CalculatedRequestTime := sUdpVars.udAdvancedRequestCycle;
  else
    //only log error if we got calculated request timeout and value is not zero
    if bGetRequestTimeout = TRUE & sUdpVars.udAdvancedResponseTimeout <> 0 & bd_SingleLog.SetUdpRequestCycleFailed = FALSE then
      LogValue("@04D6 (SafetyManager::SetUdpSchedulerGrid) [SL] The value of 0x{0} for the UDP Request Cycle is not valid! The maximum value is 0x{1}", Value:=sUdpVars.udAdvancedRequestCycle, (sUdpVars.CalculatedRequestTimeout/2));
      bd_SingleLog.SetUdpRequestCycleFailed := TRUE;
    end_if;
  end_if;

  // always use the min settings for the responses
  sUdpVars.CalculatedResponseTime    := UDP_MIN_RESPONSE_TIME;
  sUdpVars.CalculatedResponseTimeout := UDP_MIN_RESPONSE_TIMEOUT;
  
  //check for advanced response timeout from user
  if sUdpVars.udAdvancedResponseTimeout >= UDP_MIN_RESPONSE_TIMEOUT then
    sUdpVars.CalculatedResponseTimeout := sUdpVars.udAdvancedResponseTimeout;
  else
    // only log error if value is not zero
    if sUdpVars.udAdvancedResponseTimeout <> 0 & bd_SingleLog.SetUdpResponseTimeoutFailed = FALSE then
      LogValue("@04D7 (SafetyManager::SetUdpSchedulerGrid) [SL] The value of 0x{0} for the UDP Response Timeout is not valid! The minimum value is 0x{1}", Value:=sUdpVars.udAdvancedResponseTimeout, UDP_MIN_RESPONSE_TIMEOUT);
      bd_SingleLog.SetUdpResponseTimeoutFailed := TRUE;
    end_if;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::UdpResponseScheduler
  VAR
    pActResponseEntry : ^t_UdpConnection;
    pActRetryEntry    : ^t_UdpConnection;
    tmpretcode        : DINT;
    i                 : UDINT;
  END_VAR

  //point to actual request-entry
  pActResponseEntry := #sUdpVars.paConnectionsToConsumers^[sUdpVars.ResponseScheduler.ActRequestedIndex];

  //send a request every TRequestTime x Cycnt Cycle
  if ((sUdpVars.ResponseScheduler.CyCnt MOD sUdpVars.CalculatedResponseTime) = 0) then

    // search through the list if something has to be sent (check also for changes in the data)
    for i := 0 to sUdpVars.ConsumerConnectionNo-1 do
      
      //only send a request if not waiting for a response and the IP is valid (on cleaning out the connections some may be set to 0)
      if (pActResponseEntry^.WaitForResponse = FALSE) & (pActResponseEntry^.IP <> 0) & (pActResponseEntry^.SendResponse = TRUE) then
      
        //send request to mover of actual entry
        tmpretcode := SendUdpResponse(pActResponseEntry);
        if tmpretcode >= 0 then
          
          //if sending was successful we are waiting for a response
          pActResponseEntry^.WaitForResponse := TRUE;
          pActResponseEntry^.SendResponse    := FALSE;
          pActResponseEntry^.TLastRequest      := ops.tAbsolute;
          pActResponseEntry^.RequestRetries    := 0;
        
        else 
          //unable to send request
          if bd_SingleLog.SendUdpRspFailed = FALSE then
            bd_SingleLog.SendUdpRspFailed := TRUE;
            LogError("@04A0 (SafetyManager::UdpResponseScheduler) [SL] Failed to send udp response");
          end_if;
        end_if;
      
      end_if;
      
      //increase actual index
      if sUdpVars.ResponseScheduler.ActRequestedIndex < (sUdpVars.ConsumerConnectionNo-1) then
        sUdpVars.ResponseScheduler.ActRequestedIndex += 1;
      else
        //when reaching last entry -> reset index
        sUdpVars.ResponseScheduler.ActRequestedIndex := 0;
      end_if;

      pActResponseEntry := #sUdpVars.paConnectionsToConsumers^[sUdpVars.ResponseScheduler.ActRequestedIndex];
    end_for;
    
  // between the normal requests we do the retries
  elsif ((sUdpVars.ResponseScheduler.CyCnt MOD UDP_RETRY_TIME) = 0) then
    
    pActRetryEntry := #sUdpVars.paConnectionsToConsumers^[0];
    
    // check if we have to send a retry
    for i := 0 to sUdpVars.ConsumerConnectionNo-1 do
      
      // look if we actual wait for a response to send retries and check if we didn't receive data in time
      if (pActRetryEntry^.WaitForResponse = TRUE) & ((ops.tAbsolute - pActRetryEntry^.TLastRequest) > sUdpVars.CalculatedResponseTimeout) then                              

        if pActRetryEntry^.RequestRetries >= UDP_MAX_RETRIES then
        
          pActRetryEntry^.MaxRetryCnt          += 1;
          pActRetryEntry^.WaitForResponse      := FALSE;
          //don't reset flag here, because it's possible we receive an old sequence counter with next package, which leads to an error at FSoE communication
          //pActRetryEntry^.bSequenceCntReceived := FALSE; //reset flag to allow sequence counter for new udp communication
                   
        // only if the IP is valid
        elsif (pActRetryEntry^.IP <> 0) then

          //send retry to actual cpu
          tmpretcode := SendUdpResponse(pActRetryEntry);
          if tmpretcode >= 0 then
            pActRetryEntry^.TLastRequest   := ops.tAbsolute;
            pActRetryEntry^.RequestRetries += 1;
            exit;
          else
            //unable to send retry
            if bd_SingleLog.SendUdpRspRetryFailed = FALSE then
              bd_SingleLog.SendUdpRspRetryFailed := TRUE;
              LogError("@049D (SafetyManager::UdpResponseScheduler) [SL] Failed to send udp retry");
            end_if;
          end_if;
        end_if;
        
      end_if;
      
      pActRetryEntry += sizeof(t_UdpConnection);
      
    end_for;
  
  end_if;

  sUdpVars.ResponseScheduler.CyCnt += 1;
  
END_FUNCTION


FUNCTION SafetyManager::SendUdpResponse
  VAR_INPUT
    pConnection 	: ^t_UdpConnection;
  END_VAR
  VAR_OUTPUT
    retcode 	: DINT;
  END_VAR
  VAR
    udActSendLength : UDINT;
  END_VAR
  
  sUdpVars.pTempSendBufferCy^.Header.Command   := t_UdpCommand::UdpCmd_DataRsp;
  //The current header version is set here so that the other side knows which version is supported.
  sUdpVars.pTempSendBufferCy^.Header.State     := t_UdpPacketHeaderV2.State.IsFromProducer + UDP_HEADER_ACTVERSION;
  sUdpVars.pTempSendBufferCy^.Header.IFCRC     := pConnection^.IFCRC;
  sUdpVars.pTempSendBufferCy^.Header.SafetyNbr := pConnection^.SafetyNbr;   
  
  UdpSendData.SectionStart();
    
  //check for new header version with extended sequence number (4Byte)
  if pConnection^.ActSendData.usHeaderVersion >= UDP_HEADER_VERSION_V2 then  

    sUdpVars.pTempSendBufferCy^.Header.SequenceNumber := pConnection^.ActSendData.udSequenceNumber;
        
    udActSendLength := sizeof(t_UdpPacketHeaderV2);
    
    if pConnection^.ActSendData.PdoLength then
      sUdpVars.pTempSendBufferCy^.Header.PdoDataLength  := pConnection^.ActSendData.PdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^.Data, ptr2:=#pConnection^.ActSendData.PdoData, cntr:=pConnection^.ActSendData.PdoLength);
      udActSendLength += pConnection^.ActSendData.PdoLength;
    else
      sUdpVars.pTempSendBufferCy^.Header.PdoDataLength  := 0;
    end_if;

    if pConnection^.ActSendData.SdoLength then
      sUdpVars.pTempSendBufferCy^.Header.SdoDataLength  := pConnection^.ActSendData.SdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^.Data + pConnection^.ActSendData.PdoLength, ptr2:=#pConnection^.ActSendData.SdoData, cntr:=pConnection^.ActSendData.SdoLength);
      udActSendLength += pConnection^.ActSendData.SdoLength;
    else
      sUdpVars.pTempSendBufferCy^.Header.SdoDataLength  := 0;
    end_if;
  else
    //use old headerversion with 1byte Sequence Counter  
    sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SequenceNumber := pConnection^.ActSendData.usSequenceNumber;
        
    udActSendLength := sizeof(t_UdpPacketHeaderV1);
    
    if pConnection^.ActSendData.PdoLength then
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.PdoDataLength  := pConnection^.ActSendData.PdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Data, ptr2:=#pConnection^.ActSendData.PdoData, cntr:=pConnection^.ActSendData.PdoLength);
      udActSendLength += pConnection^.ActSendData.PdoLength;
    else
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.PdoDataLength  := 0;
    end_if;

    if pConnection^.ActSendData.SdoLength then
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SdoDataLength  := pConnection^.ActSendData.SdoLength;
      _memcpy(ptr1:=#sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Data + pConnection^.ActSendData.PdoLength, ptr2:=#pConnection^.ActSendData.SdoData, cntr:=pConnection^.ActSendData.SdoLength);
      udActSendLength += pConnection^.ActSendData.SdoLength;
    else
      sUdpVars.pTempSendBufferCy^$t_UdpPacketV1.Header.SdoDataLength  := 0;
    end_if;
  
  end_if;
  
  UdpSendData.SectionStop();  
  
  retcode := SendUDPData(sUdpVars.pTempSendBufferCy, udActSendLength, pConnection^.IP); 
  
  sUdpVars.SendCnt += 1;
  
  if retcode < 0 then
    // error at sending
    sUdpVars.LastSendError := retcode;
    sUdpVars.SendErrorCnt += 1;
  else
    // only valid one-time
    pConnection^.ActSendData.SdoLength    := 0;
    
    pConnection^.SentPackages += 1;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::UdpTask
  
  while TRUE do
      
        // if there is no error and we got UDP stuff going on => do it
    if (sUdpVars.ProducerConnectionNo | ((sUdpVars.UdpInit AND 2#11) = (t_UdpInit.FSoEAvailable OR t_UdpInit.ProvidesIF))) then
      if sUdpVars.UdpInit.CallBackInstalled = FALSE then
      
        //add callback info for receiving udp packages
        AddCallBackInfo(this, #ReceiveUDPData());
        
        sUdpVars.UdpInit.CallBackInstalled := TRUE;
      end_if;
    
      SafetyUDP::CyWork(0);
      
      // no SDOs or scheduling when we reinitialize
      if CommInitialized & (InitSSW > _ClearRoutingInfos) then
        // check initialisation of the connections
        if (sUdpVars.ConnectionsInitialized = FALSE) & ((LastUdpIfCfgDistribution = 0) | ((ops.tAbsolute - LastUdpIfCfgDistribution) > SM_INTERFACE_CONFIG_SEND_PAUSE)) then
        
          ShareUdpConnectionInfos();

          // remember when the config has been distributed, to not spam the other sides
          LastUdpIfCfgDistribution := ops.tAbsolute;          
        end_if;

        // only if we're a consumer and have producers to contact
        if sUdpVars.ProducerConnectionNo then
        
          // work the connections (send requests)
          UdpRequestScheduler();
          
          // forward the SDOs from the producer (responses)
          ForwardReceivedUdpSDOs(paConnections:=sUdpVars.paConnectionsToProducers, ConnectionNo:=sUdpVars.ProducerConnectionNo);
        end_if;
      
        // if there are consumers, we may distribute new SDO data
        if sUdpVars.ConsumerConnectionNo then
        
          // work the connections (send responses)
          UdpResponseScheduler();
        
          // forward the SDOs from the consumers (requests)
          ForwardReceivedUdpSDOs(paConnections:=sUdpVars.paConnectionsToConsumers, ConnectionNo:=sUdpVars.ConsumerConnectionNo);
        end_if;
      end_if;
    end_if;
    
    To_MultiTask.TASKDELAY(timeout0:=1);
  end_while;

END_FUNCTION


FUNCTION SafetyManager::TimeSyncModuleAvailable
  VAR_INPUT
    pTimeSyncElement 	: ^t_TimeSyncElement;
  END_VAR
  VAR_OUTPUT
    Available 	: BOOL;
  END_VAR

  if pTimeSyncElement^.InfoBits.IsMissingOptional | (pTimeSyncElement^.pSafety & (pTimeSyncElement^.pSafety^.SafetyNbr = 0)) then
    Available := FALSE;
  else
    Available := TRUE;
  end_if;

END_FUNCTION


FUNCTION SafetyManager::ReinitEverything
  VAR
    MyPara 	  : CmdStruct;
    MyResult 	: results;
  END_VAR

  // new safety project detected (different crc) => reinit the system (similar to disconnect)
  MyPara.uiCmd := CMD_SM_DISCONNECT;
  MyPara.aPara[0] := 0; // set length of further parameters to 0
// don't disconnect connected managing modules. if there is any difference in the communication, it will be interchanged via special SDO frames (CMD_SM_IF_MODULE_INFO...)
//  TellAllInterfaceManagingModules(#MyPara, #MyResult);
  TellAllSafetyModules(#MyPara, #MyResult);             // tell all of our local safety modules that there's a new config => reinitialize  

  ClassState := 0;      

  // reset module sync no (no more modules synchron)
  ModuleSyncNo := 0;

  // restart initialisation (maybe we had no clue about interfaceframes when initialising the first time)
  InitSSW := _ClearRoutingInfos;     

END_FUNCTION


FUNCTION SafetyManager::CheckCustomConnectionValid
  VAR_INPUT
    pConnection 	: ^t_FSoEConnectionDetails;
    pIFFSoEInfo 	: ^t_FSoEConnInfo;
  END_VAR
  VAR_OUTPUT
    bIsCustomConnValid 	: BOOL;
  END_VAR
  
  // This method checks if the passed connection and FSoE Interface are a Valid custom connection
  // This check replaces the slave address check for normal FSoE Interface frames. (s. FinalizePDOMapping)
  // (The slave addresses may not valid for custom connections at this point)
   
  bIsCustomConnValid := FALSE;
  
  if pConnection = NIL | pIFFSoEInfo = NIL | pIFFSoEInfo^.pManaging = NIL then
    return;
  end_if;

  
  if pConnection^.Flags.IsMaster then    
    
    // Check for custom ip address in compressed path
    // Check for HWT_ESCAPE_SAFETY_CUSTOM in uncompressed path
    // pConnection^.pSlavePathComp: 1 Byte Type, 4 Byte IF CRC, 4 Byte Custom IP Addr,                                          
    // example : 03 59 8C AE 3D 00 00 00 02 FF FF FF 
    // pIFFSoEInfo^.pManaging^.pModulePath: 2 Byte Length, 1 Byte Domain No, 1 Byte Escape Sequence, 1 Byte Custom IP Addr 
    // example : 03 00 00 8A 02    
    if pConnection^.pSlavePathComp & pConnection^.SlavePathCompLen > 8 &                    // Pointer + length (Custom IP = Byte 9)
       (pConnection^.pSlavePathComp^ = 3) &                                                 // Type Code
       pIFFSoEInfo^.pManaging^.pModulePath & (pIFFSoEInfo^.pManaging^.pModulePath^ >= 3) &  // Pointer + length
       ((pConnection^.pSlavePathComp+5)^$UDINT AND 16#00FFFFFF) = 0 &                       // Custom IP
       (pIFFSoEInfo^.pManaging^.pModulePath+3)^ = HWT_ESCAPE_SAFETY_CUSTOM  then            // Escape Sequence
      // Compare Custom IDs
      if (pConnection^.pSlavePathComp + 8)^ = (pIFFSoEInfo^.pManaging^.pModulePath+4)^ then
        bIsCustomConnValid := TRUE;
      end_if;

    end_if;

  else  
    
    // Check IFCRC in compressed path
    // Check for HWT_ESCAPE_SAFETY_CUSTOM in uncompressed path
    // pConnection^.pSlavePathComp: 1 Byte Type, 4 Byte IF CRC
    // example : 03 59 8C AE 3D FF FF FF
    // pIFFSoEInfo^.pManaging^.pModulePath: 2 Byte Length, 1 Byte Domain No, 1 Byte Escape Sequence, 1 Byte Custom IP Addr 
    // example : 03 00 00 8A 02    
    if pConnection^.pSlavePathComp & pConnection^.SlavePathCompLen > 5 &                    // Pointer + length (IFCRC = Byte 1, 4Byte CRC Data )
       (pConnection^.pSlavePathComp^ = 3) &                                                 // Type Code
       pIFFSoEInfo^.pManaging^.pModulePath & (pIFFSoEInfo^.pManaging^.pModulePath^ >= 3) &  // Pointer + length
       (pIFFSoEInfo^.pManaging^.pModulePath+3)^ = HWT_ESCAPE_SAFETY_CUSTOM then             // Escape Sequence
       
      // Compare IFCRC (Cannot compare Custom IDs because (pConnection^.pSlavePathComp + 8)^ may not be valid yet at this time
      if (pConnection^.pSlavePathComp+1)^$UDINT = pIFFSoEInfo^.FromOtherSide.InterfaceCRC then
        bIsCustomConnValid := TRUE;
      end_if;

    end_if;
  
  end_if;

END_FUNCTION


FUNCTION SafetyManager::ShareSingleConnectionInfos
	VAR_INPUT
		pMasterConnection 	: ^t_FSoEConnectionDetails;
		pModuleConfig 	: ^t_ModuleListElem;
	END_VAR
  VAR
  	pMasterModuleCfg  : ^t_ModuleListElem;
  	pSlaveModuleCfg   : ^t_ModuleListElem;
    pActModuleCfg     : ^t_ModuleListElem;
    pHelpSafety       : ^t_SafetyComponent;
    pMasterSafety     : ^t_SafetyComponent;
    pSlaveSafety      : ^t_SafetyComponent;
    MyPara 	          : CmdStruct;
    MyResult	        : results;
    pPDOReadData      : pVoid;
    i                 : UDINT;
    IsInList          : BOOL;
  END_VAR
  
//*****************************************************************************
//** TELL THE MODULES WHAT DATA TO COPY IN REALTIME                          **
//*****************************************************************************

  // get the master and slave module configuration of this connection
  
  // master module configuration is always the first one in the list
  pMasterModuleCfg  := pModuleConfig;
  
  // search for the slave config via the path stored in the connection  
  pSlaveModuleCfg   := NIL;
  pActModuleCfg     := pModuleConfig;
  while pActModuleCfg do
    if (pActModuleCfg^.ModuleInfo.pPath^ = pMasterConnection^.pSlavePath^) &
       (To_StdLib.MemCmp(pActModuleCfg^.ModuleInfo.pPath+1, pMasterConnection^.pSlavePath+1, pMasterConnection^.pSlavePath^+1) = 0) then
      
      pSlaveModuleCfg := pActModuleCfg;
      exit;
    end_if;
    
    pActModuleCfg := pActModuleCfg^.pNext;
  end_while;

  if pSlaveModuleCfg = NIL then
    return;
  end_if;

  // get the object this pointers via the hardware paths
  pMasterSafety := NIL;
  pSlaveSafety  := NIL;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
    if pHelpSafety^.PathLen & 
       (pMasterModuleCfg^.ModuleInfo.pPath^ = pHelpSafety^.pPath^) & 
       (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pMasterModuleCfg^.ModuleInfo.pPath + 2 + pMasterModuleCfg^.ModuleInfo.pPath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then
      pMasterSafety := pHelpSafety;
      exit;
    else
      pHelpSafety := pHelpSafety^.pNext;
    end_if;
  end_while;

  pHelpSafety := pFirstSafetyComponent;
  while pHelpSafety do
    if pHelpSafety^.PathLen & 
       (pSlaveModuleCfg^.ModuleInfo.pPath^ = pHelpSafety^.pPath^) & 
       (To_StdLib.MemCmp(pHelpSafety^.pPath + 1, pSlaveModuleCfg^.ModuleInfo.pPath + 2 + pSlaveModuleCfg^.ModuleInfo.pPath^ - pHelpSafety^.pPath^, pHelpSafety^.pPath^) = 0) then
      pSlaveSafety := pHelpSafety;
      exit;
    else
      pHelpSafety := pHelpSafety^.pNext;
    end_if;
  end_while;

  if pMasterSafety & pSlaveSafety then
    
    // we start with the safe outputs: get the data pointer from the master and give it to the slave
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@048F (SafetyManager::ShareSingleConnectionInfos) Invalid return value when asking the master safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;

  #ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
    LogValue("@DBG (SafetyManager::ShareSingleConnectionInfos) ConnectionID: {0}, SlaveAddress: {1}", Value:=pMasterConnection^.ConnectionID, Value2:=pMasterConnection^.SlaveAddress);
    LogValue("@DBG (SafetyManager::ShareSingleConnectionInfos) MasterSafetyNbr: {0}, SlaveSafetyNbr: {1}", Value:=pMasterSafety^.SafetyNbr, Value2:=pSlaveSafety^.SafetyNbr);
    LogValue("@DBG (SafetyManager::ShareSingleConnectionInfos) MasterReadOffset: {0}, MasterReadSize: {1}, SlaveWriteOffset: {2}", Value:=pMasterConnection^.MasterReadOffset, Value2:=pMasterConnection^.MasterReadSize, Value3 := pMasterConnection^.SlaveWriteOffset);
  #endif

    // check if it's in the list
    IsInList := FALSE;
    if FSoEInputPDONo then
      for i := 0 to (FSoEInputPDONo-1) do
        if ((aFSoEInputPDOs[i].pSafety = pSlaveSafety)  & (aFSoEInputPDOs[i].pMem = pPDOReadData$DINT + pMasterConnection^.MasterReadOffset)) then
          aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pMasterSafety;
          aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
          IsInList := TRUE;
        end_if;
      end_for;
    end_if;
    
    if IsInList = FALSE then
      MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
      MyPara.aPara[0] := pPDOReadData$DINT + pMasterConnection^.MasterReadOffset;
      MyPara.aPara[2] := pMasterConnection^.OwnerSafetyNbr$DINT;
      MyPara.aPara[3] := pMasterConnection^.MasterReadSize;
      MyPara.aPara[4] := pMasterSafety^.InfoBits.IsOptional;
      MyPara.aPara[7] := pMasterConnection^.SlaveWriteOffset;
      MyPara.aPara[8] := SM_IF_VALIDITY_MARK$DINT;
      MyPara.aPara[9] := pMasterConnection^.Flags.IsInterfaceConnection;
      pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

      if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
        aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pSlaveSafety;
        aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
        FSoEInputPDONo += 1;
      else
        LogValue("@0490 (SafetyManager::ShareSingleConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
      end_if;    
    end_if;

    // safe inputs: get the data pointer from the slave and give it to the master
    MyPara.uiCmd    := CMD_SM_ADD_OUTPUT_PDO;
    MyResult.uiLng  := 0;
    pSlaveSafety^.pThis^.NewInst(#MyPara, #MyResult);

    if MyResult.uiLng <> 4 then
      LogError("@0491 (SafetyManager::ShareSingleConnectionInfos) Invalid return value when asking the slave safety module for it's pointer to the PDO read data!");
      return;
    end_if;
    
    pPDOReadData := MyResult.aData[0]$pVoid;

  #ifdef SAFETY_DEBUG_LOGGING_ACTIVATED      
  #pragma message("RamAnd 20.11.2014 16:41 \ testweise mapping loggen")
    LogValue("@DBG (SafetyManager::ShareSingleConnectionInfos) SlaveReadOffset: {0}, SlaveReadSize: {1}, MasterWriteOffset: {2}", Value:=pMasterConnection^.SlaveReadOffset, Value2:=pMasterConnection^.MasterWriteSize, Value3 := pMasterConnection^.MasterWriteOffset);
  #endif

    // check if it's in the list
    IsInList := FALSE;
    if FSoEInputPDONo then
      for i := 0 to (FSoEInputPDONo-1) do
        if ((aFSoEInputPDOs[i].pSafety = pMasterSafety)  & (aFSoEInputPDOs[i].pMem = pPDOReadData$DINT + pMasterConnection^.SlaveReadOffset)) then
          aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pMasterSafety;
          aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
          IsInList := TRUE;
        end_if;
      end_for;
    end_if;

    if IsInList = FALSE then

      MyPara.uiCmd    := CMD_SM_ADD_INPUT_PDO;
      MyPara.aPara[0] := pPDOReadData$DINT + pMasterConnection^.SlaveReadOffset;
      MyPara.aPara[2] := pSlaveSafety^.SafetyNbr$DINT;
      MyPara.aPara[3] := pMasterConnection^.MasterWriteSize;
      MyPara.aPara[4] := pSlaveSafety^.InfoBits.IsOptional;
      MyPara.aPara[7] := pMasterConnection^.MasterWriteOffset;
      MyPara.aPara[8] := 0;
      MyPara.aPara[9] := 0;
      pMasterSafety^.pThis^.NewInst(#MyPara, #MyResult);
      
      if FSoEInputPDONo < MAX_FSOE_CONNECTIONS then
        aFSoEInputPDOs[FSoEInputPDONo].pSafety    := pMasterSafety;
        aFSoEInputPDOs[FSoEInputPDONo].pMem       := MyPara.aPara[0];
        FSoEInputPDONo += 1;
      else
        LogValue("@0490 (SafetyManager::ShareSingleConnectionInfos) Maximum limit of allowed FSoE input Frames exceeded (0x{0})", Value:=MAX_FSOE_CONNECTIONS);
      end_if;
    end_if;

    // remember that we already dealt with this connection
    pMasterConnection^.InternalConnectionState := FCS_CONNECTION;
  
  end_if;

END_FUNCTION


FUNCTION SafetyManager::GetObjName
	VAR_INPUT
		pThis 	: pVirtualBase;
		pTargetString 	: ^CHAR;
		TargetStringSize 	: UDINT;
	END_VAR
  VAR
  	pObjName : ^CHAR;
    ObjNameLen : UDINT;
    pEmbObjStr : ^CHAR;
    EmbObjLen : UDINT;
  END_VAR

  // check for valid this pointer
  if pThis then
    
    // get the name of the object
    pObjName := LSL_GetObjName(pThis);
    
    // check for valid name pointer
    if pObjName then
      // store the object name
      
      // cut the embedded object name for SCPs
      pEmbObjStr := "\SDIASSAFETYMANAGERFSOE1";
      EmbObjLen  := _strlen(src:=pEmbObjStr);
      ObjNameLen := _strlen(pObjName);
      if (ObjNameLen > EmbObjLen) & (_strcmp(str1:=pObjName + (ObjNameLen - EmbObjLen), str2:=pEmbObjStr) = 0) then
        ObjNameLen -= EmbObjLen;
      end_if;
      
      if ObjNameLen < TargetStringSize then
        _memcpy(pTargetString, pObjName, ObjNameLen);
        (pTargetString+ObjNameLen)^ := 0; // Insert 0 Termination
      else
        _memcpy(pTargetString, pObjName, TargetStringSize-1);
        (pTargetString + TargetStringSize-1) ^ := 0;  // Insert 0 Termination
      end_if;
    end_if;
  end_if;

END_FUNCTION
