//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

// always allocate blocks for 5 more SDOs
#define MDD2000VVSB_SDO_BUFFER_ALLOC_BLOCK 5

// time between the CRC-Checks (CRC-Check to find out if configuration has changed)
#define MDD2000VVSB_CRC_CHECK_TIME  5000

// time between the system time of plc is written to scp firmware
#define MDD2000VVSB_SYSTIME_SET_TIME  (5 SEC)  //(24 hours)

// time between we read out the retrycounter from the module.
#define MDD2000VVSB_RETRY_COUNTER_WAIT_TIME 10000

// time between the check of the flex con state
#define MDD2000VVSB_FLEX_CON_STATE_CHECK_TIME 2000

#define MDD2000VVSB_TDO_BUFFER_SIZE 50

// time for SDO transfer
#define MDD2000VVSB_SDO_TRANSFERTIME 30 // maximum measured time at mover was 22ms + 3ms reserve

// timeout for optional modules to be synchron
#define MDD2000VVSB_OPTIONAL_MODULE_SYNC_TIMEOUT  5000

// maximum number of tries to execute an asynchronous command (5 = if 5 consecutive tries to make an asynchronous communication fail, the class goes into error _AsyncComError)
#define MDD2000VVSB_SDO_MAX_TRIES 5

#define MDD2000VVSB_RETRY_COUNTER_AVAILABLE_MAJOR_REV 1000
#define MDD2000VVSB_RETRY_COUNTER_AVAILABLE_MINOR_REV 338

#define MDD2000VVSB_MAX_BUS_USAGE         128
#define MDD2000VVSB_BUS_USAGE_PER_PDO_DO  127


#define MDD2000VVSB_SDO_RESPONSE_TIMEOUT                500
#define MDD2000VVSB_SDO_MAX_RETRIES                     5
#define MDD2000VVSB_SDO_MAX_DATA_PER_PAKET              56

// if defined, the DO traffic is monitored
//#define DO_DEBUG
#ifdef DO_DEBUG
#pragma message("DO_DEBUG for Safety-Modules activated! Needs more CPU ressources!")
#endif

//}}LSL_DEFINES

//{{LSL_DECLARATION
#include "..\..\Class\SafetyManager\SafetyManager.h"
#include "..\..\Class\SafetyBaseFSoE\SafetyBaseFSoE.h"
#include "..\..\Source\interfaces\lsl_st_safety.h"
#include "..\..\Class\HwControl\HwControl.h"
#include "..\..\Source\BusInterface.h"
#include "..\..\Source\BusIFVVDrive.h"
#include "..\..\class\_drivemngbase\drivemngbase.h"
#include "..\..\Class\MDD2000MngBase\MDD2000MngBase.h"

(*!
<Class
	Name               = "MDD2000VVSafetyBase"
	Revision           = "1.1"
	GUID               = "{6CF7096D-5CC3-4836-984E-3FA0F6643EDB}"
	RealtimeTask       = "false"
	CyclicTask         = "false"
	BackgroundTask     = "false"
	DefBackground      = "100 ms"
	Sigmatek           = "true"
	OSInterface        = "false"
	HighPriority       = "false"
	Automatic          = "false"
	UpdateMode         = "Prescan"
	IconPath           = ".\Class\Safety.ico"
	SharedCommandTable = "true"
	Objectsize         = "(780,360)">
	<Channels>
		<Server Name="ClassSvr" GUID="{8DFFD4B8-8D36-484E-B247-17F83473C828}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false"/>
		<Server Name="FirmwareVersion" GUID="{D0DCE873-BA52-4336-89DC-4ABAB56F9C8E}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Firmware version&#13;&#10;&#13;&#10;Format:&#13;&#10;16#XXXXYYYY&#13;&#10;&#13;&#10;XXXX = Major Revision&#13;&#10;YYYY = Minor Revision"/>
		<Server Name="SafetyNumber" GUID="{38EC6E8A-8D64-462E-ABD4-79F68C465F26}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows the unique safety number of this module"/>
		<Server Name="SafetyState" GUID="{ECD8076F-94EE-4BF7-BBB1-6E6F426BC7BD}" Visualized="true" Initialize="false" WriteProtected="true" Retentive="false" Comment="Shows state of safety module&#13;&#10;&#13;&#10;_ModuleNotFound:&#13;&#10; - Module has not been found at the given position&#13;&#10;&#13;&#10;_SafetyClassOK:&#13;&#10; - Module has been found and no errors reported&#13;&#10;&#13;&#10;_MemAllocFailed: &#13;&#10; - Failed to allocate or reallocate some memory for the module&#13;&#10;&#13;&#10;_ReadFWVerFailed:&#13;&#10; - Failed to read firmware version of module"/>
		<Client Name="ToBusInterface" Required="true" Internal="false" Comment="This client must be connected to a FSoE Interface."/>
		<Client Name="ToMultiTask" Required="false" Internal="false"/>
		<Client Name="ToSigCLib" Required="false" Internal="false" Comment="Is automatically connected to &quot;SigClib&quot;."/>
	</Channels>
	<Dependencies>
		<Files>
			<File Path=".\Class\SafetyManager\SafetyManager.h" Include="true"/>
			<File Path=".\Class\SafetyBaseFSoE\SafetyBaseFSoE.h" Include="true"/>
			<File Path=".\Source\interfaces\lsl_st_safety.h" Include="true"/>
			<File Path=".\Class\HwControl\HwControl.h" Include="true"/>
			<File Path=".\Source\BusInterface.h" Include="true"/>
			<File Path=".\Source\BusIFVVDrive.h" Include="true"/>
			<File Path=".\class\_drivemngbase\drivemngbase.h" Include="true"/>
			<File Path=".\Class\MDD2000MngBase\MDD2000MngBase.h" Include="true"/>
		</Files>
	</Dependencies>
	<RevDoku>
		<Owner Company="Sigmatek" Author="LanSte"/>
		<Dokumentation Revision="1.1" Date="12.12.2024" Author="LanSte" Company="Sigmatek" Description="Correced bound exceed error in StopDO()"/>
		<Dokumentation Revision="1.0" Date="28.08.2023" Author="LanSte" Company="Sigmatek" Description="First library version."/>
	</RevDoku>
</Class>
*)
MDD2000VVSafetyBase : CLASS
	TYPE
	  CommonFlags : BDINT
	  [
	    1 bOnline,
	    2 bDeactivated,
	    3 bReconnect,
	    4 bVaranAvailable,
	    5 bReInitMarker,
	    6 bPostInitPassed,
	    7 bDeviceIDOK,
	    8 Bit8,
	    9 Bit9,
	    10 Bit10,
	    11 Bit11,
	    12 Bit12,
	    13 Bit13,
	    14 Bit14,
	    15 Bit15,
	    16 bManagerReadyForSDO,
	    17 Bit17,
	    18 bCheckConfigured,
	    19 bReadConfigFailed,
	    20 bQuitErrorAvailable,
	    21 bManagerInitialisationFinished,
	    22 bManualQuit,
	    23 bPDOOutputIsUsed,
	    24 bSDOsViaOS,
	    25 bPdosCreated,
	    26 bSdosCreated,
	    27 bSdoMechanismInitialized,
	    28 bReadFktFW,
	    29 bNoPDOData,
	    30 Bit30,
	    31 Bit31,
	    32 Bit32,
	  ];
	  eSSWConfigChanged :
	  (
	    _IdleConfigChanged,
	    _StartConfigChanged,
	    _Wait4Rt,
	    _UpdatePDOs,
	    _FinishedConfigChanged,
	    _ErrorConfigChanged
	  )$UDINT;
	  eSSWFwInfo :
	  (
	    _IdleFwInfo,
	    _GetSSDOMailbox,
	    _RequSafetyNrb,
	    _Wait4SafetyNrb,
	    _RequFwVersion,
	    _Wait4FwVersion,
	    _RequPDOLengthWr,
	    _Wait4PDOLengthWr,
	    _RequPDOLengthRd,
	    _Wait4PDOLengthRd,
	    _CreatePDOs,
	    _SendInfoToMng,
	    _FinishedFwInfo,
	    _ErrorFwInfo
	  )$UDINT;
	  eSSWReadSDO :
	  (
	    _ReadMBFooter,
	    _ReadMBData
	  )$UDINT;
#pragma pack(push, 1)
	  FirmwareVersionType : STRUCT
	    RevMinor : UDINT;
	    RevMajor : UINT;
	    ModuleType : UINT;
	    BootLoaderVer : UDINT;
	  END_STRUCT;
#pragma pack(pop)
	  FrameTypeType : BSINT
	  [
	    1 Cfg,
	    2 ToP,
	    3 Standard,
	    4 PDO,
	    5 TDO,
	    6 Reserved1,
	    7 Reserved2,
	    8 Response,
	  ];
#pragma pack(push, 1)
	  PDOType : STRUCT
	    CRC : HDINT;
	    Byte5_Zero : USINT;
	    FrameType : FrameTypeType;
	    Byte7_Zero : USINT;
	    Length : USINT;
	    SourceAdr : HDINT;
	    SourceAdrCopy : HDINT;
	    msTimeStamp : DINT;
	    DataAndCopy : ARRAY [0..107] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  RoutElement : STRUCT
	    ud_SafeNumber : HDINT;
	    ud_Thisp : UDINT;
	    p_CompressedPath : ^USINT;
	    ui_ComprPathLen : UINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDOHeader : STRUCT  //! <Type Public="true" Name="SDOHeader"/>
	    StdHeader : t_SDOStdHeader;
	    SrcAdr : HDINT;
	    DstAdr : HDINT;
	    SessId : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDORespStd : STRUCT
	    SDOHeader : SDOHeader;
	    us_Returncode : USINT;
	    aData : ARRAY [0..SBF_SDO_MAX_LENGTH-22] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDOBufferStructure : STRUCT
	    us_Status : USINT;
	    SDOStructure : SDORespStd;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDORequReadConfig : STRUCT
	    StdHeader : t_SDOStdHeader;
	    ud_Address : HDINT;
	    us_Length : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDORespReadConfig : STRUCT
	    StdHeader : t_SDOStdHeader;
	    us_Returncode : USINT;
	    us_Length : USINT;
	    ud_Data : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDOStdResp : STRUCT
	    StdHeader : t_SDOStdHeader;
	    us_Returncode : USINT;
	    us_Data : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  SDOviaPath : STRUCT
	    StdHeader : t_SDOStdHeader;
	    ZeroBytes : DINT;
	    TopologyPath : ARRAY [0..35] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  StandardFrameType : STRUCT
	    SDOHeader : t_SDOStdHeader;
	    aData : ARRAY [0..63] OF USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_DataObjectCheck : STRUCT
	    LastTime : UDINT;
	    TimeDiff : UDINT;
	    TimeDiffMin : UDINT;
	    TimeDiffMax : UDINT;
	    SameFrameCnt : DINT;
	    LastHeader : t_SDOStdHeader;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_DOInfo : STRUCT
	    pData : pVoid;
	    Handle : UDINT;
	    Length : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_FSoEConnections : STRUCT
	    ConnectionNo : UDINT;
	    pFirstConnection : ^t_FSoEConnectionDetails;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_IFConnInfoLocal : STRUCT
	    uiHwPathLen : UINT;
	    pHwPath : ^USINT;
	    uiCompHwPath : UINT;
	    pCompHwPath : ^USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_LastSDOCmd : STRUCT
	    StructItem : DINT;
	  END_STRUCT;
#pragma pack(pop)
	  t_LogOnceMarker : BDINT
	  [
	    1 NoThisForUnsafeIn,
	    2 WrongUnsafeVarType,
	    4 InvalidSchedulerState,
	    5 InvalidNewInstCmd,
	    7 RdPDOSizeInc,
	    8 WrPDOSizeInc,
	    9 RdPDOSizeDec,
	    10 WrPDOSizeDec,
	  ];
#pragma pack(push, 1)
	  t_PDOInputBuffer : STRUCT
	    ud_p2Buffer : UDINT;
	    ui_PDOLen : UINT;
	    b_TimeSynchron : BOOL;
	    ud_SafetyNr : HDINT;
	    b_IsOptional : BOOL;
	    b_IsInterfaceFrame : BOOL;
	    ui_TargetOffset : UINT;
	    p_TargetMemory : pVoid;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_PDOSizeInfo : STRUCT
	    Version : UDINT;
	    PDORdLen : UINT;
	    PDOWrLen : UINT;
	    FastUnsafeAvailable : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  t_SDORequStdRd : STRUCT
	    StdHeader : t_SDOStdHeader;
	    udAddress : UDINT;
	    usLength : USINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  tsAccesses : STRUCT
	    tPdoRd : STRUCT
	      uiMyPDOSize : UINT;
	      uiMaxPDOSize : UINT;
	      uiReadDOsUsed : UINT;
	      uiMaxReadDOsUsed : UINT;
	      aPDOs : ARRAY [0..8] OF t_DOInfo;
	      PDORead : t_DataObjectCheck;
	      aPDOOutputBuffer : ARRAY [0..SBF_SPDO_MAX_LENGTH-1] OF USINT;
	      sMailboxInfo : t_MapEntry;
	      pMBFooter : ^tsMailboxFooter;
	      usOldMsgCnt : USINT;
	    END_STRUCT;
	    tPdoWr : STRUCT
	      uiMyPDOSize : UINT;
	      uiMaxPDOSize : UINT;
	      uiWriteDOsUsed : UINT;
	      uiMaxWriteDOsUsed : UINT;
	      aPDOs : ARRAY [0..8] OF t_DOInfo;
	      PDOWrite : t_DataObjectCheck;
	      aPDOInputBuffer : ARRAY [0..SBF_SPDO_MAX_LENGTH-1] OF USINT;
	      sMailboxInfo : t_MapEntry;
	      sMBFooter : tsMailboxFooter;
	    END_STRUCT;
	    tSDORd : STRUCT
	      udInputBufferAllocatedNo : UDINT;
	      sActSDO : SDOBufferStructure;
	      pMBFooter : ^tsMailboxFooter;
	      SDORead : t_DataObjectCheck;
	      sDOInfo : t_DOInfo;
	      sMailboxInfo : t_MapEntry;
	      hdActOffset : HDINT;
	      udActLength : UDINT;
	      usOldMsgCnt : USINT;
	      SSWMailboxSDO : eSSWReadSDO;
	    END_STRUCT;
	    tSDOWr : STRUCT
	      p_SDOOutputBuffer : ^SDORespStd;
	      uiCntSDOInputBuffer : UINT;
	      uiCntSDOInputBufferAllocated : UINT;
	      p_SDOInputBuffer : ^UDINT;
	      sActSDO : SDOBufferStructure;
	      sMBFooter : tsMailboxFooter;
	      SDOWrite : t_DataObjectCheck;
	      sDOInfo : t_DOInfo;
	      sMailboxInfo : t_MapEntry;
	      hdActOffset : HDINT;
	      udActLength : UDINT;
	      usOldMsgCnt : USINT;
	      usStdSeqCnt : USINT;
	    END_STRUCT;
	    DOStopFailCounter : UDINT;
	    DOStartFailCounter : UDINT;
	    InvalidPDOSizeCnt : UDINT;
	  END_STRUCT;
#pragma pack(pop)
#pragma pack(push, 1)
	  tsFwInfos : STRUCT
	    SSWFwInfo : eSSWFwInfo;
	    SSWFwInfoError : eSSWFwInfo;
	    udTimeStamp : UDINT;
	    usRetries : USINT;
	    bdInfosGet : BDINT
	    [
	      1 SaferyNrb,
	      2 FwVersion,
	      3 Bit3,
	      4 Bit4,
	      5 Bit5,
	      6 Bit6,
	      7 Bit7,
	      8 Bit8,
	      9 Bit9,
	      10 Bit10,
	      11 Bit11,
	      12 Bit12,
	      13 Bit13,
	      14 Bit14,
	      15 Bit15,
	      16 Bit16,
	      17 Bit17,
	      18 Bit18,
	      19 Bit19,
	      20 Bit20,
	      21 Bit21,
	      22 Bit22,
	      23 Bit23,
	      24 Bit24,
	      25 Bit25,
	      26 Bit26,
	      27 Bit27,
	      28 Bit28,
	      29 Bit29,
	      30 Bit30,
	      31 Bit31,
	      32 Bit32,
	    ];
	    FwVersion : FirmwareVersionType;
	    sPdoInfo : t_PDOSizeInfo;
	  END_STRUCT;
#pragma pack(pop)
	END_TYPE
  //Servers:
	ClassSvr 	: SvrChCmd_t_e_VaranErrors;
	SafetyState 	: SvrCh_SafetyConfigStateType;
	FirmwareVersion 	: SvrCh_HDINT;
	SafetyNumber 	: SvrCh_HDINT;
  //Clients:
	ToBusInterface 	: CltChCmd_DINT;
	ToSigCLib 	: CltChCmd_SigCLib;
	ToMultiTask 	: CltChCmd__MultiTask;
  //Variables:
		usFirstScan 	: USINT;
		usInitStep 	: DINT;			//! <Variable Comment="contains the actual init step (0-2)" Name="usInitStep"/>
		sAccesses 	: tsAccesses;
		sFwInfos 	: tsFwInfos;
		sConnInfo 	: t_IFConnInfoLocal;
		DeviceID 	: UDINT;
		p_ObjectPath 	: ^USINT;			//! <Variable Comment="pointer to the object path which is needed to add the module to the safety task and get the pointers for SDO handling from the OS" Name="p_ObjectPath"/>
		bdCommonFlags 	: CommonFlags;
		bd_LogOnceMarker 	: t_LogOnceMarker;			//! <Variable Comment="if an error message should be printed only once even if the error occurs multiple times, this variable is used to remember the messages already printed" Name="bd_LogOnceMarker"/>
		bd_LogCy 	: t_LogOnceMarker;
		bd_LogCyDone 	: t_LogOnceMarker;
		OldFakePDORead 	: PDOType;
		OldFakePDOWrite 	: PDOType;
		ui_cnt_PDOInputBuffer 	: UINT;
		aPDOInputBuffer : ARRAY [0..SBF_PDO_MAX_NUMBER-1] OF t_PDOInputBuffer;

		pSDOLogin 	: pVirtualBase;
		pISafety 	: ^LSL_SAFETY;
		p_Handle_SDO_Read 	: ^SDOBufferStructure;
		p_Handle_SDO_Write 	: ^SDOBufferStructure;
		StandardResponse 	: StandardFrameType;
		bDS402Send 	: BOOL;
		DS402DataBuffer : ARRAY [0..2] OF DINT;

		FwFktVersion 	: DINT;
		SSWConfigChanged 	: eSSWConfigChanged;
		bdSafetyConfigChangedResult : BDINT
		[
		  1 ConfigChanged,
		  2 RdPDOSizeInc,
		  3 WrPDOSizeInc,
		  4 RdPDOSizeDec,
		  5 WrPDOSizeDec,
		];

  //Functions:
	
	FUNCTION VIRTUAL GLOBAL Init;
				//! <Function Comment="constructor" Name="MDD2000VVSafetyBase"/>
	FUNCTION MDD2000VVSafetyBase
		VAR_OUTPUT
			ret_code 	: ConfStates;
		END_VAR;
	
	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 Comment="add data objects for PDO, state etc." Name="AddSPDO"/>
	FUNCTION VIRTUAL AddSPDO
		VAR_INPUT
			bAddLater 	: BOOL;
		END_VAR
		VAR_OUTPUT
			ret_code 	: iprStates;
		END_VAR;
	
	FUNCTION VIRTUAL AddSSDO
		VAR_OUTPUT
			ret_code 	: iprStates;
		END_VAR;
	
	FUNCTION VIRTUAL AddFrame
		VAR_INPUT
			pFrame 	: ^t_SDOStdHeader;
			FrameState 	: DINT;
			CallerSafetyNr 	: HDINT;
		END_VAR;
				//! <Function Comment="add SDO to incoming message buffer; This function is called from the SDO sending class." Name="AddSDOToBuffer"/>
	FUNCTION VIRTUAL AddSDOToBuffer
		VAR_INPUT
			p_us_message 	: ^USINT;
			us_length 	: USINT;
		END_VAR
		VAR_OUTPUT
			sd_retval 	: DINT;
		END_VAR;
	
	FUNCTION ChangeRdDO
		VAR_INPUT
			AccessHandle 	: UDINT;
			NewOffset 	: HDINT;
			NewLength 	: UDINT;
		END_VAR
		VAR_OUTPUT
			Success 	: BOOL;
		END_VAR;
	
	FUNCTION ChangeWrDO
		VAR_INPUT
			AccessHandle 	: UDINT;
			NewOffset 	: HDINT;
			NewLength 	: UDINT;
		END_VAR
		VAR_OUTPUT
			Success 	: BOOL;
		END_VAR;
	
	FUNCTION VIRTUAL GetFWInfos
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
				//! <Function Comment="returns pointer to the own outgoing PDO buffer" Name="GetPointer2PDOBuffer"/>
	FUNCTION VIRTUAL GetPointer2PDOBuffer
		VAR_OUTPUT
			ud_PDOBuffer 	: UDINT;
		END_VAR;
	
	FUNCTION VIRTUAL InitBuffers
		VAR_OUTPUT
			sd_retval 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL InitSafetyBase
		VAR_OUTPUT
			retcode 	: BOOL;
		END_VAR;
				//! <Function Comment="log error messages" Name="LogError"/>
	FUNCTION LogError
		VAR_INPUT
			e_msg 	: ^CHAR;
		END_VAR;
	
	FUNCTION LogValue
		VAR_INPUT
			pString 	: ^CHAR;
			Value 	: UDINT;
			Value2 	: UDINT := 16#FFFFFFFF;
			Value3 	: UDINT := 16#FFFFFFFF;
		END_VAR;
	
	FUNCTION VIRTUAL RecordFrame
		VAR_INPUT
			pFrame 	: ^DINT;
			FrameState 	: DINT;
			IsFSoEPDO 	: BOOL := FALSE;
		END_VAR;
	
	FUNCTION VIRTUAL SDOStdRequ
		VAR_INPUT
			us_Command 	: USINT;
			usLength 	: USINT := 0;
			hdAddress 	: HDINT := 0;
		END_VAR
		VAR_OUTPUT
			sd_retval 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL SetDOAddresses
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION SetDORunState
		VAR_INPUT
			Handle 	: UDINT;
			NewState 	: t_BusIFRunState;
		END_VAR;
	
	FUNCTION VIRTUAL SetSafetyState
		VAR_INPUT
			NewState 	: SafetyConfigStateType;
		END_VAR;
				//! <Function Comment="start data objects" Name="StartDO"/>
	FUNCTION VIRTUAL StartDO;
				//! <Function Comment="stop data objects" Name="StopDO"/>
	FUNCTION VIRTUAL StopDO;
				//! <Function Comment="write incoming PDO from other module to hardware " Name="WritePDO2Module"/>
	FUNCTION VIRTUAL WritePDO2Module;
	
	FUNCTION VIRTUAL GLOBAL ConnectEvent;
	
	FUNCTION VIRTUAL GLOBAL DisconnectEvent;
	
	FUNCTION VIRTUAL GLOBAL InterruptEvent;
	
	FUNCTION VIRTUAL GLOBAL AddAccesses
		VAR_INPUT
			bAddLater 	: BOOL;
		END_VAR
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION VIRTUAL GetPDOInputBuffer
		VAR_INPUT
			ud_p2Buffer 	: UDINT;
			ud_SafetyNr 	: HDINT;
			p_FoundSafetyNr 	: ^BOOL := NIL;
			b_IsInterfaceFrame 	: BOOL;
			ui_TargetOffset 	: UINT;
		END_VAR
		VAR_OUTPUT
			p_InputBuffer 	: ^t_PDOInputBuffer;
		END_VAR;
	
	FUNCTION VIRTUAL AddPDOToInputBuffer
		VAR_INPUT
			ud_p2Buffer 	: UDINT;
			ud_SafetyNr 	: HDINT;
			b_IsOptional 	: BOOL;
			ui_PdoLen 	: UINT := 0;
			ui_TargetOffset 	: UINT := 0;
			b_IsInterfaceFrame 	: BOOL := FALSE;
		END_VAR
		VAR_OUTPUT
			sd_retval 	: DINT;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL SDOLogin
		VAR_INPUT
			pThis 	: pVirtualBase;
		END_VAR
		VAR_OUTPUT
			ret_code 	: iprStates;
		END_VAR;
	
	FUNCTION InitSDOsViaHWK;
	
	FUNCTION InitSDOMechanism;
	
	FUNCTION VIRTUAL ReadDriveFWVersion
		VAR_OUTPUT
			dRetCode 	: iprStates;
		END_VAR;
	
	FUNCTION FSoEData2FrameLen
		VAR_INPUT
			FSoEDataLen 	: DINT;
		END_VAR
		VAR_OUTPUT
			FSoEFrameLen 	: UINT;
		END_VAR;
	
	FUNCTION ChangeConfigState
		VAR_INPUT
			CallRt 	: BOOL;
		END_VAR;
	
	FUNCTION VIRTUAL GLOBAL ClassSvr::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 usingLtd _MultiTask
#pragma usingLtd SigCLib


//}}LSL_DECLARATION


FUNCTION GLOBAL TAB MDD2000VVSafetyBase::@CT_
0$UINT,
2#0100000000000000$UINT, //TY_MDD2000VVSAFETYBASE
1$UINT, 1$UINT, (SIZEOF(::MDD2000VVSafetyBase))$UINT, 
4$UINT, 3$UINT, 0$UINT, 
TO_UDINT(2110734413), "MDD2000VVSafetyBase", //Class
TO_UDINT(0), 0, 0$UINT, 0$UINT, //Baseclass
//Servers:
(::MDD2000VVSafetyBase.ClassSvr.pMeth)$UINT, _CH_CMD$UINT, 2#0000000000000000$UINT, TO_UDINT(619352855), "ClassSvr", 
(::MDD2000VVSafetyBase.SafetyState.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(1504224158), "SafetyState", 
(::MDD2000VVSafetyBase.FirmwareVersion.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(389440282), "FirmwareVersion", 
(::MDD2000VVSafetyBase.SafetyNumber.pMeth)$UINT, _CH_SVR$UINT, 2#0000000000000000$UINT, TO_UDINT(2430374202), "SafetyNumber", 
//Clients:
(::MDD2000VVSafetyBase.ToBusInterface.pCh)$UINT, _CH_CLT_CMD$UINT, 2#0000000000000010$UINT, TO_UDINT(1682766337), "ToBusInterface", 
(::MDD2000VVSafetyBase.ToSigCLib.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000000$UINT, TO_UDINT(2700849093), "ToSigCLib", TO_UDINT(77440046), "SigCLib", 0$UINT, 42$UINT, 
(::MDD2000VVSafetyBase.ToMultiTask.pCh)$UINT, _CH_CLT_OBJ$UINT, 2#0000000000000000$UINT, TO_UDINT(4206025932), "ToMultiTask", TO_UDINT(2641766936), "_MultiTask", 1$UINT, 1$UINT, 
END_FUNCTION


#define USER_CNT_MDD2000VVSafetyBase 23

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

FUNCTION MDD2000VVSafetyBase::@STD
	VAR_OUTPUT
		ret_code	: CONFSTATES;
	END_VAR
	VAR
		vmt	: _LSL_STD_VMETH;
	END_VAR

	//Command Methods
	InitCmdTable (nCmd := nSTDCMD + USER_CNT_MDD2000VVSafetyBase, pCmd := #vmt.CmdTable);
	vmt.CmdTable.NewInstr		:= #NewInst();
	vmt.CmdTable.Init		:= #Init();
	vmt.CmdTable.CyWork		:= #CyWork();
	vmt.CmdTable.RtWork		:= #RtWork();
#pragma warning (disable : 74)
	vmt.UserFcts[0]		:= #AddSPDO();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[1]		:= #AddSSDO();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[2]		:= #AddFrame();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[3]		:= #AddSDOToBuffer();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[4]		:= #GetFWInfos();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[5]		:= #GetPointer2PDOBuffer();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[6]		:= #InitBuffers();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[7]		:= #InitSafetyBase();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[8]		:= #RecordFrame();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[9]		:= #SDOStdRequ();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[10]		:= #SetDOAddresses();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[11]		:= #SetSafetyState();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[12]		:= #StartDO();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[13]		:= #StopDO();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[14]		:= #WritePDO2Module();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[15]		:= #ConnectEvent();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[16]		:= #DisconnectEvent();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[17]		:= #InterruptEvent();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[18]		:= #AddAccesses();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[19]		:= #GetPDOInputBuffer();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[20]		:= #AddPDOToInputBuffer();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[21]		:= #SDOLogin();

#pragma warning (default : 74)
#pragma warning (disable : 74)
	vmt.UserFcts[22]		:= #ReadDriveFWVersion();

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

	IF ClassSvr.pMeth THEN
		ret_code	:= C_OK;
	ELSE
		ret_code	:= C_OUTOF_NEAR;
		RETURN;
	END_IF;
	ret_code	:= MDD2000VVSafetyBase();

END_FUNCTION

//{{LSL_IMPLEMENTATION
VAR_EXTERNAL
  pAnalyzerThis   : ^DINT;
  pAnalyzerMethod : ^UDINT;
  pSafetyManagerThis   : pVirtualBase;
END_VAR

FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::Init
  VAR
    MyPara 	  : CmdStruct;
    MyResult	: results;
    dRetCode  : iprStates;
  END_VAR

  // don't initialize anything if the module is deactivated
  if bdCommonFlags.bDeactivated | ClassSvr <> _NotInitialized then
    return;
  end_if;

  case usInitStep of
//**********************************************************************************************************************************************************
    0:
      // first check if the module may be deactivated
      MyPara.uiCmd     := _BUS_IF_GET_DEACTIVATED_STATE;

      ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
        bdCommonFlags.bDeactivated := MyResult.aData[4]$BOOL;
        if bdCommonFlags.bDeactivated then
          ClassSvr := _NoHardware;
        end_if;
      else
        LogError("@01D6 (MDD2000VVSafetyBase::Init) Failed to get deactivation state via BusInterface.");
        bdCommonFlags.bDeactivated := TRUE;
      end_if;

      if bdCommonFlags.bDeactivated = FALSE then
        if InitSafetyBase() then
          usInitStep += 1;
        end_if;
      end_if;

//**********************************************************************************************************************************************************
    1:   
      // initialize
      usInitStep += 1;

//**********************************************************************************************************************************************************
   2:
      // initialize safety module specific data (cpu, input, output) in last init and send OK to CPU
      if usFirstScan = 3 then

        // tell the safety manager, that there is a FSoE module
        if pSafetyManagerThis then
          MyPara.uiCmd := CMD_SM_FSOE_AVAILABLE;
          pSafetyManagerThis^.NewInst(#MyPara, #MyResult);
        end_if;
      elsif (usFirstScan = 4) then

        //get device ID
        MyPara.uiCmd    := _BUS_IF_GET_PARAM;
        MyPara.aPara[0] := _BUS_IF_DEVICEID;
        ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

        if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
          LogError("@0750 (MDD2000VVSafetyBase::Init) Failed to get Parameter from BusIF");
          ClassSvr := _IDNotFound;
          return;
        else
          //store device id
          DeviceID:= MyResult.aData[4]$UDINT;
        end_if;

        // log in to safety manager
        if pSafetyManagerThis then
          MyPara.uiCmd := CMD_SM_ADD_MODULE;
          MyPara.aPara[0] := _ModuleWithoutConfig$DINT;
          MyPara.aPara[1] := THIS$DINT;
          MyPara.aPara[2] := sConnInfo.pHwPath$DINT;

          pSafetyManagerThis^.NewInst(#MyPara, #MyResult);

          // check if quit error is available
          MyPara.uiCmd := CMD_SM_QUIT_ERROR_AVAILABLE;
          MyPara.aPara[0] := THIS$DINT;
          pSafetyManagerThis^.NewInst(#MyPara, #MyResult);

          bdCommonFlags.bQuitErrorAvailable := MyResult.aData[0]$DINT <> 0;
        else
          // if there is no safety manager => problem, because this class won't work without
          ClassSvr := _ManagerNotExists;
        end_if;

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

  // tell class that safety doesn't work
  if ClassSvr <> _NotInitialized then
    MyPara.uiCmd    := _BUSIF_VVDRIVE_SET_SAFETY_STATE;
    MyPara.aPara[0] := 0; //deactivate safety
    dRetCode := ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

    if dRetCode <> READY then
      LogError("@081A (MDD2000VVSafetyBase::Init) Failed to set safety state at class.");
    end_if;
  end_if;

  usFirstScan += 1;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::AddAccesses
	VAR_INPUT
		bAddLater 	: BOOL;
	END_VAR
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  
  // For FSoE PDO must be sent in both direction. 
  if (sFwInfos.sPdoInfo.PDORdLen = 0) | 
     (sFwInfos.sPdoInfo.PDOWrLen = 0) then

    bdCommonFlags.bNoPDOData := TRUE;
    dRetCode := READY;  //return ready -> no safety PDOs is also a valid usecase
    return;
  else
    bdCommonFlags.bNoPDOData := FALSE;
  end_if;

  if bdCommonFlags.bPdosCreated = FALSE then
    
    //add mailbox footer size to fsoe size
    sAccesses.tPdoRd.uiMyPDOSize := sFwInfos.sPdoInfo.PDORdLen + sizeof(tsMailboxFooter);
    sAccesses.tPdoWr.uiMyPDOSize := sFwInfos.sPdoInfo.PDOWrLen + sizeof(tsMailboxFooter);

    dRetCode := AddSPDO(bAddLater);

    if dRetCode = READY then
      bdCommonFlags.bPdosCreated := TRUE;
    end_if;
  else  
    dRetCode := READY;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::RtWork
	VAR_INPUT
		EAX 	: UDINT;
	END_VAR
	VAR_OUTPUT
		state (EAX) 	: UDINT;
	END_VAR
  VAR
    calloption    : UDINT;
    PDOLength     : UINT;
    pBuffer       : ^USINT;
    pDOInfo       : ^t_DOInfo;
    i             : USINT;
  END_VAR

  // Save state
  // This must be the 1. line in this function or EAX may be overwritten by the generated AWL code
  calloption := EAX;

  // do the realtime work only, if module has been found and initialized correctly
  if bdCommonFlags.bOnline & ( SafetyState = _SafetyClassOK ) then

    if calloption = CALL_OPTION_RT_PRESCAN then    // Called from UpdateRt

      // wait until the safetymanager has initialized everything
      if bdCommonFlags.bManagerInitialisationFinished = FALSE then
        return;
      end_if;

      //read PDO from hardware, only if PDO is needed by other moduls
      if sAccesses.tPdoRd.uiReadDOsUsed 
      & bdCommonFlags.bPDOOutputIsUsed then

        PDOLength := sAccesses.tPdoRd.uiMyPDOSize;

        // check for new message number from MB
        // only if length is bigger than minimum and smaller than maximum
        if ( PDOLength >= (SBF_SPDO_MIN_LENGTH + sizeof(tsMailboxFooter)) )
        & (sAccesses.tPdoRd.pMBFooter^.bsControl and 0x07) <> sAccesses.tPdoRd.usOldMsgCnt then
          //save act message counter
          sAccesses.tPdoRd.usOldMsgCnt := (sAccesses.tPdoRd.pMBFooter^.bsControl and 0x07);
          pBuffer := #sAccesses.tPdoRd.aPDOOutputBuffer[0];
          pDOInfo := #sAccesses.tPdoRd.aPDOs[0];
          for i := 0 to (sAccesses.tPdoRd.uiReadDOsUsed-1) do
            _memcpy(ptr1:=pBuffer, ptr2:=pDOInfo^.pData, cntr:=pDOInfo^.Length);
            pBuffer += pDOInfo^.Length;
            pDOInfo += sizeof(t_DOInfo);
          end_for;

#ifdef SAFETY_PROTOCOL_ANALYZER_ACTIVATED
RecordFrame(pFrame:=(#sAccesses.tPdoRd.aPDOOutputBuffer[0])$^DINT, FrameState:=0, IsFSoEPDO:=TRUE);
#endif
        else
          sAccesses.InvalidPDOSizeCnt += 1;
        end_if;
      end_if;

    elsif calloption = CALL_OPTION_RT_POSTSCAN then // Called from UpdateRtPostScan

      //write PDO to hardware
      WritePDO2Module();

    end_if;
  end_if;
  
  if calloption = CALL_OPTION_RT_PRESCAN then    // Called from UpdateRt
    ChangeConfigState(CallRt:=TRUE);
  end_if;

  state := READY;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::CyWork
	VAR_INPUT
		EAX 	: UDINT;
	END_VAR
	VAR_OUTPUT
		state (EAX) 	: UDINT;
	END_VAR
  VAR
    MyPara 	              : CmdStruct;
    MyResult	            : results;
    us_SDO_length         : USINT;
    IsStandardResp        : BOOL;
    p_ReceiveObj          : pVirtualBase;
    bSendSDOfromBuffer    : BOOL;
    p_SDO_act             : ^SDOHeader;
    udNewOffset           : UDINT;
    udNewLength           : UDINT;
    usTmpMsgCnt           : USINT;
    LogVal1               : DINT;
  END_VAR

  //initialize returncode
  state := READY;

  // only if the pointers for the SDO handling are valid (after adding the accesses)
  if p_Handle_SDO_Read & p_Handle_SDO_Write then

    // we only have to do something here if the OS doesn't handle the SDOs for us
    if bdCommonFlags.bSDOsViaOS = FALSE then

      //check for valid data pointer
      if sAccesses.tSDORd.sDOInfo.pData then
        case sAccesses.tSDORd.SSWMailboxSDO of
          //************************************************************************
          _ReadMBFooter:
            //check for new message number from MB
            sAccesses.tSDORd.pMBFooter := sAccesses.tSDORd.sDOInfo.pData+sAccesses.tSDORd.udActLength-sizeof(tsMailboxFooter);

            if (sAccesses.tSDORd.pMBFooter^.bsControl and 0x07) <> sAccesses.tSDORd.usOldMsgCnt
            & sAccesses.tSDORd.pMBFooter^.uiDatalength & sAccesses.tSDORd.pMBFooter^.uiDatalength < SBF_SDO_MAX_LENGTH then
              //set access to new datalength and offset
              sAccesses.tSDORd.udActLength := sAccesses.tSDORd.pMBFooter^.uiDatalength+sizeof(tsMailboxFooter);
              sAccesses.tSDORd.hdActOffset := sAccesses.tSDORd.sMailboxInfo.hiAddress+sAccesses.tSDORd.sMailboxInfo.uiLength-sAccesses.tSDORd.udActLength;
              ChangeRdDO(AccessHandle:=sAccesses.tSDORd.sDOInfo.Handle
                          , NewOffset:= sAccesses.tSDORd.hdActOffset
                          , NewLength:= sAccesses.tSDORd.udActLength
                          );

              sAccesses.tSDORd.SSWMailboxSDO := _ReadMBData;
            end_if;

          //************************************************************************
          _ReadMBData:
            //get pointer to footer data
            sAccesses.tSDORd.pMBFooter := sAccesses.tSDORd.sDOInfo.pData+sAccesses.tSDORd.udActLength-sizeof(tsMailboxFooter);
            //store new message counter
            sAccesses.tSDORd.usOldMsgCnt := (sAccesses.tSDORd.pMBFooter^.bsControl and 0x07);
            //copy received data to buffer
            _memcpy(ptr1:=#p_Handle_SDO_Read^.SDOStructure
                  , ptr2:=sAccesses.tSDORd.sDOInfo.pData
                  , cntr:=sAccesses.tSDORd.udActLength-sizeof(tsMailboxFooter));

            //set act sdo to valid
            p_Handle_SDO_Read^.us_Status := SAFETY_SDO_DATA_VALID;

            //set access to read only mailbox footer
            sAccesses.tSDORd.udActLength := sizeof(tsMailboxFooter);
            sAccesses.tSDORd.hdActOffset := sAccesses.tSDORd.sMailboxInfo.hiAddress+sAccesses.tSDORd.sMailboxInfo.uiLength-sAccesses.tSDORd.udActLength;
            ChangeRdDO(AccessHandle:= sAccesses.tSDORd.sDOInfo.Handle
                     , NewOffset   := sAccesses.tSDORd.hdActOffset
                     , NewLength   := sAccesses.tSDORd.udActLength
                     );

            sAccesses.tSDORd.SSWMailboxSDO := _ReadMBFooter;

        end_case;
      end_if;

    end_if;

    //read SDO from module and send it to the target module
    //check, if SDO is available and valid
    if ( p_Handle_SDO_Read^.us_Status = SAFETY_SDO_DATA_VALID ) then

      //calculate length of SDO
      us_SDO_length := sizeof(SDOHeader.StdHeader.CRC) + p_Handle_SDO_Read^.SDOStructure.SDOHeader.StdHeader.Len;
      
      //Check for valid length
      if us_SDO_length <= sizeof(SDORespStd) then
      
        //copy SDO without first byte (=status byte)
        _memcpy(ptr1  := sAccesses.tSDOWr.p_SDOOutputBuffer
              , ptr2  := #p_Handle_SDO_Read^.SDOStructure
              , cntr  := us_SDO_length );

#ifdef SAFETY_PROTOCOL_ANALYZER_ACTIVATED
        RecordFrame(sAccesses.tSDOWr.p_SDOOutputBuffer$^DINT, 0);
#endif

        //mark data as received
        p_Handle_SDO_Read^.us_Status := SAFETY_SDO_NO_DATA;

        //find target object for routing
        p_ReceiveObj := NIL;

        IsStandardResp := FALSE;

        //check the SDO frame
        if ((sAccesses.tSDOWr.p_SDOOutputBuffer^.SDOHeader.StdHeader.Typ$USINT AND SBF_SSDO_MASK_TYPE)$t_SafetyFrameType = SSDO_MOD) then

          MyPara.uiCmd := CMD_SM_SDO_TO_MANAGER;
          MyPara.aPara[0] := to_dint(us_SDO_length);
          MyPara.aPara[1] := sAccesses.tSDOWr.p_SDOOutputBuffer$DINT;
          
          // if we have a SDO receive logged in
          if pSDOLogin then
            // => send it there
            pSDOLogin^.NewInst(#MyPara, #MyResult);
          else
            // otherwise to the safety manager
            pSafetyManagerThis^.NewInst(#MyPara, #MyResult);
          end_if;

        elsif ((sAccesses.tSDOWr.p_SDOOutputBuffer^.SDOHeader.StdHeader.Typ$USINT AND SBF_SSDO_MASK_TYPE)$t_SafetyFrameType = SDO_STD) then
        
          IsStandardResp := TRUE;
          StandardResponse := (sAccesses.tSDOWr.p_SDOOutputBuffer$^StandardFrameType)^;

          if StandardResponse.SDOHeader.Cmd = SBF_SDO_COMMAND_GET_SAFENBR then
            
            // if returncode is 0 read succeeded
            if StandardResponse.aData[0] = 0 then
              SafetyNumber := StandardResponse.aData[1]$HDINT;
              sFwInfos.bdInfosGet.SaferyNrb := FALSE;
            else
              LogVal1 := TO_DINT(StandardResponse.aData[0]);
              HWC_LogPrintf(this, "@0836 (MDD2000VVSafetyBase::CyWork) SDO Receive: Returncode %d from SDO (SBF_SDO_COMMAND_GET_SAFENBR) is not OK.", #LogVal1);
            end_if;
            
          elsif StandardResponse.SDOHeader.Cmd = SBF_SDO_COMMAND_GET_FW_VERSION then
          
            // if returncode is 0 read succeeded
            if StandardResponse.aData[0] = 0 then
              sFwInfos.FwVersion := StandardResponse.aData[1]$FirmwareVersionType;
              FirmwareVersion := ((sFwInfos.FwVersion.RevMajor shl 16) + (sFwInfos.FwVersion.RevMinor shr 16)$UINT)$HDINT;
              sFwInfos.bdInfosGet.FwVersion := FALSE;
            else
              LogVal1 := TO_DINT(StandardResponse.aData[0]);
              HWC_LogPrintf(this, "@0836 (MDD2000VVSafetyBase::CyWork) SDO Receive: Returncode %d from SDO (SBF_SDO_COMMAND_GET_FW_VERSION) is not OK.", #LogVal1);
            end_if;
            
          elsif StandardResponse.SDOHeader.Cmd = SBF_SDO_COMMAND_READ then
         
            LogVal1 := TO_DINT(StandardResponse.aData[0]);
            HWC_LogPrintf(this, "@DBG (MDD2000VVSafetyBase::CyWork) SDO Receive: Returncode %d from SDO (SBF_SDO_COMMAND_READ) was not expected.", #LogVal1);
            
          end_if;
        else       
          LogVal1 := TO_DINT((sAccesses.tSDOWr.p_SDOOutputBuffer^.SDOHeader.StdHeader.Typ$USINT AND SBF_SSDO_MASK_TYPE));
          HWC_LogPrintf(this, "@025C (MDD2000VVSafetyBase::CyWork) SDO Receive: Invalid frametype %d in Standard SDO response.", #LogVal1);
        end_if;
      else

        //mark data as received
        p_Handle_SDO_Read^.us_Status := SAFETY_SDO_NO_DATA;
      
        LogVal1 := TO_DINT(us_SDO_length);
        HWC_LogPrintf(this, "@0829 (MDD2000VVSafetyBase::CyWork) SDO Receive: Invalid size %d in Standard SDO response.", #LogVal1);      
      end_if;

    end_if;

    // check if buffer is empty
    if ( p_Handle_SDO_Write^.us_Status = SAFETY_SDO_NO_DATA ) then

      //write SDO to hardware (from buffer or external)
      bSendSDOfromBuffer := (( sAccesses.tSDOWr.uiCntSDOInputBuffer > 0 ) & sAccesses.tSDOWr.p_SDOInputBuffer);
      if bSendSDOfromBuffer then

        //set pointer to first SDO in the SDO list
        p_SDO_act   := sAccesses.tSDOWr.p_SDOInputBuffer$^SDOHeader;

        //calculate length of SDO (CRC + data length)
        us_SDO_length := sizeof (SDOHeader.StdHeader.CRC) + p_SDO_act^.StdHeader.Len;

        // if it's a standardframe, we add the sequence number here (the module SSDOs have their own count)
        if ((p_SDO_act^.StdHeader.Typ$USINT AND SBF_SSDO_MASK_TYPE)$t_SafetyFrameType = SDO_STD) then
          // the source could be the buffer or the external SDO, so we add it here to get a consistent counter

          //sequence number
          p_SDO_act^.StdHeader.SeqNr := sAccesses.tSDOWr.usStdSeqCnt OR 16#80;   // sequence number is in special area

          //increase counter for sequence number
          if sAccesses.tSDOWr.usStdSeqCnt < 16#7F then
            sAccesses.tSDOWr.usStdSeqCnt += 1;
          else
            sAccesses.tSDOWr.usStdSeqCnt := 0;
          end_if;
        end_if;

      #ifdef SAFETY_PROTOCOL_ANALYZER_ACTIVATED
        RecordFrame(p_SDO_act$^DINT, 1);
      #endif

        //copy SDO into write-buffer
        _memcpy(ptr1:=#p_Handle_SDO_Write^.SDOStructure, ptr2:=p_SDO_act, cntr:=us_SDO_length);

        if bdCommonFlags.bSDOsViaOS then
          p_Handle_SDO_Write^.us_Status := SAFETY_SDO_DATA_VALID;
        elsif sAccesses.tSDOWr.sDOInfo.pData then
        
          sAccesses.tSDOWr.sMBFooter.uiDatalength := us_SDO_length;
          sAccesses.tSDOWr.sMBFooter.usCmd        := 0;

          usTmpMsgCnt := (sAccesses.tSDOWr.sMBFooter.bsControl and 0x07) + 1;
          //check for msg counter overflow
          if usTmpMsgCnt > 0x07 then
            usTmpMsgCnt := 0;
          end_if;
          
          // increase message counter in the footer
    //      sAccesses.tSDOWr.sMBFooter.bsControl := (sAccesses.tSDOWr.sMBFooter.bsControl + 1) and 0x07;
          sAccesses.tSDOWr.sMBFooter.bsControl := usTmpMsgCnt;

          //copy mailbox footer into write-buffer
          _memcpy(ptr1:=#sAccesses.tSDOWr.sActSDO.SDOStructure+us_SDO_length
                , ptr2:=#sAccesses.tSDOWr.sMBFooter
                , cntr:=sizeof(tsMailboxFooter));

          udNewLength := us_SDO_length+sizeof(tsMailboxFooter);
          udNewOffset := sAccesses.tSDOWr.sMailboxInfo.hiAddress+sAccesses.tSDOWr.sMailboxInfo.uiLength-(udNewLength);

          //check if offset or length of dataobject has changed
          if udNewLength <> sAccesses.tSDOWr.udActLength
          | udNewOffset <> sAccesses.tSDOWr.hdActOffset then

            sAccesses.tSDOWr.hdActOffset := udNewOffset;
            sAccesses.tSDOWr.udActLength := udNewLength;

            //set new length and offset for write sdo
            ChangeWrDO(AccessHandle := sAccesses.tSDOWr.sDOInfo.Handle
                     , NewOffset    := sAccesses.tSDOWr.hdActOffset
                     , NewLength    := sAccesses.tSDOWr.udActLength
                     );
          end_if;

          _memcpy(ptr1:=sAccesses.tSDOWr.sDOInfo.pData
                , ptr2:=#sAccesses.tSDOWr.sActSDO.SDOStructure
                , cntr:=sAccesses.tSDOWr.udActLength);

          SetDORunState(Handle:=sAccesses.tSDOWr.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_SINGLE_RUN);
        end_if;

        //decrement amount of SDOs
        sAccesses.tSDOWr.uiCntSDOInputBuffer -= 1;

        //if there is at least 1 SDO left, copy the remaining memory over the actual SDO
        if ( sAccesses.tSDOWr.uiCntSDOInputBuffer > 0 ) then

          //remove first entry in the SDO list
          _memmove(sAccesses.tSDOWr.p_SDOInputBuffer, (sAccesses.tSDOWr.p_SDOInputBuffer + SBF_SDO_MAX_LENGTH), (sAccesses.tSDOWr.uiCntSDOInputBuffer * SBF_SDO_MAX_LENGTH));
        end_if;
        
      end_if;
    end_if;  
  end_if;
  
  ChangeConfigState(CallRt:=FALSE);
  
  if bd_LogCy <> bd_LogCyDone then
        
    if (bd_LogCy.RdPDOSizeInc) & (bd_LogCyDone.RdPDOSizeInc = FALSE) then
      HWC_LogPrintf(pThis:=this, format:="@08D1 (MDD2000VVSafetyBase::SetDOAddresses) Could not increase size of Rd FSoE DOs. Reset of SPS is required. New Len: %V2 Max Len: %V2", #sAccesses.tPdoRd.uiMyPDOSize, #sAccesses.tPdoRd.uiMaxPDOSize);
      bd_LogCyDone.RdPDOSizeInc := TRUE;
    end_if;
    
    if (bd_LogCy.RdPDOSizeDec) & (bd_LogCyDone.RdPDOSizeDec = FALSE) then
      HWC_LogPrintf(pThis:=this, format:="@08D1 (MDD2000VVSafetyBase::SetDOAddresses) Could not decrease size of Rd FSoE DOs. Reset of SPS is required. New Len: %V2 Max Len: %V2", #sAccesses.tPdoRd.uiMyPDOSize, #sAccesses.tPdoRd.uiMaxPDOSize);
      bd_LogCyDone.RdPDOSizeDec := TRUE;
    end_if;
    
    if (bd_LogCy.WrPDOSizeInc) & (bd_LogCyDone.WrPDOSizeInc = FALSE) then
      HWC_LogPrintf(pThis:=this, format:="@08D2 (MDD2000VVSafetyBase::SetDOAddresses) Could not increase size of Wr FSoE DOs. Reset of SPS is required. New Len: %V2 Max Len: %V2", #sAccesses.tPdoWr.uiMyPDOSize, #sAccesses.tPdoWr.uiMaxPDOSize);
      bd_LogCyDone.WrPDOSizeInc := TRUE;
    end_if;
    
    if (bd_LogCy.WrPDOSizeDec) & (bd_LogCyDone.WrPDOSizeDec = FALSE) then
      HWC_LogPrintf(pThis:=this, format:="@08D2 (MDD2000VVSafetyBase::SetDOAddresses) Could not decrease size of Wr FSoE DOs. Reset of SPS is required. New Len: %V2 Max Len: %V2", #sAccesses.tPdoWr.uiMyPDOSize, #sAccesses.tPdoWr.uiMaxPDOSize);
      bd_LogCyDone.WrPDOSizeDec := TRUE;
    end_if;
  
  end_if;


END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::ConnectEvent
  VAR
    MyPara 	              : CmdStruct;
    MyResult	            : results;
  END_VAR

  if SafetyState = _ModuleNotFound then
    SetSafetyState(NewState:=_SafetyClassOK);
  end_if;

  // if we we're already connected => tell the safety manager we're back (safetythis has to be valid if bReconnect is set, therefore we don't have to check it here)
  if bdCommonFlags.bReconnect then
    MyPara.uiCmd := CMD_SM_MODULE_RECONNECT;
    MyPara.aPara[0] := THIS$DINT;
    pSafetyManagerThis^.NewInst(#MyPara, #MyResult);
  end_if;
  
  //We only need to start the DO if the DOs are not handled by the OS
  if bdCommonFlags.bSDOsViaOS = FALSE then
    SetDORunState(Handle:=sAccesses.tSDORd.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_CONT_RUN);
  end_if;

  // bOnline is set if module init succeeds
  bdCommonFlags.bOnline := TRUE;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::DisconnectEvent
  VAR
    MyPara 	              : CmdStruct;
    MyResult	            : results;
  END_VAR

  //stop DOs
  StopDO();

  // reset time sync state and tell the manager
  if pSafetyManagerThis then
    MyPara.uiCmd := CMD_SM_MODULE_DISCONNECT;
    MyPara.aPara[0] := THIS$DINT;
    pSafetyManagerThis^.NewInst(#MyPara, #MyResult);

    bdCommonFlags.bReconnect := TRUE;
  end_if;

  SafetyNumber := 0;
  FirmwareVersion := 0;

  SetSafetyState(NewState:=_ModuleNotFound);

  // the mechanism has to be initalized again next time the device connects
  bdCommonFlags.bSdoMechanismInitialized := FALSE;
  
  // FW has to be read again incase an fw update
  bdCommonFlags.bReadFktFW := FALSE;
  
  // Reset Cy Logging, Relevant FW Config may have changed
  bd_LogCy := bd_LogCyDone := 0;

  bdCommonFlags.bOnline := FALSE;
  ClassSvr := _NoHardware;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::InterruptEvent

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::SetDORunState
	VAR_INPUT
		Handle 	: UDINT;
		NewState 	: t_BusIFRunState;
	END_VAR
  VAR
    dataSETRUNSTATE : t_SetRunStateCfg;
    MyPara         : CmdStruct;
    MyResult : results;
  END_VAR

  // set runstate of read
  dataSETRUNSTATE.Handle      := Handle;
  dataSETRUNSTATE.NewRunState := NewState;

  MyPara.uiCmd         := _BUS_IF_SET_RUN_STATE;
  MyPara.aPara[0]      := (#dataSETRUNSTATE)$DINT;

  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

  if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
    if NewState = _BUS_IF_RUN_STATE_NO_RUN then
      sAccesses.DOStopFailCounter   += 1;
    else
      sAccesses.DOStartFailCounter  += 1;
    end_if;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::AddSDOToBuffer
	VAR_INPUT
		p_us_message 	: ^USINT;
		us_length 	: USINT;
	END_VAR
	VAR_OUTPUT
		sd_retval 	: DINT;
	END_VAR

  // if the message is ok and there is space left in the buffer => save it
  if ( p_us_message ) & (us_length <= SBF_SDO_MAX_LENGTH) & (sAccesses.tSDOWr.uiCntSDOInputBuffer < 1000) then

    if ( sAccesses.tSDOWr.p_SDOInputBuffer = NIL ) then

      sd_retval := -2;
      return;

    end_if;

    // if the allocated memory is full, we allocate some more
    if sAccesses.tSDOWr.uiCntSDOInputBuffer = sAccesses.tSDOWr.uiCntSDOInputBufferAllocated then
      //reallocate list
      sAccesses.tSDOWr.uiCntSDOInputBufferAllocated += MDD2000VVSB_SDO_BUFFER_ALLOC_BLOCK;
      sAccesses.tSDOWr.p_SDOInputBuffer$^void := sigclib_realloc(ptr := sAccesses.tSDOWr.p_SDOInputBuffer$^void
                                                              , size := sAccesses.tSDOWr.uiCntSDOInputBufferAllocated * SBF_SDO_MAX_LENGTH);

      if ( sAccesses.tSDOWr.p_SDOInputBuffer = NIL ) then
        SetSafetyState(NewState:=_MemAllocFailed);
        sd_retval := -3;
        return;
      end_if;

    end_if;

    //copy SDO into new list element
    _memcpy(ptr1  := ( sAccesses.tSDOWr.p_SDOInputBuffer + ( sAccesses.tSDOWr.uiCntSDOInputBuffer * SBF_SDO_MAX_LENGTH))$^void
          , ptr2  := p_us_message
          , cntr  := us_length
          );

    sAccesses.tSDOWr.uiCntSDOInputBuffer += 1;

    sd_retval := 0;
  else

    sd_retval := -1;

  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::InitSafetyBase
	VAR_OUTPUT
		retcode 	: BOOL;
	END_VAR
  VAR
    MyPara : CmdStruct;
    MyResult : results;
    dataADDTASK     : t_TaskCfg;
    dataADDCallback : t_BusIFAddCallback;
    LogVal1 : DINT;
  END_VAR

  retcode := FALSE;

  // login for routing of newinst commands
  dataADDCallback.pthis := this$pVirtualBase;

  MyPara.uiCmd     := _BUS_IF_ADD_CALLBACK;
  MyPara.aPara[0]  := (#dataADDCallback)$DINT;
  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01C2 (MDD2000VVSafetyBase::InitSafetyBase) Failed to install callback at bus interface. Error %d", #LogVal1);
  end_if;

  MyPara.uiCmd     := _BUS_IF_GET_HW_PATH;
  MyPara.aPara[0]  := this$DINT;
  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
    LogError("@01C3 (MDD2000VVSafetyBase::InitSafetyBase) Failed to get hardware path.");
    // if there is no safety manager => problem, because this class won't work without
    ClassSvr := _IDNotFound;
    return;
  else
    p_ObjectPath := MyResult.aData[4]$^USINT;
    sConnInfo.uiHwPathLen := p_ObjectPath^$USINT + 1; // size of path (length + 1 for the length information)
    sConnInfo.pHwPath     := p_ObjectPath;
  end_if;

  // add realtime and cyclic task
  dataADDTASK.Priority    := _BUS_IF_ISO;
  dataADDTASK.callOptions := (CALL_OPTION_RT_PRESCAN or CALL_OPTION_RT_POSTSCAN);
  dataADDTASK.pthis       := this$UDINT;

  MyPara.uiCmd     := _BUS_IF_ADD_TASK;
  MyPara.aPara[0]  := (#dataADDTASK)$DINT;
  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

  if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01CD (MDD2000VVSafetyBase::InitSafetyBase) Failed to install realtime tasks. Error %d", #LogVal1);  
    return;
  end_if;

   // add cyclic task
  dataADDTASK.Priority    := _BUS_IF_ASY;
  dataADDTASK.callOptions := CALL_OPTION_CYCLIC;
  dataADDTASK.pthis       := this$UDINT;

  MyPara.uiCmd     := _BUS_IF_ADD_TASK;
  MyPara.aPara[0]  := (#dataADDTASK)$DINT;
  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

  if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01CE (MDD2000VVSafetyBase::InitSafetyBase) Failed to install a cyclic task. Error %d", #LogVal1);  
    return;
  end_if;

  // check if we're connected via VARAN
  MyPara.uiCmd     := _BUS_IF_IS_VARAN_AVAILABLE;

  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
    bdCommonFlags.bVaranAvailable := MyResult.aData[4]$BOOL;
  else
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01C4 (MDD2000VVSafetyBase::InitSafetyBase) Failed to get connection state via BusInterface. Error %d", #LogVal1);  
  end_if;

  // initialize pointers with NIL (they get valid pointers in a later init-run if an object of the analyzer or safety manager class is available)
  pAnalyzerThis   := NIL;
  pAnalyzerMethod := NIL;

  pSafetyManagerThis   := NIL;

  //initialize buffers
  if ( InitBuffers() ) then
    return;
  end_if;

  retcode := TRUE;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::AddSPDO
	VAR_INPUT
		bAddLater 	: BOOL;
	END_VAR
	VAR_OUTPUT
		ret_code 	: iprStates;
	END_VAR
  VAR
    MyPara         : CmdStruct;
    MyResult         : results;
    dataADDDO       : t_AddDOCfg;
    tPriority       : t_BusIFPriority;
    i               : USINT;
  END_VAR

  //initialize return value
  ret_code := READY;

  //set flag if dataobjects are added after init
  if bAddLater = FALSE then
    tPriority := _BUS_IF_ISO;
  else
    tPriority := _BUS_IF_ISO_ADDLATER;
  end_if;

  //reset local memory
  _memset(dest:=#dataADDDO, usByte:=0x00, cntr:=sizeof(dataADDDO));

  //*********************************************************************
  // FSOE DOs
  //*********************************************************************

  // first we add all read accesses (this part also stays the same)
  dataADDDO.Priority                := tPriority;
  dataADDDO.RdWr                    := _BUS_IF_READ;
  dataADDDO.ud_WaitForAccessHandle  := 0;
  dataADDDO.b_UseLocalCopy          := TRUE;
  dataADDDO.b_DOIsChangable         := TRUE;

  if sAccesses.tPdoRd.uiMyPDOSize then

    // calculate how many DOs are necessary
    sAccesses.tPdoRd.uiReadDOsUsed := ((sAccesses.tPdoRd.uiMyPDOSize-1) / MDD2000VVSB_BUS_USAGE_PER_PDO_DO) + 1;

    for i := 0 to sAccesses.tPdoRd.uiReadDOsUsed-1 do
      // read PDO

      // the length of the last is the rest
      if i = sAccesses.tPdoRd.uiReadDOsUsed-1 then
        sAccesses.tPdoRd.aPDOs[i].Length := to_usint(sAccesses.tPdoRd.uiMyPDOSize - MDD2000VVSB_BUS_USAGE_PER_PDO_DO*(sAccesses.tPdoRd.uiReadDOsUsed-1));
      else
      // all others are the max length
        sAccesses.tPdoRd.aPDOs[i].Length := MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      end_if;

      // the first access is on the main offset, the others start behind so the FPGA can detect the begin of new data
      if i = 0 then
        dataADDDO.ud_addressoff_read  := SBF_OFFSET_SPDO_DATA_RD;
      else
        dataADDDO.ud_addressoff_read  := SBF_OFFSET_SPDO_DATA_RD + (MDD2000VVSB_MAX_BUS_USAGE - MDD2000VVSB_BUS_USAGE_PER_PDO_DO);
      end_if;

      dataADDDO.ui_length_read := sAccesses.tPdoRd.aPDOs[i].Length;
      MyPara.uiCmd             := _BUS_IF_ADD_DO;
      MyPara.aPara[0]          := (#dataADDDO)$DINT;

      ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
        sAccesses.tPdoRd.aPDOs[i].Handle := MyResult.aData$t_AddDORet.Handle;
        sAccesses.tPdoRd.aPDOs[i].pData  := MyResult.aData$t_AddDORet.pDataRead;

        //set footer pointer to last data object
        if i = (sAccesses.tPdoRd.uiReadDOsUsed-1) then
          sAccesses.tPdoRd.pMBFooter := sAccesses.tPdoRd.aPDOs[i].pData+sAccesses.tPdoRd.aPDOs[i].Length-sizeof(tsMailboxFooter);
        end_if;

        //change retry timeout for varan do
        MyPara.uiCmd     := _BUS_IF_RESET_DO;
        MyPara.aPara[0]  := (#sAccesses.tPdoRd.aPDOs[i].Handle)$DINT;
        ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      else
        //if add later is active the call is from cyclic task
        if bAddLater = TRUE then
          LogError("@01C5 (MDD2000VVSafetyBase::AddDO) Failed to add a DO for FSOE read data.");
        end_if;
        ret_code := ERROR;
      end_if;

    end_for;
  end_if;
  
  // Safe max PDO size and max no of DOs
  sAccesses.tPdoRd.uiMaxPDOSize     := sAccesses.tPdoRd.uiMyPDOSize;
  sAccesses.tPdoRd.uiMaxReadDOsUsed := sAccesses.tPdoRd.uiReadDOsUsed;
  

  // now we add all write accesses (this part stays the same)
  dataADDDO.Priority                := tPriority;
  dataADDDO.RdWr                    := _BUS_IF_WRITE;
  dataADDDO.ud_WaitForAccessHandle  := 0;
  dataADDDO.b_UseLocalCopy          := TRUE;
  dataADDDO.b_DOIsChangable         := TRUE;

  if sAccesses.tPdoWr.uiMyPDOSize then

    // calculate how many DOs are necessary
    sAccesses.tPdoWr.uiWriteDOsUsed := ((sAccesses.tPdoWr.uiMyPDOSize-1) / MDD2000VVSB_BUS_USAGE_PER_PDO_DO) + 1;

    for i := 0 to sAccesses.tPdoWr.uiWriteDOsUsed-1 do
      // read PDO

      // the length of the last is the rest
      if i = sAccesses.tPdoWr.uiWriteDOsUsed-1 then
        sAccesses.tPdoWr.aPDOs[i].Length          := to_usint(sAccesses.tPdoWr.uiMyPDOSize - MDD2000VVSB_BUS_USAGE_PER_PDO_DO*(sAccesses.tPdoWr.uiWriteDOsUsed-1));
      else
      // all others are the max length
        sAccesses.tPdoWr.aPDOs[i].Length          := MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      end_if;

      // the first access is on the main offset, the others start behind so the FPGA can detect the begin of new data
      if i = 0 then
        dataADDDO.ud_addressoff_write   := SBF_OFFSET_SPDO_DATA_WR;
      else
        dataADDDO.ud_addressoff_write   := SBF_OFFSET_SPDO_DATA_WR + (MDD2000VVSB_MAX_BUS_USAGE - MDD2000VVSB_BUS_USAGE_PER_PDO_DO);
      end_if;

      dataADDDO.ui_length_write       := sAccesses.tPdoWr.aPDOs[i].Length;

      MyPara.uiCmd             := _BUS_IF_ADD_DO;
      MyPara.aPara[0]          := (#dataADDDO)$DINT;

      ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
        sAccesses.tPdoWr.aPDOs[i].Handle := MyResult.aData$t_AddDORet.Handle;
        sAccesses.tPdoWr.aPDOs[i].pData  := MyResult.aData$t_AddDORet.pDataWrite;

        //change retry timeout for varan do
        MyPara.uiCmd     := _BUS_IF_RESET_DO;
        MyPara.aPara[0]  := (#sAccesses.tPdoWr.aPDOs[i].Handle)$DINT;
        ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      else
        //if add later is active the call is from cyclic task
        if bAddLater = TRUE then
          LogError("@01CA (MDD2000VVSafetyBase::AddDO) Failed to add a DO for FSOE write data.");
        end_if;
        ret_code := ERROR;
      end_if;
    end_for;
  end_if;
  
  // Safe max PDO size and max no of DOs
  sAccesses.tPdoWr.uiMaxPDOSize       := sAccesses.tPdoWr.uiMyPDOSize;
  sAccesses.tPdoWr.uiMaxWriteDOsUsed  := sAccesses.tPdoWr.uiWriteDOsUsed;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::AddSSDO
	VAR_OUTPUT
		ret_code 	: iprStates;
	END_VAR
  VAR
    MyPara      : CmdStruct;
    MyResult    : results;
    dataADDDO   : t_AddDOCfg;
    LogVal1     : DINT;
  END_VAR

  //*********************************************************************
  // SSDO DOs
  //*********************************************************************
  //add read access
  MyPara.uiCmd                     := _BUS_IF_ADD_DO;
  MyPara.aPara[0]                  := (#dataADDDO)$DINT;
  dataADDDO.Priority                := _BUS_IF_ASY;
  dataADDDO.RdWr                    := _BUS_IF_READ;
  dataADDDO.ud_WaitForAccessHandle  := 0;
  dataADDDO.b_UseLocalCopy          := FALSE;

  dataADDDO.ui_length_read      := SBF_SDO_MAX_LENGTH+sizeof(tsMailboxFooter);
  dataADDDO.ud_addressoff_read  := 0;

  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
    sAccesses.tSDORd.sDOInfo.Handle := MyResult.aData$t_AddDORet.Handle;
    sAccesses.tSDORd.sDOInfo.pData  := MyResult.aData$t_AddDORet.pDataRead;

    //change retry timeout for varan do
    MyPara.uiCmd     := _BUS_IF_RESET_DO;
    MyPara.aPara[0]  := (#sAccesses.tSDORd.sDOInfo.Handle)$DINT;
    ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  else
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01C5 (MDD2000VVSafetyBase::AddDO) Failed to add a DO for SSDO read data. Error %d", #LogVal1);  
    ret_code := ERROR;
  end_if;

  //add write access
  MyPara.uiCmd                     := _BUS_IF_ADD_DO;
  MyPara.aPara[0]                  := (#dataADDDO)$DINT;
  dataADDDO.Priority                := _BUS_IF_ASY;
  dataADDDO.RdWr                    := _BUS_IF_WRITE;
  dataADDDO.ud_WaitForAccessHandle  := 0;
  dataADDDO.b_UseLocalCopy          := FALSE;

  dataADDDO.ui_length_write     := SBF_SDO_MAX_LENGTH+sizeof(tsMailboxFooter);
  dataADDDO.ud_addressoff_write := 0;

  ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if MyResult.aData[0]$t_BusIFRetvalErrorcode = _BUS_IF_RETVAL_OK then
    sAccesses.tSDOWr.sDOInfo.Handle := MyResult.aData$t_AddDORet.Handle;
    sAccesses.tSDOWr.sDOInfo.pData  := MyResult.aData$t_AddDORet.pDataWrite;

    //change retry timeout for varan do
    MyPara.uiCmd     := _BUS_IF_RESET_DO;
    MyPara.aPara[0]  := (#sAccesses.tSDOWr.sDOInfo.Handle)$DINT;
    ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  else
    LogVal1 := MyResult.aData[0]$DINT;
    HWC_LogPrintf(pThis:=this, format:="@01CA (MDD2000VVSafetyBase::AddDO) Failed to add a DO for SSDO write data. Error %d", #LogVal1);  
    ret_code := ERROR;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::GetPointer2PDOBuffer
	VAR_OUTPUT
		ud_PDOBuffer 	: UDINT;
	END_VAR

  bdCommonFlags.bPDOOutputIsUsed := TRUE;

  //set pointer to address of buffer pointer
  ud_PDOBuffer := ( #sAccesses.tPdoRd.aPDOOutputBuffer )$UDINT;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::WritePDO2Module
  VAR
    i                 : UINT;
    p_PDOInBufferAct  : ^t_PDOInputBuffer;
    usTmpMsgCnt       : USINT;
  END_VAR

  if (sAccesses.tPdoWr.uiWriteDOsUsed) & 
     (ui_cnt_PDOInputBuffer) & 
     (sAccesses.tPdoWr.uiMyPDOSize >= (SBF_SPDO_MIN_LENGTH + sizeof(tsMailboxFooter)) ) then
    p_PDOInBufferAct := #aPDOInputBuffer[0];
    for i := 1 to ui_cnt_PDOInputBuffer do
      // copy single FSoE PDO frames to target memory
      _memcpy(p_PDOInBufferAct^.p_TargetMemory, p_PDOInBufferAct^.ud_p2Buffer$pVoid, p_PDOInBufferAct^.ui_PDOLen);

      sAccesses.tPdoWr.sMBFooter.uiDatalength := sAccesses.tPdoWr.uiMyPDOSize-sizeof(tsMailboxFooter);
      sAccesses.tPdoWr.sMBFooter.usCmd        := 0;
      
      usTmpMsgCnt := (sAccesses.tPdoWr.sMBFooter.bsControl and 0x07) + 1;
      
      //check for msg counter overflow
      if usTmpMsgCnt > 0x07 then
        usTmpMsgCnt := 0;
      end_if;

      sAccesses.tPdoWr.sMBFooter.bsControl := usTmpMsgCnt$BSINT;

      _memcpy(ptr1:=p_PDOInBufferAct^.p_TargetMemory+p_PDOInBufferAct^.ui_PDOLen
            , ptr2:=#sAccesses.tPdoWr.sMBFooter
            , cntr:=sizeof(tsMailboxFooter)
            );

      p_PDOInBufferAct += sizeof(t_PDOInputBuffer);
    end_for;

  end_if;

#ifdef SAFETY_PROTOCOL_ANALYZER_ACTIVATED
  if sAccesses.tPdoWr.uiWriteDOsUsed then
    RecordFrame(pFrame:=sAccesses.tPdoWr.aPDOs[0].pData$^DINT, FrameState:=1, IsFSoEPDO:=TRUE);
  end_if;
#endif

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::StartDO
  VAR
    i : USINT;
  END_VAR

  // first check if this DO is available
  if sAccesses.tPdoRd.uiReadDOsUsed then
    for i := 0 to sAccesses.tPdoRd.uiReadDOsUsed-1 do
      // enable the DOs
      SetDORunState(Handle:=sAccesses.tPdoRd.aPDOs[i].Handle, NewState:=_BUS_IF_RUN_STATE_CONT_RUN);
    end_for;
  end_if;

  if sAccesses.tPdoWr.uiWriteDOsUsed then
    for i := 0 to sAccesses.tPdoWr.uiWriteDOsUsed-1 do
      SetDORunState(Handle:=sAccesses.tPdoWr.aPDOs[i].Handle, NewState:=_BUS_IF_RUN_STATE_CONT_RUN);
    end_for;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::StopDO
  VAR
    i : USINT;
  END_VAR

  // disable the DOs
  if sAccesses.tPdoRd.uiReadDOsUsed then
    for i := 0 to sAccesses.tPdoRd.uiReadDOsUsed-1 do
      SetDORunState(Handle:=sAccesses.tPdoRd.aPDOs[i].Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);
    end_for;
  end_if;

  if sAccesses.tPdoWr.uiWriteDOsUsed then
    for i := 0 to sAccesses.tPdoWr.uiWriteDOsUsed-1 do
      SetDORunState(Handle:=sAccesses.tPdoWr.aPDOs[i].Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);
    end_for;
  end_if;

  SetDORunState(Handle:=sAccesses.tSDORd.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);

  SetDORunState(Handle:=sAccesses.tSDOWr.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::InitBuffers
	VAR_OUTPUT
		sd_retval 	: DINT;
	END_VAR

  sd_retval := -1;

  //allocate memory for outgoing SDO (one SDO possible)
  sAccesses.tSDOWr.p_SDOOutputBuffer$^void := sigclib_malloc(size := SBF_SDO_MAX_LENGTH);

  if ( sAccesses.tSDOWr.p_SDOOutputBuffer = NIL ) then
    SetSafetyState(NewState:=_MemAllocFailed); 
    return;
  end_if;

  //allocate memory for incoming SDO list (number of SDO is dynamical)
  //following elements will be added dynamical

  //allocate list
  sAccesses.tSDOWr.uiCntSDOInputBufferAllocated := MDD2000VVSB_SDO_BUFFER_ALLOC_BLOCK;
  sAccesses.tSDOWr.p_SDOInputBuffer$^void := sigclib_malloc(size:=sAccesses.tSDOWr.uiCntSDOInputBufferAllocated * SBF_SDO_MAX_LENGTH);

  if ( sAccesses.tSDOWr.p_SDOInputBuffer = NIL ) then
    SetSafetyState(NewState:=_MemAllocFailed);
    return;
  end_if;

  //all buffers initialized
  sd_retval := 0;

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::MDD2000VVSafetyBase
	VAR_OUTPUT
		ret_code 	: ConfStates;
	END_VAR

#ifdef HWC_LogInit
  HWC_LogInit();
#endif

  //get interface for safety functions
  if ( OS_CILGET(INTERFACE_SAFETY, #pISafety)) then
    pISafety := NIL;
    
    LogError("@01DC (MDD2000VVSafetyBase::MDD2000VVSafetyBase) No Safety Interface found!");
  end_if;

  ClassSvr := _NotInitialized;

  ret_code := C_OK;

END_FUNCTION


#pragma warning(disable: 73);
FUNCTION MDD2000VVSafetyBase::LogError
	VAR_INPUT
		e_msg 	: ^CHAR;
	END_VAR

#ifdef HWC_LogError
  HWC_LogError(this, e_msg);
#endif

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::LogValue
	VAR_INPUT
		pString 	: ^CHAR;
		Value 	: UDINT;
		Value2 	: UDINT;(* := 16#FFFFFFFF *)
		Value3 	: UDINT;(* := 16#FFFFFFFF *)
	END_VAR

#ifdef HWC_LogValue3
  HWC_LogValue3(this, pString, Value, Value2, Value3);
#endif

END_FUNCTION
#pragma warning(default: 73);

#pragma warning(disable: 73)
FUNCTION VIRTUAL MDD2000VVSafetyBase::AddFrame
	VAR_INPUT
		pFrame 	: ^t_SDOStdHeader;
		FrameState 	: DINT;
		CallerSafetyNr 	: HDINT;
	END_VAR

END_FUNCTION
#pragma warning(default: 73)

FUNCTION VIRTUAL MDD2000VVSafetyBase::RecordFrame
	VAR_INPUT
		pFrame 	: ^DINT;
		FrameState 	: DINT;
		IsFSoEPDO 	: BOOL;(* := FALSE *)
	END_VAR
  VAR
    ThisBackup  : pVoid;
    TempSafetyNr: HDINT;
    Length      : USINT;
    tmpLength   : USINT;
  END_VAR

  if pAnalyzerThis & pAnalyzerMethod then

    TempSafetyNr := SafetyNumber;

    if IsFSoEPDO then

      // if write
      if FrameState then
        
        tmpLength := sAccesses.tPdoWr.uiMyPDOSize$USINT - sizeof(tsMailboxFooter);
        if tmpLength > sizeof(PDOType.DataAndCopy) then
          Length := sizeof(PDOType.DataAndCopy);
        else
          Length := tmpLength;
        end_if;

        // if it's the same content as last time
        if (_memcmp(ptr1:=pFrame, ptr2:=#OldFakePDOWrite.SourceAdr, cntr:=Length) = 0) then
          // don't record it
          return;
        end_if;

        // fake the header
        OldFakePDOWrite.Length              := sizeof(PDOType) - sizeof(PDOType.CRC) - sizeof(PDOType.DataAndCopy) + Length;;
        OldFakePDOWrite.FrameType.PDO       := TRUE;
        OldFakePDOWrite.FrameType.Reserved2 := TRUE;

        // store the PDO data (remember it as the last recorded)
        // Copy data to OldFakePDOWrite.SourceAdr because SourceAdr, SourceAdrCopy and msTimeStamp are not used by FSoE PDOs
        _memcpy(ptr1:=#OldFakePDOWrite.SourceAdr, ptr2:=pFrame, cntr:=Length);

        pFrame := (#OldFakePDOWrite)$^DINT;

      // if read
      else
        
        tmpLength := sAccesses.tPdoRd.uiMyPDOSize$USINT - sizeof(tsMailboxFooter);
        if tmpLength > sizeof(PDOType.DataAndCopy) then
          Length := sizeof(PDOType.DataAndCopy);
        else
          Length := tmpLength;
        end_if;

        // if it's the same content as last time
        if (_memcmp(ptr1:=pFrame, ptr2:=#OldFakePDORead.SourceAdr, cntr:=Length) = 0) then
          // don't record it
          return;
        end_if;

        // fake the header
        OldFakePDORead.Length              := sizeof(PDOType) - sizeof(PDOType.CRC) - sizeof(PDOType.DataAndCopy) + Length;
        OldFakePDORead.FrameType.PDO       := TRUE;
        OldFakePDORead.FrameType.Reserved2 := TRUE;

        // store the PDO data (remember it as the last recorded)
        // Copy data to OldFakePDOWrite.SourceAdr because SourceAdr, SourceAdrCopy and msTimeStamp are not used by FSoE PDOs
        _memcpy(ptr1:=#OldFakePDORead.SourceAdr, ptr2:=pFrame, cntr:=Length);

        pFrame := (#OldFakePDORead)$^DINT;
      end_if;
    end_if;

    ThisBackup := THIS;

    THIS := pAnalyzerThis$^MDD2000VVSafetyBase;
    pAnalyzerMethod$AddFrame(pFrame$^t_SDOStdHeader, FrameState, TempSafetyNr);

    THIS := ThisBackup$^MDD2000VVSafetyBase;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::ClassSvr::NewInst
	VAR_INPUT
		pPara 	: ^CmdStruct;
		pResult 	: ^results;
	END_VAR
	VAR_OUTPUT
		ret_code 	: iprStates;
	END_VAR
  VAR
    sd_retval       : DINT;
    OldClassState   : SafetyConfigStateType;
    pPDOInputBuffer : ^t_PDOInputBuffer;
    i, j            : UINT;
    FoundNo         : UINT;
    ud_p2BufferCompare : UDINT;
    LowestFoundidx : UINT;
  END_VAR

  ret_code := READY;

  CASE pPara^.uiCmd OF
//*****************************************************************************
//** COMMANDS SAFETYMANAGER                                                  **
//*****************************************************************************
//**********************************************************************************************************************************************************
    CMD_SM_INIT_FINISHED:
      // the safety manager has finished his administration work (e.g. connecting the domains)
      bdCommonFlags.bManagerInitialisationFinished := TRUE;

//**********************************************************************************************************************************************************
    CMD_SM_FIRST_CONNECT_READY:
      // the safety manager has finished first time connecting the domains
      bdCommonFlags.bManagerReadyForSDO := TRUE;

//**********************************************************************************************************************************************************
    CMD_SM_DISCONNECT:
      bdCommonFlags.bManagerInitialisationFinished  := FALSE;
      bdCommonFlags.bManagerReadyForSDO             := FALSE;

//**********************************************************************************************************************************************************
    CMD_SM_SDO_TO_MODULE:
      //add message to sdo buffer
      AddSDOToBuffer(p_us_Message := pPara^.aPara[1]$^USINT, us_length := pPara^.aPara[0]$USINT);

//**********************************************************************************************************************************************************
    CMD_SM_GET_SAFETY_NUMBER:
      // check if pointer is valid
      if pPara^.aPara[0] then
        ((pPara^.aPara[0])$^UDINT)^ := SafetyNumber;
      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_ADD_INPUT_PDO:
      // check if the information if it's an interface frame (stored at pPara^.aPara[9]$BOOL) is valid
      if pPara^.aPara[8]$HDINT = SM_IF_VALIDITY_MARK then
        sd_retval := AddPDOToInputBuffer( ud_p2Buffer         := pPara^.aPara[0]$UDINT
                                        , ud_SafetyNr         := pPara^.aPara[2]$UDINT
                                        , b_IsOptional        := pPara^.aPara[4]$BOOL
                                        , ui_PdoLen           := pPara^.aPara[3]$UINT
                                        , ui_TargetOffset     := pPara^.aPara[7]$UINT
                                        , b_IsInterfaceFrame  := pPara^.aPara[9]$BOOL
                                        );
      else
        sd_retval := AddPDOToInputBuffer( ud_p2Buffer         := pPara^.aPara[0]$UDINT
                                        , ud_SafetyNr         := pPara^.aPara[2]$UDINT
                                        , b_IsOptional        := pPara^.aPara[4]$BOOL
                                        , ui_PdoLen           := pPara^.aPara[3]$UINT
                                        , ui_TargetOffset     := pPara^.aPara[7]$UINT
                                        );
      end_if;

      if sd_retval then
        ret_code := ERROR;
      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_REMOVE_INPUT_PDO:
      // we lost connection to other systems => we have to initialize again! remove old input PDOs

      if ui_cnt_PDOInputBuffer then
        // save the old state of the class and set it temporarily to "module not found" to prevent update rt from crashing
        OldClassState := SafetyState;
        SetSafetyState(NewState:=_ModuleNotFound);
        
        // search for the PDO
        FoundNo := 0; // FSoE Data may be split over several DOs
        pPDOInputBuffer := #aPDOInputBuffer[ui_cnt_PDOInputBuffer-1];
        ud_p2BufferCompare := pPara^.aPara[0]$UDINT;
        i := ui_cnt_PDOInputBuffer - 1;
        repeat
        
          if pPDOInputBuffer^.ud_p2Buffer = ud_p2BufferCompare then
            FoundNo += 1;
            LowestFoundidx := i;
            if pPDOInputBuffer^.ui_PDOLen = MDD2000VVSB_BUS_USAGE_PER_PDO_DO then
              // Check if PDO is split over several PDOs
              ud_p2BufferCompare += MDD2000VVSB_BUS_USAGE_PER_PDO_DO; // The offset of the next PDO must be increased by MDD2000VVSB_BUS_USAGE_PER_PDO_DO, if the PDO are part of the same FSoE Frame.
            else
              exit;
            end_if;
          
          elsif FoundNo > 0 then
            // Split PDO must be after each other in memory
            exit;
          end_if;
          
          pPDOInputBuffer -= sizeof(t_PDOInputBuffer);
          i -= 1;
        
        until pPDOInputBuffer <> #aPDOInputBuffer[0] end_repeat;        

        // if we found at least one PDO => remove them from memory (move the memory behind the found PDO over the place of the found PDO)
        if FoundNo then
          // if it's last entry (no entries have to be moved) => we only have to copy some memory if it's not the last
          if LowestFoundidx < (ui_cnt_PDOInputBuffer-FoundNo) then
            for j := LowestFoundidx to (ui_cnt_PDOInputBuffer-FoundNo-1) do
              aPDOInputBuffer[j] := aPDOInputBuffer[j+FoundNo];
            end_for;
          end_if;
          ui_cnt_PDOInputBuffer -= FoundNo;
        end_if;
        
        // restore old class state
        SetSafetyState(NewState:=OldClassState);
      end_if;

//**********************************************************************************************************************************************************
    CMD_SM_ADD_OUTPUT_PDO:
      // return the pointer to the Buffer (is linear local memory, so the offset can be added later)
      pResult^.aData[0]$UDINT := GetPointer2PDOBuffer();
      pResult^.uiLng := 4;

//**********************************************************************************************************************************************************
    CMD_SM_ADD_SDO_TO_BUFFER:
      sd_retval := AddSDOToBuffer(p_us_message:=pPara^.aPara[0]$^USINT, us_length:=pPara^.aPara[1]$USINT);
      if sd_retval then
        ret_code := ERROR;
      end_if;

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

//**********************************************************************************************************************************************************
    CMD_SM_GET_MODULE_TYPE:
      pResult^.aData[0]$UDINT := SM_TYPE_SAFETY_MODULE_FSOE;

//**********************************************************************************************************************************************************
    CMD_SM_POST_INIT_FINISHED:
      // remember that we've been here
      bdCommonFlags.bPostInitPassed := TRUE;

//**********************************************************************************************************************************************************
    CMD_SM_CONFIG_SAFETY_CPU:
      pResult^.aData[0]$UDINT := SafetyNumber; //safetyNbr

//**********************************************************************************************************************************************************
    // commands to ignore
    CMD_SM_DOMAIN_NR_SEND,
    CMD_SM_MODULE_REINIT, // we don't have a config => nothing to reinitialize
    CMD_SM_SET_NEW_SYNC_MODE,
    CMD_SM_FSOE_AVAILABLE,
    CMD_SM_REMOVE_CPU_FROM_TABLE: // we don't have a routing table

//*****************************************************************************
//** COMMANDS FROM BUS INTERFACE                                             **
//*****************************************************************************
//**********************************************************************************************************************************************************
    _BUS_IF_HWCONNECT :
      // device has been connected
      ConnectEvent();
      pResult^.aData[0] := _BUS_IF_RETVAL_OK;

//**********************************************************************************************************************************************************
    _BUS_IF_HWDISCONNECT :
      // device has been disconnected
      DisconnectEvent();
      pResult^.aData[0] := _BUS_IF_RETVAL_OK;

//**********************************************************************************************************************************************************
    _BUS_IF_HWINTERRUPT :
      // error at device which may lead to a disconnect or replace the disconnect callback
      InterruptEvent();
      pResult^.aData[0] := _BUS_IF_RETVAL_OK;

//**********************************************************************************************************************************************************
    _BUS_IF_CHECK_DEVICE_ID :
      // check for correct device id
      pResult^.aData[0] := (DeviceID = pPara^.aPara[0]$UDINT);

      bdCommonFlags.bDeviceIDOk := pResult^.aData[0]$BOOL;

//**********************************************************************************************************************************************************
    _BUS_IF_RESET_DETECTED:
      if SafetyState = _SafetyClassOK then
        SafetyState := _LostPowerSupply;
      end_if;

//**********************************************************************************************************************************************************
    _BUS_IF_RESET_GONE_DETECTED:
      if SafetyState = _LostPowerSupply then
        SafetyState := _SafetyClassOK;
      end_if;


//**********************************************************************************************************************************************************
    CMD_GET_HARDWARE_DIAGNOSIS:
      pResult^.aData[0]$t_HWCGetHardwareDiagnosisOut.hd_SafetyNumber := SafetyNumber;


//*****************************************************************************
//** COMMANDS FROM Drive VV Class                                            **
//*****************************************************************************

//**********************************************************************************************************************************************************
		_BUSIF_VVDRIVE_ADD_ISO_DATAOBJECT:
      // add iso DO
      ret_code := AddAccesses(pPara^.aPara[0]$BOOL);

//**********************************************************************************************************************************************************
		_BUSIF_VVDRIVE_CHANGE_DO_ADDRESSES:
      // set correct DO addresses
      ret_code := SetDOAddresses();

 //**********************************************************************************************************************************************************
		_BUSIF_VVDRIVE_START_ISO_DATAOBJECT:
      // start FSoE data objects
      StartDO();
//      ret_code := READY; // already the default retcode

 //**********************************************************************************************************************************************************
		_BUSIF_VVDRIVE_GET_SAFETY_INFOS:
    
      if bdCommonFlags.bReadFktFW = FALSE then
      
        // Read non safety FW from drive
        // Used to check if drive supports Safey
        
        ret_code := ReadDriveFWVersion();
        
        if ret_code = READY then
        
          bdCommonFlags.bReadFktFW := TRUE;
          ret_code := BUSY;  // Set to busy so that else case is handled
          if abs(FwFktVersion) < MDD2000MNGBASE_FKTFWVERSION_SAFETYSUPPORT then
            ret_code := ERROR;
            HWC_LogPrintf(pThis:=this, format:="@08D3 (MDD2000VVSafetyBase::ClassSvr::NewInst) FW-Version of Drive: %d does not Support safety!", p0:=#FwFktVersion);
          end_if;          
        
        end_if;

      else
      
        if bdCommonFlags.bSdoMechanismInitialized = FALSE then
          // the module has to be physically available to install the handling via OS
          InitSDOMechanism();
          
          bdCommonFlags.bSdoMechanismInitialized := TRUE;
        end_if;

        // get firmware infos (safety number, firmware version)
        ret_code := GetFWInfos();
      end_if;

//*****************************************************************************
//** COMMANDS FROM Drive Mng Class                                           **
//*****************************************************************************
      
  //**********************************************************************************************************************************************************
    MDD2000MNG_IF_SAFETYCONFIG_CHANGED:
      
      ret_code := ERROR; //unsupported version
    
      //Check version 1
      if pPara^.aPara[0] = MDD2000MNG_IF_SAFETYCONFIG_CHANGED_VER_1 then
      
        if (pPara^.aPara[2] > 0) &  (pPara^.aPara[3] > 0) then  // FSoE Len must be valid
        
          if SSWConfigChanged = _IdleConfigChanged then
            
            sFwInfos.sPdoInfo.PDORdLen := FSoEData2FrameLen(pPara^.aPara[2]);
            sFwInfos.sPdoInfo.PDOWrLen := FSoEData2FrameLen(pPara^.aPara[3]);
          
            SSWConfigChanged := _StartConfigChanged;
            ret_code := READY; 
          else
            ret_code := BUSY; 
          end_if;
        
        end_if;
        
        pResult^.uiLng := 4;
        pResult^.aData[0]$DINT := MDD2000MNG_IF_SAFETYCONFIG_CHANGED_VER_MAX;  //max supported version
      end_if;

//**********************************************************************************************************************************************************
  else
    if bd_LogOnceMarker.InvalidNewInstCmd = FALSE then
      bd_LogOnceMarker.InvalidNewInstCmd := TRUE;
      LogValue("@024A (MDD2000VVSafetyBase::ClassSvr::NewInst) [SL] Received invalid command: 0x{0}", Value:=pPara^.uiCmd);
    end_if;
    ret_code := ERROR;

//**********************************************************************************************************************************************************
  END_CASE;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::SetSafetyState
	VAR_INPUT
		NewState 	: SafetyConfigStateType;
	END_VAR

  // use it locally => can also be forwarded in derivation
  SafetyState := NewState;

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::ChangeRdDO
	VAR_INPUT
		AccessHandle 	: UDINT;
		NewOffset 	: HDINT;
		NewLength 	: UDINT;
	END_VAR
	VAR_OUTPUT
		Success 	: BOOL;
	END_VAR
  VAR
  	MyPara   : CmdStruct;
    MyResult : results;
    dataUPDATEDO    : t_UpdateDOCfg;
  END_VAR

  Success := TRUE;

  // if length = zero we may just want to stop the DO
  if NewLength then
    dataUPDATEDO.Handle               := AccessHandle;
    dataUPDATEDO.ud_addressoff_read   := NewOffset;
    dataUPDATEDO.ud_length_read       := NewLength$UINT;

    MyPara.uiCmd         := _BUS_IF_UPDATE_DO;
    MyPara.aPara[0]      := (#dataUPDATEDO)$DINT;

    ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
    if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
      LogError("@0753 (MDD2000VVSafetyBase::ChangeRdDO) Failed to change asynchron read access");
      Success := FALSE;
    end_if;
  else
    Success := FALSE;
  end_if;

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::ChangeWrDO
	VAR_INPUT
		AccessHandle 	: UDINT;
		NewOffset 	: HDINT;
		NewLength 	: UDINT;
	END_VAR
	VAR_OUTPUT
		Success 	: BOOL;
	END_VAR
  VAR
  	MyPara   : CmdStruct;
    MyResult : results;
    dataUPDATEDO    : t_UpdateDOCfg;
  END_VAR

  Success := TRUE;

  // if length = zero we may just want to stop the DO
  if NewLength then
    dataUPDATEDO.Handle               := AccessHandle;
    dataUPDATEDO.ud_addressoff_write  := NewOffset;
    dataUPDATEDO.ud_length_write      := NewLength$UINT;

    MyPara.uiCmd         := _BUS_IF_UPDATE_DO;
    MyPara.aPara[0]      := (#dataUPDATEDO)$DINT;

    ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
    if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
      LogError("@0754 (MDD2000VVSafetyBase::ChangeWrDO) Failed to change asynchron write access");
      Success := FALSE;
    end_if;
  else
    Success := FALSE;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::SetDOAddresses
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR
  	MyPara   : CmdStruct;
    MyResult : results;
    usTmpCnt : USINT;
    offset   : UINT;
    sizeleft : UINT;
    size     : UINT;
  END_VAR

  //first get correct offsets from mailbox
  //FSOE
  MyPara.uiCmd    := _BUSIF_VVDRIVE_GET_MAILBOX;
  MyPara.aPara[0] := _BUSIF_VVDRIVE_SERVICE_APP_FSOE;

  dRetCode := ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
  if dRetCode = READY then
    //store mailbox infos
    sAccesses.tPdoRd.sMailboxInfo := MyResult.aData[0]$t_MailBox.Response;
    sAccesses.tPdoWr.sMailboxInfo := MyResult.aData[0]$t_MailBox.Request;
  else
    LogError("@0757 (MDD2000VVSafetyBase::SetDOAddresses) Failed to get Mailbox Infos for FSOE.");
    return;
  end_if;
  
  // Reset Info change of safety config
  bdSafetyConfigChangedResult := 0;
  bdSafetyConfigChangedResult.ConfigChanged := TRUE;
  

  // first check if this DO is available
  if sAccesses.tPdoRd.uiMaxReadDOsUsed then
  
    // Calc new size
    sAccesses.tPdoRd.uiMyPDOSize := sFwInfos.sPdoInfo.PDORdLen + sizeof(tsMailboxFooter);
    if sAccesses.tPdoRd.uiMyPDOSize = sAccesses.tPdoRd.uiMaxPDOSize then
      // Calc no of used DOs
      sAccesses.tPdoRd.uiReadDOsUsed := ((sAccesses.tPdoRd.uiMyPDOSize-1) / MDD2000VVSB_BUS_USAGE_PER_PDO_DO) + 1;
    elsif sAccesses.tPdoRd.uiMyPDOSize > sAccesses.tPdoRd.uiMaxPDOSize then
      bd_LogCy.RdPDOSizeInc := TRUE;
      bdSafetyConfigChangedResult.RdPDOSizeInc := TRUE;
      dRetCode := ERROR;
      return;
    else
      bd_LogCy.RdPDOSizeDec := TRUE;
      bdSafetyConfigChangedResult.RdPDOSizeInc := TRUE;
      dRetCode := ERROR;
      return;
    end_if;
    
    // Calculate offset of 1st DO
    offset   := sAccesses.tPdoRd.sMailboxInfo.hiAddress+sAccesses.tPdoRd.sMailboxInfo.uiLength - sAccesses.tPdoRd.uiMyPDOSize;
    sizeleft := sAccesses.tPdoRd.uiMyPDOSize;
    
    for usTmpCnt := 0 to sAccesses.tPdoRd.uiReadDOsUsed-1 do
    
      if sizeleft > MDD2000VVSB_BUS_USAGE_PER_PDO_DO then
        size      := MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
        sizeleft  -= MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      else
        size      := sizeleft;
        sizeleft  := 0;
      end_if;
      
      //set footer pointer to last data object
      if usTmpCnt = (sAccesses.tPdoRd.uiReadDOsUsed-1) then
        sAccesses.tPdoRd.pMBFooter := sAccesses.tPdoRd.aPDOs[usTmpCnt].pData + size - sizeof(tsMailboxFooter);
      end_if;
      
      sAccesses.tPdoRd.aPDOs[usTmpCnt].Length := size$USINT;
      
      ChangeRdDO( AccessHandle  := sAccesses.tPdoRd.aPDOs[usTmpCnt].Handle
                , NewOffset     := offset
                , NewLength     := size
                );
                
      offset += MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      
      if sizeleft = 0 then
        exit;
      end_if;
      
    end_for;
  elsif sAccesses.tPdoRd.uiMyPDOSize > sizeof(tsMailboxFooter) then
    // No DOs were created but new payload > 0
    bd_LogCy.RdPDOSizeInc := TRUE;
    bdSafetyConfigChangedResult.RdPDOSizeInc := TRUE;
    dRetCode := ERROR;
    return;
  end_if;

  if sAccesses.tPdoWr.uiMaxWriteDOsUsed then
  
    // Calc new size
    sAccesses.tPdoWr.uiMyPDOSize := sFwInfos.sPdoInfo.PDOWrLen + sizeof(tsMailboxFooter);
    if sAccesses.tPdoWr.uiMyPDOSize = sAccesses.tPdoWr.uiMaxPDOSize then
      // Calc no of used DOs
      sAccesses.tPdoWr.uiWriteDOsUsed := ((sAccesses.tPdoWr.uiMyPDOSize-1) / MDD2000VVSB_BUS_USAGE_PER_PDO_DO) + 1;
    elsif sAccesses.tPdoWr.uiMyPDOSize > sAccesses.tPdoWr.uiMaxPDOSize then
      bd_LogCy.WrPDOSizeInc := TRUE;
      bdSafetyConfigChangedResult.WrPDOSizeInc := TRUE;
      dRetCode := ERROR;
      return;
    else
      bd_LogCy.WrPDOSizeDec := TRUE;
      bdSafetyConfigChangedResult.WrPDOSizeDec := TRUE;
      dRetCode := ERROR;
      return;
    end_if;
  
    // Calculate offset of 1st DO
    offset   := sAccesses.tPdoWr.sMailboxInfo.hiAddress+sAccesses.tPdoWr.sMailboxInfo.uiLength - sAccesses.tPdoWr.uiMyPDOSize;
    sizeleft := sAccesses.tPdoWr.uiMyPDOSize;
    
    for usTmpCnt := 0 to sAccesses.tPdoWr.uiWriteDOsUsed-1 do
    
      if sizeleft > MDD2000VVSB_BUS_USAGE_PER_PDO_DO then
        size      := MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
        sizeleft  -= MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      else
        size      := sizeleft;
        sizeleft  := 0;
      end_if;
      
      sAccesses.tPdoWr.aPDOs[usTmpCnt].Length := size$USINT;
      
      ChangeWrDO( AccessHandle  := sAccesses.tPdoWr.aPDOs[usTmpCnt].Handle
                , NewOffset     := offset
                , NewLength     := size
                );
      
      offset += MDD2000VVSB_BUS_USAGE_PER_PDO_DO;
      
      if sizeleft = 0 then
        exit;
      end_if;
      
    end_for;
  
  elsif sAccesses.tPdoWr.uiMyPDOSize > sizeof(tsMailboxFooter) then
    // No DOs were created but new payload > 0
    bd_LogCy.WrPDOSizeInc := TRUE;
    bdSafetyConfigChangedResult.WrPDOSizeInc := TRUE;
    dRetCode := ERROR;
    return;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::SDOStdRequ
	VAR_INPUT
		us_Command 	: USINT;
		usLength 	: USINT;(* := 0 *)
		hdAddress 	: HDINT;(* := 0 *)
	END_VAR
	VAR_OUTPUT
		sd_retval 	: DINT;
	END_VAR
  VAR
    pSDORequestBuffer   : ^t_SDOStdHeader;
    RequBuffer          : ARRAY [0..127] OF USINT;
    usAdditionalLen     : USINT;
  END_VAR

  _memset(dest:=#RequBuffer, usByte:=0x00, cntr:=sizeof(RequBuffer));

  //SDO requests for this commands don't have any further data
  pSDORequestBuffer$^void := #RequBuffer[0];

  //calculate length without crc
  pSDORequestBuffer^.len := sizeof(t_SDOStdHeader) - sizeof(t_SDOStdHeader.CRC);

  //type
  pSDORequestBuffer^.typ := SDO_STD;
  
  //command to inform CPU about initializing classes is ok
  pSDORequestBuffer^.cmd := us_Command;

  //set CRC-field to 0
  pSDORequestBuffer^.CRC := 0;
  usAdditionalLen        := 0;

  if usLength & hdAddress & us_Command = SBF_SDO_COMMAND_READ then
    pSDORequestBuffer^$t_SDORequStdRd.udAddress := hdAddress;
    pSDORequestBuffer^$t_SDORequStdRd.usLength  := usLength;
    usAdditionalLen := sizeof(t_SDORequStdRd.udAddress)+sizeof(t_SDORequStdRd.usLength);
    pSDORequestBuffer^.len := pSDORequestBuffer^.len + usAdditionalLen;
  end_if;

  sd_retval := AddSDOToBuffer(p_us_message:=pSDORequestBuffer$^USINT, us_length:=(sizeof(t_SDOStdHeader)+usAdditionalLen));

  //no response
  if ( sd_retval = -1 ) then
    //log not routeable SDOs
    LogError("@075A (MDD2000VVSafetyBase::SDOStdRequ) Buffer for SDO's is full.");
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::GetFWInfos
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR
  VAR  	
    MyPara 	    : CmdStruct;
    MyResult	  : results;
		dTmpRetCode : iprStates;
    ParameterState : DINT;
  END_VAR

  dRetCode := BUSY;

  case sFwInfos.SSWFwInfo of
    //********************************************************
  	eSSWFwInfo::_IdleFwInfo:
      if bdCommonFlags.bSDOsViaOS then
        sFwInfos.SSWFwInfo := _RequSafetyNrb;
      else
        sFwInfos.SSWFwInfo := _GetSSDOMailbox;
      end_if;


    //********************************************************
  	eSSWFwInfo::_GetSSDOMailbox:
      //SSDO
      MyPara.uiCmd    := _BUSIF_VVDRIVE_GET_MAILBOX;
      MyPara.aPara[0] := _BUSIF_VVDRIVE_SERVICE_APP_SSDO;

      dTmpRetCode := ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);
      if dTmpRetCode = READY then
        //store mailbox infos
        sAccesses.tSDORd.sMailboxInfo := MyResult.aData[0]$t_MailBox.Response;
        sAccesses.tSDOWr.sMailboxInfo := MyResult.aData[0]$t_MailBox.Request;
      else
        LogError("@0759 (MDD2000VVSafetyBase::GetFWInfos) Failed to get Mailbox Infos for SSDO.");
        SetSafetyState(NewState:=_ModuleNotFound);
        sFwInfos.SSWFwInfo := _ErrorFwInfo;
        return;
      end_if;

      sAccesses.tSDORd.udActLength := sizeof(tsMailboxFooter);
      sAccesses.tSDORd.hdActOffset := sAccesses.tSDORd.sMailboxInfo.hiAddress+sAccesses.tSDORd.sMailboxInfo.uiLength-sAccesses.tSDORd.udActLength;
      ChangeRdDO(AccessHandle:=sAccesses.tSDORd.sDOInfo.Handle
        , NewOffset:= sAccesses.tSDORd.hdActOffset
        , NewLength:= sAccesses.tSDORd.udActLength
        );
        
      sAccesses.tSDOWr.udActLength := SBF_SDO_MAX_LENGTH+sizeof(tsMailboxFooter);
      sAccesses.tSDOWr.hdActOffset := sAccesses.tSDOWr.sMailboxInfo.hiAddress+sAccesses.tSDOWr.sMailboxInfo.uiLength-sAccesses.tSDOWr.udActLength;
      ChangeWrDO(AccessHandle:=sAccesses.tSDOWr.sDOInfo.Handle
        , NewOffset:=sAccesses.tSDOWr.hdActOffset
        , NewLength:=sAccesses.tSDOWr.udActLength
        );

      sFwInfos.usRetries := 0;
      sFwInfos.SSWFwInfo := _RequSafetyNrb;

    //********************************************************
  	eSSWFwInfo::_RequSafetyNrb:   
      SDOStdRequ(us_Command:=SBF_SDO_COMMAND_GET_SAFENBR);
      
      if bdCommonFlags.bSDOsViaOS = FALSE then
        //start read access
        SetDORunState(Handle:=sAccesses.tSDORd.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_CONT_RUN);
      end_if;

      sFwInfos.bdInfosGet.SaferyNrb := TRUE;
      sFwInfos.udTimeStamp := ops.tAbsolute;
      sFwInfos.SSWFwInfo   := _Wait4SafetyNrb;

    //********************************************************
  	eSSWFwInfo::_Wait4SafetyNrb:

      if sFwInfos.bdInfosGet.SaferyNrb = FALSE then
        sFwInfos.usRetries := 0;
        sFwInfos.SSWFwInfo := _RequFwVersion;
      elsif (ops.tAbsolute-sFwInfos.udTimeStamp) >= MDD2000VVSB_SDO_RESPONSE_TIMEOUT then
        if sFwInfos.usRetries < MDD2000VVSB_SDO_MAX_RETRIES then
          sFwInfos.SSWFwInfo := _RequSafetyNrb;
          sFwInfos.usRetries += 1;
        else
          LogValue("@075B (MDD2000VVSafetyBase::GetFWInfos)Failed to read SafetyNumber from SDO Address 0x{0}", Value:=sAccesses.tSDORd.hdActOffset);
          LogValue("@075B (MDD2000VVSafetyBase::GetFWInfos)Request SDO Address is 0x{0}", Value:=sAccesses.tSDOWr.hdActOffset);
          SetSafetyState(NewState:=_ReadFWVerFailed);
          sFwInfos.SSWFwInfo := _ErrorFwInfo;
        end_if;
      end_if;

    //********************************************************
  	eSSWFwInfo::_RequFwVersion:
    
      SDOStdRequ(us_Command:=SBF_SDO_COMMAND_GET_FW_VERSION);
      sFwInfos.bdInfosGet.FwVersion := TRUE;
      sFwInfos.udTimeStamp := ops.tAbsolute;
      sFwInfos.SSWFwInfo   := _Wait4FwVersion;

    //********************************************************
  	eSSWFwInfo::_Wait4FwVersion:
      
      if sFwInfos.bdInfosGet.FwVersion = FALSE then      
        
        sFwInfos.SSWFwInfo := eSSWFwInfo::_RequPDOLengthWr;

        if bdCommonFlags.bSDOsViaOS = FALSE then
          //deactivate read access
          SetDORunState(Handle:=sAccesses.tSDORd.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);
        end_if;
      elsif (ops.tAbsolute-sFwInfos.udTimeStamp) >= MDD2000VVSB_SDO_RESPONSE_TIMEOUT then
        if sFwInfos.usRetries < MDD2000VVSB_SDO_MAX_RETRIES then
          sFwInfos.SSWFwInfo := _RequFwVersion;
          sFwInfos.usRetries += 1;
        else
          LogValue("@075C (MDD2000VVSafetyBase::GetFWInfos)Failed to read FirmwareVersion from SDO Address 0x{0}", Value:=sAccesses.tSDORd.hdActOffset);
          LogValue("@075C (MDD2000VVSafetyBase::GetFWInfos)Request SDO Address is 0x{0}", Value:=sAccesses.tSDOWr.hdActOffset);
          SetSafetyState(NewState:=_ReadFWVerFailed);
          sFwInfos.SSWFwInfo := _ErrorFwInfo;
        end_if;
      end_if;

    //********************************************************
  	eSSWFwInfo::_RequPDOLengthWr:
    
      MyPara.uiCmd  := BUSIF_DRIVEMNGBASE_GET_PARA_STATE;
      MyPara.aPara[0] := 0; // to_dint(AxBuffer[usAxis].AxisIndex);
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      ParameterState := MyResult.aData[0]$DINT;
  
      if (dTmpRetCode = ERROR) | (ParameterState <> 0) then
        HWC_LogPrintf(pThis:=this, format:="@08D4 (MDD2000VVSafetyBase::GetFWInfos) Step Request _RequPDOLengthWr->GET_PARA_STATE Failed. Retcode: %d, State %d", #dTmpRetCode, #ParameterState);
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        return;
      end_if;
    
      MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_SET_PARAMETER;
      MyPara.aPara[0] := 0;                   // to_dint(usAxis);
      MyPara.aPara[1] := MDD2000_PID_SAFETY_CFG_INFO;
      MyPara.aPara[2] := MDD2000_SPID_SAFETY_CFG_IN_FSOEFRAME; // SubID
      MyPara.aPara[3] := 0; // Read
      MyPara.aPara[4] := NIL$DINT;
      MyPara.aPara[5] := 0;
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      
      if (dTmpRetCode = READY) & (MyResult.aData[0]$DINT = 0) then
        sFwInfos.SSWFwInfo   := eSSWFwInfo::_Wait4PDOLengthWr;
      else
        HWC_LogPrintf(pThis:=this, format:="@08D5 (MDD2000VVSafetyBase::GetFWInfos) Step Request _RequPDOLengthWr->SET_PARAMETER Failed. Retcode: %d, State %d", #dTmpRetCode, #(MyResult.aData[0]$DINT) );
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;        
      end_if;

    //********************************************************
  	eSSWFwInfo::_Wait4PDOLengthWr:
    
      MyPara.uiCmd  := BUSIF_DRIVEMNGBASE_GET_PARA_STATE;
      MyPara.aPara[0] := 0; // to_dint(AxBuffer[usAxis].AxisIndex);
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      ParameterState := MyResult.aData[0]$DINT;
  
      if (dTmpRetCode = ERROR) | 
         ( (ParameterState <> 0) & (ParameterState <> -1) ) then
        HWC_LogPrintf(pThis:=this, format:="@08D6 (MDD2000VVSafetyBase::GetFWInfos) Step Request _Wait4PDOLengthWr->GET_PARA_STATE Failed. Retcode: %d, State %d", #dTmpRetCode, #ParameterState);
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        return;
      end_if;
      
      if ParameterState = 0 then
          
        MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_GET_PARAMETER;
        MyPara.aPara[0] := 0; // to_dint(usAxis);
        dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
        _memcpy(ptr1:=#DS402DataBuffer[0], ptr2:=(MyResult.aData[0])$pVoid, cntr:=sizeof(DS402DataBuffer));
        
        if dTmpRetCode = READY then
          sFwInfos.sPdoInfo.PDOWrLen := FSoEData2FrameLen(DS402DataBuffer[0]);
          sFwInfos.SSWFwInfo :=  eSSWFwInfo::_RequPDOLengthRd;  
        elsif dTmpRetCode <> BUSY then // = ERROR
          HWC_LogPrintf(pThis:=this, format:="@08D7 (MDD2000VVSafetyBase::GetFWInfos) Step Request _Wait4PDOLengthWr->GET_PARAMETER Failed. Retcode: %d, State %d", #dTmpRetCode, #(MyResult.aData[0]$DINT) );
          sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        end_if;

      else // ParameterState = -1
        // dRetCode := BUSY; Try again
      end_if;
      
    //********************************************************
  	eSSWFwInfo::_RequPDOLengthRd:
      
      MyPara.uiCmd  := BUSIF_DRIVEMNGBASE_GET_PARA_STATE;
      MyPara.aPara[0] := 0; // to_dint(AxBuffer[usAxis].AxisIndex);
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      ParameterState := MyResult.aData[0]$DINT;
  
      if (dTmpRetCode = ERROR) | (ParameterState <> 0) then
        HWC_LogPrintf(pThis:=this, format:="@08D8 (MDD2000VVSafetyBase::GetFWInfos) Step Request _RequPDOLengthRd->GET_PARA_STATE Failed. Retcode: %d, State %d", #dTmpRetCode, #ParameterState);
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        return;
      end_if;
    
      MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_SET_PARAMETER;
      MyPara.aPara[0] := 0;                   // to_dint(usAxis);
      MyPara.aPara[1] := MDD2000_PID_SAFETY_CFG_INFO;
      MyPara.aPara[2] := MDD2000_SPID_SAFETY_CFG_OUT_FSOEFRAME; // SubID
      MyPara.aPara[3] := 0; // Read
      MyPara.aPara[4] := NIL$DINT;
      MyPara.aPara[5] := 0;
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      
      if (dTmpRetCode = READY) & (MyResult.aData[0]$DINT = 0) then
        sFwInfos.SSWFwInfo   := eSSWFwInfo::_Wait4PDOLengthRd;
      else
        HWC_LogPrintf(pThis:=this, format:="@08D9 (MDD2000VVSafetyBase::GetFWInfos) Step Request _RequPDOLengthRd->SET_PARAMETER Failed. Retcode: %d, State %d", #dTmpRetCode, #(MyResult.aData[0]$DINT) );
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
      end_if;
    
    //********************************************************
  	eSSWFwInfo::_Wait4PDOLengthRd:
    
      MyPara.uiCmd  := BUSIF_DRIVEMNGBASE_GET_PARA_STATE;
      MyPara.aPara[0] := 0; // to_dint(AxBuffer[usAxis].AxisIndex);
      dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      ParameterState := MyResult.aData[0]$DINT;
  
      if (dTmpRetCode = ERROR) | 
         ( (ParameterState <> 0) & (ParameterState <> -1) ) then
        HWC_LogPrintf(pThis:=this, format:="@08DA (MDD2000VVSafetyBase::GetFWInfos) Step Request _Wait4PDOLengthRd->GET_PARA_STATE Failed. Retcode: %d, State %d", #dTmpRetCode, #ParameterState);
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        return;
      end_if;
      
      if ParameterState = 0 then
          
        MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_GET_PARAMETER;
        MyPara.aPara[0] := 0; // to_dint(usAxis);
        dTmpRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
        _memcpy(ptr1:=#DS402DataBuffer[0], ptr2:=(MyResult.aData[0])$pVoid, cntr:=sizeof(DS402DataBuffer));
        
        if dTmpRetCode = READY then
          sFwInfos.sPdoInfo.PDORdLen := FSoEData2FrameLen(DS402DataBuffer[0]);
          sFwInfos.SSWFwInfo :=  _CreatePDOs;  
        elsif dTmpRetCode <> BUSY then // = ERROR
          HWC_LogPrintf(pThis:=this, format:="@08DB (MDD2000VVSafetyBase::GetFWInfos) Step Request _Wait4PDOLengthRd->GET_PARAMETER Failed. Retcode: %d, State %d", #dTmpRetCode, #(MyResult.aData[0]$DINT) );
          sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
        end_if;

      else // ParameterState = -1
        // dRetCode := BUSY; Try again
      end_if;
      
    //********************************************************
  	eSSWFwInfo::_CreatePDOs:
    
      sFwInfos.SSWFwInfo := _SendInfoToMng;
      if AddAccesses(bAddLater:=TRUE) <> READY then
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
      end_if;
      
    //********************************************************
  	eSSWFwInfo::_SendInfoToMng:
      // we have to wait for the PDOs first, otherwise the routing initiated by the safety manager wouldn't work
      // when there are no pdo data just go on
      if bdCommonFlags.bPdosCreated | bdCommonFlags.bNoPDOData then 
      
        //add infos for safety manager routing
        // give the manager more details about the module
        if pSafetyManagerThis then
          MyPara.uiCmd := CMD_SM_ADD_MODULE_INFO;
          MyPara.aPara[0] := THIS$DINT;
          MyPara.aPara[1] := SafetyNumber$DINT;
          MyPara.aPara[2] := FirmwareVersion$DINT;
          MyPara.aPara[3] := 16#FF; // fake FPGA_Version (only needed for CDIAS CPUs)
          MyPara.aPara[4] := sConnInfo.uiHwPathLen;
          MyPara.aPara[5] := sConnInfo.pHwPath$DINT;
          pSafetyManagerThis^.NewInst(#MyPara, #MyResult);

        else
          LogError("@075D (MDD2000VVSafetyBase::GetFWInfos) Couldn't find object SafetyManager of HwControl class!");
        end_if;

        sFwInfos.SSWFwInfo := _FinishedFwInfo;
      
      else
        // Cancel if flags are not set by this time! They will never if READY is not returned
        sFwInfos.SSWFwInfo :=  _ErrorFwInfo;
      end_if;

    //********************************************************
  	eSSWFwInfo::_FinishedFwInfo:

      SetSafetyState(NewState:=_SafetyClassOK);
      ClassSvr           := _ClassOk;
      sFwInfos.SSWFwInfo := _IdleFwInfo;
      dRetCode := READY;

    //********************************************************
  	eSSWFwInfo::_ErrorFwInfo:
      if bdCommonFlags.bSDOsViaOS = FALSE then
        SetDORunState(Handle:=sAccesses.tSDORd.sDOInfo.Handle, NewState:=_BUS_IF_RUN_STATE_NO_RUN);
      end_if;
      sFwInfos.SSWFwInfo := _IdleFwInfo;
      dRetCode           := ERROR;

  end_case;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::AddPDOToInputBuffer
	VAR_INPUT
		ud_p2Buffer 	: UDINT;
		ud_SafetyNr 	: HDINT;
		b_IsOptional 	: BOOL;
		ui_PdoLen 	: UINT;(* := 0 *)
		ui_TargetOffset 	: UINT;(* := 0 *)
		b_IsInterfaceFrame 	: BOOL;(* := FALSE *)
	END_VAR
	VAR_OUTPUT
		sd_retval 	: DINT;
	END_VAR
  VAR
    p_BufferElement       : ^t_PDOInputBuffer;
    ui_LenAct             : UINT;
    b_FoundOnlyBySafetyNr : BOOL;
  END_VAR

  sd_retval := 0;

  // check if we already got a write pdo which can be used as target memory
  if sAccesses.tPdoWr.aPDOs[ui_TargetOffset /MDD2000VVSB_BUS_USAGE_PER_PDO_DO].pData = NIL then
    // no valid target memory
    sd_retval := -2;
    return;
  end_if;

  //check if input buffer is already in list
  p_BufferElement := GetPDOInputBuffer(ud_p2Buffer := ud_p2Buffer, ud_SafetyNr, #b_FoundOnlyBySafetyNr, b_IsInterfaceFrame, ui_TargetOffset);

  //buffer already in list and it's no interface frame
  if ( p_BufferElement ) & (b_IsInterfaceFrame = FALSE) then
    // if it has been found by number and the length of the PDO matches => only change buffer and target offset (dynamic reassignment e.g. used for automated guided vehicles)
    if b_FoundOnlyBySafetyNr & (ui_PdoLen = p_BufferElement^.ui_PDOLen) then
      p_BufferElement^.ud_p2Buffer        := ud_p2Buffer;
      p_BufferElement^.ui_TargetOffset    := ui_TargetOffset;
      p_BufferElement^.p_TargetMemory     := sAccesses.tPdoWr.aPDOs[ui_TargetOffset / MDD2000VVSB_BUS_USAGE_PER_PDO_DO].pData + (ui_TargetOffset MOD MDD2000VVSB_BUS_USAGE_PER_PDO_DO);
    end_if;
    return;
  elsif ui_cnt_PDOInputBuffer < SBF_PDO_MAX_NUMBER then
    // check if it's on the border
    if ((ui_TargetOffset MOD MDD2000VVSB_BUS_USAGE_PER_PDO_DO) + ui_PdoLen) > MDD2000VVSB_BUS_USAGE_PER_PDO_DO then

      // calculate how many bytes we can store in the actual DO
      ui_LenAct := MDD2000VVSB_BUS_USAGE_PER_PDO_DO - (ui_TargetOffset MOD MDD2000VVSB_BUS_USAGE_PER_PDO_DO);

      // we need another virtual PDO for the rest (split up the copy commands in real time) => recursion!
      sd_retval := AddPDOToInputBuffer(ud_p2Buffer + ui_LenAct, ud_SafetyNr, b_IsOptional, ui_PdoLen - ui_LenAct, ui_TargetOffset + ui_LenAct, b_IsInterfaceFrame);

      ui_PdoLen := ui_LenAct;
    end_if;

#ifdef SAFETY_DEBUG_LOGGING_ACTIVATED
    LogValue(pString:="Adding new buffer (InputPDO). IsInterfaceFrame 0x{0}; New TargetOffset 0x{1}, SafetyNbr: 0x{2}", Value:=b_IsInterfaceFrame, Value2:=ui_TargetOffset, Value3:=ud_SafetyNr);
#endif

    aPDOInputBuffer[ui_cnt_PDOInputBuffer].ud_p2Buffer        := ud_p2Buffer;
    //*****************************************************************************

    aPDOInputBuffer[ui_cnt_PDOInputBuffer].b_TimeSynchron     := TRUE; // at FSoE we do not have to wait for time synchronisation
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].ud_SafetyNr        := ud_SafetyNr;
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].ui_PDOLen          := ui_PDOLen;
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].b_IsOptional       := b_IsOptional;
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].b_IsInterfaceFrame := b_IsInterfaceFrame;
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].ui_TargetOffset    := ui_TargetOffset;
    aPDOInputBuffer[ui_cnt_PDOInputBuffer].p_TargetMemory     := sAccesses.tPdoWr.aPDOs[ui_TargetOffset / MDD2000VVSB_BUS_USAGE_PER_PDO_DO].pData + (ui_TargetOffset MOD MDD2000VVSB_BUS_USAGE_PER_PDO_DO);

    ui_cnt_PDOInputBuffer += 1;
  else
    // no more space available
    sd_retval := -1;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::GetPDOInputBuffer
	VAR_INPUT
		ud_p2Buffer 	: UDINT;
		ud_SafetyNr 	: HDINT;
		p_FoundSafetyNr 	: ^BOOL;(* := NIL *)
		b_IsInterfaceFrame 	: BOOL;
		ui_TargetOffset 	: UINT;
	END_VAR
	VAR_OUTPUT
		p_InputBuffer 	: ^t_PDOInputBuffer;
	END_VAR
  VAR
    i1                : UINT;
    p_InputBuffer_Act : ^t_PDOInputBuffer;
  END_VAR

  p_InputBuffer := NIL;

  if p_FoundSafetyNr then
    p_FoundSafetyNr^ := FALSE;
  end_if;

  if ui_cnt_PDOInputBuffer then

    //set pointer on first input buffer element
    p_InputBuffer_Act := #aPDOInputBuffer[0];

    //check each element
    for i1 := 0 to (ui_cnt_PDOInputBuffer-1) do

      //if searched pointer is found, pointer on input buffer element is returned
      if (p_InputBuffer_Act^.ud_p2Buffer        = ud_p2Buffer )       &
         (p_InputBuffer_Act^.b_IsInterfaceFrame = b_IsInterfaceFrame) &

         // if it's an interface frame, the offset has to be the same (otherwise it could be a dynamically connected FlexCon)
        ((b_IsInterfaceFrame = FALSE) |
         (p_InputBuffer_Act^.ui_TargetOffset    = ui_TargetOffset))   then
        // even if found by SafetyNumber, if found by buffer => reset the found by safetynumber
        p_FoundSafetyNr^ := FALSE;
        p_InputBuffer := p_InputBuffer_Act;
        return;
      end_if;

      if p_FoundSafetyNr & (p_InputBuffer_Act^.ud_SafetyNr        = ud_SafetyNr)        &
                           (p_InputBuffer_Act^.b_IsInterfaceFrame = b_IsInterfaceFrame) &

                           // if it's an interface frame, the offset has to be the same (otherwise it could be a dynamically connected FlexCon)
                          ((b_IsInterfaceFrame = FALSE) |
                           (p_InputBuffer_Act^.ui_TargetOffset    = ui_TargetOffset))   then
        p_FoundSafetyNr^ := TRUE;
        p_InputBuffer := p_InputBuffer_Act;
//        return;
      end_if;


      //set pointer to next element
      p_InputBuffer_Act += sizeof ( t_PDOInputBuffer );

    end_for;
  end_if;

END_FUNCTION


FUNCTION VIRTUAL GLOBAL MDD2000VVSafetyBase::SDOLogin
	VAR_INPUT
		pThis 	: pVirtualBase;
	END_VAR
	VAR_OUTPUT
		ret_code 	: iprStates;
	END_VAR

  ret_code := ERROR;
  
  // check for valid pointer
  if pThis then
    // if we have no this pointer stored yet or we have the same stored
    if (pSDOLogin = NIL) | (pSDOLogin = pThis) then
      // store it and return ok
      pSDOLogin := pThis;
      
      ret_code := READY;
    end_if;
  end_if;

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::InitSDOsViaHWK
  VAR
  	dRetCode : iprStates;
  END_VAR
  
  if bdCommonFlags.bSdosCreated = FALSE then
    // use the class memory
    p_Handle_SDO_Read  := #sAccesses.tSDORd.sActSDO;
    p_Handle_SDO_Write := #sAccesses.tSDOWr.sActSDO;

    //add default SDO to read infos from safety
    dRetCode := AddSSDO();

    if dRetCode = ERROR then
      ClassSvr := _CreateDOFailed;
    end_if;

    if dRetCode = READY then
      bdCommonFlags.bSdosCreated := TRUE;
    end_if;
  else
    dRetCode := READY;
  end_if;

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::InitSDOMechanism
  VAR
    MyPara 	  : CmdStruct;
    MyResult	: results;
  	sd_retval : DINT;
    LogVal1   : DINT;
  END_VAR

  // if safety interface is available and it's a salamander system (use the hardware class interface if it's RTK)
  if pISafety & HWC_ops_is_salamander() then

    //register module as safety module in OS and receive pointers on SDO buffers
    sd_retval := SAFETY_iAddModuleToSafetyTask( uiMaster  := 0
                                              , pucNode   := p_ObjectPath
                                              , pucRdSDO  := ( #p_Handle_SDO_Read )$^UDINT
                                              , pucWrSDO  := ( #p_Handle_SDO_Write )$^UDINT
                                              );
  
    //look for an error
    if ( sd_retval ) then   
      // newer oS needed or SigDriveInfo.txt in VARAN Typelabel of Drive is missing
      
      HWC_LogPrintf(pThis:=this, format:="@01D5 (MDD2000VVSafetyBase::InitSDOMechanism) Module not added to Safety Communication Task. Error %d", #sd_retval);
      
      InitSDOsViaHWK();             
      
      bdCommonFlags.bSDOsViaOS := FALSE;
    else
      // no error => use OS interface
      bdCommonFlags.bSDOsViaOS := TRUE;
      
      if HWC_IsPostInitFinished() = FALSE then
        // tell the VARAN Manager that we need to give the OS some time during the post init phase for SDO processing
        MyPara.uiCmd         := _BUS_IF_NEED_POST_INIT_TASK_DELAY;

        ToBusInterface.NewInst(pPara:=#MyPara, pResult:=#MyResult);

        if MyResult.aData[0]$t_BusIFRetvalErrorcode <> _BUS_IF_RETVAL_OK then
          LogVal1 := MyResult.aData[0]$DINT;
          HWC_LogPrintf(pThis:=this, format:="@07DA (MDD2000VVSafetyBase::InitSDOMechanism) Request to bus manager for task delay during post init failed. Error %d", #LogVal1);
        end_if;
      end_if;
    end_if;               
  else
    // we have to use the HWK for SDO handling on non-Salamander systems
    InitSDOsViaHWK();
  
    bdCommonFlags.bSDOsViaOS := FALSE;
  end_if;
  
END_FUNCTION


FUNCTION VIRTUAL MDD2000VVSafetyBase::ReadDriveFWVersion
	VAR_OUTPUT
		dRetCode 	: iprStates;
	END_VAR  
  VAR  	
    MyPara   : CmdStruct;
    MyResult : results;
    ParameterState : DINT;
  END_VAR
  
  MyPara.uiCmd  := BUSIF_DRIVEMNGBASE_GET_PARA_STATE;
  MyPara.aPara[0] := 0; // to_dint(AxBuffer[usAxis].AxisIndex);
  dRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
  ParameterState := MyResult.aData[0]$DINT;
  
  if dRetCode = ERROR then    
    return;
  end_if;
  
  if (ParameterState = 0) | 
     ( (bDS402Send = FALSE) & (ParameterState <> -1) & (ParameterState <> -3) ) then
    
    if bDS402Send = FALSE then

      MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_SET_PARAMETER;
      MyPara.aPara[0] := 0;                   // to_dint(usAxis);
      MyPara.aPara[1] := MDD2000_PID_SIGMATEKPARA_INDEX_INFO;
      MyPara.aPara[2] := MDD2000_SPID_I_FW;
      MyPara.aPara[3] := 0; // Read
      MyPara.aPara[4] := NIL$DINT;
      MyPara.aPara[5] := 0;
      dRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      
      if (dRetCode = READY) & (MyResult.aData[0]$DINT = 0) then
        dRetCode    := BUSY;  // Cahnge Retcode to busy as we wait for answer
        bDS402Send  := TRUE;
      end_if;  
    
    else
    
      MyPara.uiCmd    := BUSIF_DRIVEMNGBASE_GET_PARAMETER;
      MyPara.aPara[0] := 0; // to_dint(usAxis);
      dRetCode := ToBusInterface.NewInst(#MyPara, #MyResult);
      _memcpy(ptr1:=#DS402DataBuffer[0], ptr2:=(MyResult.aData[0])$pVoid, cntr:=sizeof(DS402DataBuffer));
      
      if dRetCode = READY then
        FwFktVersion := DS402DataBuffer[0];  
        bDS402Send := FALSE;
        
      elsif dRetCode <> BUSY then // = ERROR
        bDS402Send := FALSE;
      end_if;
    end_if;
  elsif (ParameterState = -1) | (ParameterState = -3) then
    dRetCode := BUSY;
  else
    bDS402Send := FALSE;
    dRetCode := ERROR;
  end_if;    

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::FSoEData2FrameLen
	VAR_INPUT
		FSoEDataLen 	: DINT;
	END_VAR
	VAR_OUTPUT
		FSoEFrameLen 	: UINT;
	END_VAR
  
  FSoEFrameLen := 0;
  
  if FSoEDataLen > 1 then     
    FSoEFrameLen := to_uint(3 + (FSoEDataLen * 2) );   // 1 Cmd + 2 connid + (2 x FSoEDataLen) 
  elsif FSoEDataLen = 1 then
    FSoEFrameLen := 6;  // 1 Cmd + 2 connid + 1 Data + 2 crc
  end_if;  

END_FUNCTION


FUNCTION MDD2000VVSafetyBase::ChangeConfigState
	VAR_INPUT
		CallRt 	: BOOL;
	END_VAR
  VAR
    MyPara 	  : CmdStruct;
    MyResult  : results;
  END_VAR
  
  case SSWConfigChanged of
    //********************************************************
  	eSSWConfigChanged::_IdleConfigChanged:
      // Do Nothing
    
    //********************************************************
    eSSWConfigChanged::_StartConfigChanged:
      
      if (SafetyState = _SafetyClassOK) | (SafetyState = _ReinitConfig) then
        SafetyState := _ReinitConfig; // Change SafetyState to block Rt
        SSWConfigChanged := eSSWConfigChanged::_Wait4Rt;  // Wait for 1 Rt Call to ensure PDO Data is no longer copied.
      else
        SSWConfigChanged := eSSWConfigChanged::_ErrorConfigChanged;
      end_if;      
    
    //********************************************************
  	eSSWConfigChanged::_Wait4Rt:
    
      if CallRt = TRUE then
        StopDO(); // Stop DOs, no of DOs may change due to changed size
        SSWConfigChanged := eSSWConfigChanged::_UpdatePDOs;
      end_if;
    
    //********************************************************
  	eSSWConfigChanged::_UpdatePDOs:
    
      if CallRt = TRUE then
        
        //add mailbox footer size to fsoe size
        sAccesses.tPdoRd.uiMyPDOSize := sFwInfos.sPdoInfo.PDORdLen + sizeof(tsMailboxFooter);
        sAccesses.tPdoWr.uiMyPDOSize := sFwInfos.sPdoInfo.PDOWrLen + sizeof(tsMailboxFooter);
        
        if SetDOAddresses() = READY then
          StartDO();  // Start DOs again
          SSWConfigChanged := eSSWConfigChanged::_FinishedConfigChanged;
        else
          SSWConfigChanged := eSSWConfigChanged::_ErrorConfigChanged;
        end_if;
        
      end_if;
    
    //********************************************************
  	eSSWConfigChanged::_FinishedConfigChanged:
      
      if CallRt = FALSE then
        
        SafetyState := _SafetyClassOK;
        SSWConfigChanged := eSSWConfigChanged::_IdleConfigChanged;
        
        if bdSafetyConfigChangedResult.ConfigChanged then
        
          MyPara.uiCmd          := MDD2000MNG_IF_SAFETYCONFIG_CHANGEDRESULT;
          MyPara.aPara[0]       := MDD2000MNG_IF_SAFETYCONFIG_CHANGEDRESULT_VER_1;          
          MyPara.aPara[1]$BDINT := 0;  // 0 .. Ok; Bit1 .. PDO Size Increased, change not possible
          ToBusInterface.NewInst(#MyPara, #MyResult);
          
          bdSafetyConfigChangedResult.ConfigChanged := 0;
        end_if;
        
      end_if;
    
    //********************************************************
  	eSSWConfigChanged::_ErrorConfigChanged:
    
      if CallRt = FALSE then
    
        SSWConfigChanged := eSSWConfigChanged::_IdleConfigChanged;
        
        if bdSafetyConfigChangedResult.ConfigChanged then
        
          MyPara.uiCmd          := MDD2000MNG_IF_SAFETYCONFIG_CHANGEDRESULT;
          MyPara.aPara[0]       := MDD2000MNG_IF_SAFETYCONFIG_CHANGEDRESULT_VER_1;
          MyPara.aPara[1]$BDINT := 1;  // 0 .. Ok; Bit1 .. PDO Size Increased, change not possible
          
          ToBusInterface.NewInst(#MyPara, #MyResult);
          
          bdSafetyConfigChangedResult.ConfigChanged := 0;
          
        end_if;
      end_if;
    
    //********************************************************
  end_case;

END_FUNCTION
