/*
<:copyright-BRCM:2019:DUAL/GPL:standard 

   Copyright (c) 2019 Broadcom 
   All Rights Reserved

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License, version 2, as published by
the Free Software Foundation (the "GPL").

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.


A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
Boston, MA 02111-1307, USA.

:>
*/
#if defined(SPD_opteed)
#include <arch_helpers.h>
#include <assert.h>
#include <bl_common.h>
#include <debug.h>
#include <errno.h>
#include <platform.h>
#include <runtime_svc.h>
#include <stddef.h>
#include <arch.h>

static volatile uint32_t optee_smc_entry = 0;

static uint32_t monitor_stack_pointer [PLATFORM_MAX_CPUS_PER_CLUSTER];
static uint32_t optee_params_register [PLATFORM_MAX_CPUS_PER_CLUSTER][8];

extern void save_atf_sysreg(void);
extern void restore_atf_sysreg(void);

__attribute__((naked)) void opteed_jump(uint32_t jump, uint32_t *monitor_stack, uint32_t *params)
{
   /* Save all GP registers */
   asm("push {r0-r12, r14}");

   /* Find OPTEE's entry address and place to save ATF's monitor stack*/
   asm volatile ("mov r10, %0" "\n\t"
                 "mov r11, %1" "\n\t"
                 "mov r12, %2" "\n\t"
                 : : "r" (params), "r" (monitor_stack), "r" (jump));

   /* Save ATF's monitor's stack in cache memory */
   asm("str sp, [r11]");
   save_atf_sysreg();

   /* Load OPTEE's parameters */
   asm("ldm r10, {r0-r7}");

   /* Turn off MMU and Cache needed to initialize OPTEE */
   asm("mrc p15, 0, r8, c1, c0, 0");
   asm("bic r8, r8, #0x5"); // (SCTLR_M_BIT | SCTLR_C_BIT) = 0x5
   asm("mcr p15, 0, r8, c1, c0, 0");

   /* Save ATF's monitor's stack in non-cache memory */
   asm("str sp, [r11]");
   save_atf_sysreg();

   /* Jump to OPTEE */
   asm("cps #0x13"); // switch to secure MODE_SVC mode (0x13)
   asm("blx r12");
   asm("nop");
   /* Returned from OPTEE with OPTEE's smc entry point in r12. ** DO NOT corrupt r12 ** */

   /* Load ATF's monitor stack from non-cached memory */
   asm("ldr r9,=monitor_stack_pointer");
   /* Point to the monitor stack pointer for this CPU */
   asm("mrc p15, 0, r10, c0, c0, 5");
   asm("and r10, r10, #0xff");
   asm("lsl r10, r10, #2");
   asm("add r9, r10, r9");
   asm("ldr sp, [r9]");

   /* Restore ATF's system registers */
   restore_atf_sysreg();

   /* Turn ON MMU */
   asm("mrc p15, 0, r8, c1, c0, 0");
   asm("ldr r9,=0x5"); // SCTLR_M_BIT | SCTLR_C_BIT
   asm("orr r8, r8, r9");
   asm("mcr p15, 0, r8, c1, c0, 0");
   asm("nop");
   asm("nop");
   /* Save optee's SMC entry in cached memory */
   asm("ldr r8,=optee_smc_entry");
   asm("str r12,[r8]");
   /* Save the OPTEE's return registers r0-r7  in cached memory  */
   asm("ldr r8,=optee_params_register");
   asm("lsl r10, r10, #3");
   asm("add r8, r10, r8");
   asm("stm r8, {r0-r7}");
   /* Restore all GP registers */
   asm("pop {r0-r12, r14}");
   asm("bx lr");
}

/*******************************************************************************
 * OPTEE Dispatcher setup. The OPTEED finds out the OPTEE entrypoint and type
 * (aarch32/aarch64) if not already known and initialises the context for entry
 * into OPTEE for its initialization.
 ******************************************************************************/
int32_t opteed_setup(void)
{
   entry_point_info_t *optee_ep_info;
   uint32_t cpu_id = plat_my_core_pos();
   uint32_t *monitor_stack = &monitor_stack_pointer[cpu_id];
   uint32_t *optee_params = &optee_params_register[cpu_id][0];
   /*
    * Get information about the Secure Payload (BL32) image. Its
    * absence is a critical failure.  TODO: Add support to
    * conditionally include the SPD service
    */

   optee_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
   if (!optee_ep_info) {
      WARN("No OPTEE provided by BL2 boot loader, Booting device"
           " without OPTEE initialization. SMC`s destined for OPTEE"
           " will return SMC_UNK\n");
      return 1;
   }

   /*
    * If there's no valid entry point for SP, we return a non-zero value
    * signalling failure initializing the service. We bail out without
    * registering any handlers
    */
   if (!optee_ep_info->pc)
     return 1;

   opteed_jump(optee_ep_info->pc, monitor_stack, optee_params);

   return 0;
}



/*******************************************************************************
 * This function is responsible for handling all SMCs in the Trusted OS/App
 * range from the non-secure state as defined in the SMC Calling Convention
 * Document. It is also responsible for communicating with the Secure
 * payload to delegate work and return results back to the non-secure
 * state. Lastly it will also return any information that OPTEE needs to do
 * the work assigned to it.
 ******************************************************************************/
 uintptr_t opteed_smc_handler(uint32_t smc_fid,
                              u_register_t x1,
                              u_register_t x2,
                              u_register_t x3,
                              u_register_t x4,
                              void *cookie,
                              void *handle,
                              u_register_t flags)
{
   uint32_t cpu_id = plat_my_core_pos();
   uint32_t *monitor_stack = &monitor_stack_pointer[cpu_id];
   uint32_t *optee_params = &optee_params_register[cpu_id][0];
   cookie = cookie;
   handle = handle;
   flags = flags;

   /* Extract r0-r7 from saved context and pass to OPTEE */
   optee_params[0] = ((smc_ctx_t *)handle)->r0; // smc_fid;
   optee_params[1] = ((smc_ctx_t *)handle)->r1; // x1;
   optee_params[2] = ((smc_ctx_t *)handle)->r2; // x2;
   optee_params[3] = ((smc_ctx_t *)handle)->r3; // x3;
   optee_params[4] = ((smc_ctx_t *)handle)->r4; // x4;
   optee_params[5] = ((smc_ctx_t *)handle)->r5;
   optee_params[6] = ((smc_ctx_t *)handle)->r6;
   optee_params[7] = ((smc_ctx_t *)handle)->r7;

   assert(optee_params[0] == smc_fid);

   opteed_jump(optee_smc_entry, monitor_stack, optee_params);

   /* Return the response from OPTEE */
   SMC_RET4(handle, optee_params[0], optee_params[1], optee_params[2], optee_params[3]);
}

/* Define an OPTEED runtime service descriptor for fast SMC calls */
DECLARE_RT_SVC(
               opteed_fast,

               OEN_TOS_START,
               OEN_TOS_END,
               SMC_TYPE_FAST,
               opteed_setup,
               opteed_smc_handler
               );

/* Define an OPTEED runtime service descriptor for yielding SMC calls */
DECLARE_RT_SVC(
               opteed_std,

               OEN_TOS_START,
               OEN_TOS_END,
               SMC_TYPE_YIELD,
               NULL,
               opteed_smc_handler
);
#endif
