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

Go to the documentation of this file.

/*!********************************************************************************
*! FILE NAME  : mt9x001.c
*! DESCRIPTION: sensor support for Micron image sensors:
*! MT9M001(1280x1024)
*! MT9D001(1600x1200)
*! MT9T001(2048x1536)
*! MT9P001/MT9P031(2592x1944)
*!
*! Copyright (C) 2004-2007 Elphel, Inc.
*! -----------------------------------------------------------------------------**
*!
*!  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/>.
*! -----------------------------------------------------------------------------**
*!  $Log: mt9x001.c,v $
*!  Revision 1.10  2008/07/23 02:24:57  elphel
*!  typo
*!
*!  Revision 1.9  2008/05/02 15:13:35  elphel
*!  switch to hardware i2c write,added related sensor parameters
*!
*!  Revision 1.8  2008/05/01 01:28:57  elphel
*!  hardware i2c control - macros, variables
*!
*!  Revision 1.7  2008/03/20 22:29:30  elphel
*!  added trigger-related code and parameters
*!
*!  Revision 1.6  2008/02/12 21:53:20  elphel
*!  Modified I2c to support multiple buses, added raw access (no address registers) and per-slave protection bitmasks
*!
*!  Revision 1.5  2008/02/05 18:28:59  spectr_rain
*!  correct timing for MT9P sensor with binning and skipping 1,2,4
*!
*!  Revision 1.4  2007/12/27 14:27:43  spectr_rain
*!  fixed FPS, correct work with the skipping - TODO - check the binning
*!
*!  Revision 1.3  2007/10/16 23:17:51  elphel
*!  cosmetic
*!

//  Revision 1.2  2007/10/16 20:03:38  elphel
//  cosmetic
//
//  Revision 1.1.1.1  2007/09/30 03:19:56  elphel
//  This is a fresh tree based on elphel353-2.10
//
//  Revision 1.12  2007/09/30 03:19:56  elphel
//  Cleanup, fixed broken acquisition of individual JPEG images into circbuf (in mode 7)
//
//  Revision 1.11  2007/09/19 04:22:33  elphel
//  fixed debug output
//
//  Revision 1.10  2007/09/19 00:34:21  elphel
//  support for frame rate limiting, disabled extra debug  messages
//
//  Revision 1.9  2007/09/18 06:09:22  elphel
//  support for merged P_FLIP, support fro P_FPSLM (fps limit mode) that allow to limit FPS (upper limit) and maintain FPS (lower limit) preventing exposure time to exceed one divided by fps limit
//
//  Revision 1.8  2007/09/12 18:05:35  spectr_rain
//  *** empty log message ***
//
//  Revision 1.7  2007/08/17 10:23:19  spectr_rain
//  switch to GPL3 license
//
//  Revision 1.6  2007/07/20 10:17:46  spectr_rain
//  *** empty log message ***
//
//  Revision 1.10  2007/07/09 22:05:43  elphel
//  minor sensor phase adjustment (constant so far)
//
//  Revision 1.9  2007/07/09 21:10:14  elphel
//  Reducing EMI from the sensor front end cable by reducing the drive strengths on the lines. Improving phase adjustment (and phase error measurements) for the sensors. Dealing with the delayed (with respect to pixel data) line/frame valid signals in MT9P001 sensors
//
//  Revision 1.8  2007/07/09 05:15:17  elphel
//  set slowest speed on the outputs to reduce EMI
//
//  Revision 1.7  2007/06/28 02:24:40  elphel
//  Increased default frequency for 5MPix sensor to 96MHz
//
//  Revision 1.6  2007/06/18 07:57:24  elphel
//  Fixed bug working with MT9P031 - added sensor reset/reinit after frequency change
//
//  Revision 1.5  2007/05/21 17:45:11  elphel
//  boundary scan support, added 359/347 detection
//
//  Revision 1.4  2007/04/23 22:48:32  spectr_rain
//  *** empty log message ***
//
//  Revision 1.3  2007/04/17 18:28:06  spectr_rain
//  *** empty log message ***
//
//  Revision 1.2  2007/04/04 03:55:22  elphel
//  Improved i2c, added i2c as character devices (to use from php)
//
//  Revision 1.1.1.1  2007/02/23 10:11:48  elphel
//  initial import into CVS
//
//  Revision 1.12  2006/12/01 00:18:12  spectr_rain
//  *** empty log message ***
//
//  Revision 1.11  2006/11/28 18:33:26  spectr_rain
//  *** empty log message ***
//
//  Revision 1.9  2006/11/01 18:49:57  spectr_rain
//  *** empty log message ***
//
//  Revision 1.8  2006/09/12 15:21:55  spectr_rain
//  use ve for integration time if e == 0
//
//  Revision 1.7  2006/09/02 22:04:17  spectr_rain
//  *** empty log message ***
//
//  Revision 1.6  2006/09/02 00:19:49  spectr_rain
//  lock sensor while readrawimage
//
//  Revision 1.5  2006/07/17 12:22:42  spectr_rain
//  enable autoexposition
//
//  Revision 1.4  2006/07/13 04:21:15  elphel
//  mt9p031 now wants the same bayer as others - maybe DLYHOR bit was set earlier?
//
//  Revision 1.3  2006/07/12 19:32:49  spectr_rain
//  fix Bayer pattern for less than 5MPx sensors
//
//  Revision 1.2  2006/07/12 06:03:16  elphel
//  bug fix
//
//  Revision 1.1.1.1  2006/07/11 19:15:00  spectr_rain
//  unwork with less than 5MPx Micron sensor, initial branch
//
//  Revision 1.25  2006/06/05 07:31:22  spectr_rain
//  set blank vertical to 0 if pfh > 0
//
//  Revision 1.24  2006/02/16 03:37:06  elphel
//  *** empty log message ***
//
//  Revision 1.23  2006/02/08 01:53:56  spectr_rain
//  fix broken mutex in check of down_interruptible
//
//  Revision 1.22  2006/01/15 22:24:03  elphel
//  bug fix, made binning a non-stop parameter
//
//  Revision 1.21  2006/01/15 13:03:27  spectr_rain
//  fix deadlock
//
//  Revision 1.19  2006/01/12 03:52:14  elphel
//  *** empty log message ***
//
//  Revision 1.18  2006/01/10 15:41:54  spectr_rain
//  use gain compensation for color canal scale
//
//  Revision 1.17  2006/01/05 05:36:13  spectr_rain
//  *** empty log message ***
//
//  Revision 1.15  2006/01/05 05:21:32  spectr_rain
//  *** empty log message ***
//
//  Revision 1.14  2006/01/05 05:15:27  spectr_rain
//  new sensitivity/scale iface
//
//  Revision 1.13  2006/01/05 00:01:21  elphel
//  fixed hang-up - removed restarting a frame that was sometimes hanging a sensor until reset
//
//  Revision 1.12  2005/11/29 09:13:31  spectr_rain
//  add autoexposure
//
//  Revision 1.11  2005/11/23 05:13:25  spectr_rain
//  use exposition and gain with autoexposition
//
//  Revision 1.10  2005/11/22 09:21:01  elphel
//  Fixed skipping of half frames, improved period calculation (not yet for binning modes)
//
//  Revision 1.9  2005/10/12 17:58:08  elphel
//  fixed wrong fps reporting in mt9x001.c
//
//  Revision 1.8  2005/10/11 08:25:41  elphel
//  fixed long exposures without slowing down sensor (especially for 3MPix sensor)
//
//  Revision 1.7  2005/09/15 22:46:51  elphel
//  Fixed bug with 1.3MPix Micron sensor (introduced in previous release)
//
//  Revision 1.6  2005/09/10 23:33:22  elphel
//  Support of realtime clock and timestamps in the images
//
//  Revision 1.5  2005/09/06 03:40:37  elphel
//  changed parameters, added support for the photo-finish mode
//
//  Revision 1.4  2005/08/27 05:16:27  elphel
//  binning
//
//  Revision 1.3  2005/08/27 00:46:39  elphel
//  bayer control with &byr=
//
//  Revision 1.2  2005/05/10 21:08:49  elphel
//  *** empty log message ***
//

*/


/****************** INCLUDE FILES SECTION ***********************************/

#include <linux/module.h>
#include <linux/sched.h>
#include <linux/slab.h>

#include <linux/time.h>


#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/fs.h>
#include <linux/string.h>
#include <linux/init.h>
#include <linux/autoconf.h>

#include <asm/system.h>
//#include <asm/svinto.h> // obsolete
#include <asm/io.h>

#include <asm/irq.h>

#include <asm/delay.h>
#include <asm/uaccess.h>
#include <asm/elphel/c313a.h>

#include "cc3x3.h"
#include "cci2c.h"
#include "mt9x001.h"
#include "hist.h"

//#define MD9(x) printk("%s:%d:",__FILE__,__LINE__);x
#define MD9(x)
//#define D(x) printk("%s:%d:",__FILE__,__LINE__);x
#define D(x)
//#define MD7(x) printk("%s:%d:",__FILE__,__LINE__);x
#define MD7(x)

#define DEFAULT_XTRA          1000   // number of pixel times to add to the frame compression time (just a guess)

extern struct frame_params_t frame_params; // cc353.c

struct sensor_t mt9m001={
// sensor constants
   .imageWidth  = 1280,    //nominal image width for final images
   .imageHeight = 1024,    //nominal image height for final images
   .clearWidth  = 1289,    //maximal clear image width
   .clearHeight = 1033,    //maximal clear image height;
   .clearTop    = 8,       //top margin to the first clear pixel
   .clearLeft   = 16,      //left margin to the first clear pixel
   .arrayWidth  = 1312,    //total image array width (including black and boundary)
   .arrayHeight = 1048,    //total image array height (including black and boundary)
   .minWidth    = 4,       // minimal WOI width
   .minHeight   = 3,       // minimal WOI height
   .minHorBlank = 19,       // minimal horizontal blanking, in pixels in no-decimation, no-binning mode.
   .minLineDur  = 244,     // minimal total line duration, in pixels in no-decimation, no-binning mode.
   .maxHorBlank = 2047,    // maximal horizontal blanking/Virtual frame width (depends on sensor type)
   .minVertBlank= 16,       // minimal vertical blanking
   .maxVertBlank= 2047,    // maximal vertical blanking/Virtual frame height (depends on sensor type)
   .maxShutter  = 0x3fff,  // Maximal shutter duration (in lines)
   .flips       = 3,       // bit mask bit 0 - flipX, 1 - flipY
   .dcmHor      = 0x8b,    // 1,2,4,8
   .dcmVert     = 0x8b,    // 1,2,4,8
   .binHor      = 0x01,    // 1
   .binVert     = 0x01,    // 1
   .maxGain256  = 4032,    // (15.75) maximal analog gain times 0x100
   .maxClockFreq= 48000000, // Maximal clock frequency
   .nomClockFreq= 48000000, //nominal clock frequency
   .sensorType  = SENSOR_MT9X001 + MT9M_TYP,     // sensor type (for Elphel cameras)
   .i2c_addr    = MT9X001_I2C_ADDR,
   .i2c_period  = 2500,     // SCL period in ns, (standard i2c - 2500)
   .i2c_bytes   = 2,        // number of bytes/ register
   .margins     = 0,        // actually - property of FPGA code, not the sensor. Will be set in common sensor code
   .hact_delay  = 0,        // delay in ps, TBD
// dynamic variables
   .pixelWidth  = 0,        // number of the pixels output in a line (including margins, decimation)
   .pixelHeight = 0,        // number of the lines output in a frame (including margins, decimation)
   .sensorPhase90=0,        // (0..3) 90-degree shift (fpcf -w 8 80)
   .sensorPhase = 0         // positive "fpcf -w 8 20", negative - "fpcf -w 8 10"

};
struct sensor_t mt9d001={
// sensor constants
   .imageWidth  = 1600,    //nominal image width for final images
   .imageHeight = 1200,    //nominal image height for final images
   .clearWidth  = 1609,    //maximal clear image width
   .clearHeight = 1209,    //maximal clear image height;
   .clearTop    = 8,       //top margin to the first clear pixel
   .clearLeft   = 20,      //left margin to the first clear pixel
   .arrayWidth  = 1632,    //total image array width (including black and boundary)
   .arrayHeight = 1224,    //total image array height (including black and boundary)
   .minWidth    = 4,       // minimal WOI width
   .minHeight   = 3,       // minimal WOI height
   .minHorBlank = 19,      // minimal horizontal blanking, in pixels in no-decimation, no-binning mode.
   .minLineDur  = 617,     // minimal total line duration, in pixels in no-decimation, no-binning mode.
   .maxHorBlank = 2047,    // maximal horizontal blanking/Virtual frame width (depends on sensor type)
   .minVertBlank= 16,      // minimal vertical blanking
   .maxVertBlank= 2047,    // maximal vertical blanking/Virtual frame height (depends on sensor type)
   .maxShutter  = 0x3fff,  // Maximal shutter duration (in lines)
   .flips       = 3,       // bit mask bit 0 - flipX, 1 - flipY
   .dcmHor      = 0x8b,    // 1,2,4,8
   .dcmVert     = 0x8b,    // 1,2,4,8
   .binHor      = 0x01,    // 1
   .binVert     = 0x01,    // 1
   .maxGain256  = 4032,    // (15.75) maximal analog gain times 0x100
   .maxClockFreq= 48000000,      // Maximal clock frequency
   .nomClockFreq= 48000000,      //nominal clock frequency
   .sensorType  = SENSOR_MT9X001 + MT9D_TYP,     // sensor type (for Elphel cameras)
   .i2c_addr    = MT9X001_I2C_ADDR,
   .i2c_period  = 2500,     // SCL period in ns, (standard i2c - 2500)
   .i2c_bytes   = 2,        // number of bytes/ register
   .margins     = 0,        // actually - property of FPGA code, not the sensor. Will be set in common sensor code
   .hact_delay  = 0,        // delay in ps, TBD
// dynamic variables
   .pixelWidth  = 0,       // number of the pixels output in a line (including margins, decimation)
   .pixelHeight = 0,        // number of the lines output in a frame (including margins, decimation)
   .sensorPhase90=0,        // (0..3) 90-degree shift (fpcf -w 8 80)
   .sensorPhase = 0         // positive "fpcf -w 8 20", negative - "fpcf -w 8 10"
};
struct sensor_t mt9t001={
// sensor constants
   .imageWidth  = 2048,    //nominal image width for final images
   .imageHeight = 1536,    //nominal image height for final images
//   .clearWidth  = 2047,    //maximal clear image width
   .clearWidth  = 2048,    //maximal clear image width
   .clearHeight = 1545,    //maximal clear image height;
   .clearTop    = 16,      //top margin to the first clear pixel
   .clearLeft   = 28,      //left margin to the first clear pixel
   .arrayWidth  = 2112,    //total image array width (including black and boundary)
   .arrayHeight = 1568,    //total image array height (including black and boundary)
   .minWidth    = 2,       // minimal WOI width
   .minHeight   = 2,       // minimal WOI height
   .minHorBlank = 21,      // minimal horizontal blanking, in pixels in no-decimation, no-binning mode.
   .minLineDur  = 511,     // minimal total line duration, in pixels in no-decimation, no-binning mode.
   .maxHorBlank = 2047,    // maximal horizontal blanking/Virtual frame width (depends on sensor type)
   .minVertBlank= 4,       // minimal vertical blanking
   .maxVertBlank= 2047,    // maximal vertical blanking/Virtual frame height (depends on sensor type)
   .maxShutter  = 0xfffff, // Maximal shutter duration (in lines)
   .flips       = 3,       // bit mask bit 0 - flipX, 1 - flipY
   .dcmHor      = 0xff,    // 1,2,3,4,5,6,7,8
   .dcmVert     = 0xff,    // 1,2,3,4,5,6,7,8
   .binHor      = 0xff,    // 1,2,3,4,5,6,7,8
   .binVert     = 0xff,    // 1,2,3,4,5,6,7,8
   .maxGain256  = 4032,    // (15.75) maximal analog gain times 0x100
   .maxClockFreq= 48000000,      // Maximal clock frequency
   .nomClockFreq= 48000000,       //nominal clock frequency
   .sensorType  = SENSOR_MT9X001 + MT9T_TYP,     // sensor type (for Elphel cameras)
   .i2c_addr    = MT9X001_I2C_ADDR,
   .i2c_period  = 2500,     // SCL period in ns, (standard i2c - 2500)
   .i2c_bytes   = 2,        // number of bytes/ register
   .margins     = 0,        // actually - property of FPGA code, not the sensor. Will be set in common sensor code
   .hact_delay  = 0,        // delay in ps, TBD
// dynamic variables
   .pixelWidth  = 0,       // number of the pixels output in a line (including margins, decimation)
   .pixelHeight = 0,        // number of the lines output in a frame (including margins, decimation)
   .sensorPhase90=0,        // (0..3) 90-degree shift (fpcf -w 8 80)
   .sensorPhase = 0         // positive "fpcf -w 8 20", negative - "fpcf -w 8 10"
};
struct sensor_t mt9p001={
// sensor constants
   .imageWidth  = 2592,    //nominal image width for final images
   .imageHeight = 1944,    //nominal image height for final images
   .clearWidth  = 2608,    //maximal clear image width
   .clearHeight = 1952,    //maximal clear image height;
   .clearTop    = 50,      //top margin to the first clear pixel
   .clearLeft   = 10,      //left margin to the first clear pixel
   .arrayWidth  = 2752,    //total image array width (including black and boundary)
   .arrayHeight = 2003,    //total image array height (including black and boundary)
   .minWidth    = 2,       // minimal WOI width
   .minHeight   = 2,       // minimal WOI height
   .minHorBlank = 0,       // minimal horizontal blanking, in pixels in no-decimation, no-binning mode.
   .minLineDur  = 647,     // minimal total line duration, in pixels in no-decimation, no-binning mode.
   .maxHorBlank = 4095,    // maximal horizontal blanking/Virtual frame width (depends on sensor type)
   .minVertBlank= 9,       // minimal vertical blanking
   .maxVertBlank= 2047,    // maximal vertical blanking/Virtual frame height (depends on sensor type)
   .maxShutter  = 0xfffff, // Maximal shutter duration (in lines)
   .flips       = 3,       // bit mask bit 0 - flipX, 1 - flipY
   .dcmHor      = 0xff,    // 1,2,3,4,5,6,7,8 (doc show [0,6] - change to 0x7f
   .dcmVert     = 0xff,    // 1,2,3,4,5,6,7,8
   .binHor      = 0xff,    // 1,2,4 0xb{0,1,3}
   .binVert     = 0xff,    // 1,2,3,4 0xf [0,3]
   .maxGain256  = 4032,    // (15.75) maximal analog gain times 0x100
   .maxClockFreq= 96000000,      // Maximal clock frequency
//   .nomClockFreq= 48000000,       //nominal clock frequency
   .nomClockFreq= 96000000,       //nominal clock frequency - somethingv wrong in FPGA - needs a fix (fixed)
   .sensorType  = SENSOR_MT9X001 + MT9P_TYP,     // sensor type (for Elphel cameras)
   .i2c_addr    = MT9P001_I2C_ADDR,
   .i2c_period  = 2500,     // SCL period in ns, (standard i2c - 2500)
   .i2c_bytes   = 2,        // number of bytes/ register
   .margins     = 0,        // actually - property of FPGA code, not the sensor. Will be set in common sensor code
   .hact_delay  = 7500,     // 7.5ns delay in ps
// dynamic variables
   .pixelWidth  = 0,       // number of the pixels output in a line (including margins, decimation)
   .pixelHeight = 0,        // number of the lines output in a frame (including margins, decimation)
   .sensorPhase90=0,        // (0..3) 90-degree shift (fpcf -w 8 80)
   .sensorPhase = -60         // positive "fpcf -w 8 20", negative - "fpcf -w 8 10" (slow drive, 96MHz, fpga rev 0353300f)
};
extern struct sensor_t sensor; // current sensor (will be copied to by sensor driver)
static int mt9t_broken = 0x00; // what does it mean "broken" ? AF


// **************************************************************
//static int pf_stripes=0;      //number of stripes in a frame

// a place to add some general purpose register writes to sensors during init

static  unsigned int mt9m001_inits[]= \
  {
  };
// just to fix id=8501 to 8511. Maybe just some are needed
static  unsigned int mt9d001_inits[]= \
  {
   (P_MT9X001_CALTHRESH<<16) | 0xa39d,
   (P_MT9X001_CALCTRL<<16) | 0x8498
  };
static  unsigned int mt9t001_inits[]= \
  {
  };

static  unsigned int mt9p001_inits[]= \
  {
   (P_MT9X001_OUTCTRL<<16) | 0x2, // set slowesdt output signals (clock and non-clock) to reduce EMI (for FCC part 15)
   (P_MT9X001_7F     <<16) | 0x0  /* Should be written 0 to prevent blue "bluming" columns*/
  };



/* **************************************************************
 * sensor detection/initialization.
 * Noramlly called once, but it is possible to force to rerun it
 * (i.e. to recover from sensor lock up)
 ****************************************************************
 */
int     init_mt9x001(void) {
  unsigned char d[3];
  int i;
  int styp=0;
//   struct timeval tv_b;
//   struct timeval tv_e;
   unsigned char r[3];
   unsigned char w[3];
   int f;
   int first;
  d[0]=P_MT9X001_CHIPVER;
  struct sensor_t * psensor; // current sensor

// try MT9P001 first
  psensor= &mt9p001;

  i2c_writeData(0, (psensor->i2c_addr) & 0xfe,  &d[0], 1, 0); // no stop before read
  i2c_readData (0, (psensor->i2c_addr) | 1,     &d[1], 2, 0); //restart, not strart
  printk("sensor id= 0x%x\r\n",d[1]);
  if (((((d[1]<<8)+d[2]) ^ MT9P001_PARTID) & MT9X001_PARTIDMASK)==0) {
    printk("Found MT9P001 2592x1944 sensor, chip ID=%x\n",(d[1]<<8)+d[2]);
    styp=MT9P_TYP; //1;
    if(d[2] == 0x01)  mt9t_broken = 1; // what is it "broken"? Andrey
    set_imageParamsR(P_SENSOR,SENSOR_MT9X001+styp);
  }
  if (styp ==0)  { // not a 5MPix chip
    psensor= &mt9m001; //address the same for all others
    i2c_writeData(0, (psensor->i2c_addr) & 0xfe, &d[0], 1, 0); // no stop before read
    i2c_readData (0, (psensor->i2c_addr) | 1,     &d[1], 2, 0); //restart, not strart
    printk("-sensor id= 0x%x\r\n",d[1]);
    if (((((d[1]<<8)+d[2])^MT9M001_PARTID) & MT9X001_PARTIDMASK)==0) {
      printk("Found MT9M001 1280x1024 sensor, chip ID=%x\r\n",(d[1]<<8)+d[2]);
      psensor= &mt9m001;
      styp=MT9M_TYP; //1;
    } else if (((((d[1]<<8)+d[2])^MT9D001_PARTID) & MT9X001_PARTIDMASK)==0) {
      printk("Found MT9D001 1600x1200 sensor, chip ID=%x\r\n",(d[1]<<8)+d[2]);
      psensor= &mt9d001; 
      styp=MT9D_TYP; //2;
    } else if (((((d[1]<<8)+d[2])^MT9T001_PARTID) & MT9X001_PARTIDMASK)==0) {
      printk("Found MT9T001 2048x1536 sensor, chip ID=%x\r\n",(d[1]<<8)+d[2]);
      psensor= &mt9t001;
      styp=MT9T_TYP; //3;
      if(d[2] == 0x01)
      mt9t_broken = 1;
    }
   if (styp>0)   set_sensor_i2c_addr(MT9X001_I2C_ADDR);
  }
  if (styp ==0)   return 0;  // no sensor found
  memcpy(&sensor, psensor, sizeof(mt9p001));
  MD7(printk("copied %d bytes of sensor parameters\r\n",sizeof(mt9p001)));

  set_sensor_i2c_addr(sensor.i2c_addr);
//  set_imageParamsR(P_SENSOR,sensor.sensorType);
    printk("Initializing MT9X001 registers with default values:\r\n");
//#if 0

   switch (styp) {
     case MT9M_TYP:
      for (i=0;i<sizeof(mt9m001_inits)/sizeof(mt9m001_inits[0]);i++)
        writeSensorReg16(((mt9m001_inits[i]>>16) & 0xff),
                         ( mt9m001_inits[i]      & 0xffff), // i2c data (16 bit)
                          1);
      break;
     case MT9D_TYP:
      for (i=0;i<sizeof(mt9d001_inits)/sizeof(mt9d001_inits[0]);i++) {
        writeSensorReg16(((mt9d001_inits[i]>>16) & 0xff),
                         ( mt9d001_inits[i]      & 0xffff), // i2c data (16 bit)
                          1);
      }
      break;
     case MT9T_TYP:
      for (i=0;i<sizeof(mt9t001_inits)/sizeof(mt9t001_inits[0]);i++)
        writeSensorReg16(((mt9t001_inits[i]>>16) & 0xff),
                         ( mt9t001_inits[i]      & 0xffff), // i2c data (16 bit)
                          1);
      break;
     case MT9P_TYP:
      for (i=0;i<sizeof(mt9p001_inits)/sizeof(mt9p001_inits[0]);i++)
        writeSensorReg16(((mt9p001_inits[i]>>16) & 0xff),
                         ( mt9p001_inits[i]      & 0xffff), // i2c data (16 bit)
                          1);
   }
//#endif

//TODO: remove this timing hack from this function - have it separate. It can be a source of strange errors.
//*************************************
// read appropriate i2c to shadow registers
    printk("Reading all MT9X001 registers to shadows\r\n"); // let's use relevant for any of 3 sensors
   for (i=0x00;i<=0xff;i++) readSensorReg16(   i, 1);
#if 0
        /* measure read time */
        do_gettimeofday(&tv_b);
        r[0] = 0x01;
        first = 0;
        for(i = 0; i < 100; i++) {
                f = i2c_writeData(0, MT9X001_I2C_ADDR & 0xfe, &r[0], 1, 0); // no stop before read
                if(f != 0 && first == 0) {
//                      printk("read 1 error!\r\n");
                        first = 1;
                }
                f = i2c_readData(0, MT9X001_I2C_ADDR | 1, &r[1], 2, 0); //restart, not strart
                if(f != 0 && first == 0) {
//                      printk("read 1 error!\r\n");
                        first = 1;
                }
        }
        do_gettimeofday(&tv_e);
        tv_e.tv_usec += 1000000 * (tv_e.tv_sec - tv_b.tv_sec);
        tv_e.tv_usec -= tv_b.tv_usec;
        printk("\r\nsensor read delay: %ld usec\r\n", tv_e.tv_usec / 100);
        /* measure write time */
        do_gettimeofday(&tv_b);
        w[0] = 0x01;
        w[1] = r[1];
        w[2] = r[2];
        first = 0;
        for(i = 0; i < 100; i++) {
                f = i2c_writeData(0, MT9X001_I2C_ADDR & 0xfe, &w[0], 3, 1); // stop at the end of write
                if(f != 0 && first == 0) {
//                      printk("write error!\r\n");
                        first = 1;
                }
        }
        do_gettimeofday(&tv_e);
        tv_e.tv_usec += 1000000 * (tv_e.tv_sec - tv_b.tv_sec);
        tv_e.tv_usec -= tv_b.tv_usec;
        printk("\r\nsensor write delay: %ld usec\r\n", tv_e.tv_usec / 100);
        /* --==-- */
//save and restore sensor clock specified in init script (if any)
#endif
   set_imageParamsR(P_SENSOR,sensor.sensorType);
   return (sensor.sensorType);

}

/*
 * Add program_mt9x001_exposition
 */

//static int _nonstop;
//static DECLARE_MUTEX(sensor_lock);

/* ****************************************************************************************************************************************************** */
// sensor-specific corrections to decimation/binning combinations, called only when binning/decimation is changed (not nonstop)
//int program_woi_mt9x001(int nonstop);
//int program_gains_mt9x001(void);
//int program_exposure_mt9x001(void);

int adjustBinning_mt9x001(void){
        int styp=get_imageParamsR(P_SENSOR) & 0x7;
        int dh = get_imageParamsR(P_DCM_HOR);
        int dv = get_imageParamsR(P_DCM_VERT);
        int bh = get_imageParamsR(P_BIN_HOR);
        int bv = get_imageParamsR(P_BIN_VERT);
        // put here specific exceptions   
        switch(styp) {
        case MT9P_TYP: // leave same as MT9T001 for now
                // see page 45   
                break;
        case MT9T_TYP:
                break;
        }
        set_imageParamsR(P_DCM_HOR,  dh);
        set_imageParamsR(P_DCM_VERT, dv);
        set_imageParamsR(P_BIN_HOR,  bh);
        set_imageParamsR(P_BIN_VERT, bv);
}

// will use decimation, binning, flipx/y, virtual width and calcualte line duration
int program_woi_mt9x001(int nonstop) { 
        int i,dh,dv,bv,bh,ww,wh,wl,wt,pfh,flipX,flipY,d;
        int w,hbmin,p1,p2;
        int r =    0;
        int hb =   0;
        int trow = 0;
        int fclk,sclk,xtra;
        int styp = get_imageParamsR(P_SENSOR) & 0x7;

//    if  (get_imageParamsW(P_CLK_SENSOR)  > sensor.maxClockFreq) set_imageParamsW(P_CLK_SENSOR, sensor.maxClockFreq);
//       sensor.pixelHeight=h+X313_MARGINS;

        fclk=get_imageParamsW(P_CLK_FPGA);   if (fclk==0) fclk=get_imageParamsR(P_CLK_FPGA);
        sclk=get_imageParamsW(P_CLK_SENSOR); if (sclk==0) sclk=get_imageParamsR(P_CLK_SENSOR);
        xtra=get_imageParamsR(P_FPGA_XTRA);
        pfh= get_imageParamsR(P_PF_HEIGHT) & 0xffff;
        dh=  get_imageParamsR(P_DCM_HOR);
        dv=  get_imageParamsR(P_DCM_VERT);
        bh=  get_imageParamsR(P_BIN_HOR);
        bv=  get_imageParamsR(P_BIN_VERT);
//      ww=  get_imageParamsR(P_WOI_WIDTH)+X313_MARGINS;
//      wh=  get_imageParamsR(P_WOI_HEIGHT)+X313_MARGINS;
        ww=  sensor.pixelWidth*dh;
        wh=  sensor.pixelHeight*dv;
//      if (pfh) wh=pfh*dv;  // sensor frame hight (i.e. two lines) already done
        flipX = get_imageParamsR(P_FLIP) & 1;
        flipY = (get_imageParamsR(P_FLIP) & 2)?1:0;
        if(styp == MT9P_TYP) { 
                flipX = (get_imageParamsR(P_FLIP) & 1) ? 0 : 1;
                flipY = (get_imageParamsR(P_FLIP) & 2) ? 0 : 1;
        }
        MD7(printk("ww=%d, wh=%d, dh=%d, dv=%d\r\n", ww,wh,dv,dh));

        if(!nonstop) { // set decimation, woi size,  (binning, position, flip, virtual window - always)
                d = 2 * bh * (ww / (2 * bh));
                if(d != ww) { // correct window width if needed
                        sensor.pixelWidth = d / dh;
                        ww = d;
                }
                r |= writeSensorReg16(P_MT9X001_WIDTH, ww - 1, 0);
                d = 2 * (wh / 2);
                if(d != wh) { // correct window height if needed
                        sensor.pixelHeight = d / dh;
                        wh = d;
                        set_imageParamsR(P_WOI_HEIGHT, wh);
                }
                r |= writeSensorReg16(P_MT9X001_HEIGHT, wh - 1, 0);
                MD7(printk("ww=%d, wh=%d, dh=%d, dv=%d\r\n", ww, wh, dv, dh));
      set_imageParamsR(P_TRIG,(get_imageParamsR(P_TRIG) & ~4) | (get_imageParamsW(P_TRIG) & 4)); 
        } else { // nonstop
        } // end of stop/nonstop
        MD7(printk("sensor.pixelWidth=0x%lx, sensor.pixelHeight=0x%lx\r\n", sensor.pixelWidth,sensor.pixelHeight));

        wl = get_imageParamsR(P_WOI_LEFT);
        wt = get_imageParamsR(P_WOI_TOP);
        MD7(printk("wl=0x%x, wt=0x%x\r\n", wl, wt));
   if (!get_imageParamsR(P_OVERSIZE)) {
        if(flipX) {
                wl = sensor.clearWidth - ww - wl - sensor.margins * dh;
                   if(wl < 0) wl = 0;
           }
           if(flipY) {
                   wt = sensor.clearHeight - wh - wt - sensor.margins * dv;
                   if(wt < 0) wt = 0;
           }
   }
        MD7(printk("wl=0x%x, wt=0x%x\r\n", wl, wt));
// the fields that should not chnage in non-stop will not change, so it is OK to write them always
        if((styp == MT9T_TYP) || (styp == MT9P_TYP)) { 
                r |= writeSensorReg16(P_MT9X001_RAM,
                        (get_sensor_i2c_regs16(P_MT9X001_RAM) & 0xff88) | // preserve other bits from shadows
                        ((bv - 1) << 4) | (dv - 1),
                        0);
                r |= writeSensorReg16(P_MT9X001_CAM,
                        (get_sensor_i2c_regs16(P_MT9X001_CAM) & 0xff88) | // preserve other bits from shadows
                        ((bh - 1) << 4) | (dh - 1),
                        0);
                r |= writeSensorReg16(P_MT9X001_RMODE2,
                        (get_sensor_i2c_regs16(P_MT9X001_RMODE2) & 0x3fff) | // preserve other bits from shadows
                        (flipX ? (1 << 14) : 0) | // FLIPH - will control just alternative rows
                        (flipY ? (1 << 15) : 0) , // FLIPV
                        0);
                r |= writeSensorReg16(P_MT9X001_RMODE1,
                        (get_sensor_i2c_regs16(P_MT9X001_RMODE1) & 0xfeff) | // preserve other bits from shadows
                        ((get_imageParamsR(P_TRIG) & 4)?0x100:0) , //set async mode
                        0);
        } else {
                r |= writeSensorReg16(P_MT9X001_RMODE1,
                        (get_sensor_i2c_regs16(P_MT9X001_RMODE1) & 0xfec3) | // preserve other bits from shadows
                        ((get_imageParamsR(P_TRIG) & 4)?0x100:0) | //set async mode
                        ((dh == 4) ? (1 << 2) : 0) | // Column skip 4
                        ((dv == 4) ? (1 << 3) : 0) | // Row skip    4
                        ((dh == 8) ? (1 << 4) : 0) | // Column skip 8
                        ((dv == 8) ? (1 << 5) : 0) , // Row skip    8
                        0);
                r |= writeSensorReg16(P_MT9X001_RMODE2,
                        (get_sensor_i2c_regs16(P_MT9X001_RMODE2) & 0x3fe7) | // preserve other bits from shadows
                        ((dh == 2) ? (1 << 3) : 0) | // Column skip 2
                        ((dv == 2) ? (1 << 4) : 0) | // Row skip    2
                        (flipX ? (1 << 14) : 0) | // FLIPH - will control just alternative rows
                        (flipY ? (1 << 15) : 0) , // FLIPV
                        0);
        }
        // now position. Adjust wl, wt (increment only) to decimation/binning
   if (!get_imageParamsR(P_OVERSIZE)) {
        wt = (wt + sensor.clearTop) & 0xfffe; 
           wl = (wl + sensor.clearLeft) & 0xfffe; 
           MD7(printk("wl=%d, wt=%d\r\n", wl, wt));
           switch(styp) {
           case MT9P_TYP:
                   d = (bh > 1) ? ((bh > 2) ? 16 : 8) : 4;
                   if(flipX)
                           i = d * ((wl - 2) / d) + 2;
                   else
                        i = d * (wl / d);
                   if(i < wl)
                           i += d;
                   wl = i;
                   break;
           case MT9T_TYP:
                   d = (bh > 1) ? ((bh > 2) ? (16) : (8)) : (4);
                   if(flipX)
                           i = d * ((wl - 2) / d) + 2;
                   else
                           i = d * (wl / d);
                   if(i < wl)
                           i += d;
                   wl = i;
                   break;
           }
   }
        // program wl, wt (don't increase r?)
        writeSensorReg16(P_MT9X001_COLSTART, wl, 0);
        writeSensorReg16(P_MT9X001_ROWSTART, wt, 0);
        MD7(printk("wl=%d, wt=%d\r\n", wl,wt));
        // now - calculate line duration in clocks 
        int _binning_cost = 0;
        if(!nonstop)   { // should not change in nonstop mode
                switch(styp)  {
                case MT9P_TYP: //page 16
                        w = 2 * ww / (2 * dh);
                        if((w * dh) < ww)
                                w++;
//                      hbmin = 208 * bv + 64 + (40 / bh);
//                      hbmin = 208 * bv + 64 + (312 + 11) / 2;
            switch(bv) {
/*                              
                        case 1:
                                switch(bh) {
                                        case 2: _binning_cost = 0;  break;
                                        case 4: _binning_cost = 0;  break;
                                        break;
                                }
                                break;
*/
                        case 2:
                                switch(bh) {
                                        case 1: _binning_cost = 276;    break;
                                        case 2: _binning_cost = 236;    break;
                                        case 4: _binning_cost = 236;    break;
                                        break;
                                }
                                break;
                        case 4:
                                switch(bh) {
                                        case 1: _binning_cost = 826;    break;
                                        case 2: _binning_cost = 826;    break;
                                        case 4: _binning_cost = 826;    break;
                                        break;
                                }
                                break;
                        }
                        hbmin = 208 * bv + 64 + (312 + 44 + _binning_cost) / 2;
//                      hb = hbmin + 102;
                        hb = hbmin;
                        trow = w + hb * 2;
                        i = 2 * (41 + 208 * bv + 99); //strange 41 and 99, why not 140?
//                      MD7(printk("trow = %d, i = %d\r\n", trow, i));
                        if(i > trow)
                                trow = i;
                        if(((i = get_imageParamsW(P_VIRT_WIDTH))) > trow) { // extend line by adding horizontal blanking
                                hb = (i - w) / 2;
                                if(hb > sensor.maxHorBlank)
                                        hb = sensor.maxHorBlank;
                                trow = w + 2 * hb;
                        }
                        break;
                case MT9T_TYP:
                        w = 2 * ww / (2 * dh);
                        if((w * dh) < ww)
                                w++;
                        switch(bv) {
                                case 1:  p1 = 331; break;
                                case 2:  p1 = 673; break;
                                case 3:  p1 = 999; break;
                                default: p1 = 999; break; //undocumented
                        }
                        switch(bh) {
                                case 1:  p2 = 38; break;
                                case 2:  p2 = 22; break;
                                case 3:  p2 = 14; break;
                                default: p2 = 38; break; //undocumented
                        }
                        hb = sensor.minHorBlank;
                        trow = w + p1 + p2 + hb;
                        MD7(printk("trow=%d, w=%d, p1= %d, p2=%d, hb=%d\r\n", trow, w, p1, p2, hb));
                        if(((i = get_imageParamsW(P_VIRT_WIDTH))) > trow) { // extend line by adding horizontal blanking
                                hb = i - w - p1 - p2;
                                if(hb > sensor.maxHorBlank)
                                        hb = sensor.maxHorBlank;
                                trow = w + p1 + p2 + hb;
                        }
                        hb--; // confusing - for mt9p001 (not others which refer reg[0x05]) :"HB Horizontal Blanking Horizontal_Blank+1"
                        break;
                case MT9D_TYP:
//                      w = 2 * ww / (2 * dh); if ((w * dh) < ww) w++;
                        w = 2 * ((ww - 1) / (2 * dh)) + 2; //docs, p.6
                        hb = sensor.minHorBlank;
                        p1 = 322;
                        p2 = 2 - 19; //excluded hb
                        trow = w + p1 + p2 + hb;
                        i = p1 + 295;
                        if(i > trow)
                                trow = i;
                        MD7(printk("trow=%d, w=%d, p1= %d, p2=%d, hb=%d, i=%d\r\n", trow, w, p1, p2, hb, i));
                        if(((i = get_imageParamsW(P_VIRT_WIDTH))) > trow) { // extend line by adding horizontal blanking
                                hb = i - w - p1 - p2;
                                if (hb > sensor.maxHorBlank) hb = sensor.maxHorBlank;
                                        trow = w + p1 + p2 + hb;
                        }
                        break;
                case MT9M_TYP:
//                      w = 2 * ww / (2 * dh); if ((w * dh) < ww) w++;
                        w = 2 * ((ww - 1) / (2 * dh)) + 2; //docs, p.6
                        hb = sensor.minHorBlank;
                        p1 = 242;
                        p2 = 2 - 19; //excluded hb
                        trow= w + p1 + p2 + hb;
                        MD7(printk("trow=%d, w=%d, p1= %d, p2=%d, hb=%d\r\n", trow, w, p1, p2, hb));
                        if(((i = get_imageParamsW(P_VIRT_WIDTH))) > trow) { // extend line by adding horizontal blanking
                                hb = i - w - p1 - p2;
                                if(hb > sensor.maxHorBlank)
                                        hb = sensor.maxHorBlank;
                                trow = w + p1 + p2 + hb;
                        }
                        break;
                }
                set_imageParamsR(P_VIRT_WIDTH, trow); // line duration in clocks
                MD7(printk("trow = %d\r\n", trow));
                r |= writeSensorReg16(P_MT9X001_HORBLANK, hb, 0);
        }
        // calculate Bayer shift
        // seems nor MT9P031 wants the same. Maybe DLYHOR bit in FPGA CR was set?
//      if(styp == MT9P_TYP)
//              set_imageParamsR(P_BAYER, (flipX ? 1 : 0) | (flipY ? 0 : 2));
//      else
                set_imageParamsR(P_BAYER, (flipX ? 0 : 1) | (flipY ? 0 : 2));
        return r;
}

int normalize_gain(int g) {
        if(g < 0x0400)
                return (g & 0xFFE0);
        if(g < 0x0800)
                return (g & 0xFFC0);
        return (g & 0xFF00);
}

int gain_mt9x001(int g) {
        if(g > sensor.maxGain256)
                g = sensor.maxGain256;
        if(g <= 0x400)
                g >>= 5;
        else 
                g = (g >> 6) + 0x40;
        return g;
}

int program_gains_mt9x001(void) {
        int gg, gr, gb, ggb;
        int r = 0;
        gr  = get_imageParamsW(P_GAINR ); if (gr  > sensor.maxGain256) gr  = sensor.maxGain256 ; else if (gr  < 0) gr  = 0;
        gg  = get_imageParamsW(P_GAING ); if (gg  > sensor.maxGain256) gg  = sensor.maxGain256 ; else if (gg  < 0) gg  = 0;
        gb  = get_imageParamsW(P_GAINB ); if (gb  > sensor.maxGain256) gb  = sensor.maxGain256 ; else if (gb  < 0) gb  = 0;
        ggb = get_imageParamsW(P_GAINGB); if (ggb > sensor.maxGain256) ggb = sensor.maxGain256 ; else if (ggb < 0) ggb = 0;
//      printk("1: ggb == 0x%04X\r\n", ggb);
        gr = normalize_gain(gr);
        gg = normalize_gain(gg);
        gb = normalize_gain(gb);
        ggb = normalize_gain(ggb);
//      printk("2: ggb == 0x%04X\r\n", ggb);
        set_imageParamsR(P_GAINR, gr);
        set_imageParamsR(P_GAING, gg);
        set_imageParamsR(P_GAINB, gb);
        set_imageParamsR(P_GAINGB,ggb);
//      printk("\n____====____\ngains: gr == 0x%04X, gg == 0x%04X, gb == 0x%04X\n____----____\n", gr, gg, gb);
        r |= writeSensorReg16(P_MT9X001_RED,     gain_mt9x001(gr), 0);
        r |= writeSensorReg16(P_MT9X001_GREEN1,  gain_mt9x001(gg), 0);
        r |= writeSensorReg16(P_MT9X001_BLUE,    gain_mt9x001(gb), 0);
        r |= writeSensorReg16(P_MT9X001_GREEN2,  gain_mt9x001(ggb),0);
        frame_params.gain_r=  gr  >> 4;
        frame_params.gain_g=  gg  >> 4;
        frame_params.gain_b=  gb  >> 4;
        frame_params.gain_gb= ggb >> 4;

        if(r >= 0)
                r = 0; // disable wait here?
        return r;
}

// will set shutter duration and "virtual height" (vertical blank).
// line duration (imageParamsR[P_VIRT_WIDTH]) is set by program_woi_mt9x001()
// if imageParamsW[P_VEXPOS] >0 it will be used (measured in scan lines) instead of imageParamsW[P_EXPOS] (measured in 0.1 ms)
// both imageParamsR[P_VEXPOS] and imageParamsR[P_EXPOS] will be updated
// imageParamsW[P_FP100S]

void check_fps(void);

// TODO: add code to cc333.c/cc313.c to use imageParamsW[FP100S] to limit sensor frame rate if FPGA can not handle it??
int program_exposure_mt9x001(void) {
        int vh, vb, wh, h, dv, sclk, trow, ve, e, i;
        int r = 0;
        int styp = get_imageParamsR(P_SENSOR) & 0x7;
        // calculate minimal virtual heigth for current window height
        dv = get_imageParamsR(P_DCM_VERT);
        wh = get_sensor_i2c_regs16(P_MT9X001_HEIGHT) + 1;
        h = 2 * (wh / (2 * dv));
        if((h * dv) < wh)
                h++;
        vh = h + sensor.minVertBlank;
        sclk = get_imageParamsW(P_CLK_SENSOR);
        if(sclk == 0)
                sclk = get_imageParamsR(P_CLK_SENSOR);
        trow = get_imageParamsR(P_VIRT_WIDTH);
// now get_imageParamsW(P_FP100S) will be always non-zero (if specified 0 will use maximal compressor fps
   if ((get_imageParamsR(P_TRIG) & 4)==0) { 
     i = (((sclk * 1) / trow) * 100) / get_imageParamsW(P_FP100S); // number of virtual lines // changed to Hz instead of MHz
     if (i > vh) {
       vh = i;
     }
   }
        vb = vh - h;
//printk("fp100s=0x%08x vb=0x%08X, vh == 0x%08X\r\n", get_imageParamsW(P_FP100S), vb, vh);
        if(vb > sensor.maxVertBlank) {
                vb = sensor.maxVertBlank;
                vh = vb + h;
        }
        MD7(printk("vh=%d, h=%d, vb=%d\r\n", vh, h, vb));
        set_imageParamsR(P_VIRT_HEIGHT, vh);
        r |= writeSensorReg16(P_MT9X001_VERTBLANK, vb - 1, 0);
        //  exposure
        if((ve = get_imageParamsW(P_VEXPOS))) {
                e = (ve * trow) / (sclk/10000); //why was it vh+1?
        } else {
                if(((e = ((int)get_imageParamsW(P_EXPOS)))) < 0) {//use frame time as exposure time . get_imageParamsW - unsigned
                        // calculate actual frame period (in clocks)
                        e = (ve * trow) / (sclk/10000); //why was it vh+1?
                } else {
                        // here calculate exposure (1 unit= 100usec) and using actual clock frequency
                        i = get_imageParamsW(P_EXPOS) * (sclk/10000); // msec->MCLK periods. (<10.7 sec @ 20MHz)
                        MD7(printk("trow= %d, i=%d\r\r\n", trow, i));
                        ve = i / trow;
                }
        }
        if(e < 1)
                e = 1; // measured in 0.1 msec
        if(ve < 1)
                ve = 1; // in lines - can be more accurate than e
        if(ve > sensor.maxShutter)
                ve = sensor.maxShutter;
        if((get_imageParamsR(P_FPSLM) & 2) && (ve > vh)) ve = vh; // P_FPSLM bit 1 - "maintain FPS" (not lower than), exposure should not increase frame time
        set_imageParamsR(P_VEXPOS, ve);
        set_imageParamsR(P_EXPOS, e);
long t;
        if(vh > ve){
                set_imageParamsR(P_PERIOD, vh * trow);
//              set_imageParamsR(P_FP100S, t = (((sclk * 1000000) / trow) * 100) / vh);
                set_imageParamsR(P_FP100S, t = (((sclk * 1) / trow) * 100) / vh);
        } else {
                set_imageParamsR(P_PERIOD, ve * trow);
//              set_imageParamsR(P_FP100S, t = (((sclk * 1000000) / trow) * 100) / ve);
                set_imageParamsR(P_FP100S, t = (((sclk * 1) / trow) * 100) / ve);
        }
MD9(printk("vb == 0x%08X, vh == 0x%08X, fps == %d\r\n", vb, vh, t));
        r |= writeSensorReg16(P_MT9X001_VERTBLANK, vb - 1, 0);
//      r |= writeSensorReg16(P_MT9X001_VERTBLANK, 0xFFF0, 0);
        // set video exposure time
        if((styp == MT9T_TYP) || (styp == MT9P_TYP)) {
                r |= writeSensorReg16(P_MT9X001_SHTRWDTHU, ve >> 16, 0);
        }
        r |= writeSensorReg16(P_MT9X001_SHTRWDTH,  ve & 0xffff, 0);
//      check_fps();
        return r;
}

long max_(long a, long b) {
        long _max = (a > b) ? a : b;
        return _max;
}

void check_fps(void) {
        long H = readSensorReg16(P_MT9X001_HEIGHT, 1) + 1;
        long W = readSensorReg16(P_MT9X001_WIDTH, 1) + 1;
        long HB = readSensorReg16(P_MT9X001_HORBLANK, 1) + 1;
        long VB = readSensorReg16(P_MT9X001_VERTBLANK, 1) + 1;

        long HBmin = 312;
        long SW = max_(1, 65536 * readSensorReg16(P_MT9X001_SHTRWDTHU, 1) + readSensorReg16(P_MT9X001_SHTRWDTH, 1));
        long VBmin = max_(8, SW - H) + 1;

        long Trow = 2 * max_(W / 2 + max_(HB, HBmin), 41 + 208 + 99);
        long Tframe = (H + max_(VB, VBmin)) * Trow;
        MD9(printk("-->> Tframe == %d clck; Trow == %d clck\n", Tframe, Trow));
}

---