#include <EDK.h>
#include "General.h"

/*

Emulator memory is allocated in 128 kb chunks from the system memory. The
first 64 kb contain the memory and the second 64 kb contain the traps. If
you have a pointer to a byte, the according trap is at pb+65536.

You can allocate memory from 4096 to 65536 bytes, the size must be
dividable by 4096. The memory will set to zero. At each 4096 bytes, there is
a dummy trap 1 which will cause the Program Counter in the CPU to be fixed
when passing a border.

*/

struct MCB {
  struct MCB* pNext;
  byte* pbData;
  int iUsedMask;
};

const int gaiMask[16] = {
  0x0001, 0x0003, 0x0007, 0x000F,
  0x001F, 0x003F, 0x007F, 0x00FF,
  0x01FF, 0x03FF, 0x07FF, 0x0FFF,
  0x1FFF, 0x3FFF, 0x7FFF, 0xFFFF
};

static MCB* gpRoot;

static byte* Init(byte* pb, int iSize) {
  memset(pb, 0, iSize);
  memset(pb + 65536, 0, iSize);
  for (int i = 0; i < iSize; i += 4096) {
    pb[65536 + i] = 1;
  }
  return pb;
}

byte* EmuAlloc(int iSize) {
  if (iSize & 4095) {
    error("EmuAlloc(): size must be n * 4096");
  }
  if (iSize > 65536) {
    error("EmuAlloc(): size is > 64k");
  }
  // search for free block
  int iIndex = iSize / 4096 - 1;
  MCB* p = gpRoot;
  while (p) {
    int iMask = gaiMask[iIndex];
    for (int i = iIndex; i < 16; i++) {
      if ((p->iUsedMask & iMask) == 0) {
        p->iUsedMask |= iMask;
        return Init(p->pbData + (i - iIndex) * 4096, iSize);
      }
      iMask <<= 1;
    }
    p = p->pNext;
  }
  // allocate new block
  p = (MCB*)MemAlloc(sizeof MCB);
  p->pbData = MemAlloc(65536 * 2 + 1);
  p->pbData[65536 * 2] = 1;
  p->iUsedMask = gaiMask[iIndex];
  p->pNext = gpRoot;
  gpRoot = p;
  return Init(p->pbData, iSize);
}

void EmuFree(void* /*pMem*/) {
  //error("EmuFree(): not implemented");
}
