50 lines
1.2 KiB
C
50 lines
1.2 KiB
C
|
#include <fcntl.h>
|
||
|
|
#include <sys/mman.h>
|
||
|
|
#include <linux/fb.h>
|
||
|
|
#include <unistd.h>
|
||
|
|
#include <stropts.h>
|
||
|
|
#include <stdio.h>
|
||
|
|
#define SYS_CLK_FREQ 7372800
|
||
|
|
|
||
|
|
volatile unsigned long *memregs32;
|
||
|
|
volatile unsigned short *memregs16;
|
||
|
|
int memfd;
|
||
|
|
|
||
|
|
unsigned char initphys (void)
|
||
|
|
{
|
||
|
|
memfd = open("/dev/mem", O_RDWR);
|
||
|
|
if (memfd == -1)
|
||
|
|
{
|
||
|
|
printf ("Open failed for /dev/mem\n");
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
memregs32 = (unsigned long*) mmap(0, 0x10000, PROT_READ|PROT_WRITE, MAP_SHARED, memfd, 0xc0000000);
|
||
|
|
|
||
|
|
if (memregs32 == (unsigned long *)0xFFFFFFFF) return 0;
|
||
|
|
memregs16 = (unsigned short *)memregs32;
|
||
|
|
|
||
|
|
return 1;
|
||
|
|
}
|
||
|
|
|
||
|
|
void SetClock(unsigned int MHZ)
|
||
|
|
{
|
||
|
|
unsigned int v;
|
||
|
|
unsigned int mdiv,pdiv = 3,scale = 0;
|
||
|
|
MHZ*=1000000;
|
||
|
|
mdiv=(MHZ*pdiv)/SYS_CLK_FREQ;
|
||
|
|
|
||
|
|
if (!initphys()) return;
|
||
|
|
|
||
|
|
mdiv=((mdiv-8)<<8) & 0xff00;
|
||
|
|
pdiv=((pdiv-2)<<2) & 0xfc;
|
||
|
|
scale&=3;
|
||
|
|
v = mdiv | pdiv | scale;
|
||
|
|
|
||
|
|
unsigned int l = memregs32[0x808>>2]; // Get interupt flags
|
||
|
|
memregs32[0x808>>2] = 0xFF8FFFE7; //Turn off interrupts
|
||
|
|
memregs16[0x910>>1] = v; //Set frequentie
|
||
|
|
while(memregs16[0x0902>>1] & 1); //Wait for the frequentie to be ajused
|
||
|
|
memregs32[0x808>>2] = l; //Turn on interrupts
|
||
|
|
}
|