/*
 * cc -lpciaccess drift_control945.c -o drift_control945
 *
 * drift_control:
 *   - without args simply monitors drift of frame rate against loop cycle
 *   - with args adjusts frame rate to loop cycle frequency 
 *
 * 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>
#ifdef NO_LIBPCIACCESS
#include <fcntl.h>
#include <sys/mman.h>
#define I945G_IO_PHYS_BASE 0x50200000
#define I945G_IO_SIZE 0x80000
#else
#include <pciaccess.h>
#define I945G_IO_BAR 0
#endif
#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 700

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

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

#define USE_METER
#ifdef USE_METER
#define OUT_GRAPHIC meter_out
extern void meter_out(int, int);
#else
#define OUT_GRAPHIC(...)
#endif

#ifdef STANDALONE

#define DOVSTA          0x30008
#define HTOTAL_A        0x60000
#define HBLANK_A        0x60004
#define HSYNC_A         0x60008
#define PIPEA_DSL       0x70000
#define PIPEACONF       0x70008

#define ErrorF printf
#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))
#else
#define B(a) (a)
#endif
#define OC_FIELD (1 << 19)

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

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

typedef struct _drm_i945_syncf {
    struct timeval tv_now;
    struct timeval tv_vbl;
    unsigned trim;
} drm_i945_syncf_t;

#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;

/*
 * following values are hardcoded at the moment. they only work with 
 * the modeline given below. though they appear to comply with 
 * various i945 based motherboard designs.
 *
 * ModeLine "1440x576_50i"  27.75  1440 1488 1609 1769  576 580 585 625  -hsync -vsync interlace
 */
#define HTOTAL_A_OPT  0x06e8059f
#define HSYNC_A_OPT   0x064805cf

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 syf_t syf, syf_clear;
    static drm_i945_syncf_t syncf_prev;
    static struct timeval skew2vbl_prev;

    drm_i945_syncf_t syncf;
    struct timeval skew2vbl;
    struct timeval tv_usecs;
    int usecs;
    void *vptr;
    int dovsta, pipea_dsl;
#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, I945G_IO_SIZE, 
                    PROT_READ | PROT_WRITE, MAP_SHARED, fd, I945G_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 = 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)); exit(-1);
    }
    while (1) {
        if (!(dev = pci_device_next(iter))) {
            ErrorF("device not found\n"); exit(-1);
        }
        if (dev->device_id == 0x2772
#if 0
         || dev->device_id == 0x5d44 
#endif
					) {
            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[I945G_IO_BAR].base_addr, 
					dev->regions[I945G_IO_BAR].size);
    if (pci_device_map_range(dev, dev->regions[I945G_IO_BAR].base_addr, 
				dev->regions[I945G_IO_BAR].size, 1, &vptr)) {
        ErrorF("pci_device_map_range: %s\n", strerror(errno)); exit(-1);
    }
#endif
    while (1) {
    	static usleepv = SYF_FRAME_CYCLE;

/* --- 8< --- */
        syncf.trim = (INREG(HTOTAL_A) >> 16) - (HTOTAL_A_OPT >> 16);
        gettimeofday(&syncf.tv_now, 0);

        /*
         *  DOVSTA 0x00104000 => PIPEA_DSL 0x0000011e
         *  DOVSTA 0x80085000 => PIPEA_DSL 0x0000011f
         *  [...]
         *  DOVSTA 0x80104000 => PIPEA_DSL 0x00000138
         *  DOVSTA 0x80104000 => PIPEA_DSL 0x00000000
         *
         *  the chip does not provide current field status directly.
         *  but we can derive that from xor'ed dovsta and pipea_dsl contents.
         *      _______________                 ________
         * ____|               |_______________|         dovsta
         *      ___             ___             ___
         * ____|   |___________|   |___________|   |____ pipea_dsl
         *          _______________                 ____
         * ________|               |_______________|     field status
         *
         *         0          286 312
         */

#define FIELD_SWITCH_THRESHOLD 287

        /* field * 20000 + line * 64 == 39968 max. for field==1, line==312 */
	dovsta = INREG(DOVSTA);
	pipea_dsl = INREG(PIPEA_DSL);
        skew2vbl.tv_usec =
		  (pipea_dsl < FIELD_SWITCH_THRESHOLD ^ !(dovsta & OC_FIELD))
		  * SYF_PAL_FIELD_CYCLE + (pipea_dsl << 6);
	skew2vbl.tv_sec = 0;
	VSF_SUB(syncf.tv_now, skew2vbl, syncf.tv_vbl);
	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. this occurs
         * after scan line FIELD_SWITCH_THRESHOLD has been passed.
         * on a sufficiently synchronized system running at a
         * sync point of about 20000 usecs this imposes
         * no additional sleep time.
         *
         *  0      even       20000      odd      40000
         *  |-------------------|-------------------|
         *    ...---sleep--->|
         *                 18368
         */
        if (skew2vbl.tv_usec < FIELD_SWITCH_THRESHOLD << 6) {
            usleep((FIELD_SWITCH_THRESHOLD << 6) - skew2vbl.tv_usec);
        }
#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;
            }
	    if (syncf.trim != t) {
		t <<= 16;
                OUTREG(HTOTAL_A,  HTOTAL_A_OPT + t);
                OUTREG(HBLANK_A,  HTOTAL_A_OPT + t);
                OUTREG(HSYNC_A,   HSYNC_A_OPT  + (t << 1));
                OUTREG(PIPEACONF, INREG(PIPEACONF));
	    }
	}
        skew2vbl_prev = skew2vbl;
/* --- 8< --- */

main_loop_end:
        usleep(usleepv);
    }
}

