Exemplo n.º 1
0
static int lsm330dlc_gyro_resume(struct device *dev)
{
	int err = 0;
	struct i2c_client *client = to_i2c_client(dev);
	struct lsm330dlc_gyro_data *data = i2c_get_clientdata(client);

	if (data->enable) {
		mutex_lock(&data->lock);
		err = i2c_smbus_write_i2c_block_data(client,
				CTRL_REG1 | AC, sizeof(data->ctrl_regs),
							data->ctrl_regs);

		if (data->interruptible) {
			enable_irq(data->client->irq);

			lsm330dlc_gyro_restart_fifo(data);
		}

		if (!data->interruptible)
			hrtimer_start(&data->timer,
				data->polling_delay, HRTIMER_MODE_REL);

		mutex_unlock(&data->lock);
	}

	return err;
}
Exemplo n.º 2
0
static int lsm330dlc_gyro_report_values\
	(struct lsm330dlc_gyro_data *gyro_data)
{
	int res;
	struct gyro_t data;

	res = lsm330dlc_gyro_read_values(gyro_data->client, &data,
		gyro_data->entries);
	if (res < 0)
		return res;

	data.x -= gyro_data->cal_data.x;
	data.y -= gyro_data->cal_data.y;
	data.z -= gyro_data->cal_data.z;

	input_report_rel(gyro_data->input_dev, REL_RX, data.x);
	input_report_rel(gyro_data->input_dev, REL_RY, data.y);
	input_report_rel(gyro_data->input_dev, REL_RZ, data.z);
	input_sync(gyro_data->input_dev);

	lsm330dlc_gyro_restart_fifo(gyro_data);

#ifdef LOGGING_GYRO
	printk(KERN_INFO "%s, x = %d, y = %d, z = %d, count = %d\n"
		, __func__, data.x, data.y, data.z, gyro_data->count);
#endif

	return res;
}
Exemplo n.º 3
0
static int lsm330dlc_gyro_report_values\
	(struct lsm330dlc_gyro_data *data)
{
	int res, i, j;
	s16 raw[3] = {0,}, gyro_adjusted[3] = {0,};

	res = lsm330dlc_gyro_read_values(data->client,
		&data->xyz_data, data->entries);
	if (res < 0)
		return res;

	data->xyz_data.x -= data->cal_data.x;
	data->xyz_data.y -= data->cal_data.y;
	data->xyz_data.z -= data->cal_data.z;

	if (data->axis_adjust) {
		raw[0] = data->xyz_data.x;
		raw[1] = data->xyz_data.y;
		raw[2] = data->xyz_data.z;
		for (i = 0; i < 3; i++) {
			for (j = 0; j < 3; j++)
				gyro_adjusted[i]
				+= position_map[data->position][i][j] * raw[j];
		}
	} else {
		gyro_adjusted[0] = data->xyz_data.x;
		gyro_adjusted[1] = data->xyz_data.y;
		gyro_adjusted[2] = data->xyz_data.z;
	}

	input_report_rel(data->input_dev, REL_RX, gyro_adjusted[0]);
	input_report_rel(data->input_dev, REL_RY, gyro_adjusted[1]);
	input_report_rel(data->input_dev, REL_RZ, gyro_adjusted[2]);
	input_sync(data->input_dev);

	lsm330dlc_gyro_restart_fifo(data);

#ifdef LOGGING_GYRO
	pr_info("%s, x = %d, y = %d, z = %d\n"
			, __func__, gyro_adjusted[0], gyro_adjusted[1],
			gyro_adjusted[2]);
#endif

	return res;
}
Exemplo n.º 4
0
static int lsm330dlc_gyro_set_delay_ns(struct lsm330dlc_gyro_data *k3\
	, u64 delay_ns)
{
	int res = 0;
	int odr_value = ODR95_BW25;
	int i = 0, odr_table_size = ARRAY_SIZE(odr_delay_table) - 1;
	u8 ctrl;
	u64 new_delay = 0;

	mutex_lock(&k3->lock);
	if (!k3->interruptible) {
		hrtimer_cancel(&k3->timer);
		cancel_work_sync(&k3->work);
	} else
		disable_irq(k3->client->irq);

	/* round to the nearest supported ODR that is equal or above than
	 * the requested value
	 * 10ms(entries = 1), 20ms(entries = 2),
	 * 67ms(entries = 6), 200ms(entries = 20)
	 */
	if (delay_ns == 10000000LL && k3->interruptible)
		delay_ns = odr_delay_table[odr_table_size].delay_ns;

	if (delay_ns >= MAX_DELAY) {/* > max delay */
		k3->entries = MAX_ENTRY;
		odr_value = odr_delay_table[odr_table_size].odr;
		k3->time_to_read = odr_delay_table[odr_table_size].delay_ns;
		new_delay = MAX_DELAY;
	} else if (delay_ns <= odr_delay_table[0].delay_ns) { /* < min delay */
		k3->entries = 1;
		odr_value = odr_delay_table[0].odr;
		k3->time_to_read = odr_delay_table[0].delay_ns;
		new_delay = odr_delay_table[0].delay_ns;
	} else {
		for (i = odr_table_size; i >= 0; i--) {
			if (delay_ns >= odr_delay_table[i].delay_ns) {
				new_delay = delay_ns;
				do_div(delay_ns, odr_delay_table[i].delay_ns);
				k3->entries = delay_ns;
				odr_value = odr_delay_table[i].odr;
				k3->time_to_read = odr_delay_table[i].delay_ns;
				break;
			}
		}
	}

	if (k3->interruptible)
		pr_info("%s, k3->entries=%d, odr_value=0x%x\n", __func__,
			k3->entries, odr_value);

	if (odr_value != (k3->ctrl_regs[0] & ODR_MASK)) {
		ctrl = (k3->ctrl_regs[0] & ~ODR_MASK);
		ctrl |= odr_value;
		k3->ctrl_regs[0] = ctrl;
		res = i2c_smbus_write_byte_data(k3->client, CTRL_REG1, ctrl);
	}

	if (k3->interruptible) {
		enable_irq(k3->client->irq);

		/* (re)start fifo */
		lsm330dlc_gyro_restart_fifo(k3);
	}

	if (!k3->interruptible) {
		pr_info("%s, delay_ns=%lld, odr_value=0x%x\n",
					__func__, new_delay, odr_value);
		k3->polling_delay = ns_to_ktime(new_delay);
		if (k3->enable)
			hrtimer_start(&k3->timer,
				k3->polling_delay, HRTIMER_MODE_REL);
	}

	mutex_unlock(&k3->lock);

	return res;
}
Exemplo n.º 5
0
static ssize_t lsm330dlc_gyro_set_enable(struct device *dev,
		struct device_attribute *attr, const char *buf, size_t size)
{
	int err = 0;
	struct lsm330dlc_gyro_data *data  = dev_get_drvdata(dev);
	bool new_enable;

	if (sysfs_streq(buf, "1"))
		new_enable = true;
	else if (sysfs_streq(buf, "0"))
		new_enable = false;
	else {
		pr_debug("%s: invalid value %d\n", __func__, *buf);
		return -EINVAL;
	}

	printk(KERN_INFO "%s, %d enable : %d.\n", __func__, __LINE__,\
		new_enable);

	if (new_enable == data->enable)
		return size;

	mutex_lock(&data->lock);
	if (new_enable) {
		/* load cal data */
		err = lsm330dlc_gyro_open_calibration(data);
		if (err < 0 && err != -ENOENT)
			pr_err("%s: lsm330dlc_gyro_open_calibration() failed\n",
				__func__);
		/* turning on */
		err = i2c_smbus_write_i2c_block_data(data->client,
			CTRL_REG1 | AC, sizeof(data->ctrl_regs),
						data->ctrl_regs);
		if (err < 0) {
			err = -EIO;
			goto unlock;
		}

		if (data->interruptible) {
			enable_irq(data->client->irq);

			/* reset fifo entries */
			err = lsm330dlc_gyro_restart_fifo(data);
			if (err < 0) {
				err = -EIO;
				goto turn_off;
			}
		}

		if (!data->interruptible) {
			hrtimer_start(&data->timer,
				data->polling_delay, HRTIMER_MODE_REL);
		}
	} else {
		if (data->interruptible)
			disable_irq(data->client->irq);
		else {
			hrtimer_cancel(&data->timer);
			cancel_work_sync(&data->work);
		}
		/* turning off */
		err = i2c_smbus_write_byte_data(data->client,
						CTRL_REG1, 0x00);
		if (err < 0)
			goto unlock;
	}
	data->enable = new_enable;

	printk(KERN_INFO "%s, %d lsm330dlc_gyro enable is done.\n",\
		__func__, __LINE__);

turn_off:
	if (err < 0)
		i2c_smbus_write_byte_data(data->client,
						CTRL_REG1, 0x00);
unlock:
	mutex_unlock(&data->lock);

	return err ? err : size;
}