#include "handlers.h"

#ifndef CONFIG_TEEGRIS30

#include "protocol.h"
#include "tee_param_utils.h"

#include <sys/mman.h>
#include <fcntl.h>
#include <iwnotify.h>
#include <stdio.h>
#include <unistd.h>

#include <tee_internal_api.h>
#include <tees_log.h>

#define IWSHMEM_DEV_NAME    "iwshmem://"
#define IWSHMEM_DEV_NAME_LEN 64

#define PAGE_SHIFT_FIVE (12ULL)
#define PAGE_SIZE_FIVE  (1ULL << (PAGE_SHIFT_FIVE))
#define PAGE_MASK_FIVE  (~(PAGE_SIZE_FIVE-1))

#define ALIGN_TO_PAGE_SIZE_FIVE(size) (((size) + PAGE_SIZE_FIVE - 1) & PAGE_MASK_FIVE)

static const char *get_iwshmem_name(char name[], size_t size, int id) {
    int ret = snprintf(name, size, "%s%d", IWSHMEM_DEV_NAME, id);

    if (ret < 0 || (size_t)ret >= size) {
        return NULL;
    }

    return name;
}

static int iwshmem_open(int id) {
    char name_buf[IWSHMEM_DEV_NAME_LEN];
    const char *name = get_iwshmem_name(name_buf, sizeof(name_buf), id);

    return open(name, O_RDWR);
}

static void *iwshmem_mmap(int id, size_t size) {
  int fd = -1;
  void *p = NULL;
  
  MB_LOGD("mmap(id=%d, size=%zu)\n", id, size);

  fd = iwshmem_open(id);
  if (fd == -1) {
    MB_LOGE("Failed to open %d\n", id);
    return MAP_FAILED;
  }

  p = mmap(NULL, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);

  close(fd);
  return p;
}

int FiveKernelCmd(unsigned int client_id, struct kernel_cmd *kernel_cmd) {
  TEE_Param params[4];
  ProtocolParam cmd = {0};
  ProtocolCmd *command = NULL;
  size_t size_command_aligned;

  MB_LOGD("Enter to kernel handler\n");

  if (!kernel_cmd || kernel_cmd->size_in > sizeof(cmd)) {
    MB_LOGE("Invalid arguments\n");
    goto exit;
  }

  TEE_MemMove(&cmd, kernel_cmd->msg_in, kernel_cmd->size_in);

  size_command_aligned = ALIGN_TO_PAGE_SIZE_FIVE(sizeof(ProtocolCmd));
  if (!cmd.memref.buffer || size_command_aligned != cmd.memref.size) {
    MB_LOGE("Invalid command buffer\n");
    goto error_cmd;
  }

  command = iwshmem_mmap(cmd.memref.buffer, cmd.memref.size);
  if (command == MAP_FAILED) {
    MB_LOGE("Failed to mmap %d iwshmem\n", cmd.memref.buffer);
    goto error_cmd;
  }

  // Return origin: "TEE" must be set by default.
  command->return_origin = TEE_ORIGIN_TEE;

  command->cmd_ret = FillParamValues(command->param_types, params, command);
  if (TEE_SUCCESS != command->cmd_ret) {
    MB_LOGE("Failed to read command params!\n");
    command->cmd_ret = TEE_ERROR_BAD_PARAMETERS;
    goto error_unmap;
  }

  switch (command->cmd_id) {
    case PROTOCOL_COMMAND_LOAD:
    case PROTOCOL_COMMAND_UNLOAD:
      command->cmd_ret = TEE_SUCCESS;
    break;
    default:
      command->cmd_ret = FiveCommandHandler(command->cmd_id,
                                            command->param_types, params);
  }

  if (TEE_SUCCESS != FillCommandArgs(command->param_types, command, params)) {
    MB_LOGE("Failed to write command params!\n");
    command->cmd_ret = TEE_ERROR_COMMUNICATION;
    goto error_unmap;
  }

  command->return_origin = TEE_ORIGIN_TRUSTED_APP;

error_unmap:
  if (command) {
    munmap(command, cmd.memref.size);
  }

error_cmd:
  kernel_cmd->size_out = 0;
  kernel_cmd->iwnotify_flags = IWNOTIFY_OEM_NOTIFICATION_FLAG_13;

exit:
  return 0;
}

#endif