/**********************************************************************
**  cmv_train.c
**	Train CMV12K LVDS channels
**	Version 1.0
**
**  Copyright (C) 2013 H.Poetzl
**
**	This program is free software: you can redistribute it and/or
**	modify it under the terms of the GNU General Public License
**	as published by the Free Software Foundation, either version
**	2 of the License, or (at your option) any later version.
**
**********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>

#include <sys/mman.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>


#define	VERSION	"V1.0"

static char *cmd_name = NULL;

static uint32_t map_base = 0x60000000;
static uint32_t map_size = 0x00020000;

static uint32_t map_addr = 0x00000000;

static char *dev_mem = "/dev/mem";

static uint32_t pattern[3] = { 0xAAA, 0x50A, 0x5A5 };



typedef long long int (stoll_t)(const char *, char **, int);

long long int argtoll(
	const char *str, const char **end, stoll_t stoll)
{
	int bit, inv = 0;
	long long int val = 0;
	char *eptr;

	if (!str)
	    return -1;
	if (!stoll)
	    stoll = strtoll;
	
	switch (*str) {
	case '~':
	case '!':
	    inv = 1;	/* invert */
	    str++;
	default:
	    break;
	}

	while (*str) {
	    switch (*str) {
	    case '^':
		bit = strtol(str+1, &eptr, 0);
		val ^= (1LL << bit);
		break;
	    case '&':
		val &= stoll(str+1, &eptr, 0);
		break;
	    case '|':
		val |= stoll(str+1, &eptr, 0);
		break;
	    case '-':
	    case '+':
	    case ',':
	    case '=':
		break;
	    default:
		val = stoll(str, &eptr, 0);
		break;
	    }
	    if (eptr == str)
		break;
	    str = eptr;
	}

	if (end)
	    *end = eptr;
	return (inv)?~(val):(val);
}


void parse_pattern(const char *ptr, uint32_t *pattern)
{
	const char *sep = NULL;

	pattern[0] = argtoll(ptr, &sep, strtoll) & 0xFFF;
	if (sep[0] == ',') {
	    pattern[1] =
		argtoll(&sep[1], &sep, strtoll) & 0xFFF;
	    if (sep[0] == ',') {
		pattern[2] =
		    argtoll(&sep[1], &sep, strtoll) & 0xFFF;
	    }
	}
	printf("pattern 0x%03X,0x%03X,0x%03X\n",
	    pattern[0], pattern[1], pattern[2]);
}


uint16_t get_cmv_reg(unsigned reg)
{
	volatile uint32_t *ptr = (uint32_t *)0x60000000;
	return ptr[reg] & 0xFFFF;
}

void	set_cmv_reg(unsigned reg, uint16_t val)
{
	volatile uint32_t *ptr = (uint32_t *)0x60000000;
	ptr[reg] = val;
	usleep(100);
}


uint32_t get_fil_reg(unsigned reg)
{
	volatile uint32_t *ptr = (uint32_t *)0x60010000;
	return ptr[reg];
}

void	set_fil_reg(unsigned reg, uint32_t val)
{
	volatile uint32_t *ptr = (uint32_t *)0x60010000;
	ptr[reg] = val;
	usleep(100);
}

uint32_t get_del_reg(unsigned reg)
{
	volatile uint32_t *ptr = (uint32_t *)0x60011000;
	return ptr[reg];
}

void	set_del_reg(unsigned reg, uint32_t val)
{
	volatile uint32_t *ptr = (uint32_t *)0x60011000;
	ptr[reg] = val;
	usleep(100);
}


void	cmv_check(unsigned chan, unsigned *first, unsigned *last)
{
	uint32_t v0, v1;

	*first = ~0;
	*last = ~0;

	for (int d=0; d<32; d++) {
	    set_del_reg(chan, d);
	    v0 = get_del_reg(chan);
	    usleep(100);
	    v1 = get_del_reg(chan);
	
	    if ((v0 & v1 & 0x10000000) == 0)
		continue;
	
	    if ((v0 ^ v1) & 0xFF0000)
		continue;
	
	    if (*first == ~0)
		*first = d;
	    *last = d;
	}
}




int	main(int argc, char *argv[])
{
	extern int optind;
	extern char *optarg;
	int c, err_flag = 0;

	cmd_name = argv[0];
	while ((c = getopt(argc, argv, "hB:S:A:P:")) != EOF) {
	    switch (c) {
	    case 'h':
		fprintf(stderr,
		    "This is %s " VERSION "\n"
		    "options are:\n"
		    "-h        print this help message\n"
		    "-B <val>  memory mapping base\n"
		    "-S <val>  memory mapping size\n"
		    "-A <val>  memory mapping address\n"
		    "-P <val>  training pattern\n"
		    , cmd_name);
		exit(0);
		break;
	    case 'B':
		map_base = argtoll(optarg, NULL, NULL);
		break;
	    case 'S':
		map_size = argtoll(optarg, NULL, NULL);
		break;
	    case 'A':
		map_addr = argtoll(optarg, NULL, NULL);
		break;
	    case 'P':
		parse_pattern(optarg, pattern);
		break;
	    case '?':
	    default:
		err_flag++;
		break;
	    }
	}
	if (err_flag) {
	    fprintf(stderr, 
		"Usage: %s -[hvB:S:E:] path ...\n"
		"%s -h for help.\n",
		cmd_name, cmd_name);
	    exit(2);
	}


	int fd = open(dev_mem, O_RDWR | O_SYNC);
	if (fd == -1) {
	    fprintf(stderr,
		"error opening >%s<.\n%s\n",
		dev_mem, strerror(errno));
	    exit(1);
	}

	if (map_addr == 0)
	    map_addr = map_base;

	void *base = mmap((void *)map_addr, map_size,
	    PROT_READ | PROT_WRITE, MAP_SHARED,
	    fd, map_base);
	if (base == (void *)-1) {
	    fprintf(stderr,
		"error mapping 0x%08lX+0x%08lX @0x%08lX.\n%s\n",
		(long)map_base, (long)map_size, (long)map_addr,
		strerror(errno));
	    exit(2);
	}

	fprintf(stderr,
	    "mapped 0x%08lX+0x%08lX to 0x%08lX.\n",
	    (long unsigned)map_base, (long unsigned)map_size,
	    (long unsigned)base);

	printf("adjusting delay values ...\n");

	set_fil_reg(0, pattern[0]);
	set_cmv_reg(89, pattern[0]);

	for (int c=0; c<32; c++) {
	    uint32_t vis = 0xF00 | (c >> 3);
	    unsigned val = 15, len, v_a, v_b;
	
	    set_fil_reg(1, vis);	// debug override
	
	    cmv_check(c, &v_a, &v_b);
	    len = v_b - v_a;
	
	    if (len > 15) {
		if (v_a == 0)
		    val = v_b - 16;
		if (v_b == 31)
		    val = v_a + 16;

		printf("%d: [%u %u]#%u -> %u\n",
		    c, v_a, v_b, len, val);
		set_del_reg(c, val);
		continue;
	    }

	    set_del_reg(c, 0x100);	// bitslip
	    unsigned v_c, v_d;

	    cmv_check(c, &v_c, &v_d);
	    len = v_d - v_c;

	    if (len > 15) {
		if (v_c == 0)
		    val = v_d - 16;
		if (v_d == 31)
		    val = v_c + 16;

		printf("%d: [%u %u]#%u -> %u\n",
		    c, v_c, v_d, len, val);
		set_del_reg(c, val);
		continue;
	    }
	
	    if ((v_d - v_c - v_b + v_a) < 0) {
		set_del_reg(c, 0x100);	// bitslip
		
		if (v_a == 0)
		    val = 0;
		if (v_b == 31)
		    val = 31;
	    } else {
		if (v_c == 0)
		    val = 0;
		if (v_d == 31)
		    val = 31;
	    }
	    printf("%d: [%u %u %u %u] -> %u\n",
		c, v_a, v_b, v_c, v_d, val);
	    set_del_reg(c, val);
	}

	printf("adjusting bitslip ...\n");

	set_fil_reg(0, pattern[1]);
	set_cmv_reg(89, pattern[1]);

	for (int c=0; c<32; c++) {
	    uint32_t vis = 0xF00 | (c >> 3);
	
	    set_fil_reg(1, vis);	// debug override
	
	    for (int s=0; s<12; s++) {
		set_del_reg(c, 0x100);	// bitslip
		// usleep(1000);

		if (get_del_reg(c) & 0x10000000)
		    break;
	    }
	}

	printf("testing adjustments ...\n");

	for (int b=0; b<13; b++) {
	    set_fil_reg(0, (1 << b) & 0xFFF);
	    set_cmv_reg(89, (1 << b) & 0xFFF);
	    usleep(100000);
	}

	printf("adjusting control ...\n");

	set_fil_reg(1, 0xF04);
	set_fil_reg(0, 0x080);

	unsigned b_s = 11, b_v = 15, best = 0;

	for (int s=0; s<12; s++) {
	    unsigned len, v_a, v_b;

	    set_del_reg(32, 0x100);	// bitslip
	
	    cmv_check(32, &v_a, &v_b);
	    len = v_b - v_a;
	
	    if (len > best) {
		b_s = s;
		best = len;
		
		if (v_a == 0)
		    b_v = v_b - 16;
		if (v_b == 31)
		    b_v = v_a + 16;
	    }
	}

	printf("32: %u %u\n", b_s, b_v);

	set_del_reg(32, b_v & 0x1F);

	for (int s=0; s<=b_s; s++)
	    set_del_reg(32, 0x100);	// bitslip


	set_fil_reg(1, 0x000);		// unlock debug override
	set_fil_reg(0, pattern[2]);
	set_cmv_reg(89, pattern[2]);

	printf("clearing counters ...\n");

	for (int c=0; c<32; c++)
	    for (int s=0; s<12; s++)
		set_del_reg(c, 0x100);	// bitslip

	exit((err_flag)?1:0);
}