void inv_mpu_unregister_slave(struct i2c_client *slave_client,
struct ext_slave_platform_data *slave_pdata,
struct ext_slave_descr *(*get_slave_descr)(void))
{
struct mpu_private_data *mpu = mpu_private_data;
struct mldl_cfg *mldl_cfg = &mpu->mldl_cfg;
struct ext_slave_descr *slave_descr;
int result;
dev_info(&slave_client->adapter->dev, "%s\n", __func__);
if (!slave_client || !slave_pdata || !get_slave_descr)
return;
if (slave_pdata->irq)
slaveirq_exit(slave_pdata);
slave_descr = get_slave_descr();
if (!slave_descr)
return;
mutex_lock(&mpu->mutex);
if (slave_descr->exit) {
result = slave_descr->exit(slave_client->adapter,
slave_descr,
slave_pdata);
if (result)
dev_err(&slave_client->adapter->dev,
"Accel exit failed %d\n", result);
}
mldl_cfg->slave[slave_descr->type] = NULL;
mldl_cfg->pdata_slave[slave_descr->type] = NULL;
mpu->slave_modules[slave_descr->type] = NULL;
mutex_unlock(&mpu->mutex);
}
int inv_mpu_register_slave(struct module *slave_module,
struct i2c_client *slave_client,
struct ext_slave_platform_data *slave_pdata,
struct ext_slave_descr *(*get_slave_descr)(void))
{
struct mpu_private_data *mpu = mpu_private_data;
struct mldl_cfg *mldl_cfg;
struct ext_slave_descr *slave_descr;
struct ext_slave_platform_data **pdata_slave;
char *irq_name = NULL;
int result = 0;
if (!slave_client || !slave_pdata || !get_slave_descr)
return -EINVAL;
if (!mpu) {
dev_err(&slave_client->adapter->dev,
"%s: Null mpu_private_data\n", __func__);
return -EINVAL;
}
mldl_cfg = &mpu->mldl_cfg;
pdata_slave = mldl_cfg->pdata_slave;
slave_descr = get_slave_descr();
if (!slave_descr) {
dev_err(&slave_client->adapter->dev,
"%s: Null ext_slave_descr\n", __func__);
return -EINVAL;
}
mutex_lock(&mpu->mutex);
if (mpu->pid) {
mutex_unlock(&mpu->mutex);
return -EBUSY;
}
if (pdata_slave[slave_descr->type]) {
result = -EBUSY;
goto out_unlock_mutex;
}
slave_pdata->address = slave_client->addr;
slave_pdata->irq = slave_client->irq;
slave_pdata->adapt_num = i2c_adapter_id(slave_client->adapter);
dev_info(&slave_client->adapter->dev,
"%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
__func__,
slave_descr->name,
slave_descr->type,
slave_pdata->address,
slave_pdata->irq,
slave_pdata->adapt_num);
switch (slave_descr->type) {
case EXT_SLAVE_TYPE_ACCEL:
irq_name = "accelirq";
break;
case EXT_SLAVE_TYPE_COMPASS:
irq_name = "compassirq";
break;
case EXT_SLAVE_TYPE_PRESSURE:
irq_name = "pressureirq";
break;
default:
irq_name = "none";
};
if (slave_descr->init) {
result = slave_descr->init(slave_client->adapter,
slave_descr,
slave_pdata);
if (result) {
dev_err(&slave_client->adapter->dev,
"%s init failed %d\n",
slave_descr->name, result);
goto out_unlock_mutex;
}
}
pdata_slave[slave_descr->type] = slave_pdata;
mpu->slave_modules[slave_descr->type] = slave_module;
mldl_cfg->slave[slave_descr->type] = slave_descr;
goto out_unlock_mutex;
out_unlock_mutex:
mutex_unlock(&mpu->mutex);
if (!result && irq_name && (slave_pdata->irq > 0)) {
int warn_result;
dev_info(&slave_client->adapter->dev,
"Installing %s irq using %d\n",
irq_name,
slave_pdata->irq);
warn_result = slaveirq_init(slave_client->adapter,
slave_pdata, irq_name);
if (result)
dev_WARN(&slave_client->adapter->dev,
"%s irq assigned error: %d\n",
slave_descr->name, warn_result);
} else {
dev_info(&slave_client->adapter->dev,
"%s irq not assigned: %d %d %d\n",
slave_descr->name,
result, (int)irq_name, slave_pdata->irq);
}
return result;
}
int inv_mpu_register_slave(struct module *slave_module,
struct i2c_client *slave_client,
struct ext_slave_platform_data *slave_pdata,
struct ext_slave_descr *(*get_slave_descr)(void))
{
struct mpu_private_data *mpu = mpu_private_data;
struct mldl_cfg *mldl_cfg;
struct ext_slave_descr *slave_descr;
struct ext_slave_platform_data **pdata_slave;
char *irq_name = NULL;
int result = 0;
if (!slave_client || !slave_pdata || !get_slave_descr)
return -EINVAL;
if (!mpu) {
dev_err(&slave_client->adapter->dev,
"%s: Null mpu_private_data\n", __func__);
return -EINVAL;
}
mldl_cfg = &mpu->mldl_cfg;
pdata_slave = mldl_cfg->pdata_slave;
slave_descr = get_slave_descr();
if (!slave_descr) {
dev_err(&slave_client->adapter->dev,
"%s: Null ext_slave_descr\n", __func__);
return -EINVAL;
}
mutex_lock(&mpu->mutex);
if (mpu->pid) {
mutex_unlock(&mpu->mutex);
return -EBUSY;
}
if (pdata_slave[slave_descr->type]) {
result = -EBUSY;
goto out_unlock_mutex;
}
slave_pdata->address = slave_client->addr;
slave_pdata->irq = slave_client->irq;
slave_pdata->adapt_num = i2c_adapter_id(slave_client->adapter);
dev_info(&slave_client->adapter->dev,
"%s: +%s Type %d: Addr: %2x IRQ: %2d, Adapt: %2d\n",
__func__,
slave_descr->name,
slave_descr->type,
slave_pdata->address,
slave_pdata->irq,
slave_pdata->adapt_num);
switch (slave_descr->type) {
case EXT_SLAVE_TYPE_ACCEL:
irq_name = "accelirq";
break;
case EXT_SLAVE_TYPE_COMPASS:
irq_name = "compassirq";
break;
case EXT_SLAVE_TYPE_PRESSURE:
irq_name = "pressureirq";
break;
default:
irq_name = "none";
};
if (slave_descr->init) {
result = slave_descr->init(slave_client->adapter,
slave_descr,
slave_pdata);
if (result) {
dev_err(&slave_client->adapter->dev,
"%s init failed %d\n",
slave_descr->name, result);
goto out_unlock_mutex;
}
}
if (slave_descr->type == EXT_SLAVE_TYPE_ACCEL &&
slave_descr->id == ACCEL_ID_MPU6050 &&
slave_descr->config) {
/* pass a reference to the mldl_cfg data
structure to the mpu6050 accel "class" */
struct ext_slave_config config;
config.key = MPU_SLAVE_CONFIG_INTERNAL_REFERENCE;
config.len = sizeof(struct mldl_cfg *);
config.apply = true;
config.data = mldl_cfg;
result = slave_descr->config(
slave_client->adapter, slave_descr,
slave_pdata, &config);
if (result) {
LOG_RESULT_LOCATION(result);
goto out_slavedescr_exit;
}
}
pdata_slave[slave_descr->type] = slave_pdata;
mpu->slave_modules[slave_descr->type] = slave_module;
mldl_cfg->slave[slave_descr->type] = slave_descr;
goto out_unlock_mutex;
out_slavedescr_exit:
if (slave_descr->exit)
slave_descr->exit(slave_client->adapter,
slave_descr, slave_pdata);
out_unlock_mutex:
mutex_unlock(&mpu->mutex);
if (!result && irq_name && (slave_pdata->irq > 0)) {
int warn_result;
dev_info(&slave_client->adapter->dev,
"Installing %s irq using %d\n",
irq_name,
slave_pdata->irq);
warn_result = slaveirq_init(slave_client->adapter,
slave_pdata, irq_name);
if (result)
dev_WARN(&slave_client->adapter->dev,
"%s irq assigned error: %d\n",
slave_descr->name, warn_result);
} else {
dev_WARN(&slave_client->adapter->dev,
"%s irq not assigned: %d %d %d\n",
slave_descr->name,
result, (int)irq_name, slave_pdata->irq);
}
return result;
}