/*
    This file is part of SecBus.

    libtermui is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    libtermui 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.

    You should have received a copy of the GNU General Public License
    along with libtermui.  If not, see <http://www.gnu.org/licenses/>.

    Copyright 2013, Jeremie Brunel <jeremie.brunel@telecom-paristech.fr>

*/

#define ADDR_LOW  ((uint32_t)0x80f00000)
#define ADDR_HIGH ((uint32_t)0x80ff0000)

#define EROM_LOW  ((uint32_t)0x90000000)
#define EROM_HIGH ((uint32_t)0x90001000)

#include <string.h>
#include <stdlib.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <assert.h>
#include <stdbool.h>

#include <libssm/libssm.h>
#include <libssm/hsm.h>

#include <device/driver.h>
#include <device/class/dma.h>
#include <device/class/char.h>

#include <termui/term.h>
#include <termui/console.h>
#include <termui/mutekh.h>
#include <mutek/printk.h>
#include <hexo/iospace.h>

static int check_address(uint32_t addr)
{
  if (addr<ADDR_LOW || addr > ADDR_HIGH)
    return 0;
  else
    return 1;
}

#if defined(CONFIG_CPU_ARM)

static inline void __cpu_dcache_invld(void *ptr)
{
    THUMB_TMP_VAR;
	asm(
        THUMB_TO_ARM
        "mcr p15, 0, %[ptr], c7, c6, 1\n\t"
        ARM_TO_THUMB
        /*:*/ THUMB_OUT(:)
        : [ptr] "r"(ptr));
}

static inline size_t __cpu_dcache_line_size()
{
    THUMB_TMP_VAR;
	uint32_t ret;
	asm(
        THUMB_TO_ARM
        "mrc p15, 0, %[ret], c0, c0, 1\n\t"
        ARM_TO_THUMB
        : [ret] "=r"(ret) /*,*/ THUMB_OUT(,));

	return (((ret >> 12) & 0x3) + 3) << 1;
}

#else
# if defined(CONFIG_CPU_MIPS)

static inline void __cpu_dcache_invld(void *ptr)
{
  asm volatile (
# if CONFIG_CPU_MIPS_VERSION >= 32
		" cache %0, %1"
		: : "i" (0x11) , "R" (*(uint8_t*)(ptr))
# else
#  ifdef CONFIG_ARCH_SOCLIB
		" lw $0, (%0)"
		: : "r" (ptr)
#  else
		"nop"::
#  endif
# endif
		: "memory"
		);
}

static inline size_t __cpu_dcache_line_size()
{
  reg_t r0 = cpu_mips_mfc0(16, 0);
  reg_t r1 = cpu_mips_mfc0(16, 1);

  if (BIT_EXTRACT(r0, 31))
    {
      r1 = BITS_EXTRACT_FL(r1, 10, 12);

      if (r1)
	return 2 << r1;
    }

  return 8;
}

# else
#error "cpu not supported"
# endif
#endif

void __cpu_dcache_invld_buf(void *ptr, size_t size)
{
  size_t ls = __cpu_dcache_line_size();

  if (ls != 0)
    {
      uint8_t *ptr_;

      for (ptr_ = ALIGN_ADDRESS_LOW(ptr, ls);
	   ptr_ < (uint8_t*)ptr + size;
	   ptr_ += ls)
	__cpu_dcache_invld(ptr_);
    }
}

static TERMUI_CON_COMMAND_PROTOTYPE(kernel_attack_command)
{
/*  struct device_dma_s dma_dev;
  if (device_get_accessor_by_path(&dma_dev, NULL, "/fdt/dma@0", DRIVER_CLASS_DMA))
  {
    printk("error with the dma device\n");
    abort();
  }
  uint32_t s_addr = 0x90000000;
  uint32_t d_addr = (uint32_t)(&tty_soclib_char_drv+1);

  if (dev_dma_spin_copy(&dma_dev, (uintptr_t)s_addr, (uintptr_t)d_addr, 4, 0))
  {
    printk("error during dma transfert\n");
    abort();
  }
*/
  return 0;
}

static TERMUI_CON_COMMAND_PROTOTYPE(shutdown_command)
{
  cpu_mem_write_32((uintptr_t)0xd6200000, 1);

  return 0;
}

static TERMUI_CON_COMMAND_PROTOTYPE(read_command)
{
  uint32_t addr;
  size_t   size;
  char *endptr, *str = argv[0];

  addr = strtol(str, &endptr, 16);
/*  if (!check_address(addr))
  {
    printk("Address range is [0x%p:0x%p]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
    }*/   

  if (argc==2)
  {
    str = argv[1];
    size = strtol(str, &endptr, 16);
  }
  else
    size = 1;

  printk("Memory read request : %p (%u B)\n", addr, size);

  uintptr_t mem = (uintptr_t)ALIGN_VALUE_LOW(addr,0x4);

  hexdumpk(mem, (void*)mem, size);

  return 0;
}

static TERMUI_CON_COMMAND_PROTOTYPE(write_command)
{
  uint32_t addr;
  size_t   size;
  uint8_t byte;
  char *endptr, *str = argv[0];

  addr = strtol(str, &endptr, 16);
  if (!check_address(addr))
  {
    printk("Address range is [0x%x:0x%x]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
  }   

  str = argv[1];
  byte = strtol(str, &endptr, 16);

  
  if (argc==3)
  {
    str = argv[2];
    size = strtol(str, &endptr, 16);
    if (str == endptr)
    {
      termui_con_printf(con, "Arg 3 not a number (%s)\n", argv[1]);
      return 0;
    }
  }
  else
    size = 1;

  printk("Memory write request : @0x%x(0x%x)=0x%x\n", addr, size, byte);
  
  uintptr_t mem_write = (uintptr_t)ALIGN_VALUE_LOW(addr, 0x4);
/*  size_t i;
  for (i=0;i<size;i++)
  {
    mem_write[i] = byte;
    }*/
  memset((void*)mem_write, byte, size);

  return 0;
}

static TERMUI_CON_COMMAND_PROTOTYPE(write256_command)
{
  uint32_t addr;
  size_t   size;
  uint8_t byte;
  char *endptr, *str = argv[0];

  addr = strtol(str, &endptr, 16);
  if (!check_address(addr))
  {
    printk("Address range is [0x%x:0x%x]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
  }   

  str = argv[1];
  byte = strtol(str, &endptr, 16);

  
  if (argc==3)
  {
    str = argv[2];
    size = strtol(str, &endptr, 16);
    if (str == endptr)
    {
      termui_con_printf(con, "Arg 3 not a number (%s)\n", argv[1]);
      return 0;
    }
  }
  else
    size = 1;

  printk("Memory writegroup request : @0x%x(0x%x)=0x%x\n", addr, size, byte);
  
  uintptr_t mem_write = (uintptr_t)ALIGN_VALUE_LOW(addr, 0x40);
/*  size_t i;
  for (i=0;i<size;i++)
  {
    mem_write[i] = byte;
    }*/
  
  uint32_t tab256[8];
  size_t loaded_size = (size/0x20)*0x20;
  size_t i;
  for (i=0;i<8;i++)
    tab256[i] = byte | (byte << 16);

  for (i=0;i<loaded_size/0x20;i++)
  {
    sb_ssm_load_segment((uintptr_t)tab256, mem_write+i*0x20, 0x20);
  }

//  memset((void*)mem_write, byte, size);

  return 0;
}


static const char * confidentialitytostr(uint32_t c)
{
 switch(c)
  {
  case 0:
    return "NONE";
  case 1:
    return "OTP";
  case 2:
    return "CBC";
  default:
    return "";
  }
}

static const char * integritytostr(uint32_t i)
{
  switch(i)
  {
  case 0:
    return "NONE";
  case 1:
    return "MAC";
  case 2:
    return "MACTree";
  default:
    return "";
  }
}

static TERMUI_CON_COMMAND_PROTOTYPE(protect_command)
{
  uint32_t addr;
  size_t   size;
  uint32_t  integrity, confidentiality;
  char *endptr, *str = argv[0];

  addr = strtol(str, &endptr, 16);
  
  if (!check_address(addr))
  {
    printk("Address range is [0x%x:0x%x]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
  }   

  str = argv[1];
  size = strtol(str, &endptr, 16);
  
  integrity = atoi(argv[2]);
  if (integrity<0 || integrity>2)
  {
    termui_con_printf(con, "Integrity range is [0-2]\n");
    return 0;
  }

  confidentiality = atoi(argv[3]);  
  if (confidentiality<0 || confidentiality>2)
  {
    termui_con_printf(con, "Confidentiality range is [0-2]\n");
    return 0;
  }

  if ((confidentiality == 2 && (integrity != 0 && integrity != 2)) ||
      (confidentiality == 1 && integrity>1))
  {
    termui_con_printf(con, "Integrity & Confidentiality incompatible\n");
    return 0;
  }
  
  printk("Security request : @0x%x(0x%x) with I=%s, C=%s\n", addr, size, integritytostr(integrity), confidentialitytostr(confidentiality));

  if (confidentiality == 0 && integrity == 0)
  {
    sb_ssm_new_protected_segment(ALIGN_VALUE_LOW(addr, 0x1000),
				  size,
				  NULL);
  }
  else
  {
    struct sp_s *sp = sb_ssm_get_policy((confidentiality==0)?false:true, (integrity==0)?false:true, (confidentiality==1||integrity==1)?HSM_PROT_RO:HSM_PROT_RW, false);
    sb_ssm_new_protected_segment(ALIGN_VALUE_LOW(addr, 0x1000),
				  size,
				  sp);
  }

  termui_con_printf(con, "PSPEs created");

  return 0;
}

static TERMUI_CON_COMMAND_PROTOTYPE(dmaattack_command)
{
  uint32_t s_addr, d_addr;
  size_t   size;
  char *endptr, *str = argv[0];

  s_addr = strtol(str, &endptr, 16);
  
  if ((s_addr < EROM_LOW || s_addr >= EROM_HIGH) && (s_addr < ADDR_LOW || s_addr >= ADDR_HIGH))
  {
    printk("Address range for source address is [0x%x:0x%x]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
  }   

  str = argv[1];
  d_addr = strtol(str, &endptr, 16);

  if (!check_address(d_addr))
  {
    printk("Address range for destination address is [0x%x:0x%x]\n", ADDR_LOW, ADDR_HIGH);
    return 0;
  }   
  
  str = argv[2];
  size = strtol(str, &endptr, 16);

  if (ALIGN_VALUE_LOW(s_addr,0x4)+size>=EROM_HIGH ||
      ALIGN_VALUE_LOW(d_addr,0x4)+size>=ADDR_HIGH)
  {
    printk("Size out of bound\n");
    return 0;
  }

  printk("DMA request : 0x%x -> 0x%x (0x%x B)\n", s_addr, d_addr, size);

  struct device_dma_s dma_dev;
  if (device_get_accessor_by_path(&dma_dev, NULL, "/fdt/dma@0", DRIVER_CLASS_DMA))
  {
    printk("error with the dma device\n");
    abort();
  }

  if (dev_dma_spin_copy(&dma_dev, (uintptr_t)s_addr, (uintptr_t)d_addr, size, 0))
  {
    printk("error during dma transfert\n");
    abort();
  }

  __cpu_dcache_invld_buf((void*)d_addr, size);
  
  return 0;
}

/* console commands descriptors array */
static TERMUI_CON_GROUP_DECL(root_group) =
{
  /* Some useful commands provided by the library */
  TERMUI_CON_BUILTIN_HELP(-1),
  TERMUI_CON_BUILTIN_LIST(-1),
  TERMUI_CON_BUILTIN_QUIT(-1),
  
  TERMUI_CON_ENTRY(write256_command, "writegroup", 
		   TERMUI_CON_HELP("Write in memory with atomic group", 
				   "usage: writegroup ADDR CHAR [SIZE]\n"
				   "ADDR: address of the memory block (range [0x80f00000:0x80ff0000]\n"
				   "CHAR: byte to write (must be in hexadecimal)\n"
				   "SIZE: size of the memory area to write with the CHAR byte, default 1 byte")
		   TERMUI_CON_ARGS(2,3)
    ),

  TERMUI_CON_ENTRY(read_command, "read", 
		   TERMUI_CON_HELP("Read a memory block", 
				   "usage: read ADDR [SIZE]\n"
				   "ADDR: address of the memory block (range [0x80f00000:0x80ff0000]\n"
				   "SIZE: size of the memory area to read, default 1 byte")
		   TERMUI_CON_ARGS(1,2)
    ),

  TERMUI_CON_ENTRY(write_command, "write", 
		   TERMUI_CON_HELP("Write a memory block", 
				   "usage: write ADDR CHAR [SIZE]\n"
				   "ADDR: address of the memory block (range [0x80f00000:0x80ff0000]\n"
				   "CHAR: byte to write (must be in hexadecimal)\n"
				   "SIZE: size of the memory area to write with the CHAR byte, default 1 byte")
		   TERMUI_CON_ARGS(2,3)
    ),

  TERMUI_CON_ENTRY(protect_command, "protect",
		   TERMUI_CON_HELP("Protect a memory area with the given security policy",
				   "\nusage: protect ADDR SIZE INTEGRITY CONFIDENTIALITY\n"
				   "ADDR: start address of the new secure memory block\n"
				   "SIZE: size of the secure memory block\n"
				   "INTEGRITY: \n 0 none\n 1 MAC\n 2 MACTree"
				   "\nCONFIDENTIALITY: \n 0 none\n 1 OTP\n 2 CBC")
		   TERMUI_CON_ARGS(4,4)
    ),

  TERMUI_CON_ENTRY(dmaattack_command, "dma_attack",
		   TERMUI_CON_HELP("DMA attack",
				   "\nusage: S_ADDR D_ADDR SIZE\n"
				   "S_ADDR: source address ([0x80f00000:0x80ff0000])\n"
				   "D_ADDR: destination address (range [0x80f00000:0x80ff0000])\n"
				   "SIZE: memory block size to be transferred from S_ADDR to D_ADDR")
		   TERMUI_CON_ARGS(3,3)
    ),
  TERMUI_CON_ENTRY(shutdown_command, "shutdown",
		   TERMUI_CON_HELP("shutdown",
				   "stop simulation")
		   TERMUI_CON_ARGS(0,0)
    ),
/*
  TERMUI_CON_ENTRY(kernel_attack_command, "kernel_attack",
		   TERMUI_CON_HELP("Kernel attack", "Modify the TTY write function to double the printed character")
		   TERMUI_CON_ARGS(0,0)
    ),
*/
  TERMUI_CON_LIST_END
};

int main()
{
  printk("SecBus Demo\n");

  struct termui_term_s    tm;
  struct termui_console_s con;
  int res;

  sb_ssm_init(0x80000000, 0x4000000, 0x91000000);
  // clean up all policy
  struct sp_s *sp = sb_ssm_get_policy(false, false, HSM_PROT_RO, true);
  sb_ssm_new_protected_segment(0x80f00000, 0xf0000, sp);

  struct device_char_s console_dev;
  if (device_get_accessor_by_path(&console_dev, NULL, "/fdt/tty@0", DRIVER_CLASS_CHAR))
  {
    printk("error with the char device\n");
    abort();
  }

  TERMUI_CON_EMPTY_ROOT(root);

  /* open console with stdin/stdout */
  termui_dev_io_init(&tm, &console_dev, "xterm");
  termui_con_init(&con, &tm, &root);

  /* set console prompt */
  termui_con_set_prompt(&con, "[%31Ademo%A] ");

  /* register root command array */
  termui_con_register(&con, root_group);

  /* process user commands */
  do {
    res = termui_con_process(&con);
  } while (res != -EIO && res != -ECANCELED);

  /* free allocated resources */
  termui_con_cleanup(&con);
  termui_termio_cleanup(&tm);

  return 0;
}

