Esempio n. 1
0
static void brickpi_init_work(struct work_struct *work)
{
	struct brickpi_data *data = container_of(work, struct brickpi_data,
						 poll_work);
	int i, err;

	for (i = 0; i < data->num_channels; i++) {
		struct brickpi_channel_data *ch_data = &data->channel_data[i];

		ch_data->data = data;
		// TODO: add module parameter to get addresses
		ch_data->address = i + 1;
		ch_data->in_port[BRICKPI_PORT_1].ch_data = ch_data;
		ch_data->in_port[BRICKPI_PORT_1].sensor_type =
						BRICKPI_SENSOR_TYPE_FW_VERSION;
		ch_data->in_port[BRICKPI_PORT_2].ch_data = ch_data;
		ch_data->in_port[BRICKPI_PORT_2].sensor_type =
						BRICKPI_SENSOR_TYPE_FW_VERSION;
		err = brickpi_set_sensors(ch_data);
		if (err < 0) {
			dev_err(data->tty->dev,
				"Failed to init while setting sensors. (%d)\n",
				err);
			return;
		}
		ch_data->out_port[BRICKPI_PORT_1].ch_data = ch_data;
		ch_data->out_port[BRICKPI_PORT_2].ch_data = ch_data;
		err = brickpi_get_values(ch_data);
		if (err < 0) {
			dev_err(data->tty->dev,
				"Failed to init while getting values. (%d)\n",
				err);
			return;
		}
		ch_data->fw_version =
			ch_data->in_port[BRICKPI_PORT_1].sensor_values[0];
		debug_pr("address: %d, fw: %d\n", ch_data->address,
			 ch_data->fw_version);

		err = brickpi_register_in_ports(ch_data, data->tty->dev);
		if (err < 0) {
			dev_err(data->tty->dev,
				"Failed to register input ports (%d)",
				err);
			return;
		}
		err = brickpi_register_out_ports(ch_data, data->tty->dev);
		if (err < 0) {
			brickpi_unregister_in_ports(ch_data);
			dev_err(data->tty->dev,
				"Failed to register output ports (%d)",
				err);
			return;
		}
		ch_data->init_ok = true;
	}

	INIT_WORK(&data->poll_work, brickpi_poll_work);
	hrtimer_start(&data->poll_timer, ktime_set(0, 0), HRTIMER_MODE_REL);
}
int brickpi_in_port_set_i2c_mode(struct lego_device *sensor, u8 set_mode_reg,
				 u8 set_mode_data, u8 read_reg, unsigned size)
{
	struct brickpi_in_port_data *in_port =
		container_of(sensor->port, struct brickpi_in_port_data, port);
	int err;

	if (size > BRICKPI_MAX_I2C_DATA_SIZE) {
		dev_err(&sensor->dev, "I2C message size is too big.\n");
		return -EINVAL;
	}

	in_port->num_i2c_msg = 1;
	in_port->i2c_msg[0].settings |= BRICKPI_I2C_SAME;
	if (set_mode_reg) {
		in_port->i2c_msg[0].write_size = 2;
		in_port->i2c_msg[0].write_data[0] = set_mode_reg;
		in_port->i2c_msg[0].write_data[1] = set_mode_data;
		in_port->i2c_msg[0].read_size = 0;
		err = brickpi_set_sensors(in_port->ch_data);
		if (err < 0)
			return err;
		err = brickpi_get_values(in_port->ch_data);
		if (err < 0)
			return err;
	}

	in_port->i2c_msg[0].write_size = 1;
	in_port->i2c_msg[0].write_data[0] = read_reg;
	in_port->i2c_msg[0].read_size = size;

	return brickpi_set_sensors(in_port->ch_data);
}
Esempio n. 3
0
static void brickpi_poll_work(struct work_struct *work)
{
	struct brickpi_data *data = container_of(work, struct brickpi_data,
						 poll_work);
	int i, err;

	if (data->closing)
		return;

	for (i = 0; i < data->num_channels; i++) {
		struct brickpi_channel_data *ch_data = &data->channel_data[i];
		if (ch_data->init_ok) {
			err = brickpi_get_values(ch_data);
			if (err < 0)
				debug_pr("failed to get values for address %d. (%d)\n",
				ch_data->address, err);
		}
	}
}