File content as of revision 12:9e3de2ece9c4:

/*******************************************************************************
 * 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.
 *******************************************************************************
 */
#include "SH_Max8614x_BareMetal.h"
#include "SHComm.h"
#include "HostAccelHelper.h"
#include <string.h> //for memset
#include <stdint.h>

uint16_t HrmResult       = 0;
uint16_t SPO2Result      = 0;
uint8_t  HrmConfidence   = 0;
uint8_t  SPo2Confidence  = 0;



#if defined(DEBUG_INFO)
	#define __DBGMESSAGE( str , val ) {printf(str, val);}
#else
	#define __DBGMESSAGE( str , val )
#endif


// Defines
#define SSMAX8614X_REG_SIZE  		1
#define SSMAX8614X_MODE1_DATASIZE  18				//Taken from API doc
#define SSWHRM_MODE1_DATASIZE 		6				//Taken from API doc
#define SSWSPO2_MODE1_DATASIZE  	9				// added for wspo2
#define SSACCEL_MODE1_DATASIZE  	6				//Taken from API doc
#define SSAGC_MODE1_DATASIZE  		0				//Taken from API doc
#define SSBPT_MODE1_2_DATASIZE  	4				//Taken from API doc /* TODO */

#define MIN_MACRO(a,b) ((a)<(b)?(a):(b))

// sensor configuration
//#define ENABLE_SENSOR_HUB_ACCEL
#define USE_HOST_ACCEL
// algorithm configuration
#define ENABLE_WHRM_AND_SP02
#define ENABLE_WSP02
// end of senor and algorithm configuration
#define MAX_NUM_WR_ACC_SAMPLES			 5
#define BMI160_SAMPLE_RATE				25
// end of defines

//function pointer use to perform arithmetic operation
typedef void (*rx_data_callback)(uint8_t *);
typedef struct {
	int data_size;
	rx_data_callback rx_data_parser;
} ss_data_req;

typedef struct {
	int16_t x;
	int16_t y;
	int16_t z;
} accel_mode1_data;

typedef struct {
	uint32_t led1;
	uint32_t led2;
	uint32_t led3;
	uint32_t led4;
	uint32_t led5;
	uint32_t led6;
} max8614x_mode1_data;

typedef struct {
	uint16_t hr;
	uint8_t hr_conf;
	uint16_t spo2;
	uint8_t status;
} whrm_mode1_data;


typedef struct {  /// added for WSPO2
	uint16_t r;
	uint8_t spo2_conf;
	uint16_t spo2;
	uint8_t percentComplete;
	uint8_t lowSignalQualityFlag;
	uint8_t motionFlag;
	uint8_t status;   //isSpo2Calculated;
} wspo2_mode1_data;

typedef struct Max86140_SH_Status_Tracker {
	uint8_t sensor_data_from_host;
	uint8_t data_type_enabled;					// what type of data is enabled
	uint8_t sample_count_enabled;				// does me11 provide sample count
	uint32_t sample_count;
	uint8_t data_buf_storage[512];				// store data read from SH
	ss_data_req algo_callbacks[SH_NUM_CURRENT_ALGOS];
	ss_data_req sensor_callbacks[SH_NUM_CURRENT_SENSORS];
	uint8_t sensor_enabled_mode[SH_NUM_CURRENT_SENSORS];
	uint8_t algo_enabled_mode[SH_NUM_CURRENT_ALGOS];
	int input_fifo_size;
} Max86140_SH_Status_Tracker_t;

// Max8614x Default Callbacks
void max8614x_data_rx(uint8_t* data_ptr)
{
	max8614x_mode1_data sample;
	sample.led1 = (data_ptr[0] << 16) | (data_ptr[1] << 8) | data_ptr[2];
	sample.led2 = (data_ptr[3] << 16) | (data_ptr[4] << 8) | data_ptr[5];
	sample.led3 = (data_ptr[6] << 16) | (data_ptr[7] << 8) | data_ptr[8];
	sample.led4 = (data_ptr[9] << 16) | (data_ptr[10] << 8) | data_ptr[11];
	sample.led5 = (data_ptr[12] << 16) | (data_ptr[13] << 8) | data_ptr[14];
	sample.led6 = (data_ptr[15] << 16) | (data_ptr[16] << 8) | data_ptr[17];

	//printf("led1=%.6X led2=%.6X led3=%.6X led4=%.6X led5=%.6X led6=%.6X\r\n",
	//		sample.led1, sample.led2, sample.led3, sample.led4, sample.led5, sample.led6);

	//enqueue(&max8614x_queue, &sample);
}
void whrm_data_rx(uint8_t* data_ptr) {
	//See API doc for data format
	whrm_mode1_data sample;
	sample.hr = (data_ptr[0] << 8) | data_ptr[1];
	sample.hr_conf = data_ptr[2];
	sample.spo2 = (data_ptr[3] << 8) | data_ptr[4];
	sample.status = data_ptr[5];
	HrmResult  = sample.hr / 10;
	HrmConfidence = sample.hr_conf;
	//printf("hr_c=%d\r\n", HrmResult);
#if defined(DEBUG_INFO)
	printf("hr=%.1f conf=%d spo2=%d status=%d\r\n", (float)sample.hr / 10.0, sample.hr_conf, sample.spo2, sample.status);
#endif
	//enqueue(&whrm_queue, &sample);
}

void wspo2_data_rx(uint8_t* data_ptr)
{
	//See API doc for data format
	wspo2_mode1_data sample;
	sample.r = (data_ptr[0] << 8) | data_ptr[1];  // already x10
	sample.spo2_conf = data_ptr[2];
	sample.spo2 = (data_ptr[3] << 8) | data_ptr[4]; // already x10
	sample.percentComplete = data_ptr[5];
	sample.lowSignalQualityFlag = data_ptr[6];
	sample.motionFlag = data_ptr[7];
	sample.status = data_ptr[8];
	SPO2Result = sample.spo2 / 10;
	SPo2Confidence = sample.spo2_conf;
#if defined(DEBUG_INFO)
	printf("r=%.1f SpO2Conf=%d SpO2=%.1f prcntComp=%d lowSig=%d motion=%d isCalc=%d\r\n", (float)sample.r / 10.0, sample.spo2_conf, (float)sample.spo2/10.0, sample.percentComplete, sample.lowSignalQualityFlag , sample.motionFlag, sample.status);
#endif
	//enqueue(&wspo2_queue, &sample);
}

void accel_data_rx(uint8_t* data_ptr) {
	//See API doc for data format
	accel_mode1_data sample;
	sample.x = (data_ptr[0] << 8) | data_ptr[1];
	sample.y = (data_ptr[2] << 8) | data_ptr[3];
	sample.z = (data_ptr[4] << 8) | data_ptr[5];
	//printf("x:%d, y:%d, z:%d\r\n", sample.x, sample.y, sample.z);
}

void agc_data_rx(uint8_t* data_ptr) {
	//NOP: AGC does not collect data
}
// end of Max8614x Default Callbacks


static Max86140_SH_Status_Tracker * get_config_struct() {

    /* assigns a static adress to configuration struct*/
	static Max86140_SH_Status_Tracker glbl_max8614x_status_track;
	return &glbl_max8614x_status_track;
}

void initialize_config_struct() {
	Max86140_SH_Status_Tracker *p_glbl_max8614x_status_track = get_config_struct();
	/*
	 * Desc: Configuration init flow, Perform this action at init stage of data acquisition. Raw sesnsor data buffer pointer is input to each
	 *       enabled sensor/algorithm,s funtion that is responsible to extract numeric data from data byte stream from sensor hub.
	 *
	 *       - Append Sensor Raw Data structure with raw sensor data sample size and pointer to function of sensor that is reposible to parse
	 *         data byte stream from sesnor hub and extract sensor numeric data.
	 *       - Append accompanying sensors to main state of sensor. ie Accelerometer from Host with sensor data sample size and pointer to function of
	 *         sensor that is reposible to parse data byte stream from sesnor hub and extract sensor numeric data.
	 *       - Append algorithms to be enabled  with algorithm data sample size and pointer to function of
	 *         algorithm that is reposible to parse data byte stream from sensor hub and extract sensor numeric data.
	 *
	 * */

	//set all the values to 0
	memset(p_glbl_max8614x_status_track, 0, sizeof(*p_glbl_max8614x_status_track));
	// max8614x
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_MAX8614X].data_size = SSMAX8614X_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_MAX8614X].rx_data_parser = &max8614x_data_rx;
	// accelerometer
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_ACCEL].data_size = SSACCEL_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_ACCEL].rx_data_parser = &accel_data_rx;
	// agc
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_AGC].data_size = SSAGC_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_AGC].rx_data_parser = &agc_data_rx;
	// whrm
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WHRM].data_size = SSWHRM_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WHRM].rx_data_parser = &whrm_data_rx;
	// spo2
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WSPO2].data_size = SSWSPO2_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WSPO2].rx_data_parser = &wspo2_data_rx;
}


void SH_Max8614x_get_reg(uint8_t addr, uint32_t *val) {
	int status = sh_get_reg(SH_SENSORIDX_MAX8614X, addr, val);

	if (status == 0) {
		__DBGMESSAGE("\r\n reg_val=%02X err=0 \r\n", ((uint8_t)*val))
	} else {
		__DBGMESSAGE("\r\n err=%d\r\n", -1)
	}

	return;
}

int CSTMR_SH_FeedAccDataIntoSH(Max86140_SH_Status_Tracker_t *p_max8614x_status_track) {
	static accel_data_t peek_buf[MAX_NUM_WR_ACC_SAMPLES];
	static uint8_t tx_buf[MAX_NUM_WR_ACC_SAMPLES * sizeof(accel_mode1_data) + 2]; // 2 bytes for the command
	if(!p_max8614x_status_track->sensor_data_from_host) {
		return -1;
	} else {
		accel_data_t accel_data = {0};
		accel_mode1_data acc_sample;
		int num_tx, num_samples, num_bytes = 0, num_wr_bytes = 0;
		int num_written_samples, nb_expected;
		int ret = 0;

		// get accelerometer data
		ret = CSTMR_SH_HostAccelerometerGet_sensor_xyz(&accel_data);
		if (ret < 0)
			return ret;

		if(CSTMR_SH_HostAccelerometerEnqueueData(&accel_data) != 0) {
			__DBGMESSAGE("Thrown an accel sample\n", NULL)
		}

		if(CSTMR_SH_HostAccelerometerGetDataCount() < MAX_NUM_WR_ACC_SAMPLES) {
			return -1;
		}

		ret = sh_get_num_bytes_in_input_fifo(&num_bytes);
		if (ret != 0) {
			__DBGMESSAGE("Unable to read num bytes in input fifo\r\n", NULL)
			return -1;
		}
		num_tx = p_max8614x_status_track->input_fifo_size - num_bytes;
		if (num_tx <= 0) {
			__DBGMESSAGE("num_tx can't be negative\r\n",NULL)
			return -1;
		}
		num_samples = num_tx / sizeof(accel_mode1_data);
		num_samples = MIN_MACRO(num_samples, MAX_NUM_WR_ACC_SAMPLES);
		num_tx = num_samples * sizeof(accel_mode1_data);
		if (num_samples == 0) {
			__DBGMESSAGE("Input FIFO is Full\r\n",NULL)
			return -1;
		}

		for(int i = 0; i < num_samples; ++i) {
			ret |= CSTMR_SH_HostAccelerometerDequeuData(&peek_buf[i]);
		}
		if (ret != 0) {
			__DBGMESSAGE("CSTMR_SH_HostAccelerometerDequeuData failed\r\n",NULL)
			return -1;
		}


		for (int i = 2, j = 0; j < num_samples; i+= sizeof(accel_mode1_data), j++) {
			accel_data = peek_buf[j];
			acc_sample.x = (int16_t)(accel_data.x*1000);
			acc_sample.y = (int16_t)(accel_data.y*1000);
			acc_sample.z = (int16_t)(accel_data.z*1000);
			tx_buf[i] = acc_sample.x;
			tx_buf[i + 1] = acc_sample.x >> 8;
			tx_buf[i + 2] = acc_sample.y;
			tx_buf[i + 3] = acc_sample.y >> 8;
			tx_buf[i + 4] = acc_sample.z;
			tx_buf[i + 5] = acc_sample.z >> 8;

		}

		ret = sh_feed_to_input_fifo(tx_buf, num_tx + 2, &num_wr_bytes);
		if(ret != 0) {
			__DBGMESSAGE("sh_feed_to_input_fifo\r\n",NULL)
			return -1;
		}
		num_written_samples = num_wr_bytes / sizeof(accel_mode1_data);
		if(num_written_samples != num_samples) {
			__DBGMESSAGE("num_written_samples failed\r\n",NULL)
			return -1;
		}
	}
	return 0;
}


void SH_Max8614x_set_reg(uint8_t addr, uint32_t val) {
	int status;
	status = sh_set_reg(SH_SENSORIDX_MAX8614X, addr, val, SSMAX8614X_REG_SIZE);
	__DBGMESSAGE("\r\n err=%d\r\n", status);
}



int SH_Max8614x_data_report_execute(void) {

	int num_samples, databufLen;
	uint8_t *databuf;


	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	// prepare the buffer to store the results
	databuf = p_glbl_max8614x_status_track->data_buf_storage;
	databufLen = sizeof(p_glbl_max8614x_status_track->data_buf_storage);

	// poll SH
	sh_ss_execute_once(databuf, databufLen, &num_samples);

	// feed accelerometer into me11
	CSTMR_SH_FeedAccDataIntoSH(p_glbl_max8614x_status_track);

	if(num_samples) {
		//Skip status byte
		uint8_t *data_ptr = &databuf[1];

		int i = 0;
		for (i = 0; i < num_samples; i++) {
			int sh_data_type = p_glbl_max8614x_status_track->data_type_enabled;
			if (p_glbl_max8614x_status_track->sample_count_enabled) {
				p_glbl_max8614x_status_track->sample_count = *data_ptr++;
			}
			//Chop up data and send to modules with enabled sensors
			if (sh_data_type == SS_DATATYPE_RAW || sh_data_type == SS_DATATYPE_BOTH) {
				for (int i = 0; i < SH_NUM_CURRENT_SENSORS; i++) {
					if (p_glbl_max8614x_status_track->sensor_enabled_mode[i]) {
						p_glbl_max8614x_status_track->sensor_callbacks[i].rx_data_parser(data_ptr);
						data_ptr += p_glbl_max8614x_status_track->sensor_callbacks[i].data_size;
					}
				}
			}
			if (sh_data_type == SS_DATATYPE_ALGO || sh_data_type == SS_DATATYPE_BOTH) {
				for (int i = 0; i < SH_NUM_CURRENT_ALGOS; i++) {
					if (p_glbl_max8614x_status_track->algo_enabled_mode[i]) {
						p_glbl_max8614x_status_track->algo_callbacks[i].rx_data_parser(data_ptr);
						data_ptr += p_glbl_max8614x_status_track->algo_callbacks[i].data_size;
					}
				}
			}
		}
	}
	return num_samples;
}

int SH_Max8614x_algo_init(enum enAlgoMode paramAlgoMode) {

	/*
	 *
	 * */
	int status;
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();
	if(p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WHRM] ||
			p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WSPO2]) {
		    __DBGMESSAGE("\r\n Algo already enabled\r\n",NULL)
		return -1;
	}

	if(paramAlgoMode == kAlgoModeHeartRate) {
		status = sh_enable_algo(SH_ALGOIDX_WHRM, SSWHRM_MODE1_DATASIZE);
		if (status != SS_SUCCESS) {
			__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
			__DBGMESSAGE("FAILED at line %d, enable whrm\n", __LINE__)
			return status;
		}
		p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WHRM] = 0x01;
	} else {
		status = sh_enable_algo(SH_ALGOIDX_WSPO2, SSWSPO2_MODE1_DATASIZE);
		if (status != SS_SUCCESS) {
			__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
			__DBGMESSAGE("FAILED at line %d, enable whrm\n", __LINE__)
			return status;
		}
		p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WSPO2] = 0x01;
	}
}


int SH_Max8614x_default_init(enum enAlgoMode paramAlgoMode) {
	/*
	 *  Desc: Initialization flow to get algorithm estimation results:
	 *        1. initialize algorithm config struct
	 *        2. enable data type to both raw sensor and algorithm data
	 *        3. get input fifo size to learn fifo capacity
	 *        4. set fifo threshold for mfio event frequency
	 *        5. enable sensor to acquire ppg data
	 *        6. enable accompanying accel sensor
	 *        7. enable algorithm
	 *        8. Sensor Hub now starts to write raw sensor/algorithm data to its data report FIFO which
	 *           reports mfio event when data size determined by fifo threshold is written to report fifo
	 *           data can be read by SH_Max8614x_data_report_execute function.
	 *
	 * */

	int status;

	// first initialize the global config struct
	initialize_config_struct();
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	// get input fifo size
	status = sh_get_input_fifo_size(&p_glbl_max8614x_status_track->input_fifo_size);
	if (status != SS_SUCCESS) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;;
	}

	// enable both data stype
	p_glbl_max8614x_status_track->data_type_enabled = SS_DATATYPE_BOTH;
	p_glbl_max8614x_status_track->sample_count_enabled = false;
	status = sh_set_data_type(p_glbl_max8614x_status_track->data_type_enabled,
							  p_glbl_max8614x_status_track->sample_count_enabled);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}

	status = sh_set_fifo_thresh(15);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}

	status = sh_sensor_enable(SH_SENSORIDX_MAX8614X, SSMAX8614X_MODE1_DATASIZE, SH_INPUT_DATA_DIRECT_SENSOR);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n",  COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_MAX8614X] = 0x01;

#ifdef ENABLE_SENSOR_HUB_ACCEL
	status = sh_sensor_enable(SH_SENSORIDX_ACCEL, SSACCEL_MODE1_DATASIZE, SH_INPUT_DATA_DIRECT_SENSOR);
	if (status != SS_SUCCESS) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
	}
	p_glbl_max8614x_status_track->sensor_data_from_host = false;
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_ACCEL] = 0x01;
#elif defined(USE_HOST_ACCEL)
	CSTMR_SH_HostAccelerometerInitialize();
	CSTMR_SH_HostAccelerometerSetDefaults();
	status = CSTMR_SH_HostAccelerometerSetSampleRate(BMI160_SAMPLE_RATE);
	if (status != 0) {
		__DBGMESSAGE("Unable to set BMI160's sample rate\n",NULL)
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return status;
	}

	status = CSTMR_SH_HostAccelerometerEnableDataReadyInterrupt();
	if(status != 0){
		__DBGMESSAGE("Unable to enable BMI160 Interrupt, ret: %d\n", status)
		return status;
	}

	status = sh_sensor_enable(SH_SENSORIDX_ACCEL, SSACCEL_MODE1_DATASIZE, SH_INPUT_DATA_FROM_HOST);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return status;
	}
	p_glbl_max8614x_status_track->sensor_data_from_host = true;
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_ACCEL] = 0x01;
#endif
	status = SH_Max8614x_algo_init(paramAlgoMode);
	if(status != 0) {
		__DBGMESSAGE("AlgoInitFailed\r\n",NULL)
	}

	    __DBGMESSAGE("\r\n err=%d\r\n",  status)
	return status;
}

void SH_Max8614x_stop() {
	sh_disable_irq_mfioevent();
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	for(int i = 0; i < SH_NUM_CURRENT_SENSORS; ++i) {
		if(p_glbl_max8614x_status_track->sensor_enabled_mode[i]) {
			p_glbl_max8614x_status_track->sensor_enabled_mode[i] = 0;
			sh_sensor_disable(i);
		}

	}

	for(int i = 0; i < SH_NUM_CURRENT_ALGOS; ++i) {
		if(p_glbl_max8614x_status_track->algo_enabled_mode[i]) {
			p_glbl_max8614x_status_track->algo_enabled_mode[i] = 0;
			sh_disable_algo(i);
		}
	}

	if(p_glbl_max8614x_status_track->sensor_data_from_host) {
		CSTMR_SH_HostAccelerometerInitialize();
		p_glbl_max8614x_status_track->sensor_data_from_host = 0;
	}

	sh_clear_mfio_event_flag();
	sh_enable_irq_mfioevent();
}