C++ LOG_RESULT_LOCATION函数代码示例

static int lsm303dlx_m_resume(void *mlsl_handle,
			     struct ext_slave_descr *slave,
			     struct ext_slave_platform_data *pdata)
{
	int result = INV_SUCCESS;

	/* Use single measurement mode. Start at sleep state. */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    LSM_REG_MODE, LSM_MODE_SLEEP);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	/* Config normal measurement */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    LSM_REG_CONF_A, 0);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	/* Adjust gain to 320 LSB/Gauss */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    LSM_REG_CONF_B, LSM_CONF_B_GAIN_5_5);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	return result;
}

/* -------------------------------------------------------------------------- */
static int ak8975_init(void *mlsl_handle,
		       struct ext_slave_descr *slave,
		       struct ext_slave_platform_data *pdata)
{
	int result;
	unsigned char serial_data[COMPASS_NUM_AXES];

	struct ak8975_private_data *private_data;
	private_data = (struct ak8975_private_data *)
	    kzalloc(sizeof(struct ak8975_private_data), GFP_KERNEL);

	if (!private_data)
		return INV_ERROR_MEMORY_EXAUSTED;

	result = inv_serial_single_write(mlsl_handle, pdata->address,
					 AK8975_REG_CNTL,
					 AK8975_CNTL_MODE_POWER_DOWN);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	/* Wait at least 100us */
	udelay(100);

	result = inv_serial_single_write(mlsl_handle, pdata->address,
					 AK8975_REG_CNTL,
					 AK8975_CNTL_MODE_FUSE_ROM_ACCESS);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	/* Wait at least 200us */
	udelay(200);

	result = inv_serial_read(mlsl_handle, pdata->address,
				 AK8975_REG_ASAX,
				 COMPASS_NUM_AXES, serial_data);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	pdata->private_data = private_data;

	private_data->init.asa[0] = serial_data[0];
	private_data->init.asa[1] = serial_data[1];
	private_data->init.asa[2] = serial_data[2];

	result = inv_serial_single_write(mlsl_handle, pdata->address,
					 AK8975_REG_CNTL,
					 AK8975_CNTL_MODE_POWER_DOWN);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	udelay(100);
	return INV_SUCCESS;
}

/**
 * @brief Stop the DMP running
 *
 * @return INV_SUCCESS or non-zero error code
 */
static int dmp_stop(struct mldl_cfg *mldl_cfg, void *gyro_handle)
{
	unsigned char user_ctrl_reg;
	int result;

	if (mldl_cfg->inv_mpu_state->status & MPU_DMP_IS_SUSPENDED)
		return INV_SUCCESS;

	result = inv_serial_read(gyro_handle, mldl_cfg->mpu_chip_info->addr,
				 MPUREG_USER_CTRL, 1, &user_ctrl_reg);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	user_ctrl_reg = (user_ctrl_reg & (~BIT_FIFO_EN)) | BIT_FIFO_RST;
	user_ctrl_reg = (user_ctrl_reg & (~BIT_DMP_EN)) | BIT_DMP_RST;

	result = inv_serial_single_write(gyro_handle,
					 mldl_cfg->mpu_chip_info->addr,
					 MPUREG_USER_CTRL, user_ctrl_reg);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	mldl_cfg->inv_mpu_state->status |= MPU_DMP_IS_SUSPENDED;

	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 INV_SUCCESS if successful or a non-zero error code.
 */
static int bma222_resume(void *mlsl_handle,
			 struct ext_slave_descr *slave,
			 struct ext_slave_platform_data *pdata)
{
	int result;
	struct bma222_config *resume_config =
		&((struct bma222_private_data *)pdata->private_data)->resume;

	/* Soft reset */
	result = inv_serial_single_write(mlsl_handle, pdata->address,
			BMA222_SOFTRESET_REG, BMA222_SOFTRESET_MASK);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	msleep(10);

	result = bma222_set_odr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->odr);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	result = bma222_set_fsr(mlsl_handle, pdata, resume_config,
				TRUE, resume_config->fsr);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	return result;
}

static int inv_i2c_write(struct i2c_adapter *i2c_adap,
			    unsigned char address,
			    unsigned int len, unsigned char const *data)
{
	struct i2c_msg msgs[1];
	int res;

	if (!data || !i2c_adap) {
		LOG_RESULT_LOCATION(-EINVAL);
		return -EINVAL;
	}

	msgs[0].addr = address;
	msgs[0].flags = 0;	/* write */
	msgs[0].buf = (unsigned char *)data;
	msgs[0].len = len;

	res = i2c_transfer(i2c_adap, msgs, 1);
	if (res < 1) {
		if (res == 0)
			res = -EIO;
		LOG_RESULT_LOCATION(res);
		return res;
	} else
		return 0;
}

int inv_serial_read(
	void *sl_handle,
	unsigned char slave_addr,
	unsigned char register_addr,
	unsigned short length,
	unsigned char *data)
{
	int result;
	unsigned short bytes_read = 0;

	if ((slave_addr & 0x7E) == DEFAULT_MPU_SLAVEADDR
		&& (register_addr == MPUREG_FIFO_R_W ||
		    register_addr == MPUREG_MEM_R_W)) {
		LOG_RESULT_LOCATION(INV_ERROR_INVALID_PARAMETER);
		return INV_ERROR_INVALID_PARAMETER;
	}

	while (bytes_read < length) {
		unsigned short this_len =
		    min(SERIAL_MAX_TRANSFER_SIZE, length - bytes_read);
		result = inv_i2c_read((struct i2c_adapter *)sl_handle,
				      slave_addr, register_addr + bytes_read,
				      this_len, &data[bytes_read]);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		bytes_read += this_len;
	}
	return 0;
}

/* -------------------------------------------------------------------------- */
static int ami30x_suspend(void *mlsl_handle,
			  struct ext_slave_descr *slave,
			  struct ext_slave_platform_data *pdata)
{
	int result;
	unsigned char reg;
	result =
	    inv_serial_read(mlsl_handle, pdata->address, AMI30X_REG_CNTL1,
			    1, &reg);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	reg &= ~(AMI30X_BIT_CNTL1_PC1 | AMI30X_BIT_CNTL1_FS1);
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    AMI30X_REG_CNTL1, reg);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	return result;
}

static int hscdtd002b_resume(void *mlsl_handle,
			     struct ext_slave_descr *slave,
			     struct ext_slave_platform_data *pdata)
{
	int result = INV_SUCCESS;

	/* Soft reset */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    COMPASS_HSCDTD002B_CTRL3, 0x80);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	/* Force state; Power mode: active */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    COMPASS_HSCDTD002B_CTRL1, 0x82);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	/* Data ready enable */
	result =
	    inv_serial_single_write(mlsl_handle, pdata->address,
				    COMPASS_HSCDTD002B_CTRL2, 0x08);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	msleep(1);		/* turn-on time */

	return result;
}

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

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

	return result;
}

/**
 *  @brief Simply disables the IRQ since it is not usable on BMA150 devices.
 *
 *  @param mlsl_handle
 *             the handle to the serial channel the device is connected to.
 *  @param pdata
 *             a pointer to the slave platform data.
 *  @param config
 *              configuration to apply to, suspend or resume
 *  @param apply
 *             whether to apply immediately or save the settings to be applied
 *             at the next resume.
 *  @param irq_type
 *              the type of IRQ.  Valid values are
 *              - MPU_SLAVE_IRQ_TYPE_NONE
 *              - MPU_SLAVE_IRQ_TYPE_MOTION
 *              - MPU_SLAVE_IRQ_TYPE_DATA_READY
 *              The only supported IRQ type is MPU_SLAVE_IRQ_TYPE_NONE which
 *              corresponds to disabling the IRQ completely.
 *
 *  @return INV_SUCCESS if successful or a non-zero error code.
 */
static int bma150_set_irq(void *mlsl_handle,
			  struct ext_slave_platform_data *pdata,
			  struct bma150_config *config,
			  int apply,
			  long irq_type)
{
	int result = INV_SUCCESS;

	if (irq_type != MPU_SLAVE_IRQ_TYPE_NONE)
		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;

	config->irq_type = MPU_SLAVE_IRQ_TYPE_NONE;
	config->int_reg = 0x00;

	if (apply) {
		result = inv_serial_single_write(mlsl_handle, pdata->address,
			 BMA150_CTRL_REG, config->ctrl_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
		result = inv_serial_single_write(mlsl_handle, pdata->address,
			 BMA150_INT_REG, config->int_reg);
		if (result) {
			LOG_RESULT_LOCATION(result);
			return result;
		}
	}
	return result;
}

static int mpu6050_input_set_fsr(struct mpu6050_input_data *data, int fsr)
{
	unsigned char fsr_mask;
	int result;
	unsigned char reg;

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

	result =
		mpu6050_i2c_read_reg(data->client,
			MPUREG_ACCEL_CONFIG, 1, &reg);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	result =
		mpu6050_i2c_write_single_reg(data->client, MPUREG_ACCEL_CONFIG,
					 reg | fsr_mask);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	return result;
}

/**
 *  @brief suspends the device to put it in its lowest power mode.
 *
 *  @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 INV_SUCCESS if successful or a non-zero error code.
 */
static int bma222_suspend(void *mlsl_handle,
			  struct ext_slave_descr *slave,
			  struct ext_slave_platform_data *pdata)
{
	int result;
	struct bma222_config *suspend_config =
		&((struct bma222_private_data *)pdata->private_data)->suspend;

	result = bma222_set_odr(mlsl_handle, pdata, suspend_config,
				TRUE, suspend_config->odr);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}
	result = bma222_set_fsr(mlsl_handle, pdata, suspend_config,
				TRUE, suspend_config->fsr);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	result = inv_serial_single_write(mlsl_handle, pdata->address,
				BMA222_PWR_REG, BMA222_PWR_SLEEP_MASK);
	if (result) {
		LOG_RESULT_LOCATION(result);
		return result;
	}

	msleep(3); /* 3 ms powerup time maximum */
	return result;
}

static int inv_i2c_read(struct i2c_adapter *i2c_adap,
			   unsigned char address, unsigned char reg,
			   unsigned int len, unsigned char *data)
{
	struct i2c_msg msgs[2];
	int res;

	if (!data || !i2c_adap) {
		LOG_RESULT_LOCATION(-EINVAL);
		return -EINVAL;
	}

	msgs[0].addr = address;
	msgs[0].flags = 0;	/* write */
	msgs[0].buf = ®
	msgs[0].len = 1;

	msgs[1].addr = address;
	msgs[1].flags = I2C_M_RD;
	msgs[1].buf = data;
	msgs[1].len = len;

	res = i2c_transfer(i2c_adap, msgs, 2);
	if (res < 2) {
		if (res >= 0)
			res = -EIO;
		LOG_RESULT_LOCATION(res);
		return res;
	} else
		return 0;
}

/**
 *  @brief  inv_set_control_sensitivity is used to set the sensitivity for a control
 *          signal.
 *
 *  @pre    inv_dmp_open() Must be called with MLDmpDefaultOpen() or 
 *          inv_open_low_power_pedometer().
 *
 *  @param controlSignal    Indicates which control signal is being modified.
 *                          Must be one of:
 *                          - INV_CONTROL_1,
 *                          - INV_CONTROL_2,
 *                          - INV_CONTROL_3 or
 *                          - INV_CONTROL_4.
 *
 *  @param sensitivity      The sensitivity of the control signal.
 *
 *  @return error code
 */
inv_error_t inv_set_control_sensitivity(unsigned short controlSignal,
                                        long sensitivity)
{
    INVENSENSE_FUNC_START;
    unsigned char regs[2];
    long finalSens = 0;
    inv_error_t result;

    if (inv_get_state() < INV_STATE_DMP_OPENED)
        return INV_ERROR_SM_IMPROPER_STATE;

    finalSens = sensitivity * 100;
    if (finalSens > 16384) {
        finalSens = 16384;
    }
    regs[0] = (unsigned char)(finalSens / 256);
    regs[1] = (unsigned char)(finalSens % 256);
    switch (controlSignal) {
    case INV_CONTROL_1:
        result = inv_set_mpu_memory(KEY_D_0_224, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[0] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_2:
        result = inv_set_mpu_memory(KEY_D_0_228, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[1] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_3:
        result = inv_set_mpu_memory(KEY_D_0_232, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[2] = (unsigned short)sensitivity;
        break;
    case INV_CONTROL_4:
        result = inv_set_mpu_memory(KEY_D_0_236, 2, regs);
        if (result) {
            LOG_RESULT_LOCATION(result);
            return result;
        }
        cntrl_params.sensitivity[3] = (unsigned short)sensitivity;
        break;
    default:
        break;
    }
    if (finalSens != sensitivity * 100) {
        return INV_ERROR_INVALID_PARAMETER;
    } else {
        return INV_SUCCESS;
    }
}

static int ak8963_get_config(void *mlsl_handle,
			     struct ext_slave_descr *slave,
			     struct ext_slave_platform_data *pdata,
			     struct ext_slave_config *data)
{
	struct ak8963_private_data *private_data = pdata->private_data;
	int result;
	if (!data->data)
		return INV_ERROR_INVALID_PARAMETER;

	switch (data->key) {
	case MPU_SLAVE_READ_REGISTERS:
		{
			unsigned char *serial_data =
			    (unsigned char *)data->data;
			result =
			    inv_serial_read(mlsl_handle, pdata->address,
					    serial_data[0], data->len - 1,
					    &serial_data[1]);
			if (result) {
				LOG_RESULT_LOCATION(result);
				return result;
			}
			break;
		}
	case MPU_SLAVE_READ_SCALE:
		{
			unsigned char *serial_data =
			    (unsigned char *)data->data;
			serial_data[0] = private_data->init.asa[0];
			serial_data[1] = private_data->init.asa[1];
			serial_data[2] = private_data->init.asa[2];
			result = INV_SUCCESS;
			if (result) {
				LOG_RESULT_LOCATION(result);
				return result;
			}
			break;
		}
	case MPU_SLAVE_CONFIG_ODR_SUSPEND:
		(*(unsigned long *)data->data) = 0;
		break;
	case MPU_SLAVE_CONFIG_ODR_RESUME:
		(*(unsigned long *)data->data) = 8000;
		break;
	case MPU_SLAVE_CONFIG_FSR_SUSPEND:
	case MPU_SLAVE_CONFIG_FSR_RESUME:
	case MPU_SLAVE_CONFIG_MOT_THS:
	case MPU_SLAVE_CONFIG_NMOT_THS:
	case MPU_SLAVE_CONFIG_MOT_DUR:
	case MPU_SLAVE_CONFIG_NMOT_DUR:
	case MPU_SLAVE_CONFIG_IRQ_SUSPEND:
	case MPU_SLAVE_CONFIG_IRQ_RESUME:
	default:
		return INV_ERROR_FEATURE_NOT_IMPLEMENTED;
	};

	return INV_SUCCESS;
}