Diff: SHComm.cpp

Revision:

0:23dade9a66bf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SHComm.cpp	Mon Dec 17 07:56:04 2018 +0000
@@ -0,0 +1,913 @@
+/*******************************************************************************
+ * Copyright (C) 2018 Maxim Integrated Products, Inc., All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a
+ * copy of this software and associated documentation files (the "Software"),
+ * to deal in the Software without restriction, including without limitation
+ * the rights to use, copy, modify, merge, publish, distribute, sublicense,
+ * and/or sell copies of the Software, and to permit persons to whom the
+ * Software is furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included
+ * in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
+ * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
+ * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
+ * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
+ * OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * Except as contained in this notice, the name of Maxim Integrated
+ * Products, Inc. shall not be used except as stated in the Maxim Integrated
+ * Products, Inc. Branding Policy.
+ *
+ * The mere transfer of this software does not imply any licenses
+ * of trade secrets, proprietary technology, copyrights, patents,
+ * trademarks, maskwork rights, or any other form of intellectual
+ * property whatsoever. Maxim Integrated Products, Inc. retains all
+ * ownership rights.
+ *******************************************************************************
+ */
+
+/*
+#ifdef __cplusplus
+extern "C" {
+#endif
+*/
+
+#include <events/mbed_events.h>
+#include <mbed.h>
+#include "mbed.h"
+
+#include "SHComm.h"
+
+
+#define SS_I2C_8BIT_SLAVE_ADDR      0xAA
+#define SENSORHUB_I2C_ADRESS        SS_I2C_8BIT_SLAVE_ADDR
+
+#define ENABLED   ((int)(1))
+#define DISABLED  ((int)(0))
+
+#define SS_DUMP_REG_SLEEP_MS        (100)
+#define SS_ENABLE_SENSOR_SLEEP_MS   (20)
+#define SS_DEFAULT_CMD_SLEEP_MS     (2)
+#define SS_WAIT_BETWEEN_TRIES_MS    (2)
+#define SS_CMD_WAIT_PULLTRANS_MS    (5)
+#define SS_FEEDFIFO_CMD_SLEEP_MS	(30)
+
+
+#define SS_DEFAULT_RETRIES       ((int) (4))
+#define SS_ZERO_DELAY               0
+#define SS_ZERO_BYTES               0
+
+
+/*define sample size of algorithm and raw sensor data in bytes*/
+#define SH_ALGO_WHRM_SAMPLE_DATABYTES  4
+#define SH_ALGO_SP02_SAMPLE_DATABYTES  4
+
+
+/*define command sequences given in Maxim ME32664 user manual*/
+#define SH_GET_HUB_STATUS_CMDSEQ               {0x00,0x00}
+#define SH_SET_OPERATING_MODE_CMDSEQ(opMode)   {0x01,0x00,opMode}
+       #define SH_SET_OPERATING_MODE_BOOTLOADER_CMDSEQ      {0x02,0x00,0x08}
+       #define SH_SET_OPERATING_MODE_APPLICATION_CMDSEQ     {0x02,0x00,0x00}
+       #define SH_SET_OPERATING_MODE_RESET_CMDSEQ           {0x02,0x00,0x02}
+#define SH_GET_OPERATING_MODE_CMDSEQ           {0x02,0x00}
+
+#define SH_SET_OUTPUT_MODE_CMDSEQ( outMode)    {0x10,0x00, outMode}
+	   #define SH_SET_OUTMODE_NODATA_CMDSEQ                 {0x10,0x00,0x00}
+       #define SH_SET_OUTMODE_SENSORDATA_CMDSEQ             {0x10,0x00,0x01}
+       #define SH_SET_OUTMODE_ALGODATA_CMDSEQ               {0x10,0x00,0x02}
+       #define SH_SET_OUTMODE_PAUSE_CMDSEQ                  {0x10,0x00,0x04}
+       #define SH_SET_OUTMODE_SENSAMPLECNT_CMDSEQ           {0x10,0x00,0x05}
+	   #define SH_SET_OUTMODE_ALGOSAMPLECNT_CMDSEQ          {0x10,0x00,0x06}
+       #define SH_SET_OUTMODE_ALGOSENSAMPLECNT_CMDSEQ       {0x10,0x00,0x07}
+
+#define SH_GET_OUTPUT_MODE_CMDSEQ             {0x11,0x00}
+
+#define SH_DFIFO_SET_INT_THRESHOLD_CMDSEQ( ucThreshold )    {0x10,0x01,ucThreshold}
+#define SH_DFIFO_GET_INT_THRESHOLD_CMDSEQ                   {0x11,0x01}
+
+#define SH_DFIFO_GET_NSAMPLES_CMDSEQ          {0x12,0x00}
+#define SH_DFIFO_PULL_SAMPLE_CMDSEQ           {0x12,0x01}
+#define SH_GET_EXTINPUT_FIFOSZ_CMDSEQ         {0x13,0x01}
+#define SH_GET_SAMPLEBYTECNT_INPUTFIFO_CMDSEQ {0x13,0x04}
+#define SH_FEED_TO_INPUTFIFO_CMDSEQ           {0x14,0x00}
+
+#define SH_WRITE_SENSORREG_CMDSEQ( sensorIdx , regAddr )    { 0x40, sensorIdx , regAddr}
+#define SH_READ_SENSORREG_CMDSEQ( sensorIdx , regAddr )     { 0x41, sensorIdx , regAddr}
+#define SH_READ_AFE_ATTRIBUTES_CMDSEQ(sensorIdx)            { 0x42, sensorIdx}
+#define SH_READ_ALLREGISTERS_CMDSEQ(sensorIdx)              { 0x43, sensorIdx}
+
+#define SH_ENABLE_SENSOR_CMDSEQ(sensorIdx , extMode)    {0x44, sensorIdx, 0x01 , extMode }
+#define SH_DISABLE_SENSOR_CMDSEQ(sensorIdx)   {0x44, sensorIdx, 0x00}
+
+
+#define SH_AGC_SET_ADCRANGE_CMDSEQ( uiPercentage)           {0x50, 0x00, 0x00 , uiPercentage}
+#define SH_AGC_SET_STEPSZ_CMDSEQ( uiPercentage)             {0x50, 0x00, 0x01 , uiPercentage}
+#define SH_AGC_SET_SENSITIVITY_CMDSEQ( uiPercentage)        {0x50, 0x00, 0x02 , uiPercentage}
+#define SH_AGC_SET_NSAMPLESAVRAGING_CMDSEQ( ucNsamples)     {0x50, 0x00, 0x03 , uiNsamples}
+#define SH_WHRM_SET_SAMPRATE_CMDSEQ( ucNsamples)            {0x50, 0x02, 0x03 , uiNsamples}
+
+
+#define SH_ENABLE_ALGO_CMDSEQ( algoIdx)      { 0x52, algoIdx , 0x01}
+#define SH_DISABLE_ALGO_CMDSEQ( algoIdx)     { 0x52, algoIdx , 0x00}
+
+#define SH_SET_ALGO_CONFIGURATION_CMDSEQ( algoIdx, cgfIdx)  { 0x50 , algoIdx, cgfIdx }
+#define SH_GET_ALGO_CONFIGURATION_CMDSEQ( algoIdx, cgfIdx)  { 0x51 , algoIdx, cgfIdx }
+
+#define SH_COMM_CHECK_CMDSEQ                 {0xFF, 0x00}
+
+   /*
+    *   define the "platform specific" hardware interface which SSinterface requires:
+    *   1. master i2c port
+    *   2. interrupt attachable I/O pin (mfio)
+    *   3. I/O pin for reset
+    *   Note: Definitions below are for MAX32630FTR Pagasus board . Modify for your platform.
+    **/
+I2C *m_i2cBus;                /*i2c  bus sensor hub is connected to*/
+
+PinName ss_mfio(P5_4);             /* platform specific mfio event pin */
+PinName ss_reset(P5_6);            /* platform specific sensor hub reset pin */
+DigitalInOut mfio_pin(ss_mfio);    /* mfio pin mode be I/O */
+DigitalInOut reset_pin(ss_reset);  /* reset pin mode be I/O */
+InterruptIn irq_pin(ss_mfio);      /* define mfio pin interrupt attachable*/
+
+
+/*
+ * SSI API funcions
+ * NOTE: Generic functions for any platform.
+ *       exceptions: below needs needs modification according to platform and HAL drivers
+ *       1. Hard reset function
+ *       2. Enable/disable mfio event interrput
+ *       3. mfio pin interrupt routine
+ *
+ * **/
+
+/*global buffer for sensor i2c commands+data*/
+uint8_t sh_write_buf[512];
+static volatile bool m_irq_received_ = false;
+static volatile bool mfio_int_happened = false;
+
+/* sensor hub states */
+static bool sc_en     = false;
+static int data_type  = 0;
+static int is_sensor_enabled[SS_MAX_SUPPORTED_SENSOR_NUM] = {0};
+static int is_algo_enabled[SS_MAX_SUPPORTED_ALGO_NUM]     = {0};
+static int sensor_sample_sz[SS_MAX_SUPPORTED_SENSOR_NUM]  = {0};
+static int algo_sample_sz[SS_MAX_SUPPORTED_ALGO_NUM]      = {0};
+
+
+
+/* desc  :
+ *         Func to init master i2c hardware comm interface with sennor hub
+ *                 init mfio interrupt pin and attach irq to pin
+ *                 init reset pin
+ * params:
+ *         N/A
+ */
+
+
+void sh_irq_handler();
+void sh_init_hwcomm_interface(){
+	static I2C ssI2C(P3_4, P3_5);     /*set up sensor hub i2c communication at 400 kHz*/
+	ssI2C.frequency(400000);
+	m_i2cBus = &ssI2C;
+
+	reset_pin.input();
+	reset_pin.mode(PullUp);
+	mfio_pin.input();                /*set mfio as input for getting mfio event reporting when sesnor hub is on  application mode */
+	mfio_pin.mode(PullUp);
+
+	irq_pin.fall(sh_irq_handler);    /*attach falling edge interrupt to mfio pin for mfio event reporting */
+
+    return;
+}
+
+/* mfio pin event reporting related interrupt functions*/
+/*
+ * data ready event reporting isr from sensor hub
+ *
+ * params:
+ *         N/A
+ * */
+void sh_irq_handler()
+{
+	m_irq_received_ = true;
+}
+void sh_clear_mfio_event_flag(void){
+	m_irq_received_ = false;
+}
+
+bool sh_has_mfio_event(void){
+	return m_irq_received_;
+}
+
+/*  desc:
+ *       func to enable event reporting from sensor hub
+ *
+ *  params:
+ *       N/A
+ * */
+void sh_enable_irq_mfioevent(void)
+{
+	irq_pin.enable_irq();
+}
+
+/*  desc:
+ *       func to disable event reporting from sensor hub
+ *
+ *  params:
+ *       N/A
+ * */
+void sh_disable_irq_mfioevent(void)
+{
+	irq_pin.disable_irq();
+}
+
+/* desc:
+ *     reset event reporting process from sensor hub
+ *
+ *  params:
+ *       N/A
+ **/
+bool sh_reset_mfio_irq(){
+	bool ret = mfio_int_happened;
+	mfio_int_happened = false;
+	sh_disable_irq_mfioevent();
+	irq_pin.fall(sh_irq_handler);
+	sh_enable_irq_mfioevent();
+	return ret;
+}
+
+
+/*
+ * desc:
+ *    function to reset sensor hub and put to application mode after reset  interface and get data format.
+ *
+ * params:
+ *
+ *    __I wakeupMode : 0x00 : application mode
+ *                     0x08 : bootloader mode
+ * */
+int sh_hard_reset(int wakeupMode){
+
+   int status;
+   sh_disable_irq_mfioevent();
+   reset_pin.output();
+   mfio_pin.output();
+
+   reset_pin.write(0);
+   wait_ms(SS_RESET_TIME);
+
+   if( (wakeupMode & 0xFF) == 0 ) {
+
+	   mfio_pin.write(1);
+	   reset_pin.write(1);
+	   wait_ms(SS_STARTUP_TO_MAIN_APP_TIME);
+
+   }else {
+
+	    mfio_pin.write(0);
+		reset_pin.write(1);
+		wait_ms(SS_STARTUP_TO_BTLDR_TIME);
+   }
+	mfio_pin.input();
+	mfio_pin.mode(PullUp);
+	reset_pin.input();
+	sh_enable_irq_mfioevent();
+}
+
+
+/*
+ * desc:
+ *    function to init sensor comm interface and get data format.
+ *
+ * */
+void sh_init_hubinterface(void){
+
+	sh_init_hwcomm_interface();
+	//sh_get_data_type(&data_type, &sc_en);
+    return;
+}
+
+
+
+
+
+/*
+ *
+ *   SENSOR HUB COMMUNICATION INTERFACE ( Defined in MAX32664 User Guide ) API FUNCTIONS
+ *
+ *
+ * */
+
+int sh_write_cmd( uint8_t *tx_buf,
+		          int tx_len,
+				  int sleep_ms)
+{
+	int retries = SS_DEFAULT_RETRIES;
+	int ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)tx_buf, tx_len);
+	while (ret != 0 && retries-- > 0) {
+
+		wait_ms(1);
+    	ret = m_i2cBus->write(SS_I2C_8BIT_SLAVE_ADDR, (char*)tx_buf, tx_len);
+	}
+    if (ret != 0)
+       return SS_ERR_UNAVAILABLE;
+
+
+    wait_ms(sleep_ms);
+
+    char status_byte;
+    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) {
+	 	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 || try_again)
+        return SS_ERR_UNAVAILABLE;
+
+	return (int) (SS_STATUS)status_byte;
+}
+
+
+int sh_write_cmd_with_data(uint8_t *cmd_bytes,
+		                   int cmd_bytes_len,
+                           uint8_t *data,
+						   int data_len,
+                           int cmd_delay_ms)
+{
+    memcpy(sh_write_buf, cmd_bytes, cmd_bytes_len);
+    memcpy(sh_write_buf + cmd_bytes_len, data, data_len);
+    int status = sh_write_cmd(sh_write_buf,cmd_bytes_len + data_len, cmd_delay_ms);
+    return status;
+}
+
+
+int sh_read_cmd( uint8_t *cmd_bytes,
+		         int cmd_bytes_len,
+	             uint8_t *data,
+				 int data_len,
+	             uint8_t *rxbuf,
+				 int rxbuf_sz,
+                 int sleep_ms )
+{
+
+	int retries = SS_DEFAULT_RETRIES;
+
+    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) {
+		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)
+    	return SS_ERR_UNAVAILABLE;
+
+
+    wait_ms(sleep_ms);
+
+    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) {
+		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)
+        return SS_ERR_UNAVAILABLE;
+
+    return (int) ((SS_STATUS)rxbuf[0]);
+}
+
+
+
+int sh_get_sensorhub_status(uint8_t *hubStatus){
+
+	uint8_t ByteSeq[] = SH_GET_HUB_STATUS_CMDSEQ;
+	uint8_t rxbuf[2] = { 0 };
+
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+			                    0, 0,
+			                    &rxbuf[0], sizeof(rxbuf),
+								SS_DEFAULT_CMD_SLEEP_MS);
+
+	*hubStatus = rxbuf[1];
+	return status;
+}
+
+
+int sh_get_sensorhub_operating_mode(uint8_t *hubMode){
+
+	uint8_t ByteSeq[] = SH_GET_OPERATING_MODE_CMDSEQ;
+	uint8_t rxbuf[2] = { 0 };
+
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+			                    0, 0,
+			                    &rxbuf[0], sizeof(rxbuf),
+								SS_DEFAULT_CMD_SLEEP_MS);
+
+	*hubMode = rxbuf[1];
+	return status;
+}
+
+
+int sh_set_sensorhub_operating_mode(uint8_t hubMode){
+
+	uint8_t ByteSeq[] = SH_SET_OPERATING_MODE_CMDSEQ(hubMode);
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), SS_DEFAULT_CMD_SLEEP_MS);
+    return status;
+
+}
+
+
+//int sh_set_data_type( uint8_t outMode)
+int sh_set_data_type(int data_type_, bool sc_en_)
+{
+
+#if 0
+	uint8_t dataTypeSc = (uint8_t)((sc_en ? SS_MASK_OUTPUTMODE_SC_EN : 0) | ((data_type << SS_SHIFT_OUTPUTMODE_DATATYPE) & SS_MASK_OUTPUTMODE_DATATYPE));
+	uint8_t ByteSeq[] = SH_SET_OUTPUT_MODE_CMDSEQ( dataTypeSc);
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), SS_DEFAULT_CMD_SLEEP_MS);
+    if( status == 0x00){
+    	data_type = data_type_;
+        sc_en = sc_en_;
+    }
+#endif
+
+	uint8_t cmd_bytes[] = { SS_FAM_W_COMMCHAN, SS_CMDIDX_OUTPUTMODE };
+	uint8_t data_bytes[] = { (uint8_t)((sc_en_ ? SS_MASK_OUTPUTMODE_SC_EN : 0) |
+							((data_type_ << SS_SHIFT_OUTPUTMODE_DATATYPE) & SS_MASK_OUTPUTMODE_DATATYPE)) };
+
+	int status = sh_write_cmd_with_data(&cmd_bytes[0], sizeof(cmd_bytes),
+								&data_bytes[0], sizeof(data_bytes),
+								SS_DEFAULT_CMD_SLEEP_MS);
+	data_type = data_type_;
+	sc_en = sc_en_;
+
+	return status;
+}
+
+
+int sh_get_data_type(int *data_type_, bool *sc_en_){
+
+	uint8_t ByteSeq[] = SH_GET_OUTPUT_MODE_CMDSEQ;
+	uint8_t rxbuf[2] = {0};
+	int status = sh_read_cmd( &ByteSeq[0], sizeof(ByteSeq),
+							  0, 0,
+							  &rxbuf[0], sizeof(rxbuf),
+							  SS_DEFAULT_CMD_SLEEP_MS);
+	if (status == 0x00 /*SS_SUCCESS*/) {
+		*data_type_ =
+			(rxbuf[1] & SS_MASK_OUTPUTMODE_DATATYPE) >> SS_SHIFT_OUTPUTMODE_DATATYPE;
+		*sc_en_ =
+			(bool)((rxbuf[1] & SS_MASK_OUTPUTMODE_SC_EN) >> SS_SHIFT_OUTPUTMODE_SC_EN);
+
+	}
+
+	return status;
+
+}
+
+
+int sh_set_fifo_thresh( int threshold ){
+
+#if 0
+	uint8_t ucThresh = (uint8_t) (threshold & 0xFF);
+	uint8_t ByteSeq[] = SH_DFIFO_SET_INT_THRESHOLD_CMDSEQ(ucThresh );
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), SS_DEFAULT_CMD_SLEEP_MS);
+	return status;
+#endif
+
+	uint8_t cmd_bytes[] = { SS_FAM_W_COMMCHAN, SS_CMDIDX_FIFOAFULL };
+	uint8_t data_bytes[] = { (uint8_t)threshold };
+
+	int status = sh_write_cmd_with_data(&cmd_bytes[0], sizeof(cmd_bytes),
+								&data_bytes[0], sizeof(data_bytes),
+								SS_DEFAULT_CMD_SLEEP_MS
+	                            );
+	return status;
+
+}
+
+
+int sh_get_fifo_thresh(int *thresh){
+
+	uint8_t ByteSeq[] = SH_DFIFO_GET_INT_THRESHOLD_CMDSEQ;
+	uint8_t rxbuf[2] = {0};
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+							 0, 0,
+							 &rxbuf[0], sizeof(rxbuf),
+							 SS_DEFAULT_CMD_SLEEP_MS);
+
+	*thresh = (int) rxbuf[1];
+
+	return status;
+
+}
+
+
+int sh_ss_comm_check(void){
+
+
+	uint8_t ByteSeq[] = SH_COMM_CHECK_CMDSEQ;
+	uint8_t rxbuf[2];
+
+	int status = sh_read_cmd( &ByteSeq[0], sizeof(ByteSeq),
+							  0, 0,
+							  &rxbuf[0], sizeof(rxbuf),
+							  SS_DEFAULT_CMD_SLEEP_MS );
+
+	int tries = 4;
+	while (status == SS_ERR_TRY_AGAIN && tries--) {
+		wait_ms(1000);
+		status = sh_read_cmd( &ByteSeq[0], sizeof(ByteSeq),
+									  0, 0,
+									  &rxbuf[0], sizeof(rxbuf),
+									  SS_DEFAULT_CMD_SLEEP_MS );
+
+	}
+
+	return status;
+}
+
+
+int sh_num_avail_samples(int *numSamples) {
+
+	 uint8_t ByteSeq[] = SH_DFIFO_GET_NSAMPLES_CMDSEQ;
+	 uint8_t rxbuf[2] = {0};
+
+	 int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+							  0, 0,
+							  &rxbuf[0], sizeof(rxbuf),
+							  1);
+
+	 *numSamples = (int) rxbuf[1];
+
+	 return status;
+}
+
+
+int sh_read_fifo_data( int numSamples,
+		               int sampleSize,
+		               uint8_t* databuf,
+					   int databufSz) {
+
+	int bytes_to_read = numSamples * sampleSize + 1; //+1 for status byte
+
+	uint8_t ByteSeq[] = SH_DFIFO_PULL_SAMPLE_CMDSEQ;
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+							 0, 0,
+							 databuf, bytes_to_read,
+							 10);
+
+	return status;
+}
+
+
+/*
+ * desc:
+ *        func to read sample size for SmartSensor input FIFO for extrenal accel data
+ *
+ * params:
+ *		  __O sampSize:  size of data sample struct in bytes
+ * returns:
+ *        1 byte status (SS_STATUS) : 0x00 (SS_SUCCESS) on success
+ *
+ **/
+int sh_read_input_fifo_samplesz( int *sampSize){
+
+	/* NOT IMPLEMENTED IN SS INTERFACE */
+
+}
+
+/*
+ * desc:
+ *        func to write data  samples to  SmartSensor input FIFO for extrenal accel data
+ *
+ * params:
+          ...
+ * returns:
+ *        1 byte status (SS_STATUS) : 0x00 (SS_SUCCESS) on success
+ */
+int sh_write_input_fifo( void *arg){
+
+	/* NOT IMPLEMENTED IN SS INTERFACE */
+
+}
+
+
+int sh_set_reg(int idx, uint8_t addr, uint32_t val, int regSz){
+
+	uint8_t ByteSeq[] = SH_WRITE_SENSORREG_CMDSEQ( ((uint8_t)idx) , addr );
+	uint8_t data_bytes[4];
+	for (int i = 0; i < regSz; i++) {
+		data_bytes[i] = (val >> (8 * (regSz - 1)) & 0xFF);
+	}
+	int status = sh_write_cmd_with_data( &ByteSeq[0], sizeof(ByteSeq),
+							             &data_bytes[0], (uint8_t) regSz,
+										 SS_DEFAULT_CMD_SLEEP_MS);
+
+    return status;
+}
+
+
+int sh_get_reg(int idx, uint8_t addr, uint32_t *val){
+
+
+	uint32_t i32tmp;
+	uint8_t ByteSeq[] = SH_READ_AFE_ATTRIBUTES_CMDSEQ(((uint8_t) idx));
+	uint8_t rxbuf[3] = {0};
+
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+								0, 0,
+							 &rxbuf[0], sizeof(rxbuf),
+							 SS_DEFAULT_CMD_SLEEP_MS);
+
+
+    if(status == 0x00 /* SS_SUCCESS */) {
+
+    	int reg_width = rxbuf[1];
+    	uint8_t ByteSeq2[] = SH_READ_SENSORREG_CMDSEQ( ((uint8_t)idx) , addr );
+    	uint8_t rxbuf2[5] = {0};
+    	status = sh_read_cmd(&ByteSeq2[0], sizeof(ByteSeq2),
+    						0, 0,
+    						&rxbuf2[0], reg_width + 1,
+							SS_DEFAULT_CMD_SLEEP_MS);
+
+    	if (status == 0x00  /* SS_SUCCESS */) {
+    		i32tmp = 0;
+    		for (int i = 0; i < reg_width; i++) {
+    			i32tmp = (i32tmp << 8) | rxbuf2[i + 1];
+    		}
+            *val = i32tmp;
+    	}
+     }
+
+    return status;
+
+}
+
+
+int sh_sensor_enable( int idx , int sensorSampleSz , uint8_t ext_mode ){
+
+	uint8_t ByteSeq[] = SH_ENABLE_SENSOR_CMDSEQ( ((uint8_t) idx) ,  ((uint8_t) ext_mode));
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), 5 * SS_ENABLE_SENSOR_SLEEP_MS);
+	if(status == 0x00){
+
+		is_sensor_enabled[idx] = ENABLED;
+		sensor_sample_sz[idx] = sensorSampleSz;
+	}
+    return status;
+
+}
+
+
+int sh_sensor_disable( int idx ){
+
+	uint8_t ByteSeq[] = SH_DISABLE_SENSOR_CMDSEQ( ((uint8_t) idx));
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), SS_ENABLE_SENSOR_SLEEP_MS);
+	if(status == 0x00){
+
+		is_sensor_enabled[idx] = DISABLED;
+	}
+	return status;
+
+}
+
+
+int sh_get_input_fifo_size(int *fifo_size)
+{
+
+	uint8_t ByteSeq[] = SH_GET_EXTINPUT_FIFOSZ_CMDSEQ;
+	uint8_t rxbuf[3]; /* status + fifo size */
+
+
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+							  0, 0,
+							  rxbuf, sizeof(rxbuf), 2*SS_DEFAULT_CMD_SLEEP_MS);
+
+	*fifo_size = rxbuf[1] << 8 | rxbuf[2];
+	return status;
+}
+
+
+int sh_feed_to_input_fifo(uint8_t *tx_buf, int tx_buf_sz, int *nb_written)
+{
+	int status;
+
+	uint8_t ByteSeq[] = SH_FEED_TO_INPUTFIFO_CMDSEQ;
+	uint8_t rxbuf[3];
+
+	tx_buf[0] = 0x14;
+	tx_buf[1] = 0x00;
+
+	status= sh_read_cmd(tx_buf, tx_buf_sz,
+			          0, 0,
+			          rxbuf, sizeof(rxbuf), SS_FEEDFIFO_CMD_SLEEP_MS);
+
+	*nb_written = rxbuf[1] * 256 + rxbuf[2];
+	return status;
+}
+
+
+int sh_get_num_bytes_in_input_fifo(int *fifo_size)
+{
+
+    uint8_t ByteSeq[] = SH_GET_SAMPLEBYTECNT_INPUTFIFO_CMDSEQ;
+	uint8_t rxbuf[3]; /* status + fifo size */
+
+
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+							 0, 0,
+							 rxbuf, sizeof(rxbuf),
+							 2*SS_DEFAULT_CMD_SLEEP_MS);
+
+	*fifo_size = rxbuf[1] << 8 | rxbuf[2];
+	return status;
+}
+
+
+/*
+ * ALGARITIM RELATED FUNCTIONS :)
+ *
+ *
+ *
+ *
+ *
+ * */
+
+
+int sh_enable_algo(int idx , int algoSampleSz){
+
+	uint8_t ByteSeq[] = SH_ENABLE_ALGO_CMDSEQ( ((uint8_t) idx) );
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), 25 * SS_ENABLE_SENSOR_SLEEP_MS);
+	if(status == 0x00){
+
+		is_algo_enabled[idx] = ENABLED;
+		algo_sample_sz[idx]  = algoSampleSz;
+	}
+    return status;
+
+}
+
+
+int sh_disable_algo(int idx){
+
+	uint8_t ByteSeq[] = SH_DISABLE_ALGO_CMDSEQ( ((uint8_t) idx) );
+	int status = sh_write_cmd( &ByteSeq[0],sizeof(ByteSeq), SS_ENABLE_SENSOR_SLEEP_MS );
+	if(status == 0x00){
+
+		is_algo_enabled[idx] = DISABLED;
+	}
+    return status;
+
+}
+
+
+int sh_set_algo_cfg(int algo_idx, int cfg_idx, uint8_t *cfg, int cfg_sz){
+
+	uint8_t ByteSeq[] = SH_SET_ALGO_CONFIGURATION_CMDSEQ( ((uint8_t) algo_idx) ,  ((uint8_t) cfg_idx)  );
+	int status = sh_write_cmd_with_data( &ByteSeq[0], sizeof(ByteSeq),
+			                             cfg, cfg_sz,
+										 SS_DEFAULT_CMD_SLEEP_MS);
+
+	return status;
+
+}
+
+
+int sh_get_algo_cfg(int algo_idx, int cfg_idx, uint8_t *cfg, int cfg_sz){
+
+	uint8_t ByteSeq[] = SH_GET_ALGO_CONFIGURATION_CMDSEQ( ((uint8_t) algo_idx) ,  ((uint8_t) cfg_idx)  );
+	int status = sh_read_cmd(&ByteSeq[0], sizeof(ByteSeq),
+						     0, 0,
+							 cfg, cfg_sz,
+							 SS_DEFAULT_CMD_SLEEP_MS);
+	return status;
+
+}
+
+
+/*
+ * desc:
+ *      func to get active cumulative sample size of sensor hub in order to
+ *           calculate number of bytes to be read from sensor hub report data buffer
+ *
+ * params:
+ *      __I data_type : active data type of sensor hub -> no data              :0 (SS_DATATYPE_PAUSE)
+ *                                                        raw sensor data only :1 (SS_DATATYPE_RAW)
+ *                                                        algo data only       :2 (SS_DATATYPE_ALGO)
+ *                                                        algo+raw data        :3 (SS_DATATYPE_BOTH)
+ *      __O sample_size : calculated active cumulative sample size
+
+ * returns:
+ *        N/A
+ *
+ **/
+static void fifo_sample_size(int data_type_, int *sample_size)
+{
+
+    int tmpSz = 0;
+	//*sample_size = 0;
+
+	if (data_type_ == SS_DATATYPE_RAW || data_type_ == SS_DATATYPE_BOTH) {
+		for (int i = 0; i < SS_MAX_SUPPORTED_SENSOR_NUM; i++) {
+			if (is_sensor_enabled[i]) {
+				tmpSz += sensor_sample_sz[i];
+				//*sample_size += sensor_data_reqs[i]->data_size;
+			}
+		}
+	}
+
+	if (data_type_ == SS_DATATYPE_ALGO || data_type_ == SS_DATATYPE_BOTH) {
+		for (int i = 0; i < SS_MAX_SUPPORTED_ALGO_NUM; i++) {
+			if (is_algo_enabled[i]) {
+				tmpSz += algo_sample_sz[i];
+				//*sample_size += algo_data_reqs[i]->data_size;
+			}
+		}
+	}
+
+	*sample_size = tmpSz;
+}
+
+
+void sh_ss_execute_once( uint8_t *databuf , int databufLen , int *nSamplesRead){
+
+	if(m_irq_received_ == false) {
+		  *nSamplesRead = 0;
+		  return;
+	}
+
+	uint8_t sample_count;
+
+    sh_disable_irq_mfioevent();
+    sh_clear_mfio_event_flag();
+
+	uint8_t hubStatus = 0;
+	int status = sh_get_sensorhub_status(&hubStatus);
+    if(status != 0x00 /*SS_SUCCESS*/){
+    	sh_enable_irq_mfioevent();
+    	return;
+    }
+
+    if (hubStatus & SS_MASK_STATUS_DATA_RDY) {
+
+    	 int num_samples = 1;
+    	 status = sh_num_avail_samples(&num_samples);
+    	 if (status != 0x00 /*SS_SUCCESS*/){
+    		 sh_enable_irq_mfioevent();
+    	    	return;
+         }
+
+
+    	 int sample_size;
+    	 fifo_sample_size(data_type, &sample_size);
+    	 /*DEBUG *///
+    	 //printf("____DATA READY %d %d \n", (int)num_samples, sample_size);
+
+    	 int bytes_to_read = num_samples * sample_size + 1; //+1 for status byte
+         if ((uint32_t)bytes_to_read > databufLen) {
+ 			//Reduce number of samples to read to fit in buffer
+ 			num_samples = (databufLen - 1) / sample_size;
+ 		 }
+        wait_ms(5);
+        status = sh_read_fifo_data(num_samples, sample_size, &databuf[0], databufLen);
+        if(status != 0x00 /*SS_SUCCESS*/){
+        	*nSamplesRead = 0;
+        	sh_enable_irq_mfioevent();
+        	return;
+        }
+        *nSamplesRead = num_samples;
+
+    }
+
+    sh_enable_irq_mfioevent();
+    return;
+}
+
+
+
+
+
+
+/*
+#ifdef __cplusplus
+}
+#endif
+*/
+