#include "al/cpu.h"
#include "al/io.h"
#include "al/sign.h"
#include "al/sec.h"

#include "stdlib.h"
#include "ssm.h"
#include "stdio.h"
#include "string.h"
#include "types.h"
#include "macro.h"

#include "sb_hsm_drv.h"

#include "config.h"

#include "memory_footprint.h"

#define ERROR while (1) {}

struct memory_footprint_s *memory_footprint;

struct header {
  char     name[64];
  uint32_t entry_point;
  size_t   size;
  uint32_t b_addr;
  uint32_t stack;
};

static inline void init_hardware()
{
  block_init((uint32_t*)CONFIG_EEPROM_BASE);
  print_init((uint32_t*)CONFIG_TTY_BASE);
  mem_init((uint32_t*)CONFIG_IRAM_BASE+0x80);
  sb_hsm_init_drv(CONFIG_SECBUS_BASE);
  printk("\n");
  printk("   _____           ____\n");
  printk("  / ___/___  _____/ __ )__  _______\n");
  printk("  \\__ \\/ _ \\/ ___/ __  / / / / ___/\n");
  printk(" ___/ /  __/ /__/ /_/ / /_/ (__  )\n");
  printk("/____/\\___/\\___/_____/\\__,_/____/\n");
  printk("By Telecom ParisTech\n\n");
  printk("|--> HSM initialization "); 
  ssm_init(0x91000000, 0x80000000, 3);
  printk("\r[OK]\n");
}

void secbus_add_sp()
{
  ssm_add_sp(1, true, true, sp_rw);
  ssm_add_sp(2, false, true, sp_rw);
}

/*
 * initialize secbus hw module
 * the policy used for the kernel page is given by the argument
 */
void secbus_hw_init(unsigned char sp, struct header *kernel_hdr)
{
  printk("Secbus initialisation\n");

  printk("|--> Security policy ");
  secbus_add_sp();
  printk("\r[OK]\n");

  printk("|--> Kernel PSPE ");
  ssm_create_segment(1, kernel_hdr->b_addr, kernel_hdr->size+0x1000);
  ssm_create_segment(2, 0x80e00000, 0x4000);
  ssm_create_segment(2, 0x80e04000, 0xfc000);
  printk("\r[OK]\n");

}

/*
   Read header from the first block of the io block,
   compute is signature and initialize the struct 
   passed as argument
  
   @param struct header *head: pointer to the struct to fill
 */
void read_header(struct header *head, void *ctx_sign)
{
  printk("Reading kernel header\n");
  uint32_t mem_unprotected = 0x80f00000; 
  ssm_add_sp(0, false, false, sp_rw);
  ssm_create_segment(0, 0x80000000, 0x1000);
  ssm_create_segment(0, mem_unprotected, 0x10000);


  volatile uint32_t *mem_u = (volatile uint32_t*)mem_unprotected;

  size_t block_size = block_get_size();

  uint8_t hdr_in_block = sizeof(struct header)/block_size + 1;
  
  void *hdr = mem_get_block(block_get_size() * hdr_in_block);

  if (block_read((void*)mem_u/*hdr*/, hdr_in_block, 0) == 1)
  {
    printk("failed\n");
    ERROR;
  }

  memcpy(hdr, (void*)mem_u, block_size);

  /* generate the signature for the first block */
  sign_update(ctx_sign, hdr, block_size); 

  /* initialize the header block */
  memcpy(head->name, (const void*)hdr, 64);
  memcpy(&head->entry_point, (const void*)(hdr+64), sizeof(uint32_t));
  memcpy(&head->size, (const void*)(hdr+64+sizeof(uint32_t)), sizeof(size_t));
  memcpy(&head->b_addr, (const void*)(hdr+64+sizeof(uint32_t)+sizeof(size_t)), sizeof(uint32_t));
  memcpy(&head->stack, (const void*)(hdr+64+2*sizeof(uint32_t)+sizeof(size_t)), sizeof(uint32_t));

  mem_free_block(hdr);
 
  printk("Kernel name : %s\n", head->name);
  printk("Entry point : %x\n", head->entry_point);
  printk("Size        : %dko\n", head->size/1024); 
  printk("Stack       : %x\n", head->stack);

  memory_footprint->slave_mac_addr = ALIGN_VALUE_UP(head->b_addr + head->size, 0x1000) + 0x1000;
  memory_footprint->slave_ht_addr = memory_footprint->slave_mac_addr + 0x400 + 0x1000;

  return;
}

void *load_kernel(struct header *kernel_header, void *ctx_sign)
{
  volatile unsigned char *external_memory = (volatile unsigned char*)kernel_header->b_addr;

  uint32_t mem_unprotected = 0x80f00000;
  volatile uint32_t *mem_u = (volatile uint32_t*)mem_unprotected;

  size_t block_size = block_get_size();
  size_t kernel_in_block = kernel_header->size/block_size+1;
  
  size_t itr;

  char *buf_block = mem_get_block(block_size);

  printk("Loading %s:\n", kernel_header->name);
  for (itr=0;itr<kernel_in_block;itr++)
  {
    if (block_read((void*)mem_u/*buf_block*//*(void*)(external_memory+itr*block_size)*/, 1, itr+1/* jump the first block (header block) */) == 1)
    {
      printk("reading kernel fail at block %d -> %x\n", itr, buf_block);
      return NULL;
    }
    printk("\r%d/%d", itr+1, kernel_in_block);
    memcpy(buf_block, (void*)mem_u, block_size);

    sign_update(ctx_sign, (void*)buf_block, block_size);


    memcpy((void*)(external_memory+itr*block_size), (const void*)buf_block, block_size);
  }

  mem_free_block(buf_block);

  printk("\n");

  return (void*)kernel_header->entry_point;
}

int bootloader()
{
  void          (*kernel_entry_point)(uint32_t*);
  struct header kernel_header;
  void          *ctx_sign = NULL;
  void          *sign_gen = NULL;
  void          *sign_ref = NULL;
  
  init_hardware();

  memory_footprint = mem_get_block(sizeof(struct memory_footprint_s));

  sign_get_ref(sign_ref);

  ctx_sign = sign_ctx_init();
  sign_gen = sign_gen_init();
  sign_ref = sign_ref_init();
  
  read_header(&kernel_header, ctx_sign);
  
  secbus_hw_init(0, &kernel_header);

  kernel_entry_point = load_kernel(&kernel_header, ctx_sign);
  
  if (kernel_entry_point == NULL)
  {
    printk("Kernel loading failed\n");
    goto error;
  }

  sign_set(ctx_sign, sign_gen);

  if (!sign_compare(sign_ref, sign_gen))
  {
    printk("kernel corrupt !!!\n");
    goto error;
  }

  printk("Now booting %s (%x):\n", kernel_header.name, kernel_header.entry_point);
  kernel_entry_point((uint32_t*)memory_footprint);

error:
  while (1) {}

  return 0;
}
