#include "tz_hdm_interface.h"
#include "hdm_defs.h"
#include "buffer_utils.h"
#include "icccOperations.h"
#include "process_cmd.h"

char TZ_APP_NAME[] = { "tz_hdm" };

static tci_message_t sendMsgCopy;
static tci_message_t respMsgCopy;

/*
 * Reserve 24576 byte for stack.
 * Usually TAs have smaller stacks than this, but for HDM we need a bigger stack because
 * we have many big buffers to store certificates, HDM response message, etc.
 */
DECLARE_TRUSTLET_MAIN_STACK(56320)
//DECLARE_TRUSTLET_MAIN_STACK(0x80000)

/* Trustlet entry. */
_TLAPI_ENTRY void tlMain(const addr_t tciBuffer, const uint32_t tciBufferLen)
{
  /* Initialization sequence */
  uint32_t ret;
  uint32_t commandId;
  tci_message_t *sendmsg = NULL;
  tci_message_t *respmsg = NULL;

  /* Check if the size of the given TCI is sufficient */
  if ((NULL == tciBuffer) || (sizeof(tci_message_t) > tciBufferLen)) {
    /* TCI too small -> end Trustlet */
    HDM_LOG("Trustlet HDM::EXITING");
    tlApiExit(EXIT_ERROR);
  }

  HDM_LOG("Trustlet HDM::Starting");

#if defined (TZ_STACK_PROTECTION)
  HDM_LOG("init_stack_canary");
  init_stack_canary();
#endif

  /* The Trustlet main loop running infinitely */
  for (;;) {
    /* Wait for a notification to arrive (INFINITE timeout is recommended -> not polling!) */
    tlApiWaitNotification(TLAPI_INFINITE_TIMEOUT);

    if(!isBufferInRange(tciBuffer, sizeof(tci_message_t), tciBuffer, (uint8_t *) tciBuffer+tciBufferLen)) {
      HDM_LOG_DEBUG("Invalid tciBufferLen!");
      tlApiNotify();
      continue;
    }
    TEE_MemMove(&sendMsgCopy, tciBuffer, sizeof(tci_message_t));
    if(sendMsgCopy.header.len > tciBufferLen - sizeof(tci_message_t)) {
      HDM_LOG_DEBUG("invalid header len!");
      tlApiNotify();
      continue;
    }

    if(!isBufferInRange((uint8_t *) tciBuffer + sendMsgCopy.header.len, 
        sizeof(tci_message_t), 
        (uint8_t *) tciBuffer + sendMsgCopy.header.len, 
        (uint8_t *) tciBuffer+tciBufferLen)) {
      HDM_LOG_DEBUG("invalid tciBuffer or tciBufferLen!");
      tlApiNotify();
      continue;
    }
    TEE_MemMove(&respMsgCopy, (uint8_t *) tciBuffer + sendMsgCopy.header.len, sizeof(tci_message_t));

    sendmsg = &sendMsgCopy;
    respmsg = &respMsgCopy;

    /* Derefernece commandId once for further usage */
    commandId = sendmsg->header.id;

    /* Check if the message received is (still) a response */
    if (!IS_CMD(commandId)) {
      /* Tell the NWd a response is still pending (optional) */
      tlApiNotify();
      continue;
    } else {
      HDM_LOG("we got a command: %x", commandId);
    }

    /* Process Command. */
    ret = 0;
    ret = process_cmd(commandId, sendmsg, respmsg);
    HDM_LOG_DEBUG("cmd processing completed!");
    
    /* Set up response header -> mask response ID and set return code */
    respmsg->header.id = RSP_ID(commandId);
    respmsg->header.status = ret;
    TEE_MemMove((uint8_t *) tciBuffer + sendMsgCopy.header.len, &respMsgCopy, sizeof(tci_message_t));
    zeroOutBuffer(&sendMsgCopy,sizeof(sendMsgCopy));
    zeroOutBuffer(&respMsgCopy,sizeof(respMsgCopy));
    HDM_LOG_DEBUG("cmd processing completed notifying tl!");
    /* Notify back the TLC */
    tlApiNotify();
    
  }
}

/** @} */