--
--  Copyright (C) 2022 Fabien Chouteau <fabien.chouteau@gmail.com>
--
--  SPDX-License-Identifier: BSD-3-Clause
--
with System.Storage_Elements;
with RP2040_SVD.Interrupts;
with RP2040_SVD.SIO;
with Cortex_M.Hints;
with Cortex_M.NVIC;

with RP.Multicore.FIFO;

package body RP.Multicore is

   ------------------
   -- Launch_Core1 --
   ------------------

   procedure Launch_Core1 (Trap_Vector   : HAL.UInt32;
                           Stack_Pointer : HAL.UInt32;
                           Entry_Point   : HAL.UInt32)
   is
      Int_Id : constant := RP2040_SVD.Interrupts.SIO_IRQ_PROC0_Interrupt;
      Enabled : constant Boolean := Cortex_M.NVIC.Enabled (Int_Id);
   begin

      --  Disable FIFO interrupt during launch procedure with core1 to avoid
      --  running a handler in case user enabled it already.
      Cortex_M.NVIC.Disable_Interrupt (Int_Id);

      declare
         Commands : constant UInt32_Array :=
           (0, 0, 1,
            Trap_Vector,
            Stack_Pointer,
            Entry_Point or 1); -- Thumb mode

         Response : UInt32;

         Index : Natural := Commands'First;
         Cmd : UInt32;
      begin
         loop
            Cmd := Commands (Index);

            if Cmd = 0 then
               FIFO.Drain;

               Cortex_M.Hints.Send_Event;
            end if;

            --  Send a command an wait for echo
            FIFO.Push_Blocking (Cmd);
            Response := FIFO.Pop_Blocking;

            if Response = Cmd then
               --  Success:

               if Index = Commands'Last then
                  --  Launch done
                  exit;
               else
                  --  Next command
                  Index := Index + 1;
               end if;

            else
               --  Failure: start from the beginning
               Index := Commands'First;
            end if;
         end loop;
      end;

      if Enabled then
         Cortex_M.NVIC.Enable_Interrupt (Int_Id);
      end if;
   end Launch_Core1;

   procedure Launch_Core1
      (Trap_Vector, Stack_Pointer, Entry_Point : System.Address)
   is
      use System.Storage_Elements;
   begin
      Launch_Core1
         (Trap_Vector   => UInt32 (To_Integer (Trap_Vector)),
          Stack_Pointer => UInt32 (To_Integer (Stack_Pointer)),
          Entry_Point   => UInt32 (To_Integer (Entry_Point)));
   end Launch_Core1;

   function CPU_Id
      return Natural
   is (Natural (RP2040_SVD.SIO.SIO_Periph.CPUID));

end RP.Multicore;