-- Message reader & writers for SMG Communication Protocol
 -- S.MG, 2018

with Interfaces; use Interfaces;
with Serpent;
with System; use System;

package body Messages is

  procedure Write_SKeys_SMsg( Keyset  : in Serpent_Keyset;
                              Counter : in Interfaces.Unsigned_16;
                              Msg     : out Raw_Types.Serpent_Msg) is
    Pos   : Integer := Msg'First;
    Check : CRC32.CRC32;
    PadLen: Integer;
    K     : Serpent.Key;
  begin
    -- write Type ID
    Msg(Pos) := SKeys_S_Type;
    Pos := Pos + 1;

    -- write count of keys (NB: this IS 8 bits by definition)
    Msg(Pos) := Keyset.Keys'Length;
    Pos := Pos + 1;

    -- write keys
    for I in Keyset.Keys'Range loop
      -- retrieve Key to write
      K := Keyset.Keys( I );

      -- write key itself
      Msg(Pos..Pos+K'Length-1) := K;
      -- ensure little endian order in message
      Cast_LE(Msg(Pos..Pos+K'Length-1));
      Pos := Pos + K'Length;

      -- write CRC of key
      Check := CRC32.CRC( K );
      Msg(Pos..Pos+3) := Raw_Types.Cast(Check);
      Cast_LE(Msg(Pos..Pos+3));
      Pos := Pos + 4;
    end loop;

    -- write flag
    Msg(Pos) := Keyset.Flag;
    Pos := Pos + 1;

    -- write message counter
    Msg(Pos..Pos+1) := Raw_Types.Cast(Counter);
    Cast_LE(Msg(Pos..Pos+1));
    Pos := Pos + 2;

    -- write padding as needed; endianness is irrelevant here
    PadLen := Msg'Last - Pos + 1;
    if PadLen > 0 then
      declare
        Pad : Raw_Types.Octets(1..PadLen);
      begin
        RNG.Get_Octets( Pad );
        Msg(Pos..Pos+PadLen-1) := Pad;
      end;
    end if;
  end Write_SKeys_SMsg;


  -- Reads a Serpent keyset from given Serpent Message
  procedure Read_SKeys_SMsg( Msg     : in Raw_Types.Serpent_Msg;
                             Counter : out Interfaces.Unsigned_16;
                             Keyset  : out Serpent_Keyset) is
    Pos: Integer := Msg'First;
  begin
    -- read type and check
    if Msg(Pos) = SKeys_S_Type then
      Pos := Pos + 1;
    else
      raise Invalid_Msg;
    end if;

    -- read count of keys and check
    if Msg(Pos) in Keys_Count'Range then
      declare
        N     : Keys_Count := Keys_Count(Msg(Pos));
        KS    : Serpent_Keyset(N);
        K     : Serpent.Key;
        Check : CRC32.CRC32;
        O4    : Raw_Types.Octets_4; 
        O2    : Raw_Types.Octets_2;
      begin
        Pos := Pos + 1;
        --read keys and check crc for each
        for I in 1 .. N loop
          -- read key and advance pos
          K := Msg(Pos..Pos+K'Length-1);
          Cast_LE(K);
          Pos := Pos + K'Length;
          -- read crc and compare to crc32(key)
          O4 := Msg(Pos..Pos+3);
          Cast_LE(O4);
          Check   := Raw_Types.Cast(O4);
          Pos := Pos + 4;
          if Check /= CRC32.CRC(K) then
            raise Invalid_Msg;
          end if;
          -- if it got here, key is fine so add to set
          KS.Keys(KS.Keys'First + I -1) := K;
        end loop;
        -- read and set flag
        KS.Flag := Msg(Pos);
        Pos := Pos + 1;
        -- read and set message counter
        O2 := Msg(Pos..Pos+1);
        Cast_LE(O2);
        Counter := Raw_Types.Cast(O2);
        -- rest of message is padding so it's ignored
        -- copy keyset to output variable
        Keyset := KS;
      end;
    else
      raise Invalid_Msg;
    end if;
  end Read_SKeys_SMsg;

  -- private part
  procedure Cast_LE( LE: in out Raw_Types.Octets ) is
  begin
    -- flip octets ONLY if native is big endian.
    if System.Default_Bit_Order = System.High_Order_First then
      declare
        BE: constant Raw_Types.Octets := LE;
      begin
        for I in 1..LE'Length loop
          LE(LE'First+I-1) := BE(BE'Last-I+1);
        end loop;
      end;
    end if;
    -- NOTHING to do for native little endian
  end Cast_LE;

end Messages;