52 lines
928 B
C
52 lines
928 B
C
|
#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;
|
||
|
|
}
|