void init_startup_thread(uint32_t arg)
{
    /* Threads have arguments for functions they run, we don't
       need any. Silence the compiler warning by using the argument. */
    arg = arg;

    kprintf("Mounting filesystems\n");
    vfs_mount_all();

    kprintf("Initializing networking\n");
    network_init();

    if(bootargs_get("initprog") == NULL) {
        kprintf("No initial program (initprog), dropping to fallback\n");
        init_startup_fallback();
    }

    kprintf("Starting initial program '%s'\n", bootargs_get("initprog"));

    /* `process_start` no longer takes an executable as its argument, so we need
       to start initprog with `process_spawn`. */
    process_id_t pid = process_spawn(bootargs_get("initprog"));
    if (pid < 0) {
        KERNEL_PANIC("Couldn't fit initial program in process table.\n");
    }
    process_join(pid);

    /* The current process_start() should never return. */
    KERNEL_PANIC("Run out of initprog.\n");
}

// send terminate signal to process
void tlb_modified_exception(void) {
    tlb_exception_state_t tlb_es;
    _tlb_get_exception_state(&tlb_es);
    pagetable_t *current_pagetable;
    current_pagetable = thread_get_current_thread_entry()->pagetable;
    
    if (current_pagetable == NULL) {
        KERNEL_PANIC("Pagetable is non-existing");
    }
    
    uint32_t i;
    for (i = 0; i < current_pagetable->valid_count; i++) {
        tlb_entry_t *entry = &current_pagetable->entries[i];
        // find addr fra pagetable og put i tlb. 
        if (entry->VPN2 == tlb_es.badvpn2) {
            
            /* Checks if address is odd( see vm.c)
             * and thereafter checks validbit */
            if (ADDR_IS_ON_ODD_PAGE(entry->VPN2)){
                KERNEL_ASSERT(entry->D1);
            } else {
                KERNEL_ASSERT(entry->D0);
            }
            return;
        }
    }
    KERNEL_PANIC("Unhandled TLB modified exception");
}

/** Initializes interrupt handling. Allocates interrupt stacks for
 * each processor, initializes the interrupt vectors and initializes
 * the registered interrupt handler table.
 *
 * @param num_cpus Number of CPUs in the system
 */
void interrupt_init(int num_cpus) {
    int i;
    uint32_t *iv_area1 = (uint32_t *)INTERRUPT_VECTOR_ADDRESS1;
    uint32_t *iv_area2 = (uint32_t *)INTERRUPT_VECTOR_ADDRESS2;
    uint32_t *iv_area3 = (uint32_t *)INTERRUPT_VECTOR_ADDRESS3;
    uint32_t ret;

    if (num_cpus < 1 || num_cpus > CONFIG_MAX_CPUS)
        KERNEL_PANIC("Too few or many CPUs found");

    /* Allocate interrupt stacks for each processor */
    for(i = 0; i < num_cpus; i++) {
        ret = (uint32_t)kmalloc(PAGE_SIZE);
        if (ret == 0)
            KERNEL_PANIC("Unable to allocate interrupt stacks");
        interrupt_stacks[i] = ret+PAGE_SIZE-4;
    }

    /* Copy the interrupt vector code to its positions.All vectors
     * will contain the same code.
     */
    for(i = 0 ; i < INTERRUPT_VECTOR_LENGTH ; i++) {
	iv_area1[i] = ((uint32_t *) &_cswitch_vector_code)[i];
	iv_area2[i] = ((uint32_t *) &_cswitch_vector_code)[i];
	iv_area3[i] = ((uint32_t *) &_cswitch_vector_code)[i];
    }

    /* Initialize the handler table to empty */
    for (i=0; i<CONFIG_MAX_DEVICES; i++) {
	interrupt_handlers[i].device = NULL;
	interrupt_handlers[i].irq = 0;
	interrupt_handlers[i].handler = NULL;
    }
}

/**
 * Allocates permanent memory for the kernel in unmapped memory. Call
 * of this function after virtual memory has been initialized will
 * cause kernel panic. Panics if memory can't be allocated.
 *
 * @param bytes The number of bytes to be allocated.
 *
 * @return The start address of the reseved memory address.
 */
void *kmalloc(int bytes)
{
    uint32_t res;

    /* Panic if VM is initialized */
    if (free_area_start == 0xffffffff){
        KERNEL_PANIC("Attempting to use kmalloc after vm init\n");
    }

    if (free_area_start == 0) {
        KERNEL_PANIC("Attempting to use kmalloc before initialization\n");
    }    

    /* bytes == 0 may be useful for aligning memory so it is allowed */
    if (bytes < 0)
        KERNEL_PANIC("Attempting to kmalloc negative amount of bytes\n");

    if (free_area_start + bytes > memory_end)
        KERNEL_PANIC("Out of memory\n");

    res = free_area_start;

    free_area_start += bytes;

    /* Check that the start of free area is aligned on a word
       boundary */
    if (free_area_start & 0x03) {
        free_area_start += 4;
        free_area_start &= 0xfffffffc;
    }

    return (void *)res;
}

/** Registers an interrupt handler for one or more interrupts
 * (IRQs). When registered, a \texttt{handler(device)} function call
 * will be made if any of the interrupts in \texttt{irq} occured.
 *
 * @param irq Mask of interrupts this handler wants to handle
 * @param handler The interrupt handling function
 * @param device The device registered for the interrupt, will be
 * given as a parameter for handler
 */
void interrupt_register(uint32_t irq,
			void (*handler)(device_t *),
			device_t *device)
{
    int i = 0;

    /* Check that IRQ mask is sane */
    if ((irq & ~(uint32_t)INTERRUPT_MASK_ALL)!= 0) {
	kprintf("Unsupported IRQ mask:%.8x\n", irq);
	KERNEL_PANIC("interrupt_register");
    }

    /* No need for spinlock, this should not be called after other CPUs
     * are enabled.
     */

    while (interrupt_handlers[i].device != NULL && i < CONFIG_MAX_DEVICES) i++;

    if (i >= CONFIG_MAX_DEVICES)
	KERNEL_PANIC("Interrupt handler table is full");

    interrupt_handlers[i].device = device;
    interrupt_handlers[i].irq = irq;
    interrupt_handlers[i].handler = handler;
}

/**
 * Gets one request from request queue and puts the disk in
 * work. Assumes that interrupts are disabled and device spinlock is
 * held. Also assumes that the device is idle.
 *
 * @param gbd pointer to the general block device.
 */
static void disk_next_request(gbd_t *gbd) {
  disk_real_device_t *real_dev = gbd->device->real_device;
  disk_io_area_t *io = (disk_io_area_t *)gbd->device->io_address;
  volatile gbd_request_t *req;

  KERNEL_ASSERT(!(DISK_STATUS_RBUSY(io->status) ||
                  DISK_STATUS_WBUSY(io->status)));
  KERNEL_ASSERT(real_dev->request_served == NULL);

  req = real_dev->request_queue;
  if(req == NULL) {
    /* There were no requests. */
    return;
  }
  real_dev->request_queue = req->next;
  req->next = NULL;

  real_dev->request_served = req;


  io->tsector = req->block;
  io->dmaaddr = (uint32_t)req->buf;
  if(req->operation == GBD_OPERATION_READ) {
    io->command = DISK_COMMAND_READ;
  } else if(req->operation == GBD_OPERATION_WRITE) {
    io->command = DISK_COMMAND_WRITE;
  } else {
    KERNEL_PANIC("disk_next_request: Unknown gbd operation.");
  }

  if(DISK_STATUS_ERRORS(io->status)) {
    kprintf("disk error: 0x%8.8x\n", DISK_STATUS_ERRORS(io->status));
    KERNEL_PANIC("disk error occured");
  }
}

/**
 * Initializes interrupt driven tty driver. Memory is reserved for
 * data structures and tty interrupt handler is registerded.
 *
 * @param desc Pointer to a YAMS device descriptor data structure.
 *
 * @return Pointer to tty's device_t structure.
 */
device_t *tty_init(io_descriptor_t *desc) {
  device_t *dev;
  gcd_t *gcd;
  tty_real_device_t *tty_rd;
  uint32_t irq_mask;
  static int num_of_inits = 0;

  dev = (device_t*)stalloc(sizeof(device_t));
  if(dev == NULL)
    KERNEL_PANIC("Could not reserve memory for tty driver.");

  gcd = (gcd_t*)stalloc(sizeof(gcd_t));
  if(gcd == NULL)
    KERNEL_PANIC("Could not reserve memory for tty driver.");

  dev->generic_device = gcd;
  dev->io_address     = desc->io_area_base;
  dev->type           = desc->type;

  gcd->device = dev;
  gcd->write  = tty_write;
  gcd->read   = tty_read;

  tty_rd = (tty_real_device_t*)stalloc(sizeof(tty_real_device_t));
  if(tty_rd == NULL)
    KERNEL_PANIC("Could not reserve memory for tty driver.");

  dev->real_device = tty_rd;
  if (num_of_inits == 0) {
    /* First tty driver will share the device with the polling TTY.
     * That is, we use the same spinlock with it. (The spinlock is
     * kprintf's because that is the only proper way to access the
     * polling tty.) */
    tty_rd->slock = &kprintf_slock;
  } else {
    tty_rd->slock = (spinlock_t*)stalloc(sizeof(spinlock_t));
    if(tty_rd->slock == NULL)
      KERNEL_PANIC("Could not reserve memory for tty driver spinlock.");
    spinlock_reset(tty_rd->slock);
  }
  num_of_inits++;

  tty_rd->write_head = 0;
  tty_rd->write_count = 0;

  tty_rd->read_head = 0;
  tty_rd->read_count = 0;

  irq_mask = 1 << (desc->irq + 10);
  interrupt_register(irq_mask, tty_interrupt_handle, dev);

  return dev;
}

void thread_goto_userland(context_t *usercontext)
{
  /* Call platform-specific */
  _context_enter_userland(usercontext);

  KERNEL_PANIC("Userland entering returned for unknown reason.");
}

void init_startup_thread(uint32_t arg)
{
    /* Threads have arguments for functions they run, we don't
       need any. Silence the compiler warning by using the argument. */
    arg = arg;
    process_id_t pid;

    kprintf("Mounting filesystems\n");
    vfs_mount_all();

    kprintf("Initializing networking\n");
    network_init();

    if(bootargs_get("initprog") == NULL) {
	kprintf("No initial program (initprog), dropping to fallback\n");
	init_startup_fallback();
    }

    kprintf("Starting initial program '%s'\n", bootargs_get("initprog"));

    pid = process_spawn(bootargs_get("initprog"));
    if (pid < 0)
        KERNEL_PANIC("Couldn't fit initial program in process table.\n");

    process_join(pid);
    halt_kernel();
}

void tlb_common_exception(void) {
    tlb_exception_state_t tlb_es;
    _tlb_get_exception_state(&tlb_es);
    pagetable_t *current_pagetable;
    current_pagetable = thread_get_current_thread_entry()->pagetable;
    if (current_pagetable == NULL) {
        KERNEL_PANIC("Pagetable is non-existing");
    }
    uint32_t i;
    for (i = 0; i < current_pagetable->valid_count; i++) {
        tlb_entry_t *entry = &current_pagetable->entries[i];
        // find addr fra pagetable og put i tlb. 
        if (entry->VPN2 == tlb_es.badvpn2) {
            
            KERNEL_ASSERT(entry->VPN2 == tlb_es.badvaddr >> 13);

            /* Checks if address is odd( see vm.c)
             * and thereafter checks validbit */

            if (ADDR_IS_ON_ODD_PAGE(tlb_es.badvaddr)){
                KERNEL_ASSERT(entry->V1);
            } else {
                KERNEL_ASSERT(entry->V0);
            }

            // Inserting into a random entry of the tlb.
            _tlb_write_random(&current_pagetable->entries[i]);
            return;
        }
    }

/**
 * Initialize disk device driver. Reserves memory for data structures
 * and register driver to the interrupt handler.
 *
 * @param desc Pointer to the YAMS IO device descriptor of the disk
 *
 * @return Pointer to the device structure of the disk
 */
device_t *disk_init(io_descriptor_t *desc) {
  device_t *dev;
  gbd_t    *gbd;
  disk_real_device_t *real_dev;
  uint32_t irq_mask;

  dev = (device_t*)stalloc(sizeof(device_t));
  gbd = (gbd_t*)stalloc(sizeof(gbd_t));
  real_dev = (disk_real_device_t*)stalloc(sizeof(disk_real_device_t));
  if (dev == NULL || gbd == NULL || real_dev == NULL)
    KERNEL_PANIC("Could not allocate memory for disk driver.");

  dev->generic_device = gbd;
  dev->real_device = real_dev;
  dev->descriptor = desc;
  dev->io_address = desc->io_area_base;
  dev->type = desc->type;

  gbd->device = dev;
  gbd->read_block = disk_read_block;
  gbd->write_block = disk_write_block;
  gbd->block_size = disk_block_size;
  gbd->total_blocks = disk_total_blocks;

  spinlock_reset(&real_dev->slock);
  real_dev->request_queue = NULL;
  real_dev->request_served = NULL;

  irq_mask = 1 << (desc->irq + 10);
  interrupt_register(irq_mask, disk_interrupt_handle, dev);

  return dev;
}

/**
 * Handle system calls. Interrupts are enabled when this function is
 * called.
 *
 * @param user_context The userland context (CPU registers as they
 * where when system call instruction was called in userland)
 */
void syscall_handle(context_t *user_context)
{
    /* When a syscall is executed in userland, register a0 contains
     * the number of the syscall. Registers a1, a2 and a3 contain the
     * arguments of the syscall. The userland code expects that after
     * returning from the syscall instruction the return value of the
     * syscall is found in register v0. Before entering this function
     * the userland context has been saved to user_context and after
     * returning from this function the userland context will be
     * restored from user_context.
     */
    switch(user_context->cpu_regs[MIPS_REGISTER_A0]) {
    case SYSCALL_HALT:
        halt_kernel();
        break;
    case SYSCALL_READ:
        user_context->cpu_regs[MIPS_REGISTER_V0] =
            read(user_context->cpu_regs[MIPS_REGISTER_A1]
                ,(void*)user_context->cpu_regs[MIPS_REGISTER_A2]
                ,user_context->cpu_regs[MIPS_REGISTER_A3]);
        break;
    case SYSCALL_WRITE:
        // TODO
        user_context->cpu_regs[MIPS_REGISTER_V0] =
            write(user_context->cpu_regs[MIPS_REGISTER_A1]
                 ,(void*)user_context->cpu_regs[MIPS_REGISTER_A2]
                 ,user_context->cpu_regs[MIPS_REGISTER_A3]);
        break;
    default: 
        KERNEL_PANIC("Unhandled system call\n");
    }

    /* Move to next instruction after system call */
    user_context->pc += 4;
}

/**
 * Handle system calls. Interrupts are enabled when this function is
 * called.
 *
 * @param user_context The userland context (CPU registers as they
 * where when system call instruction was called in userland)
 */
void syscall_handle(context_t *user_context)
{
    /* When a syscall is executed in userland, register a0 contains
     * the number of the syscall. Registers a1, a2 and a3 contain the
     * arguments of the syscall. The userland code expects that after
     * returning from the syscall instruction the return value of the
     * syscall is found in register v0. Before entering this function
     * the userland context has been saved to user_context and after
     * returning from this function the userland context will be
     * restored from user_context.
     */

    int retval;

    switch(user_context->cpu_regs[MIPS_REGISTER_A0]) {
    case SYSCALL_HALT:
        halt_kernel();
        break;
    case SYSCALL_EXEC:
        retval = (int) process_spawn((char*) user_context->cpu_regs[MIPS_REGISTER_A1]);
        user_context->cpu_regs[MIPS_REGISTER_V0] = retval;
        break;
    case SYSCALL_EXIT:
        /* Resources are cleaned up in process_finish(...) */
        process_finish(user_context->cpu_regs[MIPS_REGISTER_A1]);
        break;
    case SYSCALL_JOIN:
        retval = process_join(user_context->cpu_regs[MIPS_REGISTER_A1]);
        user_context->cpu_regs[MIPS_REGISTER_V0] = retval;
        break;
    case SYSCALL_READ:
        {
            int fhandle = user_context->cpu_regs[MIPS_REGISTER_A1];
            int buffer = user_context->cpu_regs[MIPS_REGISTER_A2];
            int length = user_context->cpu_regs[MIPS_REGISTER_A3];

            int retval = syscall_read(fhandle, (void *)buffer, length);
            user_context->cpu_regs[MIPS_REGISTER_V0] = retval;
        }
        break;
    case SYSCALL_WRITE:
        {
            int fhandle = user_context->cpu_regs[MIPS_REGISTER_A1];
            int buffer = user_context->cpu_regs[MIPS_REGISTER_A2];
            int length = user_context->cpu_regs[MIPS_REGISTER_A3];

            int retval = syscall_write(fhandle, (void *)buffer, length);
            user_context->cpu_regs[MIPS_REGISTER_V0] = retval;
        }
        break;
    default:
        KERNEL_PANIC("Unhandled system call\n");
    }

    /* Move to next instruction after system call */
    user_context->pc += 4;
}

void thread_goto_userland(context_t *usercontext)
{
    /* Set userland bit and enable interrupts before entering userland. */
    usercontext->status = usercontext->status | USERLAND_ENABLE_BIT;
    usercontext->status = usercontext->status | INTERRUPT_MASK_ALL;
    usercontext->status = usercontext->status | INTERRUPT_MASK_MASTER;
    _cswitch_to_userland(usercontext);
    
    KERNEL_PANIC("Userland entering returned for unknown reason.");
}

uint32_t syscall_read(uint32_t fd, char* s, int len)
{
    int count = 0;
    gcd_t *gcd;
    if (fd != FILEHANDLE_STDIN) {
        KERNEL_PANIC("Can only read() from standard input.");
    }
    gcd = process_get_current_process_entry()->fds[0];
    count = gcd->read(gcd, s, len);
    return count;
}

/** Initializes the (software) shutdown device. Note that the system
 * should have only one shutdown device.
 *
 * @param desc Pointer to the YAMS IO device descriptor of the shutdown device
 * @return Pointer to the device structure of the shutdown device
 */
device_t *shutdown_init(io_descriptor_t *desc)
{
  if (shutdown_init_done)
    KERNEL_PANIC("Hardware failure: multiple SHUTDOWN devices");

  shutdown_init_done = 1;

  fill_device_t(desc, &system_shutdown);

  return &system_shutdown;
}

uint32_t syscall_write(uint32_t fd, char* s, int len)
{
    int count;
    gcd_t *gcd;
    if (fd != FILEHANDLE_STDOUT) {
        KERNEL_PANIC("Can only write() to standard output.");
    }
    gcd = process_get_current_process_entry()->fds[1];
    count = gcd->write(gcd, s, len);
    return count;
}

/** Handles an interrupt (exception code 0). All interrupt handlers
 * that are registered for any of the occured interrupts (hardware
 * 0-5, software 0-1) are called. The scheduler is called if a timer
 * interrupt (hardware 5) or a context switch request (software
 * interrupt 0) occured, or if the currently running thread for the
 * processor is the idle thread.
 *
 * @param cause The Cause register from CP0
 */
void interrupt_handle(uint32_t cause) {
    int this_cpu, i;
    
    if(cause & INTERRUPT_CAUSE_SOFTWARE_0) {
        _interrupt_clear_sw0();
    }

    this_cpu = _interrupt_getcpu();

    /* Exceptions should be handled elsewhere: */
    if((cause  & 0x0000007c) != 0) {
	kprintf("Caught exception, cause %.8x, CPU %i\n", cause, this_cpu);
	KERNEL_PANIC("Exception in interrupt_handle");
    }


    /* Call appropiate interrupt handlers.  Handlers cannot be
     * unregistered, so after the first empty * entry all others are
     * also empty.
     */
    for (i=0; i<CONFIG_MAX_DEVICES; i++) {
	if (interrupt_handlers[i].device == NULL)
	    break;
	
	/* If this handler is registered for any of the interrupts
	 * that occured, call it.
	 */
	if ((cause & interrupt_handlers[i].irq) != 0)
	    interrupt_handlers[i].handler(interrupt_handlers[i].device);
    }


    /* Timer interrupt (HW5) or requested context switch (SW0)
     * Also call scheduler if we're running the idle thread.
     */
    if((cause & (INTERRUPT_CAUSE_SOFTWARE_0 |
		 INTERRUPT_CAUSE_HARDWARE_5)) ||
       scheduler_current_thread[this_cpu] == IDLE_THREAD_TID) {
	scheduler_schedule();
	
	/* Until we have proper VM we must manually fill
	   the TLB with pagetable entries before running code using
	   given pagetable. Note that this method limits pagetable
	   rows (possible mapping pairs) to 16 and can't be used
	   with proper pagetables and VM.

           Note that if you remove this call (which you probably do when
           you implement proper VM), you must manually call _tlb_set_asid
           here. See the implementation of tlb_fill on details how to do that.
        */
    _tlb_set_asid(thread_get_current_thread());
    }
}

void tlb_seek_insert(void)
{
  tlb_exception_state_t state;
  _tlb_get_exception_state(&state);
  pagetable_t *table = thread_get_current_thread_entry()->pagetable;
  for (int i = 0; i < (int)table->valid_count; i++) {
    if (table->entries[i].VPN2 == state.badvpn2) {
      _tlb_write_random(&table->entries[i]);
      return;
    }
  }
  KERNEL_PANIC("Access violation");
}

physaddr_t physmem_allocblocks(uint32_t count)
{
  /* Get spinlock */
  physaddr_t addr = 0, i;
  interrupt_status_t intr_status = _interrupt_disable();
  spinlock_acquire(physmem_lock);

  /* Sanity */
  if(used_blocks >= total_blocks)
    {
      /* PANIC AT THE DISCO ! */
      KERNEL_PANIC("Physical Manager >> OUT OF MEMORY");
    }

  /* Get a frame */
  int64_t frame = physmem_getframes(count);

  if(frame == -1)
    {
      /* PANIC AT THE DISCO ! */
      spinlock_release(physmem_lock);
      KERNEL_PANIC("Physical Manager >> OUT OF MEMORY");
    }

  /* Mark it used */
  for(i = 0; i < count; i++)
    memmap_setbit(frame + i);

  /* Release spinlock */
  spinlock_release(physmem_lock);
  _interrupt_set_state(intr_status);

  /* Calculate Address */
  addr = (uint64_t)(frame * PMM_BLOCK_SIZE);
  used_blocks++;

  return addr;
}