#include "clock.h"
#include "mbox.h"
#include "uart.h"

unsigned int getclock(uint32_t clkid, uint8_t max) {
  mbox[0] = 8 * 4;
  mbox[1] = MBOX_REQUEST;
  mbox[2] = max > 0 ? MBOX_TAG_GETMAXCLK : MBOX_TAG_GETCLK;
  mbox[3] = 8;
  mbox[4] = 0;
  mbox[5] = clkid;
  mbox[6] = 0;
  mbox[7] = MBOX_TAG_LAST;

  if (mbox_call(MBOX_CH_PROP, mbox)) {
    puts("Failed getting clock\n");
    return 0;
  } else {
    return mbox[6];
  }
}

int setmaxclock(uint32_t clkid)
{
  uint32_t maxfreq = getclock(clkid, 1);
  if (maxfreq != 0) {
    return setclock(clkid, maxfreq);
  }

  return -1;
}

int setclock(uint32_t clkid, uint32_t freq)
{
  mbox[0] = 9 * 4;
  mbox[1] = MBOX_REQUEST;
  mbox[2] = MBOX_TAG_SETCLK;
  mbox[3] = 12;
  mbox[4] = 0;
  mbox[5] = clkid;
  mbox[6] = freq;
  mbox[7] = 0;
  mbox[8] = MBOX_TAG_LAST;

  if (mbox_call(MBOX_CH_PROP, mbox)) {
    puts("Failed setting clock\n");
    return -1;
  }

  return 0;
}