#include "mmap.h"
#include <multiboot2.h>

#ifdef DEBUG
#include <stdio.h>
#endif

__attribute__((section(".multiboot.text"))) unsigned
multiboot2_mmap(const struct multiboot_mmap_entry entries[], unsigned entry_count, unsigned bitmap[1024 * 32])
{
  unsigned i, l;
  multiboot_uint64_t avail_frames = 0;
  multiboot_uint64_t n_frames;
  multiboot_uint64_t table_idx;

  /* clear out the bitmap */
  for (i = 0; i < 1024 * 32; ++i) bitmap[i] = 0;

  /* loop through all the mmap_entry structures where type is MULTIBOOT_MEMORY_AVAILABLE */
  for (i = 0; i < entry_count; ++i) {
    if (entries[i].type != MULTIBOOT_MEMORY_AVAILABLE) continue;

    /* number of frames in this entry */
    n_frames = entries[i].len / 4096;
    avail_frames += n_frames;

#ifdef DEBUG
    printf("mmap_entry: 0x%16llx\tlen=%12llu\t%d frames (%d blocks + %d)\n", entries[i].addr, entries[i].len, n_frames,
           n_frames / 32, n_frames % 32);
#endif

    /* the bitmap is an array of blocks, each holding 32 (2^5) values */
    table_idx = (entries[i].addr >> 17); /* get the upper 15 bits */

    while (n_frames != 0) {
      if (n_frames >= 32) {
        bitmap[table_idx] = 0xffffffff;
        table_idx++;
        n_frames -= 32;
      }
      else {
        unsigned block = bitmap[table_idx];
        for (l = 0; l < n_frames; ++l) block |= (1 << l);
        bitmap[table_idx] = block;
        n_frames = 0;
      }
    }
  }

  return avail_frames;
}