static void ev3_uart_send_ack(struct work_struct *work)
{
struct ev3_uart_port_data *port = container_of(to_delayed_work(work),
struct ev3_uart_port_data, send_ack_work);
int err;
ev3_uart_write_byte(port->tty, EV3_UART_SYS_ACK);
if (!port->sensor.context && port->type_id <= EV3_UART_TYPE_MAX) {
port->sensor.context = port->tty;
err = register_lego_sensor(&port->sensor, port->tty->dev);
if (err < 0) {
port->sensor.context = NULL;
if (port->in_port) {
port->in_port = NULL;
put_device(&port->in_port->dev);
}
dev_err(port->tty->dev,
"Could not register UART sensor on tty %s",
port->tty->name);
return;
}
} else
dev_err(port->tty->dev, "Reconnected due to: %s\n",
port->last_err);
mdelay(4);
schedule_work(&port->change_bitrate_work);
}
static int ev3_uart_sensor_probe(struct lego_device *ldev)
{
struct ev3_uart_sensor_data *data;
int err;
if (WARN_ON(!ldev->entry_id))
return -EINVAL;
data = kzalloc(sizeof(struct ev3_uart_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
data->ldev = ldev;
memcpy(&data->info,
&ev3_uart_sensor_defs[ldev->entry_id->driver_data],
sizeof(struct ev3_uart_sensor_info));
data->sensor.name = ldev->entry_id->name;
data->sensor.port_name = ldev->port->port_name;
#if defined(CONFIG_NXT_I2C_SENSORS) || defined(CONFIG_NXT_I2C_SENSORS_MODULE)
/* mindsensors EV3 sensor mux only supports modes that return one value */
if (ldev->port->dev.type == &ms_ev3_smux_port_type)
data->sensor.num_modes = data->info.num_view_modes;
else
#endif
data->sensor.num_modes = data->info.num_modes;
data->sensor.mode_info = data->info.mode_info;
data->sensor.set_mode = ev3_uart_sensor_set_mode;
data->sensor.context = data;
err = register_lego_sensor(&data->sensor, &ldev->dev);
if (err)
goto err_register_lego_sensor;
dev_set_drvdata(&ldev->dev, data);
ev3_uart_sensor_set_mode(data, 0);
return 0;
err_register_lego_sensor:
kfree(data);
return err;
}
static int ev3_analog_sensor_probe(struct lego_device *ldev)
{
struct ev3_analog_sensor_data *data;
int err;
if (WARN_ON(!ldev->entry_id))
return -EINVAL;
data = kzalloc(sizeof(struct ev3_analog_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
data->ldev = ldev;
memcpy(&data->info, &ev3_analog_sensor_defs[ldev->entry_id->driver_data],
sizeof(struct ev3_analog_sensor_info));
data->sensor.name = ldev->entry_id->name;
data->sensor.port_name = ldev->port->port_name;
data->sensor.num_modes = data->info.num_modes;
data->sensor.mode_info = data->info.mode_info;
data->sensor.set_mode = ev3_analog_sensor_set_mode;
data->sensor.context = data;
err = register_lego_sensor(&data->sensor, &ldev->dev);
if (err)
goto err_register_lego_sensor;
dev_set_drvdata(&ldev->dev, data);
ev3_analog_sensor_set_mode(data, 0);
return 0;
err_register_lego_sensor:
kfree(data);
return err;
}
static int ht_nxt_smux_i2c_sensor_probe(struct lego_device *ldev)
{
struct ht_nxt_smux_i2c_sensor_data *data;
const struct nxt_i2c_sensor_info *sensor_info;
struct ht_nxt_smux_i2c_sensor_platform_data *pdata =
ldev->dev.platform_data;
size_t mode_info_size;
int err, i;
if (WARN_ON(!ldev->entry_id))
return -EINVAL;
if (WARN_ON(!pdata))
return -EINVAL;
sensor_info = &nxt_i2c_sensor_defs[ldev->entry_id->driver_data];
if (sensor_info->ops) {
dev_err(&ldev->dev, "The '%s' driver requires special operations"
" that are not supported in the '%s' module.",
ldev->entry_id->name, "ht-nxt-smux-i2c-sensor");
return -EINVAL;
}
data = kzalloc(sizeof(struct ht_nxt_smux_i2c_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
mode_info_size = sizeof(struct nxt_i2c_sensor_info) * sensor_info->num_modes;
if (!data->sensor.mode_info) {
err = -ENOMEM;
goto err_kalloc_mode_info;
}
data->ldev = ldev;
data->type = ldev->entry_id->driver_data;
data->info = sensor_info;
data->sensor.name = ldev->entry_id->name;
data->sensor.port_name = data->ldev->port->port_name;
if (data->info->num_read_only_modes)
data->sensor.num_modes = data->info->num_read_only_modes;
else
data->sensor.num_modes = data->info->num_modes;
data->sensor.num_view_modes = 1;
memcpy(data->sensor.mode_info, data->info->mode_info, mode_info_size);
data->sensor.set_mode = ht_nxt_smux_i2c_sensor_set_mode;
data->sensor.context = data;
for (i = 0; i < data->sensor.num_modes; i++) {
struct lego_sensor_mode_info *minfo = &data->sensor.mode_info[i];
if (!minfo->raw_min && !minfo->raw_max)
minfo->raw_max = 255;
if (!minfo->pct_min && !minfo->pct_max)
minfo->pct_max = 100;
if (!minfo->si_min && !minfo->si_max)
minfo->si_max = 255;
if (!minfo->data_sets)
minfo->data_sets = 1;
if (!minfo->figures)
minfo->figures = 5;
}
dev_set_drvdata(&ldev->dev, data);
err = register_lego_sensor(&data->sensor, &ldev->dev);
if (err) {
dev_err(&ldev->dev, "could not register sensor!\n");
goto err_register_lego_sensor;
}
ldev->port->nxt_i2c_ops->set_pin1_gpio(ldev->port->context,
data->info->pin1_state);
ht_nxt_smux_port_set_i2c_addr(data->ldev->port, pdata->address,
data->info->slow);
ht_nxt_smux_i2c_sensor_set_mode(data, 0);
return 0;
err_register_lego_sensor:
kfree(data->sensor.mode_info);
err_kalloc_mode_info:
kfree(data);
return err;
}
static int brickpi_i2c_sensor_probe(struct lego_device *ldev)
{
struct brickpi_i2c_sensor_data *data;
const struct nxt_i2c_sensor_info *sensor_info;
struct brickpi_i2c_sensor_platform_data *pdata =
ldev->dev.platform_data;
size_t mode_info_size;
int err, i;
if (WARN_ON(!ldev->entry_id))
return -EINVAL;
if (WARN_ON(!pdata))
return -EINVAL;
sensor_info = &nxt_i2c_sensor_defs[ldev->entry_id->driver_data];
if (sensor_info->ops) {
dev_err(&ldev->dev, "The '%s' driver requires special operations"
" that are not supported in the '%s' module.",
ldev->entry_id->name, "brickpi-i2c-sensor");
return -EINVAL;
}
data = kzalloc(sizeof(struct brickpi_i2c_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
mode_info_size = sizeof(struct lego_sensor_mode_info) * sensor_info->num_modes;
data->sensor.mode_info = kmalloc(mode_info_size, GFP_KERNEL);
if (!data->sensor.mode_info) {
err = -ENOMEM;
goto err_kalloc_mode_info;
}
data->ldev = ldev;
data->type = ldev->entry_id->driver_data;
data->info = sensor_info;
data->sensor.name = ldev->entry_id->name;
snprintf(data->address, LEGO_NAME_SIZE, "%s:i2c%d",
data->ldev->port->address, pdata->address);
data->sensor.address = data->address;
data->sensor.num_modes = data->info->num_modes;
data->sensor.num_view_modes = 1;
memcpy(data->sensor.mode_info, data->info->mode_info, mode_info_size);
data->sensor.set_mode = brickpi_i2c_sensor_set_mode;
data->sensor.send_command = brickpi_i2c_sensor_send_command;
data->sensor.context = data;
for (i = 0; i < data->sensor.num_modes; i++) {
struct lego_sensor_mode_info *minfo = &data->sensor.mode_info[i];
if (!minfo->raw_min && !minfo->raw_max)
minfo->raw_max = 255;
if (!minfo->pct_min && !minfo->pct_max)
minfo->pct_max = 100;
if (!minfo->si_min && !minfo->si_max)
minfo->si_max = 255;
if (!minfo->data_sets)
minfo->data_sets = 1;
if (!minfo->figures)
minfo->figures = 5;
}
dev_set_drvdata(&ldev->dev, data);
err = register_lego_sensor(&data->sensor, &ldev->dev);
if (err) {
dev_err(&ldev->dev, "could not register sensor!\n");
goto err_register_lego_sensor;
}
brickpi_in_port_set_i2c_data(ldev, data->info->slow, data->info->pin1_state);
brickpi_i2c_sensor_set_mode(data, 0);
return 0;
err_register_lego_sensor:
kfree(data->sensor.mode_info);
err_kalloc_mode_info:
kfree(data);
return err;
}
static int nxt_i2c_sensor_probe(struct i2c_client *client,
const struct i2c_device_id *id)
{
struct nxt_i2c_sensor_platform_data *pdata = client->dev.platform_data;
struct nxt_i2c_sensor_data *data;
struct lego_port_device *in_port = NULL;
const struct nxt_i2c_sensor_info *sensor_info;
const struct i2c_device_id *i2c_dev_id = id;
char version[NXT_I2C_ID_STR_LEN + 1] = { 0 };
size_t mode_info_size;
int err, i;
sensor_info = &nxt_i2c_sensor_defs[i2c_dev_id->driver_data];
if (pdata)
in_port = pdata->in_port;
data = kzalloc(sizeof(struct nxt_i2c_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
mode_info_size = sizeof(struct lego_sensor_mode_info) * sensor_info->num_modes;
data->sensor.mode_info = kmalloc(mode_info_size, GFP_KERNEL);
if (!data->sensor.mode_info) {
err = -ENOMEM;
goto err_kalloc_mode_info;
}
data->client = client;
data->in_port = in_port;
data->type = i2c_dev_id->driver_data;
data->info = sensor_info;
data->sensor.name = i2c_dev_id->name;
snprintf(data->address, LEGO_NAME_SIZE, "%s:i2c%d",
in_port ? in_port->address : client->adapter->name,
client->addr);
data->sensor.address = data->address;
data->sensor.num_modes = data->info->num_modes;
data->sensor.num_view_modes = 1;
memcpy(data->sensor.mode_info, data->info->mode_info, mode_info_size);
data->sensor.num_commands = data->info->num_commands;
data->sensor.cmd_info = data->info->cmd_info;
data->sensor.set_mode = nxt_i2c_sensor_set_mode;
data->sensor.send_command = nxt_i2c_sensor_send_command;
data->sensor.direct_read = nxt_i2c_sensor_direct_read;
data->sensor.direct_write = nxt_i2c_sensor_direct_write;
data->sensor.get_poll_ms = nxt_i2c_sensor_get_poll_ms;
data->sensor.set_poll_ms = nxt_i2c_sensor_set_poll_ms;
data->sensor.context = data;
i2c_smbus_read_i2c_block_data(client, NXT_I2C_FW_VER_REG,
NXT_I2C_ID_STR_LEN, version);
/*
* HiTechnic sensors have 0xfd before the version because of an early
* bug in the NXT I2C code where register 0x00 could not be read
* reliably. So, we just ignore it along with the whitespace.
*/
strncpy(data->sensor.fw_version, strim(version[0] == 0xfd ?
(version + 1) : version), NXT_I2C_ID_STR_LEN + 1);
for (i = 0; i < data->sensor.num_modes; i++) {
struct lego_sensor_mode_info *minfo = &data->sensor.mode_info[i];
if (!minfo->raw_min && !minfo->raw_max)
minfo->raw_max = 255;
if (!minfo->pct_min && !minfo->pct_max)
minfo->pct_max = 100;
if (!minfo->si_min && !minfo->si_max)
minfo->si_max = 255;
if (!minfo->data_sets)
minfo->data_sets = 1;
if (!minfo->figures)
minfo->figures = 5;
}
INIT_WORK(&data->poll_work, nxt_i2c_sensor_poll_work);
hrtimer_init(&data->poll_timer, CLOCK_MONOTONIC, HRTIMER_MODE_REL);
data->poll_timer.function = nxt_i2c_sensor_poll_timer;
data->poll_ms = default_poll_ms;
i2c_set_clientdata(client, data);
if (data->info->ops && data->info->ops->probe_cb) {
err = data->info->ops->probe_cb(data);
if (err < 0)
goto err_probe_cb;
}
err = register_lego_sensor(&data->sensor, &client->dev);
if (err) {
dev_err(&client->dev, "could not register sensor!\n");
goto err_register_lego_sensor;
}
if (data->in_port && data->in_port->nxt_i2c_ops)
data->in_port->nxt_i2c_ops->set_pin1_gpio(data->in_port->context,
data->info->pin1_state);
if (data->type == LEGO_NXT_ULTRASONIC_SENSOR)
msleep (1);
nxt_i2c_sensor_set_mode(data, data->sensor.mode);
return 0;
err_probe_cb:
err_register_lego_sensor:
i2c_set_clientdata(client, NULL);
kfree(data->sensor.mode_info);
err_kalloc_mode_info:
kfree(data);
return err;
}
static int ht_nxt_smux_i2c_sensor_probe(struct lego_device *ldev)
{
struct ht_nxt_smux_i2c_sensor_data *data;
const struct nxt_i2c_sensor_info *sensor_info;
struct ht_nxt_smux_i2c_sensor_platform_data *pdata =
ldev->dev.platform_data;
int err, i;
if (WARN_ON(!ldev->entry_id))
return -EINVAL;
if (WARN_ON(!pdata))
return -EINVAL;
sensor_info = &nxt_i2c_sensor_defs[ldev->entry_id->driver_data];
data = kzalloc(sizeof(struct ht_nxt_smux_i2c_sensor_data), GFP_KERNEL);
if (!data)
return -ENOMEM;
data->ldev = ldev;
data->type = ldev->entry_id->driver_data;
memcpy(&data->info, sensor_info, sizeof(struct nxt_i2c_sensor_info));
data->sensor.name = ldev->entry_id->name;
data->sensor.port_name = data->ldev->port->port_name;
if (data->info.num_read_only_modes)
data->sensor.num_modes = data->info.num_read_only_modes;
else
data->sensor.num_modes = data->info.num_modes;
data->sensor.num_view_modes = 1;
data->sensor.mode_info = data->info.mode_info;
data->sensor.set_mode = ht_nxt_smux_i2c_sensor_set_mode;
data->sensor.context = data;
for (i = 0; i < data->sensor.num_modes; i++) {
struct lego_sensor_mode_info *minfo = &data->info.mode_info[i];
if (!minfo->raw_min && !minfo->raw_max)
minfo->raw_max = 255;
if (!minfo->pct_min && !minfo->pct_max)
minfo->pct_max = 100;
if (!minfo->si_min && !minfo->si_max)
minfo->si_max = 255;
if (!minfo->data_sets)
minfo->data_sets = 1;
if (!minfo->figures)
minfo->figures = 5;
}
dev_set_drvdata(&ldev->dev, data);
err = register_lego_sensor(&data->sensor, &ldev->dev);
if (err) {
dev_err(&ldev->dev, "could not register sensor!\n");
goto err_register_lego_sensor;
}
ldev->port->nxt_i2c_ops->set_pin1_gpio(ldev->port->context,
data->info.pin1_state);
ht_nxt_smux_port_set_i2c_addr(data->ldev->port, pdata->address,
data->info.slow);
ht_nxt_smux_i2c_sensor_set_mode(data, 0);
return 0;
err_register_lego_sensor:
kfree(data);
return err;
}
