File content as of revision 0:ddc2fef69ef9:

/*******************************************************************************
 * 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
*/