/*
 * 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 <errno.h>
#include <stdlib.h>
#include <string.h>
#include <pciaccess.h>
#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

/*
 * prefilter to prevent stray updates.
 * our software PLL will not try to lock for these.
 */
#define SYF_CATCH_RANGE (18000 >> SYF_INPUT_DOUBLE_RATE)

/*
 * updates outside time window defined by this
 * value spawn warnings when in debug mode
 */
#define SYF_WARN_RANGE (15000 >> 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)

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

/*
 * 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

#define DEBUG
#ifdef DEBUG
#define ERRORF ErrorF
#else
#define ERRORF(...)
#endif

#define USE_METER
#ifdef USE_METER
#define OUT_GRAPHIC meter_out
#else
#define OUT_GRAPHIC(...)
#endif

#ifdef STANDALONE
#define ErrorF printf
#define B(a) (*(argv + (a)) ? *(argv + (a)) : 0)
#define min(a, b) ((a) <= (b) ? (a) : (b))
#define max(a, b) ((a) >= (b) ? (a) : (b))
#define abs(a) ((a) >= 0 ? (a) : -(a))
#else
#define B(a) (a)
#endif

typedef struct _syf {
    int cnt; 
    int tsum; 
    int trim; 
    int drift; 
    int spoint;
} syf_t;

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

    if (!symb || symb == 1) {
	if (!symb) ErrorF("%s", headr);
        if (symb == 1) ErrorF("%s", meter);
        memset(meter, '-', 80);
        return;
    }
    val /= 500;
    val = min(val,  39);
    val = max(val, -40);
    meter[40 + val] = symb;
}
/* --- 8< --- */

main(argc, argv)
    char **argv;
{
    static int fd;
    static syf_t syf, syf_clear;
    static struct timeval skew2vbl_prev;
    static drm_radeon_syncf_t syncf_prev;

    drm_radeon_syncf_t syncf;
    struct timeval skew2vbl;
    struct timeval tv_usecs;
    int usecs;
    struct pci_device *dev;
    struct pci_id_match match;
    struct pci_device_iterator *iter;
    struct drm_modeset_ctl vbl_activate;
    int intr;

    if ((fd = drmOpen("radeon", 0)) < 0) {
	ErrorF("drmOpen: %s\n", strerror(errno)); exit(-1);
    }
    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"); 
        }
        if (dev->device_id == 0x5834
#if 0
         || dev->device_id == 0x5d44 
#endif
					) {
            break;
        }
    }
    if ((intr = drmGetInterruptFromBusID(fd, dev->bus, dev->dev, dev->func)) < 0) {
//	ErrorF("drmGetInterruptFromBusID: %s\n", strerror(errno)); exit(-1);
    }
    if (drmCtlInstHandler(fd, intr) < 0) {
//	ErrorF("drmCtlInstHandler: %s\n", strerror(errno)); exit(-1);
    }
    vbl_activate.crtc = VSF_CRTC;
    vbl_activate.cmd = _DRM_PRE_MODESET;
    if (ioctl(fd, DRM_IOCTL_MODESET_CTL, &vbl_activate)) {
        ErrorF("DRM_IOCTL_MODESET_CTL: %s\n", strerror(errno)); exit(-1);
    }
    while (1) {
    	static usleepv = SYF_FRAME_CYCLE;

/* --- 8< --- */
        syncf.trim = 0;
        if (ioctl(fd, DRM_IOCTL_RADEON_SYNCF, &syncf)) {
            ErrorF("DRM_IOCTL_RADEON_SYNCF: %s\n", strerror(errno)); exit(-1);
        }
	VSF_SUB(syncf.tv_now, syncf_prev.tv_now, tv_usecs);
	VSF_TV2USEC(tv_usecs, usecs);
#ifdef STANDALONE
	if (syncf_prev.tv_now.tv_sec) {
	    usleepv += SYF_FRAME_CYCLE - usecs;
	}
#endif
	syncf_prev = syncf;
	if (abs(usecs - SYF_FRAME_CYCLE) > SYF_CATCH_RANGE) {

	    /*
	     * toss stray intervals and reset
	     */
	    syf = syf_clear;
	    skew2vbl_prev.tv_sec = ~0;
            OUT_GRAPHIC(0, 0);
	    ErrorF("      R               %11d\n", usecs);
#ifdef STANDALONE
	    goto main_loop_end;
#else
	    return;
#endif
	}
	if (abs(usecs - SYF_FRAME_CYCLE) > SYF_WARN_RANGE) {
            OUT_GRAPHIC(0, 0);
	    ErrorF("      W               %11d\n", usecs);
	}

#ifndef STANDALONE
{
        /*
         * we must delay the next double buffer update
         * until even field has been processed. 
         */
        int vline = (INREG(RADEON_CRTC_VLINE_CRNT_VLINE) & RADEON_CRTC_CRNT_VLINE_MASK) >> RADEON_CRTC_CRNT_VLINE_SHIFT;
        int stat = INREG(RADEON_CRTC_STATUS) & RADEON_CRTC_CURRENT_FIELD;
        if (!stat) {
            usleep(64 * (311 - (vline >> 1) + 1));
        }
}
#endif
	VSF_SUB(syncf.tv_now, syncf.tv_vbl, skew2vbl)
	if (skew2vbl_prev.tv_sec != ~0) {
	    syf.tsum += usecs;
	    syf.spoint += skew2vbl.tv_usec - SYF_SYNC_POINT;
	    VSF_SUB(skew2vbl, skew2vbl_prev, tv_usecs);
	    VSF_TV2USEC(tv_usecs, usecs);
	    syf.drift += usecs;
	    ++syf.cnt;
	}
	if (skew2vbl_prev.tv_sec != ~0 && !(syf.cnt % SYF_PLL_DIVIDER)) {
	    syf.spoint /= SYF_PLL_DIVIDER;
            OUT_GRAPHIC(0, '+');
            OUT_GRAPHIC(syf.spoint, '|');
            OUT_GRAPHIC(syf.drift, '*');
            OUT_GRAPHIC(0, 1);

	    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);
	    ERRORF("%7d %7d [%3d%+4d] %7d\n", syf.drift, syf.spoint, 
	    			(char)(syncf.trim & 0xff), syf.trim, syf.tsum);
	    syf.spoint = 0;
	    syf.drift = 0;
	    syf.tsum = 0;
	}
        if (B(1) && syf.trim) {
            int t = (char)(syncf.trim & 0xff);

            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;
            }
            syncf.trim = VSF_SET_TRIM | VSF_TEMPLATE | t & 0xff;
            if (ioctl(fd, DRM_IOCTL_RADEON_SYNCF, &syncf)) {
                ErrorF("DRM_IOCTL_RADEON_SYNCF: %s\n", strerror(errno)); exit(-1);
            }
	}
        skew2vbl_prev = skew2vbl;
/* --- 8< --- */

main_loop_end:
        usleep(usleepv);
    }
}

