void ms_nxtmmx_remove_cb(struct nxt_i2c_sensor_data *data)
{
struct ms_nxtmmx_data *mmx = data->callback_data;
unregister_tacho_motor(&mmx[1].tm);
unregister_tacho_motor(&mmx[0].tm);
data->callback_data = NULL;
kfree(mmx);
}
static int ev3_motor_remove(struct lego_device *ldev)
{
struct ev3_motor_data *data = dev_get_drvdata(&ldev->dev);
unregister_tacho_motor(&data->tm);
dev_set_drvdata(&ldev->dev, NULL);
kfree(data);
return 0;
}
int ms_nxtmmx_probe_cb(struct nxt_i2c_sensor_data *data)
{
struct ms_nxtmmx_data *mmx;
int i, err;
mmx = kzalloc(sizeof(struct ms_nxtmmx_data) * 2, GFP_KERNEL);
if (!mmx)
return -ENOMEM;
data->callback_data = mmx;
for (i = 0; i < 2; i++) {
mmx[i].tm.driver_name = data->info->name;
snprintf(mmx[i].port_name, LEGO_PORT_NAME_SIZE, "%s:i2c%d:mux%d",
data->port_name, data->client->addr, i + 1);
mmx[i].tm.port_name = mmx[i].port_name;
mmx[i].tm.ops = &ms_nxtmmx_tacho_motor_ops;
mmx[i].tm.context = &mmx[i];
mmx[i].i2c = data;
mmx[i].index = i;
mmx[i].command_flags = CMD_FLAGS_DEFAULT_VALUE;
}
err = register_tacho_motor(&mmx[0].tm, &data->client->dev);
if (err)
goto err_register_tacho_motor0;
err = register_tacho_motor(&mmx[1].tm, &data->client->dev);
if (err)
goto err_register_tacho_motor1;
data->poll_ms = 1000;
return 0;
err_register_tacho_motor1:
unregister_tacho_motor(&mmx[0].tm);
err_register_tacho_motor0:
data->callback_data = NULL;
kfree(mmx);
return err;
}
