-- Remote I/O Server Dispatcher for Raspberry Pi LPC1114 I/O Processor
-- Expansion Board GPIO pin commands

-- Copyright (C)2019-2021, Philip Munts, President, Munts AM Corp.
--
-- Redistribution and use in source and binary forms, with or without
-- modification, are permitted provided that the following conditions are met:
--
-- * Redistributions of source code must retain the above copyright notice,
--   this list of conditions and the following disclaimer.
--
-- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-- AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-- IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-- ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
-- LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-- CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-- SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-- INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
-- CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-- ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-- POSSIBILITY OF SUCH DAMAGE.

WITH Interfaces;
WITH LPC11xx;
WITH Logging.libsimpleio;
WITH Message64;
WITH Messaging;
WITH RemoteIO.Dispatch;
WITH RemoteIO.Executive;
WITH SPI_Agent.Commands;
WITH SPI_Agent.Messages;
WITH SPI_Agent.Pins;
WITH errno;

USE TYPE Interfaces.Integer_32;
USE TYPE Interfaces.Unsigned_32;
USE TYPE LPC11xx.pin_id_t;
USE TYPE Messaging.Byte;

PACKAGE BODY RemoteIO.GPIO_SPIAgent IS

  -- Minimal binding to libspiagent.so

  PRAGMA Link_With("-lspiagent");

  PROCEDURE open
   (server : String;
    error  : OUT Interfaces.Integer_32);

  PRAGMA Import(C, open, "spiagent_open");

  PROCEDURE command
   (command  : IN  SPI_Agent.Messages.SPIAGENT_COMMAND_MSG_t;
    response : OUT SPI_Agent.Messages.SPIAGENT_RESPONSE_MSG_t;
    error    : OUT Interfaces.Integer_32);

  PRAGMA Import(C, command, "spiagent_command");

  -- Query available LPC1114 GPIO pins

  PROCEDURE Present
   (Self : IN OUT DispatcherSubclass;
    cmd  : Message64.Message;
    resp : OUT Message64.message) IS

    byteindex : Natural;
    bitmask   : Messaging.Byte;

  BEGIN
    resp(0) := MessageTypes'Pos(GPIO_PRESENT_RESPONSE);
    resp(1) := cmd(1);
    resp(2 .. 63) := (OTHERS => 0);

    FOR c IN ChannelNumber LOOP
      byteindex  := c/8;
      bitmask    := 2**(7 - (c MOD 8));

      IF Self.pins(c).registered THEN
        resp(3 + byteindex) := resp(3 + byteindex) OR bitmask;
      END IF;
    END LOOP;
  END Present;

  -- Configure an LPC1114 GPIO pin

  PROCEDURE Configure
   (Self : IN OUT DispatcherSubclass;
    cmd  : Message64.Message;
    resp : OUT Message64.message) IS

    byteindex  : Natural;
    bitmask    : Messaging.Byte;
    selected   : Boolean;
    output     : Boolean;
    pin        : LPC11xx.pin_id_t;
    registered : Boolean;
    scmd       : SPI_Agent.Messages.SPIAGENT_COMMAND_MSG_t;
    sresp      : SPI_Agent.Messages.SPIAGENT_RESPONSE_MSG_t;
    error      : Interfaces.Integer_32;

  BEGIN
    resp(0) := MessageTypes'Pos(GPIO_CONFIGURE_RESPONSE);
    resp(1) := cmd(1);
    resp(2) := 0;
    resp(3 .. 63) := (OTHERS => 0);

    FOR c IN ChannelNumber LOOP
      byteindex  := c/8;
      bitmask    := 2**(7 - (c MOD 8));
      selected   := ((cmd(2 + byteindex) AND bitmask) /= 0);
      output     := ((cmd(18 + byteindex) AND bitmask) /= 0);
      pin        := Self.pins(c).pin;
      registered := Self.pins(c).registered;

      IF selected AND registered THEN
        BEGIN

          -- Force on-board LED to output

          IF Self.pins(c).pin = SPI_Agent.Pins.LPC1114_LED THEN
            output := True;
          END IF;

          -- Build command message

          IF output THEN
            scmd.Command :=
              Interfaces.Unsigned_32(SPI_Agent.Commands.SPIAGENT_COMMAND_t'Pos(
              SPI_Agent.Commands.SPIAGENT_CMD_CONFIGURE_GPIO_OUTPUT));
          ELSE
            scmd.Command :=
              Interfaces.Unsigned_32(SPI_Agent.Commands.SPIAGENT_COMMAND_t'Pos(
              SPI_Agent.Commands.SPIAGENT_CMD_CONFIGURE_GPIO_INPUT));
          END IF;

          scmd.Pin  := Interfaces.Unsigned_32(LPC11xx.pin_id_t'Pos(pin));

          scmd.Data := 0;

          -- Dispatch command message

          command(scmd, sresp, error);

          IF error /= 0 THEN
            resp(2) := Messaging.Byte(error);
            RETURN;
          END IF;

          IF sresp.Error /= 0 THEN
            resp(2) := Messaging.Byte(sresp.Error);
            RETURN;
          END IF;

          Self.pins(c).configured := True;
          Self.pins(c).writable   := output;
        EXCEPTION
          WHEN OTHERS =>
            Logging.libsimpleio.Error("Caught exception");
        END;
      END IF;
    END LOOP;
  END Configure;

  -- Read from an LPC1114 GPIO pin

  PROCEDURE Read
   (Self : IN OUT DispatcherSubclass;
    cmd  : Message64.Message;
    resp : OUT Message64.message) IS

    byteindex  : Natural;
    bitmask    : Messaging.Byte;
    selected   : Boolean;
    pin        : LPC11xx.pin_id_t;
    configured : Boolean;
    scmd       : SPI_Agent.Messages.SPIAGENT_COMMAND_MSG_t;
    sresp      : SPI_Agent.Messages.SPIAGENT_RESPONSE_MSG_t;
    error      : Interfaces.Integer_32;

  BEGIN
    resp(0) := MessageTypes'Pos(GPIO_READ_RESPONSE);
    resp(1) := cmd(1);
    resp(2) := 0;
    resp(3 .. 63) := (OTHERS => 0);

    FOR c IN ChannelNumber LOOP
      byteindex  := c/8;
      bitmask    := 2**(7 - (c MOD 8));
      selected   := ((cmd(2 + byteindex) AND bitmask) /= 0);
      pin        := Self.pins(c).pin;
      configured := Self.pins(c).registered AND Self.pins(c).configured;

      IF selected AND configured THEN
        BEGIN

          -- Build command message

          scmd.Command :=
              Interfaces.Unsigned_32(SPI_Agent.Commands.SPIAGENT_COMMAND_t'Pos(
              SPI_Agent.Commands.SPIAGENT_CMD_GET_GPIO));

          scmd.Pin := Interfaces.Unsigned_32(LPC11xx.pin_id_t'Pos(pin));

          scmd.Data    := 0;

          -- Dispatch command message

          command(scmd, sresp, error);

          IF error /= 0 THEN
            resp(2) := Messaging.Byte(error);
            RETURN;
          END IF;

          IF sresp.Error /= 0 THEN
            resp(2) := Messaging.Byte(sresp.Error);
            RETURN;
          END IF;

          -- Process result

          IF sresp.Data /= 0 THEN
            resp(3 + byteindex) := resp(3 + byteindex) OR bitmask;
          END IF;
        EXCEPTION
          WHEN OTHERS =>
            Logging.libsimpleio.Error("Caught exception");
        END;
      END IF;
    END LOOP;
  END Read;

  -- Write to an LPC1114 GPIO pin

  PROCEDURE Write
   (Self : IN OUT DispatcherSubclass;
    cmd  : Message64.Message;
    resp : OUT Message64.message) IS

    byteindex  : Natural;
    bitmask    : Messaging.Byte;
    selected   : Boolean;
    state      : Boolean;
    pin        : LPC11xx.pin_id_t;
    configured : Boolean;
    scmd       : SPI_Agent.Messages.SPIAGENT_COMMAND_MSG_t;
    sresp      : SPI_Agent.Messages.SPIAGENT_RESPONSE_MSG_t;
    error      : Interfaces.Integer_32;

  BEGIN
    resp(0) := MessageTypes'Pos(GPIO_WRITE_RESPONSE);
    resp(1) := cmd(1);
    resp(2) := 0;
    resp(3 .. 63) := (OTHERS => 0);

    FOR c IN ChannelNumber LOOP
      byteindex  := c/8;
      bitmask    := 2**(7 - (c MOD 8));
      selected   := ((cmd(2 + byteindex) AND bitmask) /= 0);
      state      := ((cmd(18 + byteindex) AND bitmask) /= 0);
      pin        := Self.pins(c).pin;
      configured := Self.pins(c).registered AND Self.pins(c).configured AND
        Self.pins(c).writable;

      IF selected AND configured THEN
        BEGIN

          -- Build command message

          scmd.Command :=
              Interfaces.Unsigned_32(SPI_Agent.Commands.SPIAGENT_COMMAND_t'Pos(
              SPI_Agent.Commands.SPIAGENT_CMD_PUT_GPIO));

          scmd.Pin  := Interfaces.Unsigned_32(LPC11xx.pin_id_t'Pos(pin));

          scmd.Data := Boolean'Pos(state);

          -- Dispatch command message

          command(scmd, sresp, error);

          IF error /= 0 THEN
            resp(2) := Messaging.Byte(error);
            RETURN;
          END IF;

          IF sresp.Error /= 0 THEN
            resp(2) := Messaging.Byte(sresp.Error);
            RETURN;
          END IF;
        EXCEPTION
          WHEN OTHERS =>
            Logging.libsimpleio.Error("Caught exception");
        END;
      END IF;
    END LOOP;
  END Write;

  -- Create Remote I/O Protocol command dispatcher object

  FUNCTION Create
   (executor : IN OUT RemoteIO.Executive.Executor) RETURN Dispatcher IS

    Self : Dispatcher;

  BEGIN
    Self := NEW DispatcherSubclass'(pins => (OTHERS => Unused));

    executor.Register(GPIO_PRESENT_REQUEST, RemoteIO.Dispatch.Dispatcher(Self));
    executor.Register(GPIO_CONFIGURE_REQUEST, RemoteIO.Dispatch.Dispatcher(Self));
    executor.Register(GPIO_READ_REQUEST, RemoteIO.Dispatch.Dispatcher(Self));
    executor.Register(GPIO_WRITE_REQUEST, RemoteIO.Dispatch.Dispatcher(Self));

    RETURN Self;
  END Create;

  -- Dispatch Remote I/O Protocol commands

  PROCEDURE Dispatch
   (Self : IN OUT DispatcherSubclass;
    cmd  : Message64.Message;
    resp : OUT Message64.Message) IS

    msgtype : MessageTypes;

  BEGIN
    msgtype := MessageTypes'Val(cmd(0));

    CASE msgtype IS
      WHEN GPIO_PRESENT_REQUEST =>
        Present(Self, cmd, resp);

      WHEN GPIO_CONFIGURE_REQUEST =>
        Configure(Self, cmd, resp);

      WHEN GPIO_READ_REQUEST =>
        Read(Self, cmd, resp);

      WHEN GPIO_WRITE_REQUEST =>
        Write(Self, cmd, resp);

      WHEN OTHERS =>
        Logging.libsimpleio.Error("Unexected message type: " &
          MessageTypes'Image(msgtype));
    END CASE;
  END Dispatch;

  -- Register GPIO pin

  PROCEDURE Register
   (Self : IN OUT DispatcherSubclass;
    num  : ChannelNumber;
    pin  : LPC11xx.pin_id_t) IS

  BEGIN
    IF Self.pins(num).registered THEN
      RETURN;
    END IF;

    Self.pins(num) := PinRec'(pin, True, False, False);
  END Register;

  error : Interfaces.Integer_32;

BEGIN
  open("ioctl://localhost", error);

  IF (error /= 0) AND (error /= errno.EBUSY) THEN
    RAISE Program_Error WITH "Cannot initialize libsimpleio";
  END IF;
END RemoteIO.GPIO_SPIAgent;