#include <stdbool.h>
#include <stdint.h>
#include "drive.h"

void memcpy(void *to, const void *from, uint32_t n) {
  uint32_t *tp = to;
  const uint32_t *fp = from;
  while (n >= 4) {
    *(tp++) = *(fp++);
    n -= 4;
  }
  uint8_t *tpp = (uint8_t *)tp, *fpp = (uint8_t *)fp;
  while (n--)
    *(tpp++) = *(fpp++);
}

void fmcpy(void *to, const struct drive *d, file_id_t f, uint32_t from, uint32_t n) {
  uint8_t buf[512];
  d->load_sector(d, f, from >> 9, buf);
  uint16_t from_low = from & 511;
  if (!((from_low + n) & ~511)) {
    memcpy(to, buf + from_low, n);
    return;
  }

  uint32_t i = 512 - from_low;
  memcpy(to, buf + from_low, i);

  n -= i;
  uint32_t fsi = (from + i) >> 9;

  while (n & ~511) {
    d->load_sector(d, f, fsi, buf);
    memcpy(to + i, buf, 512);
    i += 512;
    n -= 512;
    ++fsi;
  }

  d->load_sector(d, f, fsi, buf);
  memcpy(to + i, buf, n);
}