initial changes to dynamic page registering and sensors with 16 bit register addresses

src/drivers/elphel/detect_sensors.c

@@ -16,7 +16,7 @@
  *  You should have received a copy of the GNU General Public License
  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  *******************************************************************************/
-
+#define DEBUG
 //#include <linux/interrupt.h>
 #include <linux/platform_device.h>
 #include <linux/of_device.h>
@@ -39,19 +39,17 @@
 
 #define DETECT_SENSORS_MODULE_DESCRIPTION "Detect sensor type(s) attached to each of the ports"
 #define OF_PREFIX_NAME                    "elphel393-detect_sensors"
-struct sensor_port_config_t
- {
-     u32  mux;                 ///< sensor multiplexer, currently 0 (SENSOR_DETECT, SENSOR_MUX_10359 or SENSOR_NONE)
-     u32  sensor[MAX_SENSORS]; ///< Without mux only [0] is used, with 10359 - 0..2 are used (i2c addressing is shifted so 0 is broadcast)
- };
 
+struct sensor_port_config_t *pSensorPortConfig;
+
+// removed static to export
 static struct sensor_port_config_t sensorPortConfig[] = {
         {.mux=SENSOR_NONE,.sensor={SENSOR_NONE,SENSOR_NONE,SENSOR_NONE,SENSOR_NONE}},
         {.mux=SENSOR_NONE,.sensor={SENSOR_NONE,SENSOR_NONE,SENSOR_NONE,SENSOR_NONE}},
         {.mux=SENSOR_NONE,.sensor={SENSOR_NONE,SENSOR_NONE,SENSOR_NONE,SENSOR_NONE}},
         {.mux=SENSOR_NONE,.sensor={SENSOR_NONE,SENSOR_NONE,SENSOR_NONE,SENSOR_NONE}}
 };
-//struct sensor_port_config_t *pSensorPortConfig;
+
 static const struct of_device_id elphel393_detect_sensors_of_match[];
 static struct device *g_dev_ptr; ///< Global pointer to basic device structure. This pointer is used in debugfs output functions
 struct sensor_name_t {
@@ -393,17 +391,111 @@ static int elphel393_detect_sensors_sysfs_register(struct platform_device *pdev)
      }
  }
 
+/**
+ * Fills the tables in sensorPortConfig with key-value pairs
+ * @param par2addr - pointer to the look-up table
+ * @param table - pointer to the global struct
+ * @return 0
+ */
+static int par2addr_fill(const unsigned short *par2addr, u16 *table){
+
+	int i=0;
+
+	int key;
+	unsigned short value;
+
+	// reset
+	for(i=0;i<MAX_SENSOR_REGS;i++){
+		table[i] = 0;
+	}
+
+	i=0;
+	// fill with key-value pairs
+	while(true){
+		key   = par2addr[2*i];
+		value = par2addr[2*i+1];
+		if ((key==0xffff)||(i>255)){
+			break;
+		}
+		table[key] = value;
+		i++;
+	}
+
+	return 0;
+}
+
+/**
+ * Based on sensorPortConfig[i].sensor[j], filled from DT,
+ *   gets SENSOR_REGS to true register addresses table for
+ *   the specified sensor
+ * @return 0
+ */
+static int par2addr_init(void){
+
+	int port;
+	int sub_chn;
+
+	const unsigned short *par2addr;
+	const unsigned short *pages;
+
+	/*
+	struct sensor_port_config_t {
+		u32  mux;                                    ///< Sensor multiplexer, currently 0 (SENSOR_DETECT, SENSOR_MUX_10359 or SENSOR_NONE)
+	    u32  sensor[MAX_SENSORS];                    ///< Without mux only [0] is used, with 10359 - 0..2 are used (i2c addressing is shifted so 0 is broadcast)
+	    u16  par2addr[MAX_SENSORS][MAX_SENSOR_REGS]; ///< Big LUT. SENSOR_REGSxxx par to sensor reg 'yyy' internal address: haddr+laddr for 16 bit
+	    u16  haddr2rec[MAX_SENSORS][MAX_FPGA_RECS];  ///< Big LUT (but almost empty). Sensor's page address (haddr of reg addr) to fpga i2c record number (fpga line#)
+	};
+	*/
+
+	// all .mux and .sensor are already filled out
+	for (port = 0; port < SENSOR_PORTS; port++){
+
+		// that's from device tree, fpga is not programmed yet
+		dev_dbg(g_dev_ptr,"port: %d mux: %d sensors: %d %d %d %d\n",
+				port,
+				sensorPortConfig[port].mux,
+				sensorPortConfig[port].sensor[0],
+				sensorPortConfig[port].sensor[1],
+				sensorPortConfig[port].sensor[2],
+				sensorPortConfig[port].sensor[3]
+				);
+
+		// sub_chn = 3 is never used
+		for (sub_chn = 0; sub_chn < 4; sub_chn++){
+			//sensorPortConfig[port].sensor[sub_chn];
+			switch (sensorPortConfig[port].sensor[sub_chn]) {
+				case SENSOR_MT9P006:
+					// get sensor table
+					par2addr = mt9x001_par2addr;
+					pages    = mt9x001_pages;
+					break;
+				case SENSOR_MT9F002:
+					// get sensor table
+					break;
+			}
+			if (par2addr){
+				// convert to key-value
+				par2addr_fill(par2addr,sensorPortConfig[port].par2addr[sub_chn]);
+				// save pointer to static LUT
+				sensorPortConfig[port].pages_ptr[sub_chn] = pages;
+			}
+		}
+	}
 
+	return 0;
+}
 
  static int detect_sensors_probe(struct platform_device *pdev)
  {
-     unsigned int irq;
-     int res;
+     //unsigned int irq;
+     //int res;
      struct device *dev = &pdev->dev;
      const struct of_device_id *match;
-     const __be32 *bufsize_be;
-     struct device_node *node;
-//     pSensorPortConfig = sensorPortConfig;
+     //const __be32 *bufsize_be;
+     //struct device_node *node;
+
+     g_dev_ptr = dev; // for debugfs
+     pSensorPortConfig = sensorPortConfig;
 
      elphel393_detect_sensors_sysfs_register(pdev);
      pr_info ("Registered sysfs for detect_sensors");
@@ -415,6 +507,7 @@ static int elphel393_detect_sensors_sysfs_register(struct platform_device *pdev)
 
      detect_sensors_init_of(pdev);
 
+     par2addr_init();
 
      //    dev_dbg(dev, "Registering character device with name "DEV393_NAME(DEV393_DETECT_SENSORS));
      //    res = register_chrdev(DETECT_SENSORS_MAJOR, DEV393_NAME(DEV393_DETECT_SENSORS), &detect_sensors_fops);
@@ -422,7 +515,7 @@ static int elphel393_detect_sensors_sysfs_register(struct platform_device *pdev)
      //        dev_err(dev, "\nlogger_init: couldn't get a major number  %d.\n ",DETECT_SENSORS_MAJOR);
      //        return res;
      //    }
-     g_dev_ptr = dev; // for debugfs
+
      return 0;
  }
 

src/drivers/elphel/detect_sensors.h

@@ -16,10 +16,25 @@
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************/
+#ifndef DETECT_SENSORS_H
+#define DETECT_SENSORS_H
 
 #define DETECT_SENSOR 1 ///< Include sensors, May be OR-ed when looking for sensor/multiplexer code/name
 #define DETECT_MUX 2    ///< Include multiplexers, May be OR-ed when looking for sensor/multiplexer code/name
 
+#define MAX_SENSOR_REGS 256
+#define MAX_FPGA_RECS 256
+
+struct sensor_port_config_t {
+	u32  mux;                                    ///< Sensor multiplexer, currently 0 (SENSOR_DETECT, SENSOR_MUX_10359 or SENSOR_NONE)
+    u32  sensor[MAX_SENSORS];                    ///< Without mux only [0] is used, with 10359 - 0..2 are used (i2c addressing is shifted so 0 is broadcast)
+    u16  par2addr[MAX_SENSORS][MAX_SENSOR_REGS]; ///< Big LUT. SENSOR_REGSxxx par to sensor reg 'yyy' internal address: haddr+laddr for 16 bit
+    u8   haddr2rec[MAX_SENSORS][MAX_FPGA_RECS];  ///< Big LUT (but almost empty). Sensor's page address (haddr of reg addr) to fpga i2c record number (fpga line#)
+    unsigned short *pages_ptr[MAX_SENSORS];
+};
+
+extern struct sensor_port_config_t *pSensorPortConfig;
+
 typedef enum {NONE,PARALLEL12,HISPI4} sens_iface_t; ///< Sensor port interface type
 
 int          get_code_by_name(const char * name, int type);
@@ -32,3 +47,5 @@ int get_subchannels(int port);
 int set_detected_mux_code(int port, int mux_type);
 int set_detected_sensor_code(int port, int sub_chn,  int mux_type);
 sens_iface_t get_port_interface(int port);
+
+#endif

src/drivers/elphel/mt9x001.c

@@ -390,6 +390,69 @@
 #define MD9(x)
 #endif
 
+/**
+LUT to map SENSOR_REGSxxx to internal sensor register addresses
+  * needed for any sensor
+  * For better manual mapping:
+      - even elements are SENSOR_REGSxxx,
+      - odd elements are sensor's register addresses.
+  * has to be at least 16-bit/entry for 16 bit addresses
+  * (for MT9X001 it's a 1-to-1 mapping)
+*/
+const unsigned short mt9x001_par2addr[] = {
+		P_MT9X001_CHIPVER,       P_MT9X001_CHIPVER,
+		P_MT9X001_ROWSTART,      P_MT9X001_ROWSTART,
+		P_MT9X001_COLSTART,      P_MT9X001_COLSTART,
+		P_MT9X001_HEIGHT,        P_MT9X001_HEIGHT,
+		P_MT9X001_WIDTH,         P_MT9X001_WIDTH,
+		P_MT9X001_HORBLANK,      P_MT9X001_HORBLANK,
+		P_MT9X001_VERTBLANK,     P_MT9X001_VERTBLANK,
+		P_MT9X001_OUTCTRL,       P_MT9X001_OUTCTRL,
+		P_MT9X001_SHTRWDTHU,     P_MT9X001_SHTRWDTHU,
+		P_MT9X001_SHTRWDTH,      P_MT9X001_SHTRWDTH,
+		P_MT9X001_PXLCTL,        P_MT9X001_PXLCTL,
+		P_MT9X001_RESTART,       P_MT9X001_RESTART,
+		P_MT9X001_SHTRDLY,       P_MT9X001_SHTRDLY,
+		P_MT9X001_RESET,         P_MT9X001_RESET,
+		P_MT9X001_PLL1,          P_MT9X001_PLL1,
+		P_MT9X001_PLL2,          P_MT9X001_PLL2,
+		P_MT9X001_PLL3,          P_MT9X001_PLL3,
+		P_MT9X001_RMODE1,        P_MT9X001_RMODE1,
+		P_MT9X001_RMODE2,        P_MT9X001_RMODE2,
+		P_MT9X001_RMODE3,        P_MT9X001_RMODE3,
+		P_MT9X001_RAM,           P_MT9X001_RAM,
+		P_MT9X001_CAM,           P_MT9X001_CAM,
+		P_MT9X001_GREEN1,        P_MT9X001_GREEN1,
+		P_MT9X001_BLUE,          P_MT9X001_BLUE,
+		P_MT9X001_RED,           P_MT9X001_RED,
+		P_MT9X001_GREEN2,        P_MT9X001_GREEN2,
+		P_MT9X001_ALLGAINS,      P_MT9X001_ALLGAINS,
+		P_MT9X001_DESIRBLACK,    P_MT9X001_DESIRBLACK,
+		P_MT9X001_ROWRBLACKOFFS, P_MT9X001_ROWRBLACKOFFS,
+		P_MT9X001_COARSETHRSH,   P_MT9X001_COARSETHRSH,
+		P_MT9X001_CALTHRESH,     P_MT9X001_CALTHRESH,
+		P_MT9X001_CALGREEN1,     P_MT9X001_CALGREEN1,
+		P_MT9X001_CALGREEN2,     P_MT9X001_CALGREEN2,
+		P_MT9X001_CALCTRL,       P_MT9X001_CALCTRL,
+		P_MT9X001_CALRED,        P_MT9X001_CALRED,
+		P_MT9X001_CALBLUE,       P_MT9X001_CALBLUE,
+		P_MT9X001_7F,            P_MT9X001_7F,
+		P_MT9X001_TEST,          P_MT9X001_TEST,
+		P_MT9X001_CHIPEN,        P_MT9X001_CHIPEN,
+		P_MT9X001_CHIPEN1,       P_MT9X001_CHIPEN1,
+		0xffff // END indicator
+};
+
+/**
+ * get at least one parameter for a page
+ */
+const unsigned short mt9x001_pages[] = {
+		P_MT9X001_CHIPVER,
+		P_MT9X001_ROWSTART, // for testing
+		0x3102,             // for testing (page 0x31)
+		0xffff // END indicator
+};
+
 static struct device *g_dev_ptr=NULL; ///< Global pointer to basic device structure. This pointer is used in debugfs output functions
 void mt9x001_set_device(struct device *dev) // do nothing, now it has it's own device
 {

src/drivers/elphel/mt9x001.h

@@ -15,6 +15,10 @@
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************/
+
+#ifndef _MT9X001_H
+#define _MT9X001_H
+
 #define MT9M001_PARTID     0x8411 ///< MT9M00* PartID register value
 #define MT9D001_PARTID     0x8511 ///< MT9D00* PartID register value
 #define MT9T001_PARTID     0x1601 ///< MT9T00* PartID register value
@@ -186,6 +190,19 @@
 ///< <li> --+bit  0   -  1 - normal operation, 0 - stop readout (same as reg 0x07, bit 1)
 ///< <li> --+bit  1   -  0 - normal, appropriate changes are made at frame boudary. 1 - do not update (same as reg 7 bit 0)</ul>
 /** Detect one of Micron/Aptina/On Semiconductor sensors MT9M*, MT9D*,MT9T*, andMT9P* with parallel interface */
+
+/**
+LUT to map SENSOR_REGSxxx to internal sensor register addresses
+  * needed for any sensor
+  * For better manual mapping:
+      - even elements are SENSOR_REGSxxx,
+      - odd elements are sensor's register addresses.
+  * has to be at least 16-bit/entry for 16 bit addresses
+  * (for MT9X001 it's a 1-to-1 mapping)
+*/
+extern const unsigned short mt9x001_par2addr[];
+extern const unsigned short mt9x001_pages[];
+
 int mt9x001_pgm_detectsensor   (int sensor_port,               ///< sensor port number (0..3)
                                 struct sensor_t * sensor,      ///< sensor static parameters (capabilities)
                                 struct framepars_t * thispars, ///< sensor current parameters
@@ -201,3 +218,5 @@ int program_woi_mt9x001(int nonstop);
 int program_gains_mt9x001(void);
 int program_exposure_mt9x001(void);
 #endif
+
+#endif

src/drivers/elphel/pgm_functions.c

@@ -215,7 +215,7 @@
 #include "x393.h"
 #include "sensor_i2c.h"
 #include "x393_videomem.h"
-#include "detect_sensors.h"
+//#include "detect_sensors.h"
 #include "x393_fpga_functions.h"
 
 // NC393 debug macros
@@ -422,7 +422,8 @@ int pgm_detectsensor   (int sensor_port,               ///< sensor port number (
     i2c_stop_run_reset      (sensor_port, I2C_CMD_RUN); // also programs status update
 
     // TODO: move to specific sensor driver
-    legacy_i2c(1<<sensor_port); // Setup i2c pages for legacy i2c commands. TODO NC393: update for compatibility with 14MPix
+    //legacy_i2c(1<<sensor_port); // Setup i2c pages for legacy i2c commands. TODO NC393: update for compatibility with 14MPix
+    register_i2c_sensor(1<<sensor_port);
 
 #ifdef INIT_IN_TRIGGERED
     camsync_mode.trig =     1; // start in stopped triggered mode

src/drivers/elphel/pgm_functions.h

@@ -16,7 +16,12 @@
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *******************************************************************************/
 
+#ifndef PGM_FUNCTIONS_H
+#define PGM_FUNCTIONS_H
+
 #include "sensor_i2c.h"
+#include "detect_sensors.h"
+
 #define COLOR_MARGINS 2 // add this many pixels each side
 #define X313_TIMESTAMPLEN 28 // pixels used for timestamp (in linescan mode added after the line)
 #define X393_TILEHOR 16
@@ -63,6 +68,17 @@ unsigned long sensor_to_camsync(unsigned long pixel_time, unsigned long sensor_c
 #define SETFRAMEPARS_COND(p,v)       { if (unlikely((v)!=thispars->pars[(p) & 0xffff])) { pars_to_update[nupdate  ].num= (p) ;  pars_to_update[nupdate++].val=(v);} }
 //#define SETFRAMEPARS_COND(p,v)       { if (unlikely((v)!=thispars->pars[p])) { pars_to_update[nupdate  ].num= (p) ;  pars_to_update[nupdate++].val=(v);} }
 
+/*
+struct sensor_port_config_t {
+	u32  mux;                                    ///< Sensor multiplexer, currently 0 (SENSOR_DETECT, SENSOR_MUX_10359 or SENSOR_NONE)
+    u32  sensor[MAX_SENSORS];                    ///< Without mux only [0] is used, with 10359 - 0..2 are used (i2c addressing is shifted so 0 is broadcast)
+    u16  par2addr[MAX_SENSORS][MAX_SENSOR_REGS]; ///< Big LUT. SENSOR_REGSxxx par to sensor reg 'yyy' internal address: haddr+laddr for 16 bit
+    u8   haddr2rec[MAX_SENSORS][MAX_FPGA_RECS];  ///< Big LUT (but almost empty). Sensor's page address (haddr of reg addr) to fpga i2c record number (fpga line#)
+    unsigned short *pages_ptr[MAX_SENSORS];
+};
+
+extern struct sensor_port_config_t *pSensorPortConfig;
+*/
 
 /**Set parameter for the sensor register and send to hardware i2c sequencer
  * @param port Sensor port number
@@ -70,9 +86,9 @@ unsigned long sensor_to_camsync(unsigned long pixel_time, unsigned long sensor_c
  * @param sa7 I2C slave address, 7 bit
  * @param reg sensor register address (8-bit)
  * @param data value to set (16 bits) */
-#define SET_SENSOR_PAR(port,frame,sa7,reg,data) { pars_to_update[nupdate  ].num= P_SENSOR_REGS+(reg) ;\
+#define SET_SENSOR_PAR(port,frame,sa7,reg,data) { pars_to_update[nupdate  ].num= P_SENSOR_REGS+(reg);\
                                                   pars_to_update[nupdate++].val=(data);\
-                                                 X3X3_I2C_SEND2((port),(frame), (sa7), (reg), (data)); \
+                                                  X3X3_I2C_SEND2((port),(frame), (sa7), (reg), (data));\
                                                 }
 /**Set parameter for the same register in multiple multiplexed sensors and send to hardware i2c sequencer
  * Similar to SET_SENSOR_PAR, but broadcast set for parameters with individual values.
@@ -238,3 +254,4 @@ unsigned long sensor_to_camsync(unsigned long pixel_time, unsigned long sensor_c
                                        } \
                                      }
 
+#endif

src/drivers/elphel/sensor_common.c

@@ -23,7 +23,7 @@
  *  You should have received a copy of the GNU General Public License
  *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
-
+#define DEBUG
 //copied from cxi2c.c - TODO:remove unneeded
 #include <linux/sched.h>
 #include <linux/kernel.h>
@@ -31,11 +31,13 @@
 #include <linux/interrupt.h>
 #include <linux/time.h>
 #include <linux/platform_device.h>
-#include <asm/outercache.h>
-#include <asm/cacheflush.h>
 
+#include <linux/string.h>
 #include <linux/spinlock.h>
 
+#include <asm/outercache.h>
+#include <asm/cacheflush.h>
+
 
 #include <uapi/elphel/c313a.h>
 #include <uapi/elphel/exifa.h>
@@ -51,6 +53,7 @@
 #include "quantization_tables.h"
 #include "x393_macro.h"
 #include "x393.h"
+#include "detect_sensors.h"
 //#include "x393_helpers.h"
 
 #include <asm/delay.h> // just for usleep1000()
@@ -1142,6 +1145,159 @@ int image_acq_stop(struct platform_device *pdev)
 {
 	return 0;
 }
+
+/**
+ * Register i2c pages equal to slave address
+ * @param port
+ * @param sub_chn
+ * @param i2c_dev
+ * @return
+ */
+int fpga_register_i2c_pages(int port, int sub_chn, x393_i2c_device_t i2c_dev){
+
+	int i;
+	int line_num;
+
+	int haddr;
+
+	u16 *table = pSensorPortConfig[port].pages_ptr[sub_chn];
+	u8  *h2r   = pSensorPortConfig[port].haddr2rec[sub_chn];
+
+	bool used_page[256];
+
+	// init
+	for(i=0;i<256;i++){
+		used_page[i] = false;
+	}
+
+	// loop through pages table
+	i=0;
+	while(true){
+
+		haddr = (table[i]>>8)&0xff;
+
+		if ((table[i]==0xffff)||(i>255)){
+			dev_dbg(g_dev_ptr,"haddr2rec table registered (number of entries: %d)\n",i);
+			break;
+		}
+
+		if(!used_page[haddr]){
+			used_page[haddr] = true;
+
+			line_num = i2c_page_alloc(port);
+			if (line_num<0){
+				return line_num;
+			}
+
+			dev_dbg(g_dev_ptr,"Registering page %d for haddr=0x%02x. slave7= 0x%02x\n",line_num, haddr, i2c_dev.slave7);
+
+			//line_num = i2c_dev.slave7;
+			//haddr = 0;
+
+			h2r[haddr] = line_num;
+			i2c_page_register(port,line_num);
+			set_xi2c_wrc(&i2c_dev,port,line_num,haddr);
+		}
+		i++;
+	}
+
+	return 0;
+}
+
+/**
+ * Takes care of a single/multiple ports - registers all required 'write' and 'read' pages for
+ * muxes and sensors
+ * @param ports_mask
+ * @return
+ */
+int register_i2c_sensor(int ports_mask) ///< bitmask of the sensor ports to use
+	                                    ///< @return 0 (may add errors)
+
+{
+	int port, subchn;
+	x393_i2c_device_t *class_mux, *class_sensor;
+	x393_i2c_device_t dev_sensor;
+	struct sensor_port_config_t pcfg;
+	const char *name;
+	const char *name10359;
+
+	bool mux;
+
+	for(port=0;port<SENSOR_PORTS;port++) if ((1<<port)&ports_mask) {
+
+		i2c_page_alloc_init(port); // reset all pages allocation
+
+		pcfg = pSensorPortConfig[port];
+
+		// pcfg.mux==0 equals 'DETECT'
+		//mux = (pcfg.mux!=SENSOR_NONE)&&(pcfg.mux!=0);
+		mux = (pcfg.mux!=SENSOR_NONE);
+
+		// 'write' recs for mux
+		if (mux){
+
+			// returns 'mux10359', need 'el10359'
+			name = get_name_by_code(pcfg.mux,DETECT_MUX);
+			// get reference name 'mux10359'
+			name10359 = get_name_by_code(SENSOR_MUX_10359,DETECT_MUX);
+
+			dev_dbg(g_dev_ptr,"Comparing %s to reference %s\n",name,name10359);
+
+			if (name!=NULL){
+				// compare in case pcfg.mux was something else
+				if (strncmp(name,name10359,strlen(name10359))==0){
+					name = name_10359;
+				}
+			}
+
+		    class_mux = xi2c_dev_get(name);
+
+		    // TODO: request a line# from fpga table and register it (not class_mux->slave7)
+		    dev_dbg(g_dev_ptr,"Registering page %d for haddr=0x%02x. slave7= 0x%02x\n",class_mux->slave7, 0, class_mux->slave7);
+		    i2c_page_register(port, class_mux->slave7);
+		    set_xi2c_wrc(class_mux, port, class_mux->slave7, 0);
+		}
+
+		// 'write' recs for sensors
+		for(subchn=0;subchn<4;subchn++){
+			if (pcfg.sensor[subchn]!=SENSOR_NONE){
+
+				name = get_name_by_code(pcfg.sensor[subchn],DETECT_SENSOR);
+				class_sensor = xi2c_dev_get(name);
+
+				// copy reference data
+				memcpy(&dev_sensor, class_sensor, sizeof(x393_i2c_device_t));
+				// i2c address rule for MUX ports
+				dev_sensor.slave7 = class_sensor->slave7 + I2C359_INC * subchn;
+				// register line#s for available sensors (w or w/o mux)
+				fpga_register_i2c_pages(port,subchn,dev_sensor);
+			}
+		}
+
+		// Now register one page for reading 10359 and the sensor using sensor speed data
+
+		// 'read' recs for sensors,
+		// TODO: request the # from fpga, do not use LEGACY_READ_PAGE2, check read functions
+		name = get_name_by_code(pcfg.sensor[0],1);
+		class_sensor = xi2c_dev_get(name);
+		memcpy(&dev_sensor, class_sensor, sizeof(x393_i2c_device_t));
+		i2c_page_register(port, LEGACY_READ_PAGE2);
+		set_xi2c_rdc(&dev_sensor, port, LEGACY_READ_PAGE2);
+
+		if (mux){
+			// 'read' recs for 10359
+			// use 'el10359_32' - in DT?
+			// TODO: request the # from fpga, do not use LEGACY_READ_PAGE4, check read functions
+			i2c_page_register(port, LEGACY_READ_PAGE4);
+			dev_sensor.data_bytes=4; // for reading 10359 in 32-bit mode
+			set_xi2c_rdc(&dev_sensor, port, LEGACY_READ_PAGE4);
+		}
+
+	}
+
+	return 0;
+}
+
 //#define I2C359_INC                    2   ///< slave address increment between sensors in 10359A board (broadcast, 1,2,3)
 /** Register i2c pages equal to slave address,
  * Use to convert 353 code  */
@@ -1170,7 +1326,7 @@ int legacy_i2c(int ports) ///< bitmask of the sensor ports to use
         }
         // Now register one page for reading 10359 and the sensor using sensor speed data
         memcpy(&dev_sensor, class_sensor, sizeof(x393_i2c_device_t)); // dev_sensor));
-        dev_dbg(g_dev_ptr, "Registering page to read senors 16-bit on port %d, page= 0x%x\n",sensor_port,LEGACY_READ_PAGE2);
+        dev_dbg(g_dev_ptr, "Registering page to read sensors 16-bit on port %d, page= 0x%x\n",sensor_port,LEGACY_READ_PAGE2);
         i2c_page_register(sensor_port, LEGACY_READ_PAGE2);
         set_xi2c_rdc(&dev_sensor, sensor_port, LEGACY_READ_PAGE2);
         dev_dbg(g_dev_ptr, "Registering page to read 32-bit data for 10359 on port %d, page= 0x%x\n",sensor_port,LEGACY_READ_PAGE4);

src/drivers/elphel/sensor_common.h

@@ -5,6 +5,8 @@
 #ifndef _SENSOR_COMMON_H
 #define _SENSOR_COMMON_H
 
+#include "detect_sensors.h"
+
 //extern struct sensor_t sensor; // current sensor (will be copied to by sensor driver), made external for the cc353.c to read/write i2c
 extern struct sensorproc_t * asensorproc;
 //extern struct sensorproc_t * sensorproc;
@@ -92,6 +94,9 @@ long long get_frame_pos(unsigned int chn, unsigned int pos);
 #define LEGACY_READ_PAGE4 0xfe
 #define  name_10359   "el10359" // Get name from DT (together with port mask)
 #define  name_sensor  "mt9p006" // Get name from DT (together with port mask)
+#define  name_mt9p006 "mt9p006" // Get name from DT (together with port mask)
+#define  name_mt9f002 "mt9f002" // Get name from DT (together with port mask)
+
 #define I2C359_INC                    2   ///< slave address increment between sensors in 10359A board (broadcast, 1,2,3) (7 bits SA)
 
 /** Perform I2C write (8  bits address, 16 bits data in "legacy" mode,
@@ -104,7 +109,11 @@ long long get_frame_pos(unsigned int chn, unsigned int pos);
  * @param sa7 I2C slave address, 7 bit
  * @param reg sensor register address (8-bit)
  * @param data value to set (16 bits) */
-#define X3X3_I2C_SEND2(port,frame,sa7,reg,data) write_xi2c_reg16_abs_asap((port),(sa7),(frame),(reg),(data))
+#define X3X3_I2C_SEND2(port,frame,sa7,reg,data) {\
+													int _ADDR = pSensorPortConfig[(port)].par2addr[0][(reg)];\
+													int _PAGE = pSensorPortConfig[(port)].haddr2rec[0][(_ADDR>>8)&0xff];\
+													write_xi2c_reg16_abs_asap((port),_PAGE,(frame),_ADDR&0xff,(data));\
+												}
 
 /** Perform I2C write in immediate mode (8  bits address, 16 bits data in "legacy" mode,
  * pages matching slave address should be registered.
@@ -114,7 +123,11 @@ long long get_frame_pos(unsigned int chn, unsigned int pos);
  * @param sa7 I2C slave address, 7 bit
  * @param reg sensor register address (8-bit)
  * @param data value to set (16 bits) */
-#define X3X3_I2C_SEND2_ASAP(port,sa7,reg,data) write_xi2c_reg16((port),(sa7),(reg),(data))
+#define X3X3_I2C_SEND2_ASAP(port,sa7,reg,data) {\
+												   int _ADDR = pSensorPortConfig[(port)].par2addr[0][(reg)];\
+												   int _PAGE = pSensorPortConfig[(port)].haddr2rec[0][(_ADDR>>8)&0xff];\
+	                                               write_xi2c_reg16((port),(sa7),(reg),(data));\
+											   }
 
 /** Perform I2C read (8  bits address, 16 bits data in "legacy" mode (sensors and 10359),
  * page LEGACY_READ_PAGE2 (==0xff) should be registered - legacy_i2c.
@@ -139,6 +152,8 @@ long long get_frame_pos(unsigned int chn, unsigned int pos);
 #define X3X3_I2C_RCV4(port,sa7,reg,datap) legacy_read_i2c_reg((port),(LEGACY_READ_PAGE4),(sa7),(reg),4,(int*)(datap))
 
 int legacy_i2c  (int ports);
+int register_i2c_sensor(int ports_mask);
+
 void udelay1000(int ms);
 
 #endif

src/drivers/elphel/sensor_i2c.c

@@ -911,7 +911,7 @@ int x393_xi2c_wait_wr(int chn)  ///< sensor port number
     int rdy;
     unsigned long       timeout_end;
 
-    dev_dbg(sdev, "Waitoing i2c sequencer ready for channel %d\n",chn);
+    dev_dbg(sdev, "Waiting i2c sequencer ready for channel %d\n",chn);
     timeout_end = jiffies + tenth_sec;
     while (jiffies < timeout_end){
         if ((rdy=x393_xi2c_ready_wr( chn)))

src/drivers/elphel/sensor_i2c.h

@@ -17,15 +17,14 @@
 
 #ifndef SENSOR_I2C_H
 #define SENSOR_I2C_H
+
 #define I2C_CMD_STOP  0
 #define I2C_CMD_RUN   1
 #define I2C_CMD_RESET 2
 #define SDA_DRIVE_HIGH 1
 #define SDA_RELEASE    1
 
-
-
-/** I2C device description to be used with i2c sequencer*/
+/** I2C device description to be used with i2c sequencer */
 typedef struct{
 	      char                         name[32];      ///< Device class name (up to 31 characters)
 	      u8                           slave7;        ///< Device class base slave address (7-bit). Instances may have it
@@ -34,7 +33,7 @@ typedef struct{
 	      u8                           data_bytes;    ///< Number of data bytes (1..10), for writes it includes register address bytes
 	      int                          scl_khz;       ///< maximal SCL frequency in KHz (currently limited by 200KHz slowest)
 } x393_i2c_device_t;
-void i2c_page_alloc_init( int chn); // reset page allocation for selscted channel
+void i2c_page_alloc_init( int chn); // reset page allocation for selected channel
 int i2c_stop_run_reset(int chn, int cmd);
 int i2c_drive_mode    (int chn, int sda_drive_high, int sda_release);
 int read_xi2c_frame   (int chn);