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