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 int brickpi_in_port_set_mode(void *context, u8 mode)
{
struct brickpi_in_port_data *in_port = context;
const char *name = NULL;
int err;
brickpi_in_port_unregister_sensor(in_port);
switch (mode) {
case BRICKPI_IN_PORT_MODE_NONE:
case BRICKPI_IN_PORT_MODE_EV3_UART:
/* We don't want to load the wrong UART sensor because it causes problems. */
in_port->sensor_type = BRICKPI_SENSOR_TYPE_FW_VERSION;
return brickpi_set_sensors(in_port->ch_data);
case BRICKPI_IN_PORT_MODE_NXT_ANALOG:
in_port->sensor_type = BRICKPI_SENSOR_TYPE_NXT_ANALOG;
name = GENERIC_NXT_ANALOG_SENSOR_NAME;
break;
case BRICKPI_IN_PORT_MODE_EV3_ANALOG:
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_TOUCH;
name = LEGO_EV3_TOUCH_SENSOR_NAME;
break;
}
/* TODO: This check shouldn't be needed when we have all of the modes implemented */
if (!name)
return -EOPNOTSUPP;
err = brickpi_set_sensors(in_port->ch_data);
if (err < 0)
return err;
return brickpi_in_port_register_sensor(in_port,
&brickpi_device_type[mode], name);
}
static int brickpi_in_port_set_pin5_gpio(void *context,
enum lego_port_gpio_state state)
{
struct brickpi_in_port_data *in_port = context;
if (unlikely(in_port->sensor_type > BRICKPI_SENSOR_TYPE_NXT_ANALOG_MAX))
return -EINVAL;
switch (state) {
case LEGO_PORT_GPIO_FLOAT:
in_port->sensor_type &= ~BRICKPI_NXT_ANALOG_FLAG_PIN5_OUT;
in_port->sensor_type &= ~BRICKPI_NXT_ANALOG_FLAG_PIN5_HIGH;
break;
case LEGO_PORT_GPIO_LOW:
in_port->sensor_type |= BRICKPI_NXT_ANALOG_FLAG_PIN5_OUT;
in_port->sensor_type &= ~BRICKPI_NXT_ANALOG_FLAG_PIN5_HIGH;
break;
case LEGO_PORT_GPIO_HIGH:
in_port->sensor_type |= BRICKPI_NXT_ANALOG_FLAG_PIN5_OUT;
in_port->sensor_type |= BRICKPI_NXT_ANALOG_FLAG_PIN5_HIGH;
break;
}
return brickpi_set_sensors(in_port->ch_data);
}
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);
}
static int brickpi_in_port_set_ev3_uart_sensor_mode(void *context, u8 type_id,
u8 mode)
{
struct brickpi_in_port_data *in_port = context;
if (type_id == ev3_uart_sensor_defs[LEGO_EV3_COLOR].type_id)
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_COLOR_M0 + mode;
else if (type_id == ev3_uart_sensor_defs[LEGO_EV3_ULTRASONIC].type_id)
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_US_M0 + mode;
else if (type_id == ev3_uart_sensor_defs[LEGO_EV3_GYRO].type_id)
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_GYRO_M0 + mode;
else if (type_id == ev3_uart_sensor_defs[LEGO_EV3_INFRARED].type_id)
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_INFRARED_M0 + mode;
else
return -EINVAL;
return brickpi_set_sensors(in_port->ch_data);
}
int brickpi_set_uart_sensor_mode(struct lego_device *sensor, u8 mode)
{
struct brickpi_in_port_data *in_port =
container_of(sensor->port, struct brickpi_in_port_data, port);
if (!strncmp(sensor->name, LEGO_EV3_COLOR_NAME, LEGO_NAME_SIZE))
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_COLOR_M0 + mode;
else if (!strncmp(sensor->name, LEGO_EV3_ULTRASONIC_NAME, LEGO_NAME_SIZE))
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_US_M0 + mode;
else if (!strncmp(sensor->name, LEGO_EV3_GYRO_NAME, LEGO_NAME_SIZE))
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_GYRO_M0 + mode;
else if (!strncmp(sensor->name, LEGO_EV3_INFRARED_NAME, LEGO_NAME_SIZE))
in_port->sensor_type = BRICKPI_SENSOR_TYPE_EV3_INFRARED_M0 + mode;
else
return -EINVAL;
return brickpi_set_sensors(in_port->ch_data);
}
