#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <sys/mman.h> // for mmap()
#include <fcntl.h>    // for open()

const int FPGA_CLK_CTRL[4] = { 0x170, 0x180, 0x190, 0x1A0 };
#define PL_FREQ 1000


int main (int argc, char **argv)
{
    int fd = open("/dev/mem", O_RDWR);
    off_t  slcr_base   = 0xF8000000;
    size_t slcr_length = 0x00001000;

    uint32_t *slcr_ptr = (uint32_t *)mmap(NULL, slcr_length,
            PROT_READ | PROT_WRITE,
            MAP_SHARED,
            fd, slcr_base);

    if (slcr_ptr == MAP_FAILED)
    {
        printf("ERROR: mmap call failed.\n");
        return 1;
    }

    if (argc != 4)
    {
        printf("ERROR: incorrect number of arguments.\n");
        printf("USAGE: %s <clock num> <div0> <div1>.\n", argv[0]);
        return 1;
    }

    long int addr  = FPGA_CLK_CTRL[strtol(argv[1], NULL,  0)];
    long int div0, div1;
    div0 = strtol(argv[2], NULL, 0);
    div1 = strtol(argv[3], NULL, 0);

    uint32_t value = 0;
    //This is used to read clock values
    //    v = slcr_ptr[FPGA_CLK_CTRL[i]/4];
    //    divisor1 = (v >> 20) & 0x3F;
    //    divisor0 = (v >> 8)  & 0x3F;
    //    srcsel   = (v >> 4)  & 0x03;
    value = (div1 << 20) | (div0 << 8) | ((slcr_ptr[addr] >> 4) & 0x03);

    if (addr < 0 || addr > 0x1000 || (addr % 4 != 0))
    {
        printf("ERROR: invalid address 0x%016lX.\n",addr);
        return 1;
    }

    printf("Setting slcr address 0x%08X to value 0x%08X\n",(uint32_t)addr,(uint32_t)value);
    slcr_ptr[addr/4] = (uint32_t) value;

    munmap(slcr_ptr,slcr_length);
    return 0;
}

