File content as of revision 2:3b5d2467e6c7:

/*
 * SHcomm.cpp
 *
 *  Created on: Nov 16, 2018
 *      Author: Yagmur.Gok
 */

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



//phase2 additions
#define SH_CHECKIF_BOOTLDRMODE_CMDSEQ   { 0x02, 0x00 }
#define SH_SELFTEST_CMDSEQ(idx)         { 0x70, (uint8_t)idx }
#define SH_EXIT_BOOTLDRMODE_CMDSEQ      { 0x01, 0x00 }
#define SH_GETLOGSIZE_CMDSEQ            { 0x90, 0x01 }
#define SH_READHUBLOGS_CMDSEQ           { 0x90, 0x00 }

#define SH_GET_BOOTLDRPAGESIZE_CMDSEQ   { 0x81, 0x01 }
#define SH_SET_BOOTLDRPAGECOUNT_CMDSEQ  { 0x80, 0x02 }

#define BOOTLOADER_MAX_PAGE_SIZE 8192

/* BOOTLOADER HOST */
#define EBL_CMD_TRIGGER_MODE	0
#define EBL_GPIO_TRIGGER_MODE	1



/*
 *   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 enabled_algo_mode[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};

/* Mode to control sesnor hub resets. ie via GPIO based hard reset or Command based soft reset*/
static uint8_t ebl_mode = EBL_GPIO_TRIGGER_MODE;

/* 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, on host side
 *
 *  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();


}


int sh_set_ebl_mode(const uint8_t mode)
{
	int status;
	if (mode == EBL_CMD_TRIGGER_MODE || mode == EBL_GPIO_TRIGGER_MODE) {
		ebl_mode = mode;
		status =  SS_SUCCESS;
	} else
		status = SS_ERR_INPUT_VALUE;

	return status;
}

const int sh_get_ebl_mode(void)
{
   return ebl_mode;
}

int sh_reset_to_bootloader(void){

	int status;
	uint8_t hubMode;

     if(ebl_mode == EBL_GPIO_TRIGGER_MODE)
    	 sh_hard_reset(0x08);
     if(ebl_mode == EBL_CMD_TRIGGER_MODE)
    	 status = sh_set_sensorhub_operating_mode(0x08);

     status = sh_get_sensorhub_operating_mode(&hubMode);
     if( status != 0x00 /*SS_SUCCESS*/ || hubMode != 0x08 ){
    	 status = -1;
     }

     return status;

}

static bool in_bootldr;


int in_bootldr_mode()
{
	uint8_t cmd_bytes[] = { SS_FAM_R_MODE, SS_CMDIDX_MODE };
	uint8_t rxbuf[2] = { 0 };

	int status = sh_read_cmd(&cmd_bytes[0], sizeof(cmd_bytes),
			0, 0,
			&rxbuf[0], sizeof(rxbuf), SS_DEFAULT_CMD_SLEEP_MS);
	if (status != SS_SUCCESS)
		return -1;

	return (rxbuf[1] & SS_MASK_MODE_BOOTLDR);
}

int exit_from_bootloader(void)
{
	uint8_t cmd_bytes[] = { SS_FAM_W_MODE, SS_CMDIDX_MODE };
	uint8_t data[] = { 0x00 };

	int status = sh_write_cmd_with_data( &cmd_bytes[0], sizeof(cmd_bytes),
										 &data[0], 1 /*sizeof(data)*/,
										 10*SS_DEFAULT_CMD_SLEEP_MS);

	in_bootldr = (status == SS_SUCCESS) ? true : false;

	return status;
}

int stay_in_bootloader()
{
	uint8_t cmd_bytes[] = { SS_FAM_W_MODE, SS_CMDIDX_MODE };
	uint8_t data[] = { SS_MASK_MODE_BOOTLDR };

	int status = sh_write_cmd_with_data(
			&cmd_bytes[0], sizeof(cmd_bytes),
			&data[0], sizeof(data), SS_DEFAULT_CMD_SLEEP_MS);

	in_bootldr = (status == SS_SUCCESS) ? true : false;
	return status;
}


static void cfg_mfio(PinDirection dir)
{
	if (dir == PIN_INPUT) {
		mfio_pin.input();
		mfio_pin.mode(PullUp);
	} else {
		sh_enable_irq_mfioevent();
		mfio_pin.output();
	}
}

int sh_debug_reset_to_bootloader(void)
{

	int status = -1;

	sh_disable_irq_mfioevent();
	if (ebl_mode == EBL_GPIO_TRIGGER_MODE) {

		reset_pin.output();
		cfg_mfio(PIN_OUTPUT);
		reset_pin.write(0);
		wait_ms(SS_RESET_TIME);
		mfio_pin.write(0);
		reset_pin.write(1);
		wait_ms(SS_STARTUP_TO_BTLDR_TIME);
		cfg_mfio(PIN_INPUT);
		reset_pin.input();
		sh_enable_irq_mfioevent();
		stay_in_bootloader();
		if (in_bootldr_mode() < 0)
			status = SS_ERR_UNKNOWN;
		else
			status = SS_SUCCESS;

	}else{
		stay_in_bootloader();
		sh_enable_irq_mfioevent();
		status = SS_SUCCESS;

	}

    return status;
}


int sh_reset_to_main_app(void)
{
	int status = -1;
	sh_disable_irq_mfioevent();
	if (ebl_mode == EBL_GPIO_TRIGGER_MODE) {

		reset_pin.output();
		cfg_mfio(PIN_OUTPUT);
		mfio_pin.write(0);
		wait_ms(SS_RESET_TIME - 5);
		reset_pin.write(0);
		wait_ms(SS_RESET_TIME - 5);
		mfio_pin.write(1);
		wait_ms(SS_RESET_TIME - 5);
		reset_pin.write(1);
		//wait_ms(50);
		//mfio_pin.write(0);
		wait_ms(2*SS_STARTUP_TO_MAIN_APP_TIME);
		cfg_mfio(PIN_INPUT);
		reset_pin.input();

    	sh_enable_irq_mfioevent();
		// Verify we exited bootloader mode
		if (in_bootldr_mode() == 0)
			status = SS_SUCCESS;
		else
			status = SS_ERR_UNKNOWN;
	}else{
		status = exit_from_bootloader();
		sh_enable_irq_mfioevent();
	}

	return status;

}

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


//PHASE2 ADDITIONS:

int sh_self_test(int idx, uint8_t *result, int sleep_ms){

	uint8_t cmd_bytes[] = { SS_FAM_R_SELFTEST, (uint8_t)idx }; // = SH_SELFTEST_CMDSEQ;
    uint8_t rxbuf[2];
    result[0] = 0xFF;

    int status = sh_read_cmd(&cmd_bytes[0],sizeof(cmd_bytes) ,
                             0, 0,
						     &rxbuf[0], sizeof(rxbuf),
						     sleep_ms  );

	if (status != SS_SUCCESS)
		return SS_ERR_TRY_AGAIN;

    result[0] = rxbuf[1];
	return status;
}

const char* sh_get_hub_fw_version(void)
{
    uint8_t cmd_bytes[2];
    uint8_t rxbuf[4];

    static char fw_version[32] = "SENSORHUB";

	int bootldr = sh_checkif_bootldr_mode();

	if (bootldr > 0) {
		cmd_bytes[0] = SS_FAM_R_BOOTLOADER;
		cmd_bytes[1] = SS_CMDIDX_BOOTFWVERSION;
	} else if (bootldr == 0) {
		cmd_bytes[0] = SS_FAM_R_IDENTITY;
		cmd_bytes[1] = SS_CMDIDX_FWVERSION;
	} else {

		return &fw_version[0];
	}

    int status = sh_read_cmd( &cmd_bytes[0], sizeof(cmd_bytes),
             	 	 	 	  0, 0,
							  &rxbuf[0], sizeof(rxbuf),
							  SS_DEFAULT_CMD_SLEEP_MS );

    if (status == SS_SUCCESS) {
        snprintf(fw_version, sizeof(fw_version),
            "%d.%d.%d", rxbuf[1], rxbuf[2], rxbuf[3]);
	}

    return &fw_version[0];
}


const char* sh_get_hub_algo_version(void)
{
    uint8_t cmd_bytes[3];
    uint8_t rxbuf[4];

    static char algo_version[64] = "SENSORHUBALGORITHMS";

	int bootldr = sh_checkif_bootldr_mode();

	if (bootldr > 0) {
		cmd_bytes[0] = SS_FAM_R_BOOTLOADER;
		cmd_bytes[1] = SS_CMDIDX_BOOTFWVERSION;
		cmd_bytes[2] = 0;
	} else if (bootldr == 0) {
		cmd_bytes[0] = SS_FAM_R_IDENTITY;
		cmd_bytes[1] = SS_CMDIDX_ALGOVER;
		cmd_bytes[2] = SS_CMDIDX_AVAILSENSORS;
	} else {

		return &algo_version[0];
	}

    int status = sh_read_cmd( &cmd_bytes[0], sizeof(cmd_bytes),
                              0, 0,
                              &rxbuf[0], sizeof(rxbuf),
						      SS_DEFAULT_CMD_SLEEP_MS   );

    if (status == SS_SUCCESS) {
        snprintf(algo_version, sizeof(algo_version),
            "%d.%d.%d", rxbuf[1], rxbuf[2], rxbuf[3]);

    }

    return &algo_version[0];
}

int sh_send_raw(uint8_t *rawdata, int rawdata_sz)
{
	return sh_write_cmd(&rawdata[0], rawdata_sz, 5 * SS_ENABLE_SENSOR_SLEEP_MS);
}

int sh_get_log_len(int *log_len)
{
	uint8_t cmd_bytes[] = { SS_FAM_R_LOG, SS_CMDIDX_R_LOG_LEN }; // = SH_GETLOGSIZE_CMDSEQ;
	uint8_t rxbuf[2] = {0};
    int logLen = 0;

	int status = sh_read_cmd(&cmd_bytes[0], sizeof(cmd_bytes),
								   0, 0,
								   &rxbuf[0], sizeof(rxbuf),
								   SS_DEFAULT_CMD_SLEEP_MS   );

	if (status == SS_SUCCESS) {
		logLen = (rxbuf[1] << 8) | rxbuf[0];
	}
	*log_len = logLen;

	return status;
}

int sh_read_ss_log(int num_bytes, uint8_t *log_buf, int log_buf_sz)
{
	int bytes_to_read = num_bytes + 1; //+1 for status byte
	//mxm_assert_msg((bytes_to_read <= log_buf_sz), "log_buf too small");

	uint8_t cmd_bytes[] = { SS_FAM_R_LOG, SS_CMDIDX_R_LOG_DATA }; // = SH_READHUBLOGS_CMDSEQ;

	int status = sh_read_cmd(&cmd_bytes[0], sizeof(cmd_bytes),
						     0, 0,
							 log_buf, bytes_to_read,
							 SS_CMD_WAIT_PULLTRANS_MS  );

	return status;
}

// END OF PHASE2 ADDITIONS




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_enable_algo_withmode(int idx, int mode, int algoSampleSz)
{

	uint8_t cmd_bytes[] = { SS_FAM_W_ALGOMODE, (uint8_t)idx, (uint8_t)mode };

	int status = sh_write_cmd_with_data(&cmd_bytes[0], sizeof(cmd_bytes), 0, 0, 25 * SS_ENABLE_SENSOR_SLEEP_MS);

	if (status == SS_SUCCESS) {
		is_algo_enabled[idx]   = ENABLED;
		algo_sample_sz[idx]    = algoSampleSz;
		enabled_algo_mode[idx] = mode;
	}

	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;
}


int sh_ss_execute_once( uint8_t *databuf , int databufLen , int *nSamplesRead){

	if(m_irq_received_ == false) {
		  *nSamplesRead = 0;
		  return -1;
	}

	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*/){
    	*nSamplesRead = 0;
    	sh_enable_irq_mfioevent();
        return status;
    }

    if (hubStatus & SS_MASK_STATUS_DATA_RDY) {

    	 int num_samples = 1;
    	 status = sh_num_avail_samples(&num_samples);
      	 if (status != 0x00 /*SS_SUCCESS*/){
    		 *nSamplesRead = 0;
    		 sh_enable_irq_mfioevent();
    		 return status;
         }


    	 int sample_size;
    	 fifo_sample_size(data_type, &sample_size);
    	 /*DEBUG *///


    	 int bytes_to_read = num_samples * sample_size + 1; //+1 for status byte
         if ( 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 status;
        }
        *nSamplesRead = num_samples;
    }

    sh_enable_irq_mfioevent();
    return status;
}



/*
 * BOOTLOADER RELATED FUNCTIONS
 *
 *
 * */

static const int aes_nonce_sz = 11;
static const int aes_auth_sz  = 16;
static int bl_comm_delay_factor = 1;



int sh_set_bootloader_delayfactor(const int factor ) {

	int status = -1;
	if( factor >= 1  && factor < 51){
	    bl_comm_delay_factor = factor;
	    status = 0x00;
	}

	return status;

}

const int sh_get_bootloader_delayfactor(void){

     return bl_comm_delay_factor;
}

int sh_exit_from_bootloader(void)
{

	return sh_reset_to_main_app(); //sh_set_sensorhub_operating_mode(0x00);
}

int sh_put_in_bootloader(void)
{
	return sh_set_sensorhub_operating_mode( 0x08);
}

int sh_checkif_bootldr_mode(void)
{
	uint8_t hubMode;
	int status = sh_get_sensorhub_operating_mode(&hubMode);
	return (status != SS_SUCCESS)? -1:(hubMode & SS_MASK_MODE_BOOTLDR);
}

int sh_get_bootloader_pagesz(int *pagesz){

	//uint8_t ByteSeq[]= SH_GET_BOOTLDRPAGESIZE_CMDSEQ;
	uint8_t ByteSeq[]= { SS_FAM_R_BOOTLOADER, SS_CMDIDX_PAGESIZE };
    uint8_t rxbuf[3];
    int sz = 0;

    int status = sh_read_cmd( &ByteSeq[0], sizeof(ByteSeq),
                          0, 0,
                          &rxbuf[0], sizeof(rxbuf),
						  SS_DEFAULT_CMD_SLEEP_MS);
    if (status == 0x00) {
           //rxbuf holds page size in big-endian format
            sz = (256*(int)rxbuf[1]) + rxbuf[2];
            if(sz > BOOTLOADER_MAX_PAGE_SIZE ) {
                   sz = -2;
            }
    }

    *pagesz = sz;

    return status;

}

int sh_set_bootloader_numberofpages(const int pageCount){

	//uint8_t ByteSeq[] = SH_SET_BOOTLDRPAGECOUNT_CMDSEQ;
    uint8_t ByteSeq[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETNUMPAGES };
    //num pages = 256*MSB + LSB
    uint8_t data_bytes[] = { (uint8_t)((pageCount >> 8) & 0xFF), (uint8_t)(pageCount & 0xFF) };

    int status = sh_write_cmd_with_data(&ByteSeq[0], sizeof(ByteSeq),
								        &data_bytes[0], sizeof(data_bytes),
										bl_comm_delay_factor * SS_DEFAULT_CMD_SLEEP_MS );

    return status;

}

int sh_set_bootloader_iv(uint8_t iv_bytes[aes_nonce_sz]){

	 uint8_t ByteSeq[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETIV };
	 int status = sh_write_cmd_with_data( &ByteSeq[0], sizeof(ByteSeq),
			                              &iv_bytes[0], aes_nonce_sz /*sizeof(iv_bytes)*/,
										  bl_comm_delay_factor * SS_DEFAULT_CMD_SLEEP_MS
										  );

     return status;

}


int sh_set_bootloader_auth(uint8_t auth_bytes[aes_auth_sz]){

	 uint8_t ByteSeq[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SETAUTH };
	 int status = sh_write_cmd_with_data( &ByteSeq[0], sizeof(ByteSeq),
			                              &auth_bytes[0], aes_auth_sz /*sizeof(auth_bytes)*/,
										  bl_comm_delay_factor * SS_DEFAULT_CMD_SLEEP_MS
										  );

     return status;

}


int sh_set_bootloader_erase(void){

    uint8_t ByteSeq[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_ERASE };

    int status = sh_write_cmd_with_data(&ByteSeq[0], sizeof(ByteSeq),
                                        0, 0,
										bl_comm_delay_factor * SS_BOOTLOADER_ERASE_DELAY);

    return status;

}


int sh_bootloader_flashpage(uint8_t *flashDataPreceedByCmdBytes , const int page_size){

	static const int flash_cmdbytes_len   = 2;
	static const int check_bytes_len      = 16;
	static const int page_write_time_ms   = 200;

    //static const uint8_t ByteSeq[] = { SS_FAM_W_BOOTLOADER, SS_CMDIDX_SENDPAGE };
    int status = -1;

    if( (*flashDataPreceedByCmdBytes == SS_FAM_W_BOOTLOADER) &&  ( *(flashDataPreceedByCmdBytes+1) == SS_CMDIDX_SENDPAGE ) ) {

		/* We do not use sh_write_cmd_with_data function because internal buffers of the function
		   is limited to 512 bytes which does not support if flashing page size is bigger */
		status = sh_write_cmd(flashDataPreceedByCmdBytes, page_size + check_bytes_len + flash_cmdbytes_len, bl_comm_delay_factor * page_write_time_ms);

    }
	return status;

}



/*
#ifdef __cplusplus
}
#endif
*/