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);
}
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);
}
}
}