/*
 * cc -I/usr/include/drm -lpciaccess -ldrm drift_control810.c -o drift_control810
 *
 * drift_control:
 *   - without args simply monitors drift of frame rate against loop cycle
 *   - otherwise 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
 */
#include <stdio.h>
#include <errno.h>
#include <stdlib.h>
#include <string.h>
#include <pciaccess.h>
#include <xf86drm.h>
#include <i810_drm.h>

#define min(a, b) ((a) <= (b) ? (a) : (b))
#define max(a, b) ((a) >= (b) ? (a) : (b))

#define STANDALONE

/* --- 8< --- */

/*
 * dependent on the frame rate 25 (50) we set this to 0 (1).
 * other params should adjust automatically.
 */
#define SYF_DOUBLE_RATE 1

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

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

/*
 * frame cycle duration in usecs for 25 (50) fps
 */
#define SYF_FRAME_CYCLE (40000 >> SYF_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.
 */
#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)
#else
#define B(a) (a)
#endif

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 int cnt;
    static int sum;
    static int trim;
    static int ds_usecs;
    static int sync_point_disp;
    static struct timeval skew_prev;
    static drm_i810_syncf_t syncf_prev;

    struct timeval skew;
    struct timeval drift_speed;
    struct drm_modeset_ctl vbl_activate;
    drm_i810_syncf_t syncf;
    int tmp;

    struct pci_device *dev;
    struct pci_id_match match;
    struct pci_device_iterator *iter;
    int intr;

    if ((fd = drmOpen("i810", 0)) < 0) {
	ErrorF("drmOpen: %s\n", strerror(errno));
    }
    if (pci_system_init()) {
        ErrorF("pci_system_init: %s\n", strerror(errno)); 
    }
    match.vendor_id = 0x8086;
    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)); 
    }
    while (1) {
        if (!(dev = pci_device_next(iter))) {
            ErrorF("device not found\n"); 
        }
        if (dev->device_id == 0x7125
#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)); 
    }
    if (drmCtlInstHandler(fd, intr) < 0) {
	ErrorF("drmCtlInstHandler: %s\n", strerror(errno)); 
    }
    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));
    }
    while (1) {
    	static usleepv = SYF_FRAME_CYCLE;

/* --- 8< --- */
        syncf.trim = 0;
        if (ioctl(fd, DRM_IOCTL_I810_SYNCF, &syncf)) {
            ErrorF("DRM_IOCTL_I810_SYNCF: %s\n", strerror(errno));
        }
#if 0
	ErrorF("%10d %10d %10d %10d %10d\n",
	    (int)syncf.tv_now.tv_sec,
	    (int)syncf.tv_now.tv_usec,
	    (int)syncf.tv_vbl.tv_sec,
	    (int)syncf.tv_vbl.tv_usec,
	    syncf.trim);
#endif
	VSF_SUB(syncf.tv_now, syncf_prev.tv_now, drift_speed);
	VSF_TV2USEC(drift_speed, tmp);
#ifdef STANDALONE
	if (syncf_prev.tv_now.tv_sec) {
	    usleepv += SYF_FRAME_CYCLE - tmp;
	}
#endif
	syncf_prev = syncf;
	if (tmp < SYF_FRAME_CYCLE - SYF_CATCH_RANGE || tmp > SYF_FRAME_CYCLE + SYF_CATCH_RANGE) {

	    /*
	     * toss stray intervals and reset
	     */
	    cnt = 0;
	    sum = 0;
	    trim = 0;
	    ds_usecs = 0;
	    sync_point_disp = 0;
	    skew_prev.tv_sec = ~0;
            OUT_GRAPHIC(0, 0);
	    ErrorF("      R               %11d\n", tmp);
#ifdef STANDALONE
	    goto main_loop_end;
#else
	    return;
#endif
	}
	if (tmp < SYF_FRAME_CYCLE - SYF_STRAY_WARN || tmp > SYF_FRAME_CYCLE + SYF_STRAY_WARN) {
            OUT_GRAPHIC(0, 0);
	    ErrorF("      W               %11d\n", tmp);
	}
#ifndef STANDALONE
{
        int vline = (INREG(I810_CRTC_VLINE_CRNT_VLINE) & I810_CRTC_CRNT_VLINE_MASK) >> I810_CRTC_CRNT_VLINE_SHIFT;
        int stat = INREG(I810_CRTC_STATUS) & I810_CRTC_CURRENT_FIELD;
        if (!stat) {
            usleep(64 * (311 - (vline >> 1) + 1));
        }
}
#endif
	VSF_SUB(syncf.tv_now, syncf.tv_vbl, skew)
	if (skew_prev.tv_sec != ~0) {
	    sum += tmp;
	    sync_point_disp += skew.tv_usec - SYF_SYNC_POINT;
	    VSF_SUB(skew, skew_prev, drift_speed);
	    VSF_TV2USEC(drift_speed, tmp);
	    ds_usecs += tmp;
	    ++cnt;
	}
	if (skew_prev.tv_sec != ~0 && !(cnt % SYF_PLL_DIVIDER)) {
	    sync_point_disp /= SYF_PLL_DIVIDER;
            OUT_GRAPHIC(0, '+');
            OUT_GRAPHIC(sync_point_disp, '|');
            OUT_GRAPHIC(ds_usecs, '*');
            OUT_GRAPHIC(0, 1);
	  
	    trim = (ds_usecs + sync_point_disp / SYF_DISP_DRIFT_FACTOR) / SYF_MIN_STEP_USEC;
	    trim = max(trim, -SYF_MAX_TRIM_REL);
	    trim = min(trim,  SYF_MAX_TRIM_REL);
	    ERRORF("%7d %7d [%3d%+4d] %7d\n", ds_usecs, sync_point_disp, (char)(syncf.trim & 0xff), trim, sum);
	   
	    sync_point_disp = 0;
	    ds_usecs = 0;
	    sum = 0;
	}
        if (B(1) && trim) {
            tmp = (char)(syncf.trim & 0xff);
            if (trim > 0) {
#if 0
                tmp = min(tmp + 1,  SYF_MAX_TRIM_ABS);
#else
                tmp = min(tmp + 1,  30);
#endif

                --trim;
            } else {
#if 0
                tmp = max(tmp - 1, -SYF_MAX_TRIM_ABS);
#else
                tmp = max(tmp - 1, -0);
#endif

                ++trim;
            }
            syncf.trim = VSF_SET_TRIM | VSF_TEMPLATE | tmp & 0xff;
  	    /*ErrorF("*trim:                   0x%08x\n", syncf.trim);*/
            if (ioctl(fd, DRM_IOCTL_I810_SYNCF, &syncf)) {
                ErrorF("DRM_IOCTL_I810_SYNCF: %s\n", strerror(errno));
            }
	}
        skew_prev = skew;
/* --- 8< --- */

main_loop_end:
        usleep(usleepv);
    }
}

