#define _GNU_SOURCE
#include <stdio.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/time.h>
#include <unistd.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include "devopen.h"

#include "dev/pci/sis1100_var.h"
#include "dev/pci/sis5100_map.h"

typedef int (*camacproc)(struct devinfo* dev, u_int32_t N, u_int32_t A,
        u_int32_t F, u_int32_t* data);

#define CAMAC(base, N, A, F) \
    ((base)+(((F)<<9)|((N)<<4)|(A)))

static int
camac_mapped(struct devinfo* dev, u_int32_t N,
        u_int32_t A, u_int32_t F, u_int32_t* data)
{
    volatile u_int32_t* camaddr=CAMAC(dev->base_remote, N, A, F);
    if ((F&0x18)==0x10) {
        u_int32_t err;
        *camaddr=*data;
        err=*(dev->base_ctrl+0x2B);
        *data=(~err<<24)&0xc0000000;
        if (err && !(err&0xc0)) {
            printf("camac_mapped: err=0x%x\n", err);
            return -1;
        }
    } else { /* read or control */
        *data=*camaddr^0xc0000000;
    }
    return 0;
}

static int 
camac_mapped_w(struct devinfo* dev, u_int32_t N,
        u_int32_t A, u_int32_t F, u_int32_t* data)
{
    volatile u_int32_t* camaddr=CAMAC(dev->base_remote, N, A, F);
    u_int32_t err;

    *camaddr=*data;
    err=*(dev->base_ctrl+0x2B);
    *data=(~err<<24)&0xc0000000;
    if (err && !(err&0xc0)) {
        printf("camac_mapped: err=0x%x\n", err);
        return -1;
    }
    return 0;
}

static int
camac_mapped_r(struct devinfo* dev, u_int32_t N,
        u_int32_t A, u_int32_t F, u_int32_t* data)
{
    *data=(*CAMAC(dev->base_remote, N, A, F))^0xc0000000;
    return 0;
}

static int
camac_driver_vme(struct devinfo* dev, u_int32_t N,
        u_int32_t A, u_int32_t F, u_int32_t* data)
{
    struct sis1100_vme_req req;
    unsigned int camac_addr;
    int res;

    camac_addr=((F&0x1f)<<11)+((N&0x1f)<<6)+((A&0xf)<<2);

    req.size=4;
    req.am=-1;
    req.addr=camac_addr;
    req.data=*data;
    req.error=0;

    if ((F&0x18)==0x10) {
        res=ioctl(dev->p_remote, SIS3100_VME_WRITE, &req);
        *data=(~req.error<<24)&0xc0000000;
    } else {
        res=ioctl(dev->p_remote, SIS3100_VME_READ, &req);
        *data=req.data^0xc0000000;
    }
    if (res) {
        printf("driver_vme: res=%d\n", res);
        return -1;
    }
    if (req.error&~0x2c0) {
        printf("driver_vme: error=0x%x\n", req.error);
        return -1;
    }
    return 0;
}

static __inline int
camac_driver(struct devinfo* dev, u_int32_t N, u_int32_t A, u_int32_t F,
    u_int32_t* data)
{
    struct sis1100_camac_req req;
    int res;

    req.N=N;
    req.A=A;
    req.F=F;
    req.data=*data;
    req.error=0;

    res=ioctl(dev->p_remote, SIS5100_CNAF, &req);
    if (res) {
        printf("driver_camac: res=%d\n", res);
        return -1;
    }
    if (req.error) {
        printf("driver_camac: error=0x%x\n", req.error);
        return -1;
    }
    *data=req.data;
    return 0;
}
#if 0
static void
test_inhibit(int p)
{
    struct sis1100_ctrl_reg reg;

    while (1) {
        reg.offset=0x100;
        reg.val=0xffff;
        reg.error=0;
        if (ioctl(p, SIS1100_CTRL_WRITE, &reg)<0) {
            fprintf(stderr, "ioctl(SIS1100_CTRL_WRITE, 0x100): %s\n",
                strerror(errno));
            return;
        }
        if (reg.error!=0) {
            fprintf(stderr, "ioctl(SIS1100_CTRL_WRITE, 0x100): error=0x%x\n",
                reg.error);
            return;
        }
        sleep(1);

        reg.offset=0x100;
        reg.val=0xffff0000;
        reg.error=0;
        if (ioctl(p, SIS1100_CTRL_WRITE, &reg)<0) {
            fprintf(stderr, "ioctl(SIS1100_CTRL_WRITE, 0x100): %s\n",
                strerror(errno));
            return;
        }
        if (reg.error!=0) {
            fprintf(stderr, "ioctl(SIS1100_CTRL_WRITE, 0x100): error=0x%x\n",
                reg.error);
            return;
        }
        sleep(1);
    }
}
#else
static void
test_inhibit(int p)
{
    int d=1;

    while (1) {
        if (ioctl(p, SIS5100_CCCI, &d)<0) {
            fprintf(stderr, "ioctl(SIS5100_CCCI, %d): %s\n",
                d, strerror(errno));
            return;
        }
        d=1-d;
        sleep(1);
    }    
}
#endif

static int
camac_count_driver_vme(struct devinfo* dev, int num)
{
    struct sis1100_vme_req req;
    unsigned int camac_addr;
    int N=20;
    int A=0;
    int F=16;
    int p;

    p=dev->p_ctrl;
    if (p<0) p=dev->p_remote;
    if (p<0) return -1;

    camac_addr=((F&0x1f)<<11)+((N&0x1f)<<6)+((A&0xf)<<2);

    req.size=4;
    req.am=-1;
    req.addr=camac_addr;

    for (; num; num--) {
        int res;

        req.data=num;

        res=ioctl(p, SIS3100_VME_WRITE, &req);
        if (res) {
            printf("camac_write: res=%d\n", res);
            return -1;
        }
        if (req.error&0x3f) {
            printf("camac_write: error=0x%x\n", req.error);
            return -1;
        }
    }
    return 0;
}

static int
camac_count_driver_camac(struct devinfo* dev, int num)
{
    struct sis1100_camac_req req;
    int p;

    p=dev->p_ctrl;
    if (p<0) p=dev->p_remote;
    if (p<0) return -1;

    req.N=20;
    req.A=0;
    req.F=16;

    for (; num; num--) {
        int res;
        req.data=num;

        res=ioctl(p, SIS5100_CNAF, &req);
        if (res) {
            printf("camac_write: res=%d\n", res);
            return -1;
        }
        if (req.error) {
            printf("camac_write: error=0x%x\n", req.error);
            return -1;
        }
    }
    return 0;
}

static int
camac_count_mapped(struct devinfo* dev, int num)
{
    int F=16;
    int N=20;
    int A=0;
    volatile u_int32_t *addr;

    if (!dev->base_remote) return -1;
    addr=CAMAC(dev->base_remote, N, A, F);
    
    for (; num; num--) {
        *addr=num;
    }
    return 0;
}

typedef int (*countproc)(struct devinfo* dev, int num);

static void
camac_count(struct devinfo* dev, countproc proc, const char* text, int num)
{
    struct timeval tv0, tv1;
    float tdiff, tcycle;
    int res;

    gettimeofday(&tv0, 0);
    res=proc(dev, num);
    gettimeofday(&tv1, 0);
    if (res) return;
    tdiff=tv1.tv_sec-tv0.tv_sec;
    tdiff+=(tv1.tv_usec-tv0.tv_usec)/1000000.;
    tcycle=(tdiff*1000000.)/num;
    printf("%s: %f us\n", text, tcycle);
}

static void
fill_4302(struct devinfo* dev, camacproc proc, const char* text, int N,
    int num)
{
    u_int32_t d, i, x;
    struct timeval tv0, tv1;
    float tdiff, tcycle;

    d=1;
    proc(dev, N, 1, 17, &d); /* set MODE to CAMAC */
    camac_mapped(dev, N, 1, 1, &d); printf("mode: %x\n", d&3);
    gettimeofday(&tv0, 0);
    for (x=100; x; x--) {
        d=0;
        proc(dev, N, 0, 17, &d); /* set ADDR */
        for (i=0; i<num; i++) {
            d=i;
            proc(dev, N, 0, 16, &d); /* write data */
            if (!(d&0xc0000000)) {
                printf("fill_4302: QX: 0x%08x\n", d);
                break;
            }
        }
    }
    gettimeofday(&tv1, 0);
    camac_mapped(dev, N, 0, 1, &d); printf("addr after write: 0x%x; i=%d\n", d, i);
    tdiff=tv1.tv_sec-tv0.tv_sec;
    tdiff+=(tv1.tv_usec-tv0.tv_usec)/1000000.;
    tcycle=(tdiff*1000000.)/i/100.;
    printf("%s: %f us\n", text, tcycle);
}

static void
fill_4302_mapped(struct devinfo* dev, const char* text, int N,
    int num)
{
    struct timeval tv0, tv1;
    float tdiff, tcycle;
    u_int32_t d, i=0, x;
    volatile u_int32_t err;
    volatile u_int32_t* addr;
    volatile u_int32_t* base=dev->base_remote;

    err=*(dev->base_ctrl+0x2B);
    d=*(dev->base_ctrl+0x2A);
    printf("(b) initial err: 0x%x, balance=%d\n", err, d);

    *CAMAC(base, N, 1, 17)=1; /* set MODE to CAMAC */
    err=*(dev->base_ctrl+0x2B);
    d=*(dev->base_ctrl+0x2A);
    printf("(set mode) err: 0x%x, balance=%d\n", err, d);

    /* read mode */
    d=*CAMAC(base, N, 1, 1); printf("mode: %x\n", d&3);
    err=*(dev->base_ctrl+0x2B);
    d=*(dev->base_ctrl+0x2A);
    printf("(read mode) err: 0x%x, balance=%d\n", err, d);

    gettimeofday(&tv0, 0);
    for (x=100; x; x--) {
        addr=CAMAC(base, N, 0, 17);
        *addr=0; /* set ADDR */
        err=*(dev->base_ctrl+0x2B);
        if (err) {
            d=*(dev->base_ctrl+0x2A);
            printf("(set addr) err: 0x%x, balance=%d\n", err, d);
            goto raus;
        }
        addr=CAMAC(base, N, 0, 16);
        for (i=0; i<num; i++) {
            *addr=i;
            /*err=*(dev->base_ctrl+0x2B);*/
/*
 *             if (err&0xc0) {
 *                 printf("fill_4302_mapped: QX: 0x%x\n", err);
 *                 goto raus;
 *             }
 */
        }
        do {
            err=*(dev->base_ctrl+0x2B);
            d=*(dev->base_ctrl+0x2A);
        } while (err==0x107);
        if (err) {
            printf("(after write) err: 0x%x, balance=%d\n", err, d);
            goto raus;
        }
    }
raus:
    gettimeofday(&tv1, 0);
    addr=CAMAC(base, N, 0, 1);
    d=*addr; printf("addr after write: 0x%x; i=%d\n", d, i);
    tdiff=tv1.tv_sec-tv0.tv_sec;
    tdiff+=(tv1.tv_usec-tv0.tv_usec)/1000000.;
    tcycle=(tdiff*1000000.)/i/100.;
    printf("%s: %f us\n", text, tcycle);
}

static void
read_4302(struct devinfo* dev, camacproc proc, const char* text, int N, int num)
{
    struct timeval tv0, tv1;
    float tdiff, tcycle;
    u_int32_t d, i=0, x;

    gettimeofday(&tv0, 0);
    for (x=100; x; x--) {
        d=0;
        camac_mapped(dev, N, 0, 17, &d); /* set ADDR */
        for (i=0; i<num; i++) {
            proc(dev, N, 0, 0, &d); /* read data */
            if (!(d&0xc0000000)) {
                printf("read_4302: QX: 0x%08x\n", d);
                goto raus;
            }
            if ((d&~0xc0000000)!=(i&0xffefff)) {
                printf("read_4302[0x%04x]: 0x%x\n", i, d);
                goto raus;
            }
        }
    }
raus:
    gettimeofday(&tv1, 0);
    proc(dev, N, 0, 1, &d); printf("addr after read: 0x%x, i=%d\n", d, i);
    tdiff=tv1.tv_sec-tv0.tv_sec;
    tdiff+=(tv1.tv_usec-tv0.tv_usec)/1000000.;
    tcycle=(tdiff*1000000.)/i/100.;
    printf("%s: %f us\n", text, tcycle);
}

static void
read_4302_mapped(struct devinfo* dev, const char* text, int N, int num)
{
    struct timeval tv0, tv1;
    float tdiff, tcycle;
    u_int32_t d, i=0, x;
    volatile u_int32_t err;
    volatile u_int32_t* addr;
    volatile u_int32_t* base=dev->base_remote;

    addr=CAMAC((u_int32_t*)0, N, 0, 17); /* set ADDR */
    printf("offset=%p\n", addr);

    gettimeofday(&tv0, 0);
    for (x=1; x; x--) {
        addr=CAMAC(base, N, 0, 17); /* set ADDR */
        *addr=0;
        err=*(dev->base_ctrl+0x2B);
        if (err) {
            d=*(dev->base_ctrl+0x2A);
            printf("(set addr) err: 0x%x, balance=%d\n", err, d);
            goto raus;
        }
        addr=CAMAC(base, N, 0, 0); /* read data */
        for (i=0; i<num; i++) {
            d=*addr;
            if (d&0xc0000000) {
                printf("read_4302_mapped: QX: 0x%08x\n", d);
                goto raus;
            }
            if (d!=(i&0xffefff)) {
                printf("read_4302_mapped[0x%04x]: 0x%x\n", i, d);
                goto raus;
            }
        }
    }
raus:
    gettimeofday(&tv1, 0);
    addr=CAMAC(base, N, 0, 1);
    d=*addr; printf("addr after read: 0x%x; i=%d\n", d, i);
    tdiff=tv1.tv_sec-tv0.tv_sec;
    tdiff+=(tv1.tv_usec-tv0.tv_usec)/1000000.;
    tcycle=(tdiff*1000000.)/i/100.;
    printf("%s: %f us\n", text, tcycle);
}

int main(int argc, char* argv[])
{
    struct devinfo devinfo;
    struct sis1100_ident ident;

    if (argc<2)
        {
        fprintf(stderr, "usage: %s path_1 path_2 ...\n", argv[0]);
        return 1;
        }

    if (open_dev(argv+1, &devinfo)<0) return 2;

    {
        int p;

        p=devinfo.p_ctrl;
        if (p<0) p=devinfo.p_remote;
        if (p<0) {
            printf("neither ctrl nor remote device open.\n");
            return 3;
        }
        if (ioctl(p, SIS1100_IDENT, &ident)<0) {
            fprintf(stderr, "ioctl(SIS1100_IDENT): %s\n", strerror(errno));
            return 4;
        }
        printf("local:\n");
        printf("  hw_type   : %d\n",   ident.local.hw_type);
        printf("  hw_version: %d\n",   ident.local.hw_version);
        printf("  fw_type   : %d\n",   ident.local.fw_type);
        printf("  fw_version: %d\n\n", ident.local.fw_version);
        printf("remote:\n");
        printf("  hw_type   : %d\n",   ident.remote.hw_type);
        printf("  hw_version: %d\n",   ident.remote.hw_version);
        printf("  fw_type   : %d\n",   ident.remote.fw_type);
        printf("  fw_version: %d\n\n", ident.remote.fw_version);

        printf("  remote side is %s and %svalid\n",
            ident.remote_online?"online":"offline",
            ident.remote_ok?"":"not ");

        if ((ident.local.hw_type!=1)||(ident.local.hw_version!=1)||
            (ident.local.fw_type!=1)) {
            fprintf(stderr, "unsupported bord version\n");
            return 4;
        }
    }

    if (devinfo.base_ctrl!=MAP_FAILED) {
        printf("size of mapped ctrl space  : %d\n", devinfo.size_ctrl);
    }
    if (devinfo.base_remote!=MAP_FAILED) {
        struct sis1100_ctrl_reg reg;
        const int mapidx=0;
        u_int32_t offs=0x400+16*mapidx;
        u_int32_t header=0x0f010000; /* 4 byte; remote space 1; no AM */
        int res=0;
        int p=devinfo.p_ctrl;

        printf("size of mapped remote space: %d\n", devinfo.size_remote);

        reg.offset=offs+0;
        reg.val=header;
        res|=ioctl(p, SIS1100_CTRL_WRITE, &reg);

        reg.offset=offs+4;
        reg.val=0; /* address modifier */
        res|=ioctl(p, SIS1100_CTRL_WRITE, &reg);

        reg.offset=offs+8;
        reg.val=0; /* address base */
        res|=ioctl(p, SIS1100_CTRL_WRITE, &reg);

        reg.offset=offs+12;
        reg.val=0; /* high part of 64 bit address */
        res|=ioctl(p, SIS1100_CTRL_WRITE, &reg);
        if (res) {
            printf("mapping of CAMAC space failed.\n");
        }
    }

#if 0
    if (devinfo.p_remote>=0) {
        test_inhibit(devinfo.p_ctrl);
    } else {
        printf("remote path not open.\n");
    }
#endif

#if 0
    camac_count(&devinfo, camac_count_driver_vme,   "vme   ", 1000000);
    camac_count(&devinfo, camac_count_driver_camac, "camac ", 1000000);
    camac_count(&devinfo, camac_count_mapped,       "mapped", 1000000);
#endif

#if 0
    {
        int p;
        p=devinfo.p_ctrl;
        if (p<0) p=devinfo.p_remote;
        if (p>=0) {
            camac_count_driver_vme(p);
        } else {
            printf("neither ctrl nor remote device open.\n");
        }
    }
    {
        int p;
        p=devinfo.p_ctrl;
        if (p<0) p=devinfo.p_remote;
        if (p>=0) {
            camac_count_driver_camac(p);
        } else {
            printf("neither ctrl nor remote device open.\n");
        }
    }
    {
        if (devinfo.base_remote!=MAP_FAILED) {
            camac_count_mapped(devinfo);
        } else {
            printf("CAMAC space not mapped.\n");
        }
    }
#endif

#if 0
    fill_4302(&devinfo, camac_driver_vme, "vme", 21, 16384);
    read_4302(&devinfo, camac_driver_vme, "vme", 21, 16384);
    fill_4302(&devinfo, camac_driver, "camac", 21, 16384);
    read_4302(&devinfo, camac_driver, "camac", 21, 16384);
    fill_4302_mapped(&devinfo, "fill_mapped_m", 21, 1000);
    fill_4302(&devinfo, camac_mapped_w, "fill_mapped_w", 21, 1000);
    read_4302(&devinfo, camac_mapped_r, "read_mapped_r", 21, 1000);
    read_4302(&devinfo, camac_mapped, "read_mapped", 21, 1000);
#endif
    read_4302_mapped(&devinfo, "read_mapped_m", 21, 1000);

    return 0;
}