os/linux-2.6-tag--devboard-R2_10-4/arch/cris/arch-v32/drivers/elphel/hist.c

Go to the documentation of this file.

/*
 * histogram for autoexposure and control
 *
 *  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 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Copyright (C) 2005-2007 Elphel, Inc
 *
 * Author: Mikhajlo Malishko <spectr@gmail.com>
 *
 */

#include <linux/kernel.h>
#include <linux/sched.h>
#include <linux/mm.h>
//#include <linux/tqueue.h>
#include <linux/wait.h>

#include <asm/unistd.h>
#include <asm/semaphore.h>
//#include <asm/smplock.h>
#include <asm/atomic.h>

#include <linux/module.h>
#include <linux/slab.h>
#include <linux/errno.h>
#include <linux/fs.h>
#include <linux/string.h>
#include <linux/init.h>
#include <linux/mm.h>
//#include <linux/iobuf.h>
//#include <linux/compatmac.h>
#include <linux/vmalloc.h>
#include <linux/rwsem.h>

#include <linux/netdevice.h>
#include <linux/skbuff.h>
#include <linux/time.h>
#include <linux/wait.h>
#include <asm/io.h>
#include <asm/atomic.h>
#include <asm/page.h>
//#include <linux/wrapper.h>

#include "fpgactrl.h"  // defines port_csp0_addr, port_csp4_addr 

#include <asm/elphel/c313a.h>
#include <asm/elphel/hist.h>
#include <asm/elphel/autoexp.h>

#include "cc3x3.h"
#include "cxdma.h"
#include "x3x3.h"
#include "hist.h"
#include "fpga_io.h"

//#include "helper.h"

#define HIST_MAJOR 130
#define AUTOEXP_MAJOR 131

#if ELPHEL_DEBUG
 #define MD1(x) printk("%s:%d:",__FILE__,__LINE__);x
  //#define MD1(x)
#else
  #define MD1(x)
#endif



/* log data... */
DECLARE_MUTEX(ae_log_lock);
static struct autoexp_log_t ae_log_buf[LOG_C];
static struct autoexp_log_t *ae_log_p = &ae_log_buf[LOG_C - 1];
static unsigned ae_log_c = 0;

DECLARE_MUTEX(sensor_lock);
atomic_t sensor_refresh;
DECLARE_MUTEX(iface_lock);
atomic_t autoexp_on;
atomic_t autoexp_enable;
DECLARE_WAIT_QUEUE_HEAD(wq_hist);
static int first_exp = 0;

DECLARE_MUTEX(autoexp_iface_lock);

#define BUF_C   2

struct gamma_t {
        unsigned short *buf;
        struct timeval tv;
};
struct gamma_t gamma[BUF_C];

/* RGGB + white reverse + white original */
static unsigned short gamma_table_1[256 * 6];
static unsigned short gamma_table_2[256 * 6];

static unsigned long c_gamma = 0;
static unsigned long c_hist = 0;

DECLARE_MUTEX(gamma_lock);
static unsigned short gamma_table[256 * 6] __attribute__ ((aligned (PAGE_SIZE))); // aligne for remap_pfn_range
struct timeval tv_gamma;

/* --==-- */
struct buf_t {
        unsigned long *buf;
        struct timeval tv;
        struct semaphore lock;
};
struct buf_t buf[BUF_C];

DECLARE_MUTEX(hist0_lock);
static struct timeval tv_gamma_hb_0;
static unsigned long hb_0[TABLES_LEN] __attribute__ ((aligned (PAGE_SIZE)));

static unsigned long hb_1[256 * 4 + 256 * 2] __attribute__ ((aligned (PAGE_SIZE)));
static unsigned long hb_2[256 * 4 + 256 * 2] __attribute__ ((aligned (PAGE_SIZE)));

#define HIST_TABLE_SIZE (256 * 4 * sizeof(u32))

#define HIST_EXP_MAX_START              4000
#define HIST_OVEREXP_MAX_START  50
#define HIST_EXP_START                  1000

DECLARE_MUTEX(autoexp_conf_lock);
int autoexp_conf_new = 1;
#if 0
struct autoexp_t autoexp_conf = {
        .on = 1,
        .width = 80,    /* in percent */
        .height = 80,
        .left = 50,
        .top = 50,
        .exp_max = HIST_EXP_MAX_START,
        .overexp_max = HIST_OVEREXP_MAX_START,
        .s_index = 0,
        .s_percent = 0,
        .exp = HIST_EXP_START,
        .skip_pmin = 500,
        .skip_pmax = 5000,
        .skip_t = 2,
};
#endif
#if 0
struct autoexp_t state = {
        .on = 1,
        .exp_max = HIST_EXP_MAX_START,
        .overexp_max = HIST_OVEREXP_MAX_START,
        .exp = HIST_EXP_START,
        .s_index = 0,
        .s_percent = 0,
        .skip_pmin = 500,
        .skip_pmax = 5000,
        .skip_t = 2,
};
#endif
int tv_less(struct timeval *tv_1, struct timeval *tv_2) {
        if(tv_1->tv_sec < tv_2->tv_sec)
                return 1;
        if(tv_1->tv_sec == tv_2->tv_sec)
                if(tv_1->tv_usec <= tv_2->tv_usec)
                        return 1;
        return 0;
}

#define GAMMA_COUNT                     200
//#define GAMMA_TABLE_SIZE      256
#define GAMMA_TABLE_SIZE        257
#define _GAMMA_TABLE_SIZE       257

static unsigned short gamma_tbl[_GAMMA_TABLE_SIZE * GAMMA_COUNT];
//static unsigned short *gamma_ptr = gamma_tbl;
static unsigned short *gamma_ptr = NULL;
//static unsigned long gamma_ptr_order = 0;


/*******************************************************************************
 * image primitives
 */
/*
struct aexp_window_t {
        unsigned long width;
        unsigned long height;
        unsigned long top;
        unsigned long left;
};
*/
/* real histogram window size & offset */
#if 0
struct aexp_window_t window = {
        .width = 0,
        .height = 0,
        .left = 0,
        .top = 0,
};
#endif
/* image from sensor size */
struct aexp_window_t window_sensor = {
        .width = 0,
        .height = 0,
};
#if 0
/* windows size in percent from user settings */
struct aexp_window_t window_set = {
        .width = 80,
        .height = 80,
        .left = 50,
        .top = 50,
};
#endif
atomic_t image_new;
atomic_t image_reprogrammed;
DECLARE_MUTEX(image_lock);
atomic_t sensor_window;

struct hist_sensor_t sensor_desc;
/*******************************************************************************
 *
 * init
 */

static int hist__open(struct inode *inode, struct file *filp);
static int hist__release(struct inode *inode, struct file *filp);
static int hist__ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg);
static int hist__mmap(struct file *filp, struct vm_area_struct *vma);

static struct file_operations hist__fops = {
        owner:          THIS_MODULE,
        open:           hist__open,
        release:        hist__release,
        ioctl:          hist__ioctl,
        mmap:           hist__mmap,
};

static int autoexp__open(struct inode *inode, struct file *filp);
static int autoexp__release(struct inode *inode, struct file *filp);
static ssize_t autoexp__write(struct file *filp, const char *buf, size_t count, loff_t *offp);
static int autoexp__ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg);
static int autoexp__mmap(struct file *filp, struct vm_area_struct *vma);

static struct file_operations autoexp__fops = {
        owner:          THIS_MODULE,
        open:           autoexp__open,
        release:        autoexp__release,
        write:          autoexp__write,
        ioctl:          autoexp__ioctl,
        mmap:           autoexp__mmap,
};

static int __init hist__init(void) {
        int i, j;
        int res;
    autoexp_set->on = 1;
    autoexp_set->width = 80;    /* in percent */
    autoexp_set->height = 80;
    autoexp_set->left = 50;
    autoexp_set->top = 50;
    autoexp_set->exp_max = HIST_EXP_MAX_START;
    autoexp_set->overexp_max = HIST_OVEREXP_MAX_START;
    autoexp_set->s_index = 0;
    autoexp_set->s_percent = 0;
    autoexp_set->exp = HIST_EXP_START;
    autoexp_set->skip_pmin = 500;
    autoexp_set->skip_pmax = 5000;
    autoexp_set->skip_t = 2;

    autoexp_state->on = 1;
    autoexp_state->exp_max = HIST_EXP_MAX_START;
    autoexp_state->overexp_max = HIST_OVEREXP_MAX_START;
    autoexp_state->exp = HIST_EXP_START;
    autoexp_state->s_index = 0;
    autoexp_state->s_percent = 0;
    autoexp_state->skip_pmin = 500;
    autoexp_state->skip_pmax = 5000;
    autoexp_state->skip_t = 2;

        gamma_ptr = gamma_tbl;

//static unsigned short gamma_tbl[_GAMMA_TABLE_SIZE * GAMMA_COUNT];
//static unsigned short *gamma_ptr = gamma_tbl;
//      gamma_ptr = (unsigned short *)kmalloc(GAMMA_TABLE_SIZE * GAMMA_COUNT * 2, GFP_ATOMIC);
/*
        int size = GAMMA_TABLE_SIZE * GAMMA_COUNT * 2;
        printk("gamma sizes == %d\n", size);
//      if(size % PAGE_SIZE != 0)
//              gamma_ptr_order = 1;
//      gamma_ptr_order += size / PAGE_SIZE;
        if(size % PAGE_SIZE != 0)
                size += PAGE_SIZE;
        size /= PAGE_SIZE;
        gamma_ptr_order = 0;
        while(size) {
                gamma_ptr_order++;
                size >>= 1;
        }
//      gamma_ptr = (unsigned short *)kmalloc(GFP_KERNEL, GAMMA_TABLE_SIZE * GAMMA_COUNT * 4);
        gamma_ptr = (unsigned short *)kmalloc(GAMMA_TABLE_SIZE * GAMMA_COUNT * 4, GFP_KERNEL);
        gamma_ptr = (unsigned short *)__get_free_pages(GFP_KERNEL, gamma_ptr_order);
printk("gamma_ptr == 0x%08X, order == %d\n", gamma_ptr, gamma_ptr_order);
*/
        /* register autoexposure device */
        res = register_chrdev(HIST_MAJOR, "hist", &hist__fops);
        if(res < 0) {
                printk(KERN_ERR "hist: couldn't get a major number.\n");
                return res;
        }
        /* init buffers */
        buf[0].buf = hb_1;
        buf[1].buf = hb_2;
        for(i = 0; i < BUF_C; i++) {
                buf[i].tv.tv_sec = 0;
                buf[i].tv.tv_usec = 0;
                init_MUTEX(&buf[i].lock);
        }
        gamma[0].buf = gamma_table_1;
        gamma[1].buf = gamma_table_2;
        gamma[0].tv.tv_sec = 0;
        gamma[0].tv.tv_usec = 0;
        gamma[1].tv.tv_sec = 0;
        gamma[1].tv.tv_usec = 1;
        tv_gamma.tv_sec = 0;
        tv_gamma.tv_usec = 0;
        tv_gamma_hb_0.tv_sec = 0;
        tv_gamma_hb_0.tv_usec = 0;
        printk("hist: init ok\n");
        /* register control device */
        res = register_chrdev(AUTOEXP_MAJOR, "autoexp", &autoexp__fops);
        if(res < 0) {
                printk(KERN_ERR "autoexp: couldn't get a major number.\n");
                return res;
        }
        /* -==- */
        for(i = 0; i < 4; i++)
                for(j = 0; j < 256; j++)
                        hb_0[i * 256 + j] = j;
        for(i = 0; i < 6; i++)
                for(j = 0; j < 256; j++) {
                        gamma_table[i * 256 + j] = j + (j << 8);
                        gamma_table_1[i * 256 + j] = j + (j << 8);
                        gamma_table_2[i * 256 + j] = j + (j << 8);
                }
        /* set it for correct start stepping */
        atomic_set(&sensor_refresh, 1);
        return res;
}

static void __exit hist__exit(void) {
        unregister_chrdev(HIST_MAJOR, "autoexp");
        printk("autoexp: exit ok\n");
        unregister_chrdev(HIST_MAJOR, "hist");
        printk("hist: exit ok\n");
//      __free_pages((unsigned long)gamma_ptr, gamma_ptr_order);
//      kfree(gamma_ptr);
}

module_init(hist__init);
module_exit(hist__exit);

/*******************************************************************************
 *
 * io
 */


int hist__open(struct inode *inode, struct file *filp) {
        if(down_trylock(&iface_lock))
                return -EBUSY;
        atomic_set(&autoexp_on, 1);
        if(autoexp_state->on)
                atomic_set(&autoexp_enable, 1);
        printk("/dev/hist: open()\r\n");
        return 0;
}

int hist__release(struct inode *inode, struct file *filp) {
        printk("/dev/hist: release()\r\n");
        up(&iface_lock);
        /* free/reinit all lock and flags */
        atomic_set(&autoexp_on, 0);
        atomic_set(&autoexp_enable, 0);
        atomic_set(&image_new, 0);
        atomic_set(&sensor_window, 0);
        init_MUTEX(&sensor_lock);
        return 0;
}

int hist__ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg) {
        int i;
        struct autoexp_t set;
        struct autoexp_log_t log;

        switch(_IOC_TYPE(cmd)) {
        case IOC_HIST_SET:
                copy_from_user(&set, (const void *)arg, sizeof(set));
                /* refresh sensor */
                if(down_interruptible(&sensor_lock))
                        return -EFAULT;
                if(set.exp != 0) {
                        if(first_exp >= 3) {
                                sensor_desc.exposure = set.exp;
                                autoexp_set->exp = set.exp;
                                atomic_set(&sensor_refresh, 1);
                        }
                }
                up(&sensor_lock);
                /*
                 * FPGA window must be refresh immediatelly after done_compress IRQ!
                 * in this place - with settin up flag "for_fist"
                 */
                if(down_interruptible(&image_lock) == 0) {
                        if(aexp_window_set->width != set.width || aexp_window_set->height != set.height || aexp_window_set->left != set.left || aexp_window_set->top != set.top) {
                                if(set.width != HIST_NOT_CHANGE)
                                        aexp_window_set->width = set.width;
                                if(set.height != HIST_NOT_CHANGE)
                                        aexp_window_set->height = set.height;
                                if(set.left != HIST_NOT_CHANGE)
                                        aexp_window_set->left = set.left;
                                if(set.top != HIST_NOT_CHANGE)
                                        aexp_window_set->top = set.top;
                                atomic_set(&image_new, 1);
                        }
                        up(&image_lock);
                }

                break;
        case IOC_HIST_SET_LOG:
                copy_from_user(&log, (const void *)arg, sizeof(log));
                down(&ae_log_lock);
                if(ae_log_p == ae_log_buf)
                        ae_log_p = &ae_log_buf[LOG_C - 1];
                else
                        ae_log_p--;
                if(ae_log_c < LOG_C)
                        ae_log_c++;
                port_csp0_addr[X313_WA_RTC_LATCH] = 0xFF;
                log.tv_sec = port_csp0_addr[X313_RA_RTC_SEC];
                log.tv_usec = port_csp0_addr[X313_RA_RTC_USEC];
                memcpy((void *)ae_log_p, (void *)&log, sizeof(struct autoexp_log_t));
                up(&ae_log_lock);
                break;
        case IOC_HIST_GET:
                /* wait for new, or if exist, last, histogramm */
                while(1) {
                        /* check for new settings... */
                        i = 0;
                        down(&autoexp_conf_lock);
                        if(autoexp_conf_new) {
                                autoexp_conf_new = 0;
//                              copy_to_user((void *)arg, &autoexp_conf, sizeof(autoexp_conf));
                                copy_to_user((void *)arg, autoexp_set, sizeof(*autoexp_set));
                                i = 1;
                        }
                        up(&autoexp_conf_lock);
                        if(i != 0)
                                return REZ_HIST_CONF;
                        /* try to get new histogramm */
                        for(i = 0; i < BUF_C; i++) {
                                if(down_trylock(&buf[i].lock))
                                        continue;
                                if(buf[i].tv.tv_sec == 0 && buf[i].tv.tv_usec == 0) {
                                        up(&buf[i].lock);
                                        continue;
                                }
                                /* copy image size to user */
                                filp->private_data = (void *)&buf[i];
                                return REZ_HIST_TABLE;
                        }
                        interruptible_sleep_on(&wq_hist);
                }
                break;
        default:
                return -EFAULT;
        }
        return 0;
}

static void hist__vma_open(struct vm_area_struct *vma);
static void hist__vma_close(struct vm_area_struct *vma);
static struct vm_operations_struct hist__vm_ops = {
        .open = hist__vma_open,
        .close = hist__vma_close,
};

int hist__mmap(struct file *filp, struct vm_area_struct *vma) {
        int res = 0;
        struct buf_t *b;

        b = (struct buf_t *)filp->private_data;
        if(b == NULL)
                return -EFAULT;
        if(b->tv.tv_sec == 0 && b->tv.tv_usec == 0)
                return -EFAULT;
        vma->vm_private_data = filp->private_data;
        vma->vm_ops = &hist__vm_ops;
//printk("vma->vm_start == %d; vma->vm_end == %d\r\n", vma->vm_start, vma->vm_end);
//      res = remap_page_range(   vma->vm_start, virt_to_phys(b->buf), vma->vm_end - vma->vm_start, vma->vm_page_prot);

//NOTE: hope b->buf is one of aligned (PAGE_SIZE)
   res = remap_pfn_range(vma,vma->vm_start, virt_to_phys(b->buf) >> PAGE_SHIFT,vma->vm_end-vma->vm_start,vma->vm_page_prot);

        hist__vma_open(vma);
        return res;
}

void hist__vma_open(struct vm_area_struct *vma) {
}

void hist__vma_close(struct vm_area_struct *vma) {
        struct buf_t *b;
        b = (struct buf_t *)vma->vm_private_data;
        b->tv.tv_sec = 0;
        b->tv.tv_usec = 0;
        up(&b->lock);
}

/*******************************************************************************
 *
 * work
 */

#define STEP_INIT       0x00
#define STEP_1          0x01
#define STEP_2          0x02
#define STEP_3          0x03
#define STEP_4          0x04

//extern int program_sensor_exposition(void);

void hist_irq(unsigned long src) {
        static int step = STEP_INIT;
//      static int may_read_histogram = 0;
//      static int window_changed = 0;
        int i;
        int flag;
//      static int ae_on_last = 0;
        int ae_on = 0;
        struct timeval tv;

        struct buf_t *b;
        unsigned short *_g;
        struct gamma_t *g_1;
        struct gamma_t *g_2;

        do_gettimeofday(&tv);

//printk("\n\nhist_irq\n");
        if(atomic_read(&autoexp_enable) != 0)
                ae_on = 1;
        /* reprogramm FPGA histogram window... */
        /*
         * after this - start to STEP_2
         */
        if(atomic_read(&sensor_window) != 0) {
                atomic_set(&sensor_window, 0);
                step = STEP_1;
        }
        if(atomic_read(&image_new) != 0) {
                /* not "real" lock - simple check for correct struct! */
                if(!down_trylock(&image_lock)) {
                        atomic_set(&image_new, 0);
                        /* calculate window width */
                        aexp_window->width = (window_sensor.width * aexp_window_set->width) / 100;
                        aexp_window->left = ((window_sensor.width - aexp_window->width) * aexp_window_set->left) / 100;
                        aexp_window->left += 2;
                        aexp_window->height = (window_sensor.height * aexp_window_set->height) / 100;
                        aexp_window->top = ((window_sensor.height - aexp_window->height) * aexp_window_set->top) / 100;
                        aexp_window->top += 2;
                        aexp_window->top &= 0xFFFE;
                        /* align histogram window */
                        if(aexp_window->width != 0 && aexp_window->height != 0) {
                                port_csp0_addr[X313_WA_HIST_SIZE] = (((aexp_window->height - 1) & 0xFFFF) << 16) + ((aexp_window->width - 1) & 0xFFFF);
                                port_csp0_addr[X313_WA_HIST_POS] = ((aexp_window->top & 0xFFFF) << 16) + (aexp_window->left & 0xFFFF);
                        }
                        if(step != STEP_INIT)
                                step = STEP_2;
                        else
                                step = STEP_1;
                        up(&image_lock);
                }
        }
        /*
         * reprogram sensor exposition time...
         * start in STEP_1
         */
        if(ae_on) {
                if(atomic_read(&sensor_refresh) != 0) {
                        if(program_sensor_exposition() != 0)
                                return;
                        atomic_set(&sensor_refresh, 0);
                        step = STEP_1;
                }
        }
        /*
         * if histogramm is really correct...
         * if apply any changes - step != STEP_4 in previous code, and histogramm changes skipped
         */
//      if(step == STEP_4 || ae_on == 0) {
        if(step == STEP_4) {
                for(i = 0; i < 256 * 4; i++) {
                        port_csp0_addr[X313_WA_HIST_ADDR] = i;
                        hb_0[i] = port_csp4_addr[X313_RA_HIST_DATA];
                }
                memcpy(((void *)hb_0) + OFFSET_HIST_C, &c_hist, 4);
                c_hist++;
                /* update gamma, if needed... */
                if(tv_less(&tv_gamma_hb_0, &tv_gamma)) {
                        if(!down_trylock(&gamma_lock)) {
                                memcpy(((void *)hb_0) + 256 * 4 * 4, (void *)gamma_table, 256 * 6 * 2);
//                              do_gettimeofday(&tv_gamma_hb_0);
                                tv_gamma_hb_0 = tv;
                                up(&gamma_lock);
                        }
                }
        }
        /* check steps */
//      if(ae_on == 1 && ae_on_last == 0)
//              step = STEP_4;
//      ae_on_last = ae_on;
//      if(ae_on == 0 )
//              return;

        switch(step) {
        /* in this part - wait for camera initialization... */
        case STEP_INIT:
                break;
        /* in this step, some parts reprogrammed - and changes apply only in STEP_2 */
        case STEP_1:
                step = STEP_2;
                break;
        /* changes applyed - and this frame is "bad" - skip it! */
        case STEP_2:
                step = STEP_3;
                break;
        /* this frame is "good" - wait to result of this frame... */
        case STEP_3:
                step = STEP_4;
                break;
        /* ok! - this time histogramm is correct... */
        case STEP_4:
                if(ae_on == 0)
                        break;
                /* ...write histogram in free buffer... */
                flag = 1;
                for(i = 0; i < BUF_C; i++) {
                        if(down_trylock(&buf[i].lock))
                                continue;
                        flag = 0;
                        b = &buf[i];
                        _g = (unsigned short *)(((void *)b->buf) + 1024 * 4);
                        memcpy((void *)b->buf, (void *)hb_0, 256 * 4 * 4);
                        b->tv = tv;
                        /* copy reverse and original gamma */
                        if(tv_less(&gamma[0].tv, &gamma[1].tv)) {
                                g_1 = &gamma[0];
                                g_2 = &gamma[1];
                        } else {
                                g_1 = &gamma[1];
                                g_2 = &gamma[0];
                        }
                        if(g_1->tv.tv_sec == 0)
                                g_1 = g_2;
                        else {
                                if(tv_less(&b->tv, &g_2->tv))
                                        g_1 = g_1;
                                else
                                        g_1 = g_2;
                        }
                        memcpy((void *)_g, (void *)g_1->buf + 256 * 4 * 2, 256 * 2 * 2);
                        /* --==-- */
                        up(&b->lock);
                        break;
                }
                /* ...and awake for mmap */
                if(flag == 0)
                        wake_up_interruptible(&wq_hist);
                /*
                 * ok - not apply changes - only wait for sensor_refresh to change step, if application want it
                 * or - window changed, and step's cycle restart...
                 * i.e. in this part step not changed directly!
                 */
                break;
        default:
                break;
        }

        return;
}

void hist_irq__(unsigned long src) {

        static int step = STEP_INIT;

        unsigned long i;
        struct buf_t *b;
        static unsigned long _width = 0;
        static unsigned long _height = 0;
        unsigned short *_g;
        struct gamma_t *g_1;
        struct gamma_t *g_2;
        struct timeval tv;

        static int src_done_c = 0;
        int in_busy_skip = 1;

        if(src == IRQ_SRC_VACT)
                src_done_c = 0;
        else {
                src_done_c++;
                if(src_done_c < 2)
                        return;
                src_done_c--;
        }
        do_gettimeofday(&tv);
//*/
        /* --==-- */
        for(i = 0; i < 256 * 4; i++) {
                port_csp0_addr[X313_WA_HIST_ADDR] = i;
                hb_0[i] = port_csp4_addr[X313_RA_HIST_DATA];
                if((hb_0[i] & 0xFFFC0000) != 0x00000000)
                        printk("hist[%ld] == 0x%08lX (0x%08lX) - %d:%d\r\n", i, hb_0[i], hb_0[i] & 0xFFFC0000, (int) tv.tv_sec, (int) tv.tv_usec);
        }
        memcpy(((void *)hb_0) + OFFSET_HIST_C, &c_hist, 4);
        c_hist++;
        /* update gamma, if needed... */
        if(tv_less(&tv_gamma_hb_0, &tv_gamma)) {
                if(down_trylock(&gamma_lock)) {
                        memcpy(((void *)hb_0) + 256 * 4 * 4, (void *)gamma_table, 256 * 6 * 2);
                        do_gettimeofday(&tv_gamma_hb_0);
                        up(&gamma_lock);
                }
        }
        /* this work not needed for any - skip */
        if(atomic_read(&autoexp_enable) == 0) {
                /* reset stepping cycle */
                if(step != STEP_INIT)
                        step = STEP_1;
                return;
        }
        /* 4 - stepping cycle */
restep:
        switch(step) {
        /* all driver system not initialized - check for image reprogramming, and skip next frame  */
        case STEP_INIT:
                if(atomic_read(&image_reprogrammed) != 0)
                        atomic_inc(&image_reprogrammed);
                if(atomic_read(&image_reprogrammed) == 3) {
                        atomic_set(&image_reprogrammed, 0);
                        step = STEP_1;
//                      atomic_set(&sensor_refresh, 1);
                        goto restep;
                }
                break;
        /*
         * apply (new histogram windowing) and new exposure
         */
        case STEP_1:
                if(atomic_read(&image_reprogrammed) == 1) {
                        step = STEP_INIT;
                        goto restep;
                }
//              atomic_set(&image_reprogrammed, 0);
                /* set new histogram window */
                _width = aexp_window->width;
                _height = aexp_window->height;
                if(atomic_read(&image_new) != 0) {
                        /* not "real" lock - simple check for correct struct! */
                        if(down_trylock(&image_lock))
                                break;
                        atomic_set(&image_new, 0);
                        /* calculate window width */
                        aexp_window->width = (window_sensor.width * aexp_window_set->width) / 100;
                        aexp_window->left = ((window_sensor.width - aexp_window->width) * aexp_window_set->left) / 100;
                        aexp_window->left += 2;
                        aexp_window->height = (window_sensor.height * aexp_window_set->height) / 100;
                        aexp_window->top = ((window_sensor.height - aexp_window->height) * aexp_window_set->top) / 100;
                        aexp_window->top += 2;
                        aexp_window->top &= 0xFFFE;
                        /* align histogram window */
                        if(aexp_window->width != 0 && aexp_window->height != 0) {
                                port_csp0_addr[X313_WA_HIST_SIZE] = (((aexp_window->height - 1) & 0xFFFF) << 16) + ((aexp_window->width - 1) & 0xFFFF);
                                port_csp0_addr[X313_WA_HIST_POS] = ((aexp_window->top & 0xFFFF) << 16) + (aexp_window->left & 0xFFFF);
                        }
                        up(&image_lock);
                }
                /* reprogram sensor exposition time */
                /* wait for new exposure value */
                if(atomic_read(&sensor_refresh) == 0)
                        return;
                if(program_sensor_exposition() != 0)
                        break;
//              atomic_set(&sensor_refresh, 0);
                atomic_set(&image_reprogrammed, 0);
                step = STEP_2;
/*
                if(atomic_read(&image_reprogrammed) == 1) {
                        step = STEP_INIT;
                        goto restep;
                }
*/
                break;
        /*
         * it's bad frame - just skip it, and relook if image reprogramming - return to STEP_1
         */
        case STEP_2:
                if(atomic_read(&image_reprogrammed) != 0) {
                        step = STEP_1;
                        goto restep;
                }
                step = STEP_3;
                break;
        /*
         * it's good frame - simple wait for next interrupt with 'good' histogram, if image reprogramming - return to STEP_2
         */
        case STEP_3:
                if(atomic_read(&image_reprogrammed) != 0) {
                        step = STEP_1;
                        goto restep;
                }
                step = STEP_4;
                break;
        /*
         * ok - now return correct histogram and wait for results in next interrupt
         */
        case STEP_4:
                /* previous histogram not correct programmed - wait for new... */
                if(_width == 0 || _height == 0) {
                        step = STEP_1;
                        return;
                }
                /* ...read histogram in free buffer... */
                for(i = 0; i < BUF_C; i++) {
                        if(down_trylock(&buf[i].lock))
                                continue;
                        in_busy_skip = 0;
                        b = &buf[i];
                        _g = (unsigned short *)(((void *)b->buf) + 1024 * 4);
                        memcpy((void *)b->buf, (void *)hb_0, 256 * 4 * 4);
                        b->tv = tv;
                        /* copy reverse and original gamma */
                        if(tv_less(&gamma[0].tv, &gamma[1].tv)) {
                                g_1 = &gamma[0];
                                g_2 = &gamma[1];
                        } else {
                                g_1 = &gamma[1];
                                g_2 = &gamma[0];
                        }
                        if(g_1->tv.tv_sec == 0)
                                g_1 = g_2;
                        else {
                                if(tv_less(&b->tv, &g_2->tv))
                                        g_1 = g_1;
                                else
                                        g_1 = g_2;
                        }
                        memcpy((void *)_g, (void *)g_1->buf + 256 * 4 * 2, 256 * 2 * 2);
                        /* --==-- */
                        up(&b->lock);
                        break;
                }
                /* ...and awake for mmap */
                if(in_busy_skip == 0) {
                        wake_up_interruptible(&wq_hist);
                        step = STEP_1;
                }
                break;
        default:
                return;
        }
}

struct hist_sensor_t *hist_sensor_lock(void) {
        if(atomic_read(&autoexp_enable) == 0)
                return NULL;
        if(down_interruptible(&sensor_lock))
                return NULL;
        if(first_exp < 3) {
                first_exp++;
//printk("hist_sensor_up: autoexp_state->exp = %lu, first_exp == %d\r\n", autoexp_state->exp, first_exp);
                sensor_desc.exposure = autoexp_state->exp;
        }
        return &sensor_desc;
}

void hist_sensor_unlock(void) {
        up(&sensor_lock);
}

void hist_image_exp(unsigned long exp) {
        if(first_exp < 3) {
                autoexp_set->exp = exp;
                autoexp_state->exp = exp;
//printk("hist_image_exp: exp == %lu\r\n", exp);
        }
}

void hist_image_size(unsigned long width, unsigned long height) {
        if(window_sensor.width != width || window_sensor.height != height) {
                down(&image_lock);
                window_sensor.width = width - 4;
                window_sensor.height = height - 4;
                atomic_set(&image_new, 1);
//              atomic_set(&image_reprogrammed, 1);
                atomic_set(&sensor_window, 1);
                up(&image_lock);
        }
}

void window2state (void) {
                        autoexp_state->width = aexp_window_set->width;
                        autoexp_state->height = aexp_window_set->height;
                        autoexp_state->left = aexp_window_set->left;
                        autoexp_state->top = aexp_window_set->top;
//                      autoexp_state->exp = sensor_desc.exposure;
                        autoexp_state->exp = get_imageParamsR(P_EXPOS);
//                      copy_to_user((void *)arg, &state, sizeof(state));
}
void get_autoexposure_parameters(void) 
{
                if(down_interruptible(&autoexp_conf_lock) == 0) {
//                      copy_from_user(autoexp_set, (const void *)arg, sizeof(*autoexp_set));
                        window2state();
                        up(&autoexp_conf_lock);
                }
}


void set2state(void) 
{
                        autoexp_conf_new = 1;
                        if(autoexp_set->on != 0 && autoexp_set->on != HIST_NOT_CHANGE) {
                                autoexp_state->on = 1;
//                              if(atomic_read(&autoexp_on) != 0)
                                atomic_set(&autoexp_enable, 1);
                        }
                        if(autoexp_set->on == 0) {
                                autoexp_state->on = 0;
                                atomic_set(&autoexp_enable, 0);
                                exposition_unlock();
//printk("");
                        }
                        if(autoexp_set->s_index != HIST_NOT_CHANGE)
                                autoexp_state->s_index = autoexp_set->s_index;
                        if(autoexp_set->s_percent != HIST_NOT_CHANGE)
                                autoexp_state->s_percent = autoexp_set->s_percent;
                        if(autoexp_set->overexp_max != HIST_NOT_CHANGE)
                                autoexp_state->overexp_max = autoexp_set->overexp_max;
                        if(autoexp_set->exp_max != HIST_NOT_CHANGE)
                                autoexp_state->exp_max = autoexp_set->exp_max;
                        if(autoexp_set->skip_pmin != HIST_NOT_CHANGE)
                                autoexp_state->skip_pmin = autoexp_set->skip_pmin;
                        if(autoexp_set->skip_pmax != HIST_NOT_CHANGE)
                                autoexp_state->skip_pmax = autoexp_set->skip_pmax;
                        if(autoexp_set->skip_t != HIST_NOT_CHANGE)
                                autoexp_state->skip_t = autoexp_set->skip_t;
}
void set_autoexposure_parameters(void) 
{
                if(down_interruptible(&autoexp_conf_lock) == 0) {
//                      copy_from_user(autoexp_set, (const void *)arg, sizeof(*autoexp_set));
                        set2state(); 
                        up(&autoexp_conf_lock);
                        wake_up_interruptible(&wq_hist);
                }
}

/*******************************************************************************
 *
 * gamma
 */

/*
#define GAMMA_COUNT                     200
//#define GAMMA_TABLE_SIZE      256
#define GAMMA_TABLE_SIZE        257
#define _GAMMA_TABLE_SIZE       257

//static unsigned short gamma_tbl[_GAMMA_TABLE_SIZE * GAMMA_COUNT];
//static unsigned short *gamma_ptr = gamma_tbl;
static unsigned short *gamma_ptr = NULL;
*/

void __add_gamma(unsigned short index, unsigned short *table) {
        unsigned short *ptr;
        unsigned short value;
        int i;

        if(index >= GAMMA_COUNT) {
                printk("gamma fault! index == %d\r\n", index);
                return;
        }
        value = table[0];
        if(value == 0 || value > GAMMA_COUNT) {
                printk("gamma fault! value == %d\r\n", value);
                return;
        }
        ptr = gamma_ptr + (value - 1) * GAMMA_TABLE_SIZE;
        for(i = 0; i < GAMMA_TABLE_SIZE; i++)
                ptr[i] = (table[i + 1] >> 8) + (0xFF00 & (table[i + 1] << 8));
}

/*******************************************************************************
 *
 * autoexp
 */

extern unsigned long *ccam_dma;

#define _WRITE_GAMMA                    0x10
#define _WRITE_GAMMA_BUF                (2 + _GAMMA_TABLE_SIZE * 2)

struct autoexp_context_t {
        int write;
        int count;
        unsigned char *buf;
        int buf_fill;
        int c;
};

static int autoexp__open(struct inode *inode, struct file *filp) {
        filp->private_data = NULL;
        return 0;
}

static int autoexp__release(struct inode *inode, struct file *filp) {
        if(filp->private_data)
                kfree(filp->private_data);
        return 0;
}

static int fill_buffer(int c_query, const char *buf, size_t count, size_t *offset, struct autoexp_context_t *c) {
        int r = 0;
        int len;

        len = c_query - c->buf_fill;
        if(len > count)
                len = count;
        r = copy_from_user((void *)&c->buf[c->buf_fill], &buf[*offset], len);
        c->buf_fill += len;
        *offset += len;
        return len;
}

static ssize_t autoexp__write(struct file *filp, const char *buf, size_t count, loff_t *offp) {
        struct autoexp_context_t *c;
        size_t offset = 0;
        size_t ret = count;

        c = (struct autoexp_context_t *)filp->private_data;
        if(c == NULL) {
                return -EFAULT;
//              printk("filp->private_data == NULL\n");
        }
        switch(c->write) {
                case _WRITE_GAMMA:
                        while(count) {
                                count -= fill_buffer(_WRITE_GAMMA_BUF, buf, count, &offset, c);
                                if(c->buf_fill == _WRITE_GAMMA_BUF) {
                                        __add_gamma(c->c, (unsigned short *)c->buf);
                                        c->c++;
                                        c->buf_fill = 0;
                                }
                        }
                        break;
                default: {
//                      printk("c->write != _WRITE_GAMMA\n");
                        return -EFAULT;
                }
        }
        *offp += ret;
        return ret;
}

static int autoexp__ioctl(struct inode *inode, struct file *filp, unsigned int cmd, unsigned long arg) {
        struct autoexp_context_t *c;
        int ae_c;
        int r, t;

        switch(_IOC_TYPE(cmd)) {
        case IOC_AUTOEXP_GET_LOG:
                ae_c = _IOC_NR(cmd);
                if(ae_c > LOG_C)
                        return -EFAULT;
                if(ae_log_c == 0)
                        return -EFAULT;
                if(down_interruptible(&ae_log_lock))
                        return -EFAULT;
                if(ae_c > ae_log_c)
                        ae_c = ae_log_c;
                r = ae_c;
                if(ae_log_c == LOG_C) {
                        t = &ae_log_buf[LOG_C] - ae_log_p;
                        if(ae_c <= t) {
                                copy_to_user((void *)arg, (void *)ae_log_p, ae_c * sizeof(struct autoexp_log_t));
                        } else {
                                copy_to_user((void *)arg, (void *)ae_log_p, t * sizeof(struct autoexp_log_t));
                                ae_c -= t;
                                copy_to_user((void *)(arg + sizeof(struct autoexp_log_t) * t), (void *)ae_log_buf, ae_c * sizeof(struct autoexp_log_t));
                        }
                } else {
                        copy_to_user((void *)arg, (void *)ae_log_p, ae_c * sizeof(struct autoexp_log_t));
                }
                up(&ae_log_lock);
                return r;
        case IOC_AUTOEXP_SET:
                /* if width and height changed - refresh */
//printk("s ");
                if(down_interruptible(&autoexp_conf_lock) == 0) {
//                      copy_from_user(&autoexp_conf, (const void *)arg, sizeof(autoexp_conf));
                        copy_from_user(autoexp_set, (const void *)arg, sizeof(*autoexp_set));
                        set2state(); 
#if 0
                        autoexp_conf_new = 1;
                        if(autoexp_set->on != 0 && autoexp_set->on != HIST_NOT_CHANGE) {
                                autoexp_state->on = 1;
//                              if(atomic_read(&autoexp_on) != 0)
                                atomic_set(&autoexp_enable, 1);
                        }
                        if(autoexp_set->on == 0) {
                                autoexp_state->on = 0;
                                atomic_set(&autoexp_enable, 0);
                                exposition_unlock();
//printk("");
                        }
                        if(autoexp_set->s_index != HIST_NOT_CHANGE)
                                autoexp_state->s_index = autoexp_set->s_index;
                        if(autoexp_set->s_percent != HIST_NOT_CHANGE)
                                autoexp_state->s_percent = autoexp_set->s_percent;
                        if(autoexp_set->overexp_max != HIST_NOT_CHANGE)
                                autoexp_state->overexp_max = autoexp_set->overexp_max;
                        if(autoexp_set->exp_max != HIST_NOT_CHANGE)
                                autoexp_state->exp_max = autoexp_set->exp_max;
                        if(autoexp_set->skip_pmin != HIST_NOT_CHANGE)
                                autoexp_state->skip_pmin = autoexp_set->skip_pmin;
                        if(autoexp_set->skip_pmax != HIST_NOT_CHANGE)
                                autoexp_state->skip_pmax = autoexp_set->skip_pmax;
                        if(autoexp_set->skip_t != HIST_NOT_CHANGE)
                                autoexp_state->skip_t = autoexp_set->skip_t;
#endif
                        up(&autoexp_conf_lock);
                        wake_up_interruptible(&wq_hist);
                }
//printk("S ");
                break;
        case IOC_AUTOEXP_GET:
//printk("g ");
                if(down_interruptible(&autoexp_conf_lock) == 0) {
         window2state();
#if 0
//                      autoexp_state->exp_max = autoexp_set->exp_max;
//                      autoexp_state->overexp_max = autoexp_set->overexp_max;
//                      autoexp_state->exp = autoexp_set->exp;
//                      autoexp_state->on = atomic_read(&autoexp_on);
                        autoexp_state->width = aexp_window_set->width;
                        autoexp_state->height = aexp_window_set->height;
                        autoexp_state->left = aexp_window_set->left;
                        autoexp_state->top = aexp_window_set->top;
//                      autoexp_state->exp = sensor_desc.exposure;
                        autoexp_state->exp = get_imageParamsR(P_EXPOS);
#endif
//                      copy_to_user((void *)arg, &state, sizeof(state));
                        copy_to_user((void *)arg, autoexp_state, sizeof(*autoexp_state));
                        up(&autoexp_conf_lock);
                }
//printk("G ");
                break;
        case IOC_AUTOEXP_GAMMA_TABLE:
//printk("set wrote gamma table\n");
                c = (struct autoexp_context_t *)kmalloc(sizeof(struct autoexp_context_t), GFP_KERNEL);
                if(c == NULL) {
                        printk("fail kmalloc context\r\n");
                        return -EFAULT;
                }
                c->write = _WRITE_GAMMA;
                c->count = 0;
                c->c = 0;
                c->buf_fill = 0;
                c->buf = (unsigned char *)ccam_dma;
                filp->private_data = (void *)c;
                break;
        default:
                return -EFAULT;
        }
        return 0;
}

static void autoexp__vma_close(struct vm_area_struct *vma);
static struct vm_operations_struct autoexp__vm_ops = {
        .close = autoexp__vma_close,
};

int autoexp__mmap(struct file *filp, struct vm_area_struct *vma) {

        vma->vm_ops = &autoexp__vm_ops;
        if(vma->vm_start == 0x00) {
//printk("m1 ");
//              return remap_page_range(   vma->vm_start, virt_to_phys(hb_0),              vma->vm_end - vma->vm_start, vma->vm_page_prot);
      return remap_pfn_range(vma,vma->vm_start, virt_to_phys(hb_0) >> PAGE_SHIFT,vma->vm_end - vma->vm_start, vma->vm_page_prot);
        }
        if(vma->vm_start == 0x10000000) {
//printk("m2 ");
//              return remap_page_range   (vma->vm_start, virt_to_phys(gamma_table),              vma->vm_end - vma->vm_start, vma->vm_page_prot);
      return remap_pfn_range(vma,vma->vm_start, virt_to_phys(gamma_table) >> PAGE_SHIFT,vma->vm_end - vma->vm_start, vma->vm_page_prot);
        }
        return -EFAULT;
}

void autoexp__vma_close(struct vm_area_struct *vma) {
//printk("Mx ");
}

/*******************************************************************************
 *
 *
 */

static unsigned long _gamma = 0xFFFFFFFF;
static unsigned long _black = 0x00;

static unsigned short _scale_red = 0x0100;
static unsigned short _scale_green = 0x0100;
static unsigned short _scale_blue = 0x0100;
static unsigned short _scale_green1 = 0x0100; 

#define FPGA333_TABLE_ADDR_GAMMA        0x0400

// make a interpolation of the value between of the two table values;
// index is a X coordinate of the point in 8.8 fixed point format
unsigned long gamma_v(unsigned long index, unsigned short *table) {
        unsigned long v1, v2;
        unsigned long i;
        unsigned long step;
        unsigned long delta;

        if(index >= 0xFF00)
                return 0xFFFF;
        i = index >> 8;
        v1 = table[i];
        v2 = table[i + 1];
        step = index & 0xFF;
        delta = v2 - v1;
        delta = (step * delta) >> 8;
        v1 += delta;
        if(v1 > 0xFFFF)
                v1 = 0xFFFF;
        return v1;
}

unsigned short _gamma_table_or[GAMMA_TABLE_SIZE];
unsigned short _gamma_table[256 * 6];
unsigned long gamma_table_to_fpga[256 * 4];

void set_gamma(unsigned long __gamma, unsigned long black, unsigned long scale_red, unsigned long scale_green, unsigned long scale_blue, unsigned long scale_green1) {
        u16 *g;
        u32 v;
        int i;
        int j;
        int k;
        int l;
        long x1, y1, x2, y2, a, b;
        long value;
        unsigned long scale = 0x100;
        unsigned short *_gt;
/*
        unsigned short _gamma_table[256 * 6];
        unsigned long gamma_table_to_fpga[256 * 4];
*/

//      unsigned short *_gamma_table;
//      unsigned long *gamma_table_to_fpga;
        
        struct timeval tv;
        int base, diff, d;

MD1(printk("!!! ----->>>>>>  set gamma: 0x%08X  <<<<<<<<---------- !!!!!!!!!!\n", __gamma));
//printk("\n\nscale_red == 0x%04X; scale_green == 0x%04X; scale_blue == 0x%04X\n", scale_red, scale_green, scale_blue);

        // TODO - use from 100 to 199 for the custom tables, from the start - fill it with a linear tables (i.e. gamma == 1.0)
        if(__gamma >= 200)
                __gamma = 199;
        if(black > 32)
                black = 32;
        if(black > 256)
                black = 0;
        if(_gamma == __gamma && black == _black && scale_red == _scale_red && scale_green == _scale_green && scale_blue == _scale_blue  && scale_green1 == _scale_green1)
                return;
/*
        if(_gamma == __gamma && black == _black && scale_red == _scale_red && scale_green == _scale_green && scale_blue == _scale_blue  && scale_green1 == _scale_green1) {
                printk("set gamma - skip\n");
                return;
        } else {
                printk("_gamma == %d, __gamma == %d\n", _gamma, __gamma);
                printk("black == %d, _black == %d\n", black, _black);
                printk("scale_red == %d, _scale_red == %d\n", scale_red, _scale_red);
                printk("scale_green == %d, _scale_green == %d\n", scale_green, _scale_green);
                printk("scale_blue == %d, _scale_blue == %d\n", scale_blue, _scale_blue);
                printk("scale_green1 == %d, _scale_green1 == %d\n", scale_green1, _scale_green1);
                printk("set gamma...\n");
        }
*/
//      _gamma_table = (unsigned short *)kmalloc(GFP_ATOMIC, 256 * 6 * sizeof(unsigned short));
//      gamma_table_to_fpga = (unsigned long *)kmalloc(GFP_ATOMIC, 256 * 4 * sizeof(unsigned long));
//      unsigned short *_gamma_table = (unsigned short *)kmalloc(GFP_KERNEL, 256 * 6 * sizeof(unsigned short));
//      unsigned long *gamma_table_to_fpga = (unsigned long *)kmalloc(GFP_KERNEL, 256 * 4 * sizeof(unsigned long));
//      unsigned short *_gamma_table_or = (unsigned short *)kmalloc(GFP_KERNEL, GAMMA_TABLE_SIZE * sizeof(unsigned short));
//printk("_gamma_table == 0x%08X\n", _gamma_table);
//printk("gamma_table_to_fpga == 0x%08X\n", gamma_table_to_fpga);
//printk("_gamma_table_or == 0x%08X\n", _gamma_table_or);
//*/

        _gamma = __gamma;
        _black = black;
        _scale_red = scale_red;
        _scale_green = scale_green;
        _scale_blue = scale_blue;
        _scale_green1 = scale_green1;
        g = (u16 *)(gamma_ptr + (_gamma * _GAMMA_TABLE_SIZE));
        // recalculate and write gamma table
        l = 0;
        int skip_interpolation = 0;
        if(_black == 0 && scale_red == 0x100 && scale_green == 0x100 && scale_blue == 0x100 && scale_green1 == 0x100) {
//printk("for the gamma - must to skip interpolation\n");
                skip_interpolation = 1;
        }
        for(j = 0; j < 4; j++) {
                if(skip_interpolation) {
//printk("........gamma - ........skip interpolation\n");
                        for(i = 0; i < GAMMA_TABLE_SIZE; i++) {
                                if(i > 0) {
                                        _gamma_table[j * 256 + i - 1] = g[i - 1] & 0xFFFF;
                                }
                                _gamma_table_or[i] = g[i] & 0xFFFF;
                        }
                } else {
                        switch(j) {
                                case 0: scale = scale_red;              break;
                                case 1: scale = scale_green;    break;
                                case 2: scale = scale_green1;   break;
                                case 3: scale = scale_blue;             break;
                        }
                        // Y = A * X + B
                        // calculate A & -B factors in fixed point 8.8 format
                        x1 = (_black << 8);
                        y1 = 0;
                        x2 = 0xFFFFFF / scale;
                        y2 = 0xFFFF;
                        a = ((y2 - y1) << 8) / (x2 - x1);
                        b = ((a * x2) >> 8) - y2;
                        for(i = 0; i < GAMMA_TABLE_SIZE; i++) {
                                if(i <= _black) {
                                        value = 0x00;
                                } else {
                                        v = (a * ((i << 8) + i)) >> 8;
                                        value = gamma_v(v - b, g);
                                        if(value > 0xFFFF)
                                                value = 0xFFFF;
                                }
                                if(i + 1 == GAMMA_TABLE_SIZE)
                                        value = 0xFFFF;
                                if(i > 0) {
//                                      _gamma_table[j * 256 + i - 1] = g[i] & 0xFFFF;
                                        _gamma_table[j * 256 + i - 1] = value & 0xFFFF;
                                }
                                _gamma_table_or[i] = (value & 0xFFFF) >> 6;
                        }
                }
                // convert gamma table to FPGA format
                for(i = 0; i < 256; i++) {
                        base = _gamma_table_or[i];
                        diff = _gamma_table_or[i + 1];
                        diff -= base;
                        if((diff > 63) || (diff < -64)) {
                                diff = (diff + 8) >> 4;
                                d = (base & 0x3ff) | ((diff & 0x7f) << 10) | (1 << 17);
                        } else {
                                d = (base & 0x3ff) | ((diff & 0x7f) << 10);
                        }
                        gamma_table_to_fpga[l] = d;  // 18-bit data, will autoincrement address
                        l++;
                }
        }
//      fpga_table_write(FPGA333_TABLE_ADDR_GAMMA, gamma_table_to_fpga, 256 * 4);
        // write gamma table to FPGA
        fpga_table_write(FPGA333_TABLE_ADDR_GAMMA, gamma_table_to_fpga, 256 * 4);
/*
        port_csp0_addr[0x0E] = FPGA333_TABLE_ADDR_GAMMA;
        for(i = 0; i < 256 * 4; i++) {
                        port_csp0_addr[0x0F] = gamma_table_to_fpga[i];
        }
        // to flush the last write data
        port_csp0_addr[0x0E] = FPGA333_TABLE_ADDR_GAMMA;
*/
//*/

        // calculate reversed gamma
//printk("g2 ");
        // clear gamma table
        _gt = _gamma_table + 256 * 4;
        memset((void *)_gt, 0, 256 * 2);
        if(_scale_red < _scale_green && _scale_red < _scale_blue && _scale_red < _scale_green1)
                k = 0;
        else {
                if(_scale_green < _scale_blue  && _scale_green < _scale_green1) { 
                        k = 256;
                } else {        
                        if(_scale_green1 < _scale_blue) {
                                k = 512;
                        } else {        
                                k = 768;
                        }
                }
        }
//printk("g3 ");
        // simple copy
        for(i = 0; i < 256; i++)
                _gt[_gamma_table[i + k] >> 8] = i << 8;
        // make interpolation
        for(i = 1; i < 256; i++) {
                if(_gt[i] == 0) {
                        for(j = i; j < 256; j++) {
                                if(_gt[j] != 0)
                                        break;
                        }
                        a = 0x100 / (j - i + 1);
                        for(; i < j; i++)
                                _gt[i] = _gt[i - 1] + a;
                }
        }
//printk("g4 ");
        // make copy of original gamma
        _gt = _gamma_table + 256 * 5;
        memcpy((void *)_gt, (void *)g, 256 * 2);
        // set new gamma table...
        do_gettimeofday(&tv);
//printk("gl ");
        if(down_interruptible(&gamma_lock) == 0) {
                tv_gamma = tv;
                memcpy((void *)gamma_table, (void *)_gamma_table, 256 * 6 * 2);
                memcpy(((void *)hb_0) + OFFSET_GAMMA_C, &c_gamma, 4);
                c_gamma++;
                up(&gamma_lock);
        }

MD1(printk("gamma... OK!\n"));
//      kfree((void *)_gamma_table);
//      kfree((void *)gamma_table_to_fpga);
//      kfree((void *)_gamma_table_or);
//*/
}

---