int mantis_suspend(void *mlsl_handle,
		   struct ext_slave_descr *slave,
		   struct ext_slave_platform_data *pdata)
{
	unsigned char reg;
	int result;
	struct mantis_private_data *private_data =
			(struct mantis_private_data *)pdata->private_data;

	result = mantis_set_odr(mlsl_handle, pdata, &private_data->suspend,
				TRUE, private_data->suspend.odr);
	ERROR_CHECK(result);

	result = mantis_set_irq(mlsl_handle, pdata, &private_data->suspend,
				TRUE, private_data->suspend.irq_type);
	ERROR_CHECK(result);

	result = inv_serial_read(mlsl_handle, pdata->address,
				 MPUREG_PWR_MGMT_2, 1, &reg);
	ERROR_CHECK(result);
	reg |= (BIT_STBY_XA | BIT_STBY_YA | BIT_STBY_ZA);

	result = inv_serial_single_write(mlsl_handle, pdata->address,
					 MPUREG_PWR_MGMT_2, reg);
	ERROR_CHECK(result);

	return INV_SUCCESS;
}

void NetShrVarInterface::updateParamCNVImpl(int param_index, CNVData data, CNVDataType type, unsigned int nDims, bool do_asyn_param_callbacks)
{
	if (nDims == 0)
	{
	    typename CNV2C<cnvType>::ctype val;
	    int status = CNVGetScalarDataValue (data, type, &val);
	    ERROR_CHECK("CNVGetScalarDataValue", status);
	    updateParamValue(param_index, val, do_asyn_param_callbacks);
        CNV2C<cnvType>::free(val);
	}
	else
	{
	    typename CNV2C<cnvType>::ctype* val;
	    size_t dimensions[10];
	    int status = CNVGetArrayDataDimensions(data, nDims, dimensions);
	    ERROR_CHECK("CNVGetArrayDataDimensions", status);
		size_t nElements = 1;
		for(unsigned i=0; i<nDims; ++i)
		{
		    nElements *= dimensions[i];
		}
		val = new typename CNV2C<cnvType>::ctype[nElements];
		status = CNVGetArrayDataValue(data, type, val, nElements);
	    ERROR_CHECK("CNVGetArrayDataValue", status);
	    updateParamArrayValue(param_index, val, nElements);
		delete[] val;
	}
}

	void report(FILE* fp, const char* conn_type, void* handle, bool buffered)
	{
		int error, conn_error;
		CNVConnectionStatus status;
		fprintf(fp, "  Connection type: %s", conn_type);
		if (handle == 0)
		{
		    fprintf(fp, " Status: <not being used>\n");
		    return;
		}
		error = CNVGetConnectionAttribute(handle, CNVConnectionStatusAttribute, &status);
		ERROR_CHECK("CNVGetConnectionAttribute", error);
		fprintf(fp, " status: %s", connectionStatus(status));
		error = CNVGetConnectionAttribute(handle, CNVConnectionErrorAttribute, &conn_error);
		ERROR_CHECK("CNVGetConnectionAttribute", error);
		if (conn_error < 0)
		{
			fprintf(fp, " error present: %s", CNVGetErrorDescription(conn_error));
		}
		if (buffered)
		{
			int nitems, maxitems;
			error = CNVGetConnectionAttribute(handle, CNVClientBufferNumberOfItemsAttribute, &nitems);
			ERROR_CHECK("CNVGetConnectionAttribute", error);
			error = CNVGetConnectionAttribute(handle, CNVClientBufferMaximumItemsAttribute, &maxitems);
			ERROR_CHECK("CNVGetConnectionAttribute", error);
			fprintf(fp, " Client buffer: %d items (buffer size = %d)", nitems, maxitems);
		}
		fprintf(fp, "\n");
	}

/**
 Parses the Sync Session response contained in the parseStruct to retrieve Tper session ID.  If the Sync Session response
 parameters do not match the comID, extended ComID and host session ID then a failure is returned.

 @param[in/out]   ParseStruct          Structure used to parse received TCG response, contains Sync Session response.
 @param[in]       ComId                Expected ComID that is compared to actual ComID of response
 @param[in]       ComIdExtension       Expected Extended ComID that is compared to actual Extended ComID of response
 @param[in]       HostSessionId        Expected Host Session ID that is compared to actual  Host Session ID of response
 @param[in/out]   TperSessionId        Tper Session ID retrieved from the Sync Session response.

**/
TCG_RESULT
EFIAPI
TcgParseSyncSession(
  const TCG_PARSE_STRUCT  *ParseStruct,
  UINT16                  ComId,
  UINT16                  ComIdExtension,
  UINT32                  HostSessionId,
  UINT32                  *TperSessionId
  )
{
  UINT8 MethodStatus;
  TCG_PARSE_STRUCT TmpParseStruct;
  UINT16 ParseComId;
  UINT16 ParseExtComId;
  TCG_UID InvokingUID;
  TCG_UID MethodUID;
  UINT32 RecvHostSessionId;

  NULL_CHECK(ParseStruct);
  NULL_CHECK(TperSessionId);

  CopyMem (&TmpParseStruct, ParseStruct, sizeof(TCG_PARSE_STRUCT));

  // verify method status is good
  ERROR_CHECK(TcgGetMethodStatus(&TmpParseStruct, &MethodStatus));
  METHOD_STATUS_ERROR_CHECK (MethodStatus, TcgResultFailure);

  // verify comids
  ERROR_CHECK(TcgGetComIds(&TmpParseStruct, &ParseComId, &ParseExtComId));

  if ((ComId != ParseComId) || (ComIdExtension != ParseExtComId)) {
    DEBUG ((DEBUG_INFO, "unmatched comid (exp: 0x%X recv: 0x%X) or comid extension (exp: 0x%X recv: 0x%X)\n", ComId, ParseComId, ComIdExtension, ParseExtComId));
    return TcgResultFailure;
  }
  ERROR_CHECK(TcgGetNextCall(&TmpParseStruct));
  ERROR_CHECK(TcgGetNextTcgUid(&TmpParseStruct, &InvokingUID));
  ERROR_CHECK(TcgGetNextTcgUid(&TmpParseStruct, &MethodUID));
  ERROR_CHECK(TcgGetNextStartList(&TmpParseStruct));
  ERROR_CHECK(TcgGetNextUINT32(&TmpParseStruct, &RecvHostSessionId));
  ERROR_CHECK(TcgGetNextUINT32(&TmpParseStruct, TperSessionId));
  ERROR_CHECK(TcgGetNextEndList(&TmpParseStruct));
  ERROR_CHECK(TcgGetNextEndOfData(&TmpParseStruct));

  if (InvokingUID != TCG_UID_SMUID) {
    DEBUG ((DEBUG_INFO, "Invoking UID did not match UID_SMUID\n"));
    return TcgResultFailure;
  }

  if (MethodUID != TCG_UID_SM_SYNC_SESSION) {
    DEBUG ((DEBUG_INFO, "Method UID did not match UID_SM_SYNC_SESSION\n"));
    return TcgResultFailure;
  }

  if (HostSessionId != RecvHostSessionId) {
    DEBUG ((DEBUG_INFO, "unmatched HostSessionId (exp: 0x%X recv: 0x%X)\n", HostSessionId, RecvHostSessionId));
    return TcgResultFailure;
  }

  return TcgResultSuccess;
}

int hscdtd004a_read(void *mlsl_handle,
		    struct ext_slave_descr *slave,
		    struct ext_slave_platform_data *pdata,
		    unsigned char *data)
{
	unsigned char stat;
	tMLError result = ML_SUCCESS;
	int status = ML_SUCCESS;

	/* Read status reg. to check if data is ready */
	result =
		MLSLSerialRead(mlsl_handle, pdata->address,
			   COMPASS_HSCDTD004A_STAT, 1, &stat);
	ERROR_CHECK(result);
	if (stat & 0x48) {
		result =
			 MLSLSerialRead(mlsl_handle, pdata->address,
				   COMPASS_HSCDTD004A_DATAX, 6,
				   (unsigned char *) data);
		ERROR_CHECK(result);
		status = ML_SUCCESS;
	} else if (stat & 0x68) {
		status = ML_ERROR_COMPASS_DATA_OVERFLOW;
	} else {
		status = ML_ERROR_COMPASS_DATA_NOT_READY;
	}
	/* trigger next measurement read */
	result =
		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			COMPASS_HSCDTD004A_CTRL3, 0x40);
	ERROR_CHECK(result);
	return status;

}

int hscdtd00xx_resume(void *mlsl_handle,
		      struct ext_slave_descr *slave,
		      struct ext_slave_platform_data *pdata)
{
	int result = ML_SUCCESS;

	/* Soft reset */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  COMPASS_HSCDTD00XX_CTRL3, 0x80);
	ERROR_CHECK(result);
	/* Force state; Power mode: active */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  COMPASS_HSCDTD00XX_CTRL1, 0x82);
	ERROR_CHECK(result);
	/* Data ready enable */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  COMPASS_HSCDTD00XX_CTRL2, 0x08);
	ERROR_CHECK(result);
	MLOSSleep(1);		/* turn-on time */

	return result;
}

int ami30x_resume(void *mlsl_handle,
		  struct ext_slave_descr *slave,
		  struct ext_slave_platform_data *pdata)
{
	int result = ML_SUCCESS;

	/* Set CNTL1 reg to power model active */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  AMI30X_REG_CNTL1,
				  AMI30X_BIT_CNTL1_PC1|AMI30X_BIT_CNTL1_FS1);
	ERROR_CHECK(result);
	/* Set CNTL2 reg to DRDY active high and enabled */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  AMI30X_REG_CNTL2,
				  AMI30X_BIT_CNTL2_DREN |
				  AMI30X_BIT_CNTL2_DRP);
	ERROR_CHECK(result);
	/* Set CNTL3 reg to forced measurement period */
	result =
		MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  AMI30X_REG_CNTL3, AMI30X_BIT_CNTL3_F0RCE);

	return result;
}

int CVxParamDistribution::CompareFrame(int frameNumber)
{
	// check if there is no user request to compare
	if (m_fileNameCompare.length() < 1) return 0;

	// make sure m_bufForRead is allocated
	if (!m_bufForCompare) NULLPTR_CHECK(m_bufForCompare = new vx_uint32[m_numBins]);

	// reading data from reference file
	char fileName[MAX_FILE_NAME_LENGTH]; sprintf(fileName, m_fileNameCompare.c_str(), frameNumber);
	FILE * fp = fopen(fileName, m_compareFileIsBinary ? "rb" : "r");
	if (!fp) {
		ReportError("ERROR: Unable to open: %s\n", fileName);
	}
	int status = ReadFileIntoBuffer(fp, m_bufForCompare);
	fclose(fp);
	if (status) ReportError("ERROR: distribution compare reference doesn't have enough data: %s\n", fileName);

	// compare and report error if mismatched
	vx_uint32 * bufRef = nullptr;
	ERROR_CHECK(vxAccessDistribution(m_distribution, (void **)&bufRef, VX_READ_ONLY));
	status = memcmp(bufRef, m_bufForCompare, m_numBins * sizeof(vx_uint32)) ? -1 : 0;
	ERROR_CHECK(vxCommitDistribution(m_distribution, bufRef));
	if (status) {
		m_compareCountMismatches++;
		printf("ERROR: distribution COMPARE MISMATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
		if (!m_discardCompareErrors) return -1;
	}
	else {
		m_compareCountMatches++;
		if (m_verbose) printf("OK: distribution COMPARE MATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
	}
	return 0;
}

int ak8975_read(void *mlsl_handle,
		struct ext_slave_descr *slave,
		struct ext_slave_platform_data *pdata, unsigned char *data)
{
	unsigned char regs[6];
	int result = ML_SUCCESS;
	int status = ML_SUCCESS;

	mpu_dbg("%s\n", __func__);

	result = MLSLSerialRead(mlsl_handle, pdata->address,
				 AK8975_REG_HXL, 6, regs);
	ERROR_CHECK(result);

	memcpy(data, &regs[0], 6);
	status = ML_SUCCESS;
	/*
	 * trigger next measurement if:
	 *    - stat is non zero;
	 *    - if stat is zero and stat2 is non zero.
	 * Won't trigger if data is not ready and there was no error.
	 */

	result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				  AK8975_REG_CNTL,
				  AK8975_CNTL_MODE_SINGLE_MEASUREMENT);
	ERROR_CHECK(result);

	return status;
}

// Update Body
inline void Kinect::updateBody()
{
    // Retrieve Body Frame
    ComPtr<IBodyFrame> bodyFrame;
    const HRESULT ret = bodyFrameReader->AcquireLatestFrame( &bodyFrame );
    if( FAILED( ret ) ){
        return;
    }

    // Release Previous Bodies
    Concurrency::parallel_for_each( bodies.begin(), bodies.end(), []( IBody*& body ){
        SafeRelease( body );
    } );

    // Retrieve Body Data
    ERROR_CHECK( bodyFrame->GetAndRefreshBodyData( static_cast<UINT>( bodies.size() ), &bodies[0] ) );
    Concurrency::parallel_for( 0, BODY_COUNT, [&]( const int count ){
        const ComPtr<IBody> body = bodies[count];
        BOOLEAN tracked;
        ERROR_CHECK( body->get_IsTracked( &tracked ) );
        if( !tracked ){
            return;
        }

        // Retrieve Tracking ID
        UINT64 trackingId;
        ERROR_CHECK( body->get_TrackingId( &trackingId ) );

        // Registration Tracking ID
        ComPtr<IFaceFrameSource> faceFrameSource;
        ERROR_CHECK( faceFrameReader[count]->get_FaceFrameSource( &faceFrameSource ) );
        ERROR_CHECK( faceFrameSource->put_TrackingId( trackingId ) );
    } );
}

bool CVxParamArray::CompareFrameBitwiseExact(size_t numItems, size_t numItemsRef, vx_uint8 * bufItems, int frameNumber, const char * fileName)
{
	// bitwise exact compare
	size_t numItemsMin = min(numItems, numItemsRef);
	size_t numMismatches = 0;
	if (numItemsMin > 0) {
		void * ptr = nullptr;
		vx_size stride = 0;
		ERROR_CHECK(vxAccessArrayRange(m_array, 0, numItems, &stride, &ptr, VX_READ_ONLY));
		for (size_t i = 0; i < numItems; i++) {
			vx_uint8 * item = vxFormatArrayPointer(ptr, i, stride);
			if (memcmp(item, bufItems + i * m_itemSize, m_itemSize) != 0) {
				numMismatches++;
			}
		}
		ERROR_CHECK(vxCommitArrayRange(m_array, 0, numItems, ptr));
	}
	numMismatches += max(numItems, numItemsRef) - numItemsMin;
	bool mismatchDetected = false;
	if (numMismatches > 0) {
		printf("ERROR: array COMPARE MISMATCHED %d/%d for %s with frame#%d of %s\n", (int)numMismatches, (int)numItems, GetVxObjectName(), frameNumber, fileName);
		mismatchDetected = true;
	}
	else {
		if (m_verbose) printf("OK: array COMPARE MATCHED for %s with frame#%d of %s\n", GetVxObjectName(), frameNumber, fileName);
	}
	return mismatchDetected;
}

  void initialize()
  {
    createInstance();

    // Kinectの設定を初期化する
    ERROR_CHECK( kinect->NuiInitialize( NUI_INITIALIZE_FLAG_USES_COLOR | NUI_INITIALIZE_FLAG_USES_DEPTH_AND_PLAYER_INDEX | NUI_INITIALIZE_FLAG_USES_SKELETON ) );

    // RGBカメラを初期化する
    ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_COLOR, CAMERA_RESOLUTION,
      0, 2, 0, &imageStreamHandle ) );

    // 距離カメラを初期化する
    ERROR_CHECK( kinect->NuiImageStreamOpen( NUI_IMAGE_TYPE_DEPTH_AND_PLAYER_INDEX, CAMERA_RESOLUTION,
      0, 2, 0, &depthStreamHandle ) );

    // Nearモード
    //ERROR_CHECK( kinect->NuiImageStreamSetImageFrameFlags(
    //  depthStreamHandle, NUI_IMAGE_STREAM_FLAG_ENABLE_NEAR_MODE ) );

    // スケルトンを初期化する
    ERROR_CHECK( kinect->NuiSkeletonTrackingEnable( 0, NUI_SKELETON_TRACKING_FLAG_ENABLE_SEATED_SUPPORT ) );

    // フレーム更新イベントのハンドルを作成する
    streamEvent = ::CreateEvent( 0, TRUE, FALSE, 0 );
    ERROR_CHECK( kinect->NuiSetFrameEndEvent( streamEvent, 0 ) );

    // 指定した解像度の、画面サイズを取得する
    ::NuiImageResolutionToSize(CAMERA_RESOLUTION, width, height );
  }

int mma8451_resume(void *mlsl_handle,
		   struct ext_slave_descr *slave,
		   struct ext_slave_platform_data *pdata)
{
	int result = ML_SUCCESS;
	unsigned char reg;

	result =
	    MLSLSerialRead(mlsl_handle, pdata->address, 0x0E, 1, &reg);
	ERROR_CHECK(result);

	/* data rate = 200Hz */

	/* Full Scale */
	reg &= ~ACCEL_MMA8451_CTRL_MASK;
	if (slave->range.mantissa == 4)
		reg |= 0x1;
	else if (slave->range.mantissa == 8)
		reg |= 0x2;
	else {
		slave->range.mantissa = 2;
		reg |= 0x0;
	}
	slave->range.fraction = 0;

	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x0E, reg);
	ERROR_CHECK(result);
	/* 200Hz + active mode */
	result =
	    MLSLSerialWriteSingle(mlsl_handle, pdata->address, 0x2A, 0x11);
	ERROR_CHECK(result);

	return result;
}

/**
 * Destroys the background thread.  This function will block until all tasks are
 * completed.
 *
 * @param thread  the thread to destroy.
 * @return CUDA_SUCCESS on success,
 *         CUDA_ERROR_OPERATING_SYSTEM if the thread could not be stopped.
 */
static inline CUresult cuThreadDestroy(CUthread thread) {
  // Lock the queue
  ERROR_CHECK(pthread_mutex_lock(&thread->mutex));

  // Place a NULL task on the queue to signal the thread to exit the main loop
  CU_ERROR_CHECK(cuTaskQueuePush(&thread->queue, NULL));

  // Unlock the queue
  ERROR_CHECK(pthread_mutex_unlock(&thread->mutex));

  // Signal to the thread that there are tasks waiting
  ERROR_CHECK(pthread_cond_signal(&thread->nonEmpty));

  // Wait for the thread to exit
  ERROR_CHECK(pthread_join(thread->thread, (void **)&thread));

  // Destroy the queue
  cuTaskQueueDestroy(&thread->queue);

  // Copy the thread error status
  CUresult error = thread->error;

  // Free the thread object
  free(thread);

  // Return the thread error status
  return error;
}

void KinectApp::initialize() {
	// デフォルトのKinectを取得
	ERROR_CHECK(::GetDefaultKinectSensor(&kinect));
	ERROR_CHECK(kinect->Open());

	// 座標変換インターフェースを取得
	ERROR_CHECK(kinect->get_CoordinateMapper(&coordinateMapper));

	// フレームの初期化
	initializeColorFrame();
	initializeDepthFrame();

	WindowName[0] = "color";
	WindowName[1] = "banana";
	WindowName[2] = "depth";
	WindowName[3] = "thresh_depth";
	WindowName[4] = "d_depth";
	for (int i = 0; i < 5; i++) {
		cv::namedWindow(WindowName[i]);
	}
	cv::moveWindow(WindowName[0], 700, 0);
	cv::moveWindow(WindowName[1], 1200, 0);
	cv::moveWindow(WindowName[2], 700, 400);
	cv::moveWindow(WindowName[3], 1100, 400);
	cv::moveWindow(WindowName[4], 1500, 400);

	cv::FileStorage fs("../backImg.xml", cv::FileStorage::READ);
	if (fs.isOpened()) {
		std::cout << "backImg exist." << std::endl;
		fs["backImg"] >> backImg;
	}

int ami30x_read(void *mlsl_handle,
		struct ext_slave_descr *slave,
		struct ext_slave_platform_data *pdata, unsigned char *data)
{
	unsigned char stat;
	int result = ML_SUCCESS;

	/* Read status reg and check if data ready (DRDY) */
	result =
	    MLSLSerialRead(mlsl_handle, pdata->address, AMI30X_REG_STAT1,
			   1, &stat);
	ERROR_CHECK(result);

	if (stat & 0x40) {
		result =
		    MLSLSerialRead(mlsl_handle, pdata->address,
				   AMI30X_REG_DATAX, 6,
				   (unsigned char *) data);
		ERROR_CHECK(result);
		/* start another measurement */
		result =
			MLSLSerialWriteSingle(mlsl_handle, pdata->address,
					      AMI30X_REG_CNTL3,
					      AMI30X_BIT_CNTL3_F0RCE);
		ERROR_CHECK(result);

		return ML_SUCCESS;
	}

	return ML_ERROR_COMPASS_DATA_NOT_READY;
}

// read image
int ReadImage(vx_image image, vx_rectangle_t * rectFull, FILE * fp)
{
	// get number of planes, image format, and pixel type
	vx_df_image format = VX_DF_IMAGE_VIRT;
	vx_size num_planes = 0;
	ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_FORMAT, &format, sizeof(format)));
	ERROR_CHECK(vxQueryImage(image, VX_IMAGE_ATTRIBUTE_PLANES, &num_planes, sizeof(num_planes)));
	// read all image planes into vx_image and check if EOF has occured while reading
	bool eofDetected = false;
	for (vx_uint32 plane = 0; plane < (vx_uint32)num_planes; plane++){
		vx_imagepatch_addressing_t addr;
		vx_uint8 * src = NULL;
		ERROR_CHECK(vxAccessImagePatch(image, rectFull, plane, &addr, (void **)&src, VX_WRITE_ONLY));
		vx_size width = (addr.dim_x * addr.scale_x) / VX_SCALE_UNITY;
		vx_size width_in_bytes = (format == VX_DF_IMAGE_U1_AMD) ? ((width + 7) >> 3) : (width * addr.stride_x);
		for (vx_uint32 y = 0; y < addr.dim_y; y += addr.step_y){
			vx_uint8 *srcp = (vx_uint8 *)vxFormatImagePatchAddress2d(src, 0, y, &addr);
			if (fread(srcp, 1, width_in_bytes, fp) != width_in_bytes) {
				eofDetected = true;
				break;
			}
		}
		ERROR_CHECK(vxCommitImagePatch(image, rectFull, plane, &addr, src));
	}
	// return 1 if EOF detected, other 0
	return eofDetected ? 1 : 0;
}

static int mantis_set_fsr(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct mantis_config *config, long apply, long fsr)
{
	unsigned char fsr_mask;
	int result;

	if (fsr <= 2000) {
		config->fsr = 2000;
		fsr_mask = 0x00;
	} else if (fsr <= 4000) {
		config->fsr = 4000;
		fsr_mask = 0x08;
	} else if (fsr <= 8000) {
		config->fsr = 8000;
		fsr_mask = 0x10;
	} else { /* fsr = [8001, oo) */
		config->fsr = 16000;
		fsr_mask = 0x18;
	}

	if (apply) {
		unsigned char reg;
		result = inv_serial_read(mlsl_handle, pdata->address,
					 MPUREG_ACCEL_CONFIG, 1, &reg);
		ERROR_CHECK(result);
		result = inv_serial_single_write(mlsl_handle, pdata->address,
						 MPUREG_ACCEL_CONFIG,
						 reg | fsr_mask);
		ERROR_CHECK(result);
		MPL_LOGV("FSR: %d\n", config->fsr);
	}
	return INV_SUCCESS;
}

static int bma250_set_irq(void *mlsl_handle,
			struct ext_slave_platform_data *pdata,
			struct bma250_config *config,
			int apply,
			long irq_type)
{
	unsigned char irq_bits = 0;
	int result = ML_SUCCESS;

	
	return ML_SUCCESS;

	if (irq_type == MPU_SLAVE_IRQ_TYPE_MOTION)
		return ML_ERROR_FEATURE_NOT_IMPLEMENTED;

	config->irq_type = (unsigned char)irq_type;

	if (irq_type == MPU_SLAVE_IRQ_TYPE_DATA_READY) {
		irq_bits = 0x20;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
	} else {
		irq_bits = 0x00;
		config->int_reg &= ACCEL_BOSCH_INT_MASK_WUP;
	}

	config->int_reg &= ACCEL_BOSCH_INT_MASK_IRQ;
	config->int_reg |= irq_bits;

	if (apply) {

		if (!config->power_mode) {
			
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_REG_SOFT_RESET,
					0xB6);
			ERROR_CHECK(result);
			MLOSSleep(1);
		}

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_CTRL_REG, config->ctrl_reg);
		ERROR_CHECK(result);

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
			BOSCH_INT_REG, config->int_reg);
		ERROR_CHECK(result);

		if (!config->power_mode) {
			result = MLSLSerialWriteSingle(mlsl_handle,
				pdata->address, BOSCH_PWR_REG, 0x80);
			ERROR_CHECK(result);
			MLOSSleep(1);
		} else {
			result = set_normal_mode(mlsl_handle, pdata);
			ERROR_CHECK(result);
		}
	}
	return result;
}

/**
 *  @brief resume the device in the proper power state given the configuration
 *         chosen.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param slave
 *             a pointer to the slave descriptor data structure.
 *  @param pdata
 *             a pointer to the slave platform data.
 *
 *  @return ML_SUCCESS if successful or a non-zero error code.
 */
static int bma250_resume(void *mlsl_handle,
			 struct ext_slave_descr *slave,
			 struct ext_slave_platform_data *pdata)
{
	int result;

	struct bma250_config *resume_config =
		&((struct bma250_private_data *)pdata->private_data)->resume;

	if (pdata->check_sleep_status && pdata->vote_sleep_status) {
		mutex_lock(&bma250_power_lock);
		if (pdata->check_sleep_status() == BMA250_SLEEP) {
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_SOFTRESET_REG,
					BMA250_SOFTRESET_MASK);
			ERROR_CHECK_MUTEX(result, bma250_power_lock);
			/* 4 ms needed for to be stable after reset. */
			MLOSSleep(4);
		}

		result = bma250_set_odr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->odr);
		ERROR_CHECK_MUTEX(result, bma250_power_lock);

		result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->fsr);
		ERROR_CHECK_MUTEX(result, bma250_power_lock);

		if (pdata->check_sleep_status() == BMA250_SLEEP) {
			result = MLSLSerialWriteSingle(mlsl_handle,
					pdata->address, BMA250_PWR_REG,
					BMA250_PWR_AWAKE_MASK);
			ERROR_CHECK_MUTEX(result, bma250_power_lock);
		}
		pdata->vote_sleep_status(BMA250_SLAVE2, BMA250_AWAKE);
		mutex_unlock(&bma250_power_lock);
	} else {
		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				BMA250_SOFTRESET_REG, BMA250_SOFTRESET_MASK);
		ERROR_CHECK(result);
		/* 4 ms needed for to be stable after reset. */
		MLOSSleep(4);

		result = bma250_set_odr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->odr);
		ERROR_CHECK(result);

		result = bma250_set_fsr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->fsr);
		ERROR_CHECK(result);

		result = MLSLSerialWriteSingle(mlsl_handle, pdata->address,
				BMA250_PWR_REG, BMA250_PWR_AWAKE_MASK);
		ERROR_CHECK(result);
	}
	return result;
}