/*
 * cc -I/usr/include/drm -lpciaccess -ldrm drift_control.c -o drift_control
 *
 * drift_control:
 *   - without args simply monitors drift of frame rate against loop cycle
 *   - with args adjusts frame rate to loop cycle frequency +-0.001441383Hz
 *
 * prerequisites:
 *   - Xserver must be running to initialize a proper 50Hz modeline
 *   - XV extension may NOT be actively used when using this tool
.*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
/*#define NO_LIBPCIACCESS*/
#ifdef NO_LIBPCIACCESS
#include <fcntl.h>
#include <sys/mman.h>
#define IO_PHYS_BASE 0xfdef0000
#define IO_SIZE 0x00010000
#else /**/
#include <pciaccess.h>
#define IO_BAR 2
#endif /**/
#include <xf86drm.h>
#include <radeon_drm.h>
#define STANDALONE

/* --- 8< --- */
/*
 * field cycle duration in usecs for PAL
 */
#define SYF_PAL_FIELD_CYCLE 20000

/*
 * frame cycle duration in usecs for PAL
 */
#define SYF_PAL_FRAME_CYCLE (SYF_PAL_FIELD_CYCLE << 1)

/*
 * dependent on interlaced (progressive) input frame rate 25 (50) fps
 * we set this to 0 (1). other params should adjust accordingly.
 */
#define SYF_INPUT_DOUBLE_RATE 0

/* NOT CURRENTLY USED
 * prefilter to prevent stray updates.
 * our software PLL will not try to lock for these.
 */
#define SYF_CATCH_RANGE (SYF_PAL_FRAME_CYCLE - 500 >> SYF_INPUT_DOUBLE_RATE)

/*
 * we average 25 (50) frames to yield a cycle time of
 * about one second for analysis of frame rate data.
 * this serves as frequency divider for our software PLL.
 */
#define SYF_PLL_DIVIDER (25 << SYF_INPUT_DOUBLE_RATE)

/*
 * input frame cycle duration in usecs for 25 (50) fps
 */
#define SYF_FRAME_CYCLE (SYF_PAL_FRAME_CYCLE >> SYF_INPUT_DOUBLE_RATE)

/* NOT CURRENTLY USED
 * we slightly displace sync point from center position. this makes the
 * system less susceptible to temporary field delivery irregularities.
 */
#define SYF_SYNC_POINT_OFFSET 0

/*
 * offset in usecs from double buffer switch where we preferredly try to place double
 * buffer updates. 
 * this minimizes sensivity to jitter of our software PLL phase comparator.
 */
#define SYF_SYNC_POINT (SYF_PAL_FIELD_CYCLE - SYF_SYNC_POINT_OFFSET)

/*
 * one trim increment compensates drift speed for about 29usec/sec.
 * this represents resolution of our VCO input. 
 */
#define SYF_MIN_STEP_USEC 100

/* NOT CURRENTLY USED
 * factor weighting drift against sync point displacement
 * when calculating overall compensation 
 */
#define SYF_DISP_DRIFT_FACTOR 1

/* NOT CURRENTLY USED
 * to allow for smooth adaption also on slower devices we delimit maximum
 * trim change per step size.
 * this implements some kind of low pass filter for our VCO input.
 */
#define SYF_MAX_TRIM_REL 1000

/*
 * maximum absolute trim values allowed due to current
 * hardware/driver contraints.
 * this delimits 'maximum voltage' controlling our VCO.
 * currently allowed range is defined symmetrically around the center voltage.
 */
#define SYF_MAX_TRIM_ABS 37

#ifdef STANDALONE

#define RADEON_CRTC_STATUS                  0x005c
#define RADEON_CRTC_VLINE_CRNT_VLINE        0x0210
#define RADEON_CRTC_CURRENT_FIELD           (1 << 3)
#define RADEON_CRTC_CRNT_VLINE_MASK         (0x7ff << 16)
#define RADEON_CRTC_CRNT_VLINE_SHIFT        16

#define ErrorF printf
#define FatalError ErrorF
#define B(a) (*(argv + (a)) ? *(argv + (a)) : 0)
#define INREG(reg) *(volatile unsigned *)(vptr + (reg))
#define OUTREG(reg, val) INREG(reg) = (val)
#define min(a, b) ((a) <= (b) ? (a) : (b))
#define max(a, b) ((a) >= (b) ? (a) : (b))
#define abs(a) ((a) >= 0 ? (a) : -(a))

#define VSF_SUB(a, b, c) \
    if ((a).tv_usec < (b).tv_usec) { \
        (c).tv_sec = (a).tv_sec - (b).tv_sec - 1; \
        (c).tv_usec = (a).tv_usec - (b).tv_usec + 1000000; \
    } else { \
        (c).tv_sec = (a).tv_sec - (b).tv_sec; \
        (c).tv_usec = (a).tv_usec - (b).tv_usec; \
    }

#define VSF_TV2USEC(a, b) \
    (b) = (a).tv_sec * 1000000 + (a).tv_usec;

struct _info_s {
    int SYF_debug;
    int drmFD;
} info = {
    801,
    0,
}, *pinfo = &info;

#else

#define B(a) (a)
#define pI830 pinfo

#endif

#include <time.h>
#include <errno.h>
#include <unistd.h>
#include <sys/shm.h>

#define SHMKEY 0x0815
#define LFIELD 312
#define LFRAME 625
#define TSHIFT 6

typedef struct _syf_t {
    int log;
    int cnt; 
    int trim; 
    int drift; 
    int spoint;
    int *shm;

    /* HW specific */
    int triml;
} syf_t;

extern void log_graph(int, int);

void
log_graph(val, symb)
{
    static char headr[] = "|<- -20ms                               0                              +20ms ->|";
    static char meter[sizeof(headr)];

    if (!symb || symb == 1) {
        time_t t; struct tm *tm;

        time(&t); tm = localtime(&t); 
        ErrorF("%02d:%02d:%02d %s", tm->tm_hour, tm->tm_min, tm->tm_sec, symb ? meter : headr);
        memset(meter, '-', sizeof(headr) - 1);
        return;
    }
    val = (SYF_SYNC_POINT + val) / 500;
    val = min(val, (int)sizeof(headr) - 2);
    val = max(val, 0);
    if (symb == ':') {
        meter[val] = meter[val] == '%' ? '#' : symb;
    } else if (symb == '*') {
        meter[val] = meter[val] == '%' ? '1' :
                     meter[val] == ':' ? '2' :
                     meter[val] == '#' ? '3' : symb;
    } else {
        meter[val] = symb;
    }
}
/* --- 8< --- */

main(argc, argv)
    char **argv;
{
    static syf_t syf;
    static int vbl_usec_prev = ~0;
    int stat, stat_1, vline, vline_1, vbl_usec;

    static usleepv = SYF_FRAME_CYCLE;
    static struct timeval tv_call_prev;
    struct timeval tv_tmp, tv_call;
    void *vptr;
    int usecs;
#ifdef NO_LIBPCIACCESS
    int fd;

    if ((fd = open("/dev/mem", O_RDWR | O_SYNC)) == -1) {
        ErrorF("open: %s\n", strerror(errno)); exit(-1);
    }
    vptr = mmap(0, IO_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, IO_PHYS_BASE);
    if ((long)vptr == -1) {
        ErrorF("mmap: %s\n", strerror(errno)); exit(-1);
    }
#else
    struct pci_device *dev;
    struct pci_id_match match;
    struct pci_device_iterator *iter;

    if (pci_system_init()) {
        ErrorF("pci_system_init: %s\n", strerror(errno)); exit(-1);
    }
    match.vendor_id = 0x1002;
    match.device_id = PCI_MATCH_ANY;
    match.subvendor_id = PCI_MATCH_ANY;
    match.subdevice_id = PCI_MATCH_ANY;
    match.device_class = 0;
    match.device_class_mask = 0;
    if (!(iter = pci_id_match_iterator_create(&match))) {
        ErrorF("pci_id_match_iterator_create: %s\n", strerror(errno)); exit(-1);
    }
    while (1) {
        if (!(dev = pci_device_next(iter))) {
            ErrorF("device not found\n"); exit(-1);
        }
        if (dev->device_id == 0x5834
         || dev->device_id == 0x5b60
         || dev->device_id == 0x5964
                                        ) {
            break;
        }
    }
    if (pci_device_probe(dev)) {
        ErrorF("pci_device_probe: %s\n", strerror(errno)); exit(-1);
    }
    ErrorF("phys_base: 0x%08llx phys_len: 0x%08llx\n", 
                                        dev->regions[IO_BAR].base_addr, 
                                        dev->regions[IO_BAR].size);
    if (pci_device_map_range(dev, dev->regions[IO_BAR].base_addr, 
                                dev->regions[IO_BAR].size, 1, &vptr)) {
        ErrorF("pci_device_map_range: %s\n", strerror(errno)); exit(-1);
    }
#endif
    if ((pinfo->drmFD = drmOpen("radeon", 0)) < 0) {
        ErrorF("drmOpen: %s\n", strerror(errno)); exit(-1);
    }
    while (1) {
        gettimeofday(&tv_call, 0);
        if (tv_call_prev.tv_sec) {
            VSF_SUB(tv_call, tv_call_prev, tv_tmp);
            VSF_TV2USEC(tv_tmp, usecs);
            usleepv += SYF_FRAME_CYCLE - usecs;
        }
        tv_call_prev = tv_call;

/* --- 8< --- */
    if (vbl_usec_prev == ~0) {
        int shmid;
        drm_radeon_setparam_t vbl_activate;

        vbl_activate.param = RADEON_SETPARAM_VBLANK_CRTC;
        vbl_activate.value = DRM_RADEON_VBLANK_CRTC1;
        if (ioctl(pinfo->drmFD, DRM_IOCTL_RADEON_SETPARAM, &vbl_activate)) {
            FatalError("DRM_IOCTL_RADEON_SETPARAM: %s\n", strerror(errno));
        }
        if ((shmid = shmget(SHMKEY, sizeof(*syf.shm), IPC_CREAT | 0666)) < 0) {
            FatalError("shmget: %s\n", strerror(errno));
        }
        if ((int)(syf.shm = shmat(shmid, 0, 0)) == -1) {
            FatalError("shmat: %s\n", strerror(errno));
        }
    }

    /*
     * the next few lines implement a simple glitch detection/correction
     */
    stat = INREG(RADEON_CRTC_STATUS);
    vline = INREG(RADEON_CRTC_VLINE_CRNT_VLINE);
    stat_1 = INREG(RADEON_CRTC_STATUS);
    vline_1 = INREG(RADEON_CRTC_VLINE_CRNT_VLINE);
    if ((stat ^ stat_1) & RADEON_CRTC_CURRENT_FIELD 
     && (vline & RADEON_CRTC_CRNT_VLINE_MASK) == (vline_1 & RADEON_CRTC_CRNT_VLINE_MASK)) {
        stat ^= ~0;
    }

    /*
     * CRTC_STATUS 0x00000002 CRTC_VLINE 0x000003ff   0   0     0
     * CRTC_STATUS 0x00000006 CRTC_VLINE 0x000203ff   2   1    64
     * [...]
     * CRTC_STATUS 0x00000006 CRTC_VLINE 0x026e03ff 622 311 19904
     * CRTC_STATUS 0x0000000a CRTC_VLINE 0x000003ff   0 312 19968
     * [...]
     * CRTC_STATUS 0x0000000a CRTC_VLINE 0x027003ff 624 624 39936
     * CRTC_STATUS 0x00000002 CRTC_VLINE 0x000003ff   0   0     0
     * ________                 _______________
     *         |_______________|               |____ field status
     *         0              622
     */
    stat = !!(stat & RADEON_CRTC_CURRENT_FIELD);
    vline = (vline & RADEON_CRTC_CRNT_VLINE_MASK) >> RADEON_CRTC_CRNT_VLINE_SHIFT >> 1;
    vbl_usec = (stat * LFIELD + vline) % LFRAME << TSHIFT;

    if (vbl_usec_prev == ~0) {
        vbl_usec_prev = vbl_usec;
        log_graph(0, 0);
        ErrorF("      R\n");
#ifdef STANDALONE
        goto main_loop_end;
#else
        return;
#endif
    }
    if (pinfo->SYF_debug) {
        if (abs(vbl_usec - vbl_usec_prev) > pinfo->SYF_debug) {
            log_graph(vbl_usec - vbl_usec_prev, '%'); ++syf.log;
        }
        if (abs(vbl_usec - SYF_SYNC_POINT) > pinfo->SYF_debug) {
            log_graph(vbl_usec - SYF_SYNC_POINT, ':'); ++syf.log;
        }
    }

#ifndef STANDALONE

    /*
     * we must delay the next double buffer update
     * until even field has been processed.
     */
    if (SYF_PAL_FIELD_CYCLE - vbl_usec > 0) {
        usleep(SYF_PAL_FIELD_CYCLE - vbl_usec);
    }
#endif
    syf.spoint += vbl_usec - SYF_SYNC_POINT;
    syf.drift += vbl_usec - vbl_usec_prev;
    vbl_usec_prev = vbl_usec;
    ++syf.cnt;
    if (!(syf.cnt % SYF_PLL_DIVIDER)) {
        syf.spoint /= SYF_PLL_DIVIDER;
        syf.trim = (syf.drift + syf.spoint / SYF_DISP_DRIFT_FACTOR) / SYF_MIN_STEP_USEC;
        syf.trim = max(syf.trim, -SYF_MAX_TRIM_REL);
        syf.trim = min(syf.trim,  SYF_MAX_TRIM_REL);

        if (pinfo->SYF_debug & 1 || syf.log) {
            log_graph(0, '+');
            log_graph(syf.spoint, '|');
            log_graph(syf.drift, '*');
            log_graph(0, 1);
            ErrorF("%7d %7d [%3d%+4d]\n",
                syf.drift, syf.spoint, syf.triml, syf.trim);
        }
        *syf.shm = syf.spoint;
        syf.spoint = 0;
        syf.drift = 0;
        syf.log = 0;
    }
    if (B(1) && syf.trim) {
        int t = syf.triml;

        if (syf.trim > 0) {
            t = min(t + 1,  SYF_MAX_TRIM_ABS);
            --syf.trim;
        } else {
            t = max(t - 1, -SYF_MAX_TRIM_ABS);
            ++syf.trim;
        }
        if (syf.triml != t) {
            syf.triml = t;
            t = VSF_SET_TRIM | VSF_TEMPLATE | t & 0xff;
            if (ioctl(pinfo->drmFD, DRM_IOCTL_RADEON_SYNCF, &t)) {
                FatalError("DRM_IOCTL_RADEON_SYNCF: %s\n", strerror(errno));
            }
        }
    }
/* --- 8< --- */

main_loop_end:
        usleep(usleepv);
    }
}

