Raspberry Pi Legacy

FUNCTION_BLOCK MCP23S17_FB EXTENDS spi
VAR_INPUT
    byOutputsA: WORD;
    byOutputsB: WORD;
END_VAR
VAR_OUTPUT
    auiValue: ARRAY [..] OF ;
    byInputsA: BYTE;
    byInputsB: BYTE;
    _byGPIOSelectInputA: BYTE;
    _byGPIOSelectInputB: BYTE;
    _byGPIOPullupA: BYTE;
    _byGPIOPullupB: BYTE;
END_VAR
VAR
    _byHardwareAddress: BYTE;
END_VAR

SUPER^();

CASE _iState OF
0:
    IF SUPER^.init() THEN
        _iState := 1;
    END_IF  
1:
    write8(_byHardwareAddress, IOCON, 16#28); // set HAEN and SEQOP
    write8(_byHardwareAddress, IODIRA, _byGPIOSelectInputA);    
    write8(_byHardwareAddress, IODIRB, _byGPIOSelectInputB);    
    write8(_byHardwareAddress, GPIOA, 0);   //reset outputs
    write8(_byHardwareAddress, GPIOB, 0);   
    write8(_byHardwareAddress, GPPUA, _byGPIOPullupA);  
    write8(_byHardwareAddress, GPPUB, _byGPIOPullupB);  

    _iState := 10;
END_CASE

METHOD read8: BYTE
VAR_INPUT
    byHardwareAddress: subrangeSigned;
    byPort: BYTE;
END_VAR
VAR
    abyTxBuffer: ARRAY [..] OF ;
    abyRxBuffer: ARRAY [..] OF ;
END_VAR

abyTxBuffer[0] := 16#40 + SHL(byHardwareAddress, 1) + 1; //SPI_READ_CMD;
abyTxBuffer[1] := byPort;
abyTxBuffer[2] := 16#FF;

IF transfer(pabyTxBuffer:=ADR(abyTxBuffer) , pabyRxBuffer:=ADR(abyRxBuffer) , udiLen:=3 , uiDelayus:=5 ) THEN
    read8 := abyRxBuffer[2];
ELSE
    _iState := 1000;
END_IF

METHOD write8: BOOL
VAR_INPUT
    byHardwareAddress: subrangeSigned;
    byPort: BYTE;
    byValue: BYTE;
END_VAR
VAR
    abyTxBuffer: ARRAY [..] OF ;
    abyRxBuffer: ARRAY [..] OF ;
END_VAR

abyTxBuffer[0] := 16#40 + SHL(byHardwareAddress, 1); //SPI_WRITE_CMD;
abyTxBuffer[1] := byPort;
abyTxBuffer[2] := byValue;

write8 := transfer(pabyTxBuffer:=ADR(abyTxBuffer) , pabyRxBuffer:=ADR(abyRxBuffer) , udiLen:=3 , uiDelayus:=5 );

IF NOT write8 THEN
    _iState := 1000;
END_IF

METHOD writeBit: BOOL
VAR_INPUT
    byHardwareAddress: subrangeSigned;
    byPort: BYTE;
    byBit: subrangeSigned;
    xValue: BOOL;
END_VAR
VAR
    byValue: BYTE;
END_VAR

byValue := read8(byHardwareAddress, byPort);
IF xValue THEN
    byValue := byValue OR SHL(USINT#1, byBit);
ELSE    
    byValue := byValue AND NOT SHL(USINT#1, byBit);
END_IF
writeBit := write8(byHardwareAddress, byPort, byValue);

METHOD AfterReadInputs: INT
VAR
    aby: ARRAY [..] OF ;
    usiChannel: USINT;
    ui: UINT;
END_VAR

SUPER^.AfterReadInputs();

byInputsA := read8(_byHardwareAddress, GPIOA);
byInputsB := read8(_byHardwareAddress, GPIOB);

METHOD BeforeWriteOutputs: INT
VAR
    i: USINT;
END_VAR

SUPER^.BeforeWriteOutputs();

IF _byGPIOSelectInputA < 255 THEN
    write8(_byHardwareAddress, GPIOA, WORD_TO_BYTE(byOutputsA));
END_IF
IF _byGPIOSelectInputB < 255 THEN
    write8(_byHardwareAddress, GPIOB, WORD_TO_BYTE(byOutputsB));
END_IF

METHOD Initialize: UDINT
VAR_INPUT
    wModuleType: UINT;
    dwInstance: UDINT;
    pConnector: pointer;
END_VAR
VAR
    pParam: pointer;
    udiResult: UDINT;
END_VAR

SUPER^.Initialize(wModuleType, dwInstance, pConnector);


pParam := ConfigGetParameter(_pConnector, 1);
IF pParam <> 0 THEN
    _byHardwareAddress := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF

pParam := ConfigGetParameter(_pConnector, 2);
IF pParam <> 0 THEN
    _byGPIOSelectInputA := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF

pParam := ConfigGetParameter(_pConnector, 3);
IF pParam <> 0 THEN
    _byGPIOSelectInputB := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF

pParam := ConfigGetParameter(_pConnector, 4);
IF pParam <> 0 THEN
    _byGPIOPullupA := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF

pParam := ConfigGetParameter(_pConnector, 5);
IF pParam <> 0 THEN
    _byGPIOPullupB := IoStandard.ConfigGetParameterValueByte(pParam, ADR(udiResult));
END_IF