/*
Copyright 2019 Benji Dial

Permission to use, copy, modify, and/or distribute this
software for any purpose with or without fee is hereby
granted, provided that the above copyright notice and this
permission notice appear in all copies.

THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS
ALL WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO
EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER
RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION,
ARISING OUT OF OR IN CONNECTION WITH THE USE OR PERFORMANCE
OF THIS SOFTWARE.
*/

#include "mem.h"

uint32_t last_byte = 0;
uint32_t last_mask = 0x00000001;

void *allocate_block(uint32_t size, uint16_t proc_n) {
  struct mmap_entry *i = mmap_start;
  while (i->whose || (i->length < size))
    if (i->next)
      i = i->next;
    else
      return (void *)0;
  if (i->length == size) {
    i->whose = proc_n;
    return (void *)i->base;
  }
  uint32_t remaining = i->length - size;
  i->whose = proc_n;
  i->length = size;

  uint32_t byte = last_byte;
  uint32_t mask = last_mask;
  do {
    if (!(mask <<= 1)) {
      mask = 0x00000001;
      if (++byte == MMAP_SIZE)
        byte = 0;
    }
    if ((byte == last_byte) && (mask == last_mask))
      return 0;
  } while (mmap_bitmap[byte] & mask);
  last_byte = byte;
  last_mask = mask;
  mmap_bitmap[byte] |= mask;
  uint8_t bit = 0;
  while (!(mask & 0x00000001)) {
    ++bit;
    mask >>= 1;
  }

  struct mmap_entry *new = mmap_bss + (byte << 5) + bit;
  new->base = i->base + size;
  new->length = remaining;
  new->whose = FREE;
  new->next = i->next;
  i->next = new;
  return (void *)i->base;
}

void deallocate_block(void *start) {
  struct mmap_entry *find = mmap_start;
  while (find->base != (uint32_t)start)
    if (find->next)
      find = find->next;
    else
      return;
  find->whose = FREE;

  for (struct mmap_entry *find_after = mmap_start; find_after; find_after = find_after->next)
    if (find_after->base == find->base + find->length) {
      if (!find_after->whose) {
        find->length += find_after->length;
        find->next = find_after->next;
        unmark_entry(find_after);
      }
      break;
    }

  for (struct mmap_entry *find_before = mmap_start; find_before; find_before = find_before->next)
    if (find->base == find_before->base + find_before->length) {
      if (!find_before->whose) {
        find_before->length += find->length;
        find_before->next = find->next;
        unmark_entry(find);
      }
      break;
    }
}