#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/mman.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <string.h>

#define MAP_SIZE 4096Ul
#define MAP_MASK (MAP_SIZE - 1)

#define MAP_PMC 0xfffffc00 /* Power Manager Controller*/
#define PMC_SCER 0x00 /* PMC System Clock Enable Register */
#define CKGR_PLLBR 0x2c /* PLL B Register */
#define PMC_PCK1 0x44 /* Programmable Clock 1 Register */
#define PMC_SCSR 0x08 /* System Clock Status Register */
#define PMC_SR 0x68 /* PMC status register */


int Set_PLLB_Clock()
{
int pllb;
void *map;
unsigned long read, write;

if ((pllb = open("/dev/mem", O_RDWR | O_SYNC)) == -1)
{
fprintf(stderr, "Cannot open /dev/mem.\n");
exit(EXIT_FAILURE);
}
fprintf(stderr, "/dev/mem opened.\n");

map = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, pllb, MAP_PMC & ~MAP_MASK);

if (map == (void *) -1)
{
fprintf(stderr, "Memory mapping error.\n");
exit(EXIT_FAILURE);
}
fprintf(stderr, "Memory block mapped at address %p.\n", map);
map+=(MAP_PMC & MAP_MASK);
fprintf(stderr, "Target address mapped 0x%08x-->0x%08x\n",(int) MAP_PMC,(int)map);

//PMC_SCER - 0000 0000 0000 0000 0000 0011 1100 0001 - 0x3C1
//PCK1 - 1, PCK0 - 1, UDP - 1, UHP - 1
write = 0x3C0;
*(unsigned long*)(map+PMC_SCER) = write;
printf("PMC System Clock Enable Register PMC_SCER - Written 0x%X\n", write);

//USBDIV - 01, MULB - 00000011000, OUTB - 10, PLLBCOUNT - 111110, DIVB - 00000011
//CKGR_PLLBR=0001 0000 0001 1000 1011 1110 0000 0011 - 0x1018BE03
write = 0x1018BE03;
*(unsigned long*)(map+CKGR_PLLBR) = write;
read = *(unsigned long*)(map+CKGR_PLLBR);
printf("PLL B Register CKGR_PLLBR - Written 0x%X - Read 0x%X\n", write, read);

//PRESS - 100, CSS - 11
//PMC_PCK = 0000 0000 0000 0000 0000 0000 00010011 - 0x13
write = 0x13;
*(unsigned long*)(map+PMC_PCK1) = write;
read = *(unsigned long*)(map+PMC_PCK1);
printf("Programmable Clock 1 Register PMC_PCK1 - Written 0x%X - Read 0x%X\n", write, read);

//Регистры статуса
read = *(unsigned long*)(map+PMC_SCSR);
printf("System Clock Status Register PMC_SCSR; readback 0x%X\n", read);
read = *(unsigned long*)(map+PMC_SR);
printf("PMC status register PMC_SR; readback 0x%X\n", read);

if (close (pllb) != 0)
{
fprintf (stderr, "Cannon close /dev/mem (descriptor=%d)\n", pllb);
exit(1);
}
close(pllb);
}

int main (int argc, char *argv[])
{
Set_PLLB_Clock();
}