Diff: Interfaces/SmartSensor/SSInterface.cpp

Revision:

5:795cffb6f01a

Parent:

1:7a55c0c7d6d9

--- a/Interfaces/SmartSensor/SSInterface.cpp	Thu Apr 12 13:04:59 2018 -0700
+++ b/Interfaces/SmartSensor/SSInterface.cpp	Thu May 24 14:40:48 2018 -0700
@@ -35,9 +35,12 @@
 #include "Peripherals.h"
 #include "assert.h"
 #include "utils.h"
+#include "i2cm.h"
 
 SSInterface::SSInterface(I2C &i2cBus, PinName ss_mfio, PinName ss_reset)
-	:m_i2cBus(&i2cBus), m_spiBus(NULL), mfio_pin(ss_mfio), reset_pin(ss_reset), irq_pin(ss_mfio)
+	:m_i2cBus(&i2cBus), m_spiBus(NULL),
+	mfio_pin(ss_mfio), reset_pin(ss_reset), irq_pin(ss_mfio),
+	irq_evt(1000000, "irq")
 {
 	reset_pin.input();
 	irq_pin.fall(callback(this, &SSInterface::irq_handler));
@@ -47,7 +50,9 @@
 }
 
 SSInterface::SSInterface(SPI &spiBus, PinName ss_mfio, PinName ss_reset)
-	:m_i2cBus(NULL), m_spiBus(&spiBus), mfio_pin(ss_mfio), reset_pin(ss_reset), irq_pin(ss_mfio)
+	:m_i2cBus(NULL), m_spiBus(&spiBus),
+	mfio_pin(ss_mfio), reset_pin(ss_reset), irq_pin(ss_mfio),
+	irq_evt(1000000, "irq")
 {
 	reset_pin.input();
 	irq_pin.fall(callback(this, &SSInterface::irq_handler));
@@ -139,7 +144,7 @@
 }
 
 SS_STATUS SSInterface::self_test(int idx, uint8_t *result, int sleep_ms){
-    uint8_t cmd_bytes[] = {SS_FAM_R_SELFTEST, idx};
+    uint8_t cmd_bytes[] = { SS_FAM_R_SELFTEST, (uint8_t)idx };
     uint8_t rxbuf[2];
     SS_STATUS ret;
 
@@ -256,6 +261,8 @@
 
     if (total_len <= SS_SMALL_BUF_SIZE) {
         return write_cmd_small(cmd_bytes, cmd_bytes_len, data, data_len, sleep_ms);
+    } else if (total_len <= SS_MED_BUF_SIZE) {
+        return write_cmd_medium(cmd_bytes, cmd_bytes_len, data, data_len, sleep_ms);
     } else if (total_len <= SS_LARGE_BUF_SIZE) {
         return write_cmd_large(cmd_bytes, cmd_bytes_len, data, data_len, sleep_ms);
     } else {
@@ -272,8 +279,14 @@
 	}
 	pr_info("\r\n");
 
-    int ret = 0;
-    ret |= m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)tx_buf, tx_len);
+    int ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)tx_buf, tx_len);
+
+	int retries = 4;
+	while (ret != 0 && retries-- > 0) {
+		pr_err("i2c wr retry\r\n");
+		wait_ms(1);
+    	ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)tx_buf, tx_len);	    
+	}
 
     if (ret != 0) {
     	pr_err("m_i2cBus->write returned %d\r\n", ret);
@@ -283,10 +296,18 @@
     wait_ms(sleep_ms);
 
     char status_byte;
-    ret |= m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, &status_byte, 1);
+    ret = m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, &status_byte, 1);
+	bool try_again = (status_byte == SS_ERR_TRY_AGAIN);
+	while ((ret != 0 || try_again) 
+			&& retries-- > 0) {
+		pr_info("i2c rd retry\r\n");
+		wait_ms(sleep_ms);
+    	ret = m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, &status_byte, 1);
+		try_again = (status_byte == SS_ERR_TRY_AGAIN);
+	}
 
-    if (ret != 0) {
-    	pr_err("m_i2cBus->read returned %d\r\n", ret);
+    if (ret != 0 || try_again) {
+    	pr_err("m_i2cBus->read returned %d, ss status_byte %d\r\n", ret, status_byte);
         return SS_ERR_UNAVAILABLE;
     }
 
@@ -307,6 +328,18 @@
 	return status;
 }
 
+SS_STATUS SSInterface::write_cmd_medium(uint8_t *cmd_bytes, int cmd_bytes_len,
+                       uint8_t *data, int data_len,
+                       int sleep_ms)
+{
+    uint8_t write_buf[SS_MED_BUF_SIZE];
+    memcpy(write_buf, cmd_bytes, cmd_bytes_len);
+    memcpy(write_buf + cmd_bytes_len, data, data_len);
+
+	SS_STATUS status = write_cmd(write_buf, cmd_bytes_len + data_len, sleep_ms);
+	return status;
+}
+
 SS_STATUS SSInterface::write_cmd_large(uint8_t *cmd_bytes, int cmd_bytes_len,
                        uint8_t *data, int data_len,
                        int sleep_ms)
@@ -324,20 +357,30 @@
 	uint8_t *rxbuf, int rxbuf_sz,
     int sleep_ms)
 {
-    int ret = 0;
-
 	pr_info("read_cmd: ");
 	for (int i = 0; i < cmd_bytes_len; i++) {
 		pr_info("0x%02X ", cmd_bytes[i]);
 	}
 	pr_info("\r\n");
 
-    ret |= m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)cmd_bytes, cmd_bytes_len, (data_len != 0));
+
+	int retries = 4;
+
+    int ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)cmd_bytes, cmd_bytes_len, (data_len != 0));
 
     if (data_len != 0) {
         ret |= m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)data, data_len, false);
     }
 
+	while (ret != 0 && retries-- > 0) {
+		pr_err("i2c wr retry\r\n");
+		wait_ms(1);
+    	ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)cmd_bytes, cmd_bytes_len, (data_len != 0));
+	    if (data_len != 0) {
+	        ret |= m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)data, data_len, false);
+	    }
+	}
+
     if (ret != 0) {
     	pr_err("m_i2cBus->write returned %d\r\n", ret);
         return SS_ERR_UNAVAILABLE;
@@ -345,14 +388,19 @@
 
     wait_ms(sleep_ms);
 
-    ret |= m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, (char*)rxbuf, rxbuf_sz);
-
-    if (ret != 0) {
-    	pr_err("m_i2cBus->read returned %d\r\n", ret);
+    ret = m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, (char*)rxbuf, rxbuf_sz);
+	bool try_again = (rxbuf[0] == SS_ERR_TRY_AGAIN);
+	while ((ret != 0 || try_again) && retries-- > 0) {
+		pr_info("i2c rd retry\r\n");
+		wait_ms(sleep_ms);
+    	ret = m_i2cBus->read(SS_I2C_8BIT_SLAVE_ADDR, (char*)rxbuf, rxbuf_sz);
+		try_again = (rxbuf[0] == SS_ERR_TRY_AGAIN);
+	}
+    if (ret != 0 || try_again) {
+    	pr_err("m_i2cBus->read returned %d, ss status_byte %d\r\n", ret, rxbuf[0]);
         return SS_ERR_UNAVAILABLE;
     }
 
-    wait_ms(sleep_ms);
 	pr_info("status_byte: %d\r\n", rxbuf[0]);
 	pr_info("data: ");
 	for (int i = 1; i < rxbuf_sz; i++) {
@@ -384,7 +432,7 @@
 
 	status = read_cmd(&cmd_bytes2[0], ARRAY_SIZE(cmd_bytes2),
 						0, 0,
-						&rxbuf[0], ARRAY_SIZE(rxbuf));
+						&rxbuf[0], reg_width + 1);
 
 	if (status == SS_SUCCESS) {
 		*val = 0;
@@ -463,6 +511,7 @@
 	assert_msg((mode <= SS_MAX_SUPPORTED_MODE_NUM), "mode must be < SS_MAX_SUPPORTED_MODE_NUM, or update code to handle variable length mode values");
 	assert_msg((mode != 0), "Tried to enable sensor to mode 0, but mode 0 is disable");
 
+
 	uint8_t cmd_bytes[] = { SS_FAM_W_SENSORMODE, (uint8_t)idx, (uint8_t)mode };
 
 	SS_STATUS status = write_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes), 0, 0, SS_ENABLE_SENSOR_SLEEP_MS);
@@ -497,7 +546,7 @@
 
 	uint8_t cmd_bytes[] = { SS_FAM_W_ALGOMODE, (uint8_t)idx, (uint8_t)mode };
 
-	SS_STATUS status = write_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes), 0, 0, SS_ENABLE_SENSOR_SLEEP_MS);
+	SS_STATUS status = write_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes), 0, 0, 4*SS_ENABLE_SENSOR_SLEEP_MS);
 
 	if (status == SS_SUCCESS) {
 		algo_enabled_mode[idx] = mode;
@@ -522,6 +571,31 @@
 	return status;
 }
 
+SS_STATUS SSInterface::set_algo_cfg(int algo_idx, int cfg_idx, uint8_t *cfg, int cfg_sz)
+{
+	assert_msg((algo_idx <= SS_MAX_SUPPORTED_ALGO_NUM), "idx must be < SS_MAX_SUPPORTED_ALGO_NUM, or update code to handle variable length idx values");
+	assert_msg((cfg_idx <= SS_MAX_SUPPORTED_ALGO_CFG_NUM), "idx must be < SS_MAX_SUPPORTED_ALGO_CFG_NUM, or update code to handle variable length idx values");
+
+	uint8_t cmd_bytes[] = { SS_FAM_W_ALGOCONFIG, (uint8_t)algo_idx, (uint8_t)cfg_idx };
+	SS_STATUS status = write_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+								 cfg, cfg_sz);
+
+	return status;
+}
+
+SS_STATUS SSInterface::get_algo_cfg(int algo_idx, int cfg_idx, uint8_t *cfg, int cfg_sz)
+{
+	assert_msg((algo_idx <= SS_MAX_SUPPORTED_ALGO_NUM), "idx must be < SS_MAX_SUPPORTED_ALGO_NUM, or update code to handle variable length idx values");
+	assert_msg((cfg_idx <= SS_MAX_SUPPORTED_ALGO_CFG_NUM), "idx must be < SS_MAX_SUPPORTED_ALGO_CFG_NUM, or update code to handle variable length idx values");
+
+	uint8_t cmd_bytes[] = { SS_FAM_R_ALGOCONFIG, (uint8_t)algo_idx, (uint8_t)cfg_idx };
+	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+								0, 0,
+								cfg, cfg_sz);
+
+	return status;
+}
+
 SS_STATUS SSInterface::set_data_type(int data_type, bool sc_en)
 {
 	assert_msg((data_type >= 0) && (data_type <= 3), "Invalid value for data_type");
@@ -557,6 +631,53 @@
 	return status;
 }
 
+SS_STATUS SSInterface::set_fifo_thresh(int thresh)
+{
+	assert_msg((thresh > 0 && thresh <= 255), "Invalid value for fifo a full threshold");
+	uint8_t cmd_bytes[] = { SS_FAM_W_COMMCHAN, SS_CMDIDX_FIFOAFULL };
+	uint8_t data_bytes[] = { (uint8_t)thresh };
+
+	SS_STATUS status = write_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+								&data_bytes[0], ARRAY_SIZE(data_bytes));
+	return status;
+}
+
+SS_STATUS SSInterface::get_fifo_thresh(int *thresh)
+{
+	uint8_t cmd_bytes[] = { SS_FAM_R_COMMCHAN, SS_CMDIDX_FIFOAFULL };
+	uint8_t rxbuf[2] = {0};
+
+	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+								0, 0,
+								&rxbuf[0], ARRAY_SIZE(rxbuf));
+
+	if (status == SS_SUCCESS) {
+		*thresh = rxbuf[1];
+	}
+
+	return status;
+}
+
+SS_STATUS SSInterface::ss_comm_check()
+{
+	uint8_t cmd_bytes[] = { SS_FAM_R_IDENTITY, SS_CMDIDX_PLATTYPE };
+	uint8_t rxbuf[2];
+
+	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+								0, 0,
+								&rxbuf[0], ARRAY_SIZE(rxbuf));
+
+	int tries = 4;
+	while (status == SS_ERR_TRY_AGAIN && tries--) {
+		wait_ms(1000);
+		status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
+							0, 0,
+							&rxbuf[0], ARRAY_SIZE(rxbuf));
+	}
+
+	return status;
+}
+
 void SSInterface::fifo_sample_size(int data_type, int *sample_size)
 {
 	*sample_size = 0;
@@ -587,7 +708,7 @@
 
 	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
 								0, 0,
-								&rxbuf[0], ARRAY_SIZE(rxbuf));
+								&rxbuf[0], ARRAY_SIZE(rxbuf), 1);
 
 	if (status == SS_SUCCESS) {
 		*num_samples = rxbuf[1];
@@ -605,23 +726,28 @@
 
 	uint8_t cmd_bytes[] = { SS_FAM_R_OUTPUTFIFO, SS_CMDIDX_READFIFO };
 
-	pr_err("[reading %d bytes (%d samples)\r\n", bytes_to_read, num_samples);
+	pr_info("[reading %d bytes (%d samples)\r\n", bytes_to_read, num_samples);
 
 	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
 								0, 0,
-								databuf, bytes_to_read, 15);
+								databuf, bytes_to_read, 5);
 
 	return status;
 }
 
-void SSInterface::irq_handler()
-{
-	uint8_t databuf[512];
+static uint8_t databuf[512];
+void SSInterface::ss_execute_once(){
+
+	if(m_irq_received_ == false)
+		return;
+
 	uint8_t sample_count;
-
+	m_irq_received_ = false;
 	uint8_t cmd_bytes[] = { SS_FAM_R_STATUS, SS_CMDIDX_STATUS };
 	uint8_t rxbuf[2] = {0};
 
+	irq_evt.start();
+
 	irq_pin.disable_irq();
 
 	SS_STATUS status = read_cmd(&cmd_bytes[0], ARRAY_SIZE(cmd_bytes),
@@ -631,6 +757,7 @@
 	if (status != SS_SUCCESS) {
 		pr_err("Couldn't read status byte of SmartSensor!");
 		irq_pin.enable_irq();
+		irq_evt.stop();
 		return;
 	}
 
@@ -645,39 +772,37 @@
 	}
 
 	if (rxbuf[1] & SS_MASK_STATUS_DATA_RDY) {
-		int num_samples;
+		int num_samples = 1;
 		status = num_avail_samples(&num_samples);
 		if (status != SS_SUCCESS)
 		{
 			pr_err("Couldn't read number of available samples in SmartSensor Output FIFO");
 			irq_pin.enable_irq();
+			irq_evt.stop();
 			return;
 		}
 
-		if (num_samples <= 0)
-		{
-			irq_pin.enable_irq();
-			return;
-		}
-
-
 		int sample_size;
 		fifo_sample_size(data_type, &sample_size);
 
-		size_t bytes_to_read = num_samples * sample_size + 1; //+1 for status byte
-		if (bytes_to_read > sizeof(databuf)) {
+		int bytes_to_read = num_samples * sample_size + 1; //+1 for status byte
+		if ((uint32_t)bytes_to_read > sizeof(databuf)) {
 			//Reduce number of samples to read to fit in buffer
 			num_samples = (sizeof(databuf) - 1) / sample_size;
 		}
 
+		wait_ms(5);
 		status = read_fifo_data(num_samples, sample_size, &databuf[0], sizeof(databuf));
 		if (status != SS_SUCCESS)
 		{
 			pr_err("Couldn't read from SmartSensor Output FIFO");
 			irq_pin.enable_irq();
+			irq_evt.stop();
 			return;
 		}
 
+		pr_info("read %d samples\r\n", num_samples);
+
 		//Skip status byte
 		uint8_t *data_ptr = &databuf[1];
 
@@ -712,6 +837,16 @@
 		}
 	}
 	irq_pin.enable_irq();
+	irq_evt.stop();
+}
+
+void SSInterface::ss_clear_interrupt_flag(){
+	m_irq_received_ = false;
+}
+
+void SSInterface::irq_handler()
+{
+	m_irq_received_ = true;
 }
 
 void SSInterface::irq_handler_selftest(){