diff options
| -rw-r--r-- | drivers/media/i2c/smiapp/smiapp-core.c | 82 |
1 files changed, 54 insertions, 28 deletions
diff --git a/drivers/media/i2c/smiapp/smiapp-core.c b/drivers/media/i2c/smiapp/smiapp-core.c index a6bc5eff266a..7b13617a74c9 100644 --- a/drivers/media/i2c/smiapp/smiapp-core.c +++ b/drivers/media/i2c/smiapp/smiapp-core.c @@ -2467,6 +2467,56 @@ static const struct v4l2_subdev_ops smiapp_ops; static const struct v4l2_subdev_internal_ops smiapp_internal_ops; static const struct media_entity_operations smiapp_entity_ops; +static int smiapp_register_subdevs(struct smiapp_sensor *sensor) +{ + struct i2c_client *client = v4l2_get_subdevdata(&sensor->src->sd); + struct smiapp_subdev *ssds[] = { + sensor->scaler, + sensor->binner, + sensor->pixel_array, + }; + unsigned int i; + int rval; + + for (i = 0; i < SMIAPP_SUBDEVS - 1; i++) { + struct smiapp_subdev *this = ssds[i + 1]; + struct smiapp_subdev *last = ssds[i]; + + if (!last) + continue; + + rval = media_entity_init(&this->sd.entity, + this->npads, this->pads, 0); + if (rval) { + dev_err(&client->dev, + "media_entity_init failed\n"); + return rval; + } + + rval = media_entity_create_link(&this->sd.entity, + this->source_pad, + &last->sd.entity, + last->sink_pad, + MEDIA_LNK_FL_ENABLED | + MEDIA_LNK_FL_IMMUTABLE); + if (rval) { + dev_err(&client->dev, + "media_entity_create_link failed\n"); + return rval; + } + + rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev, + &this->sd); + if (rval) { + dev_err(&client->dev, + "v4l2_device_register_subdev failed\n"); + return rval; + } + } + + return 0; +} + static int smiapp_registered(struct v4l2_subdev *subdev) { struct smiapp_sensor *sensor = to_smiapp_sensor(subdev); @@ -2705,37 +2755,13 @@ static int smiapp_registered(struct v4l2_subdev *subdev) this->sd.owner = THIS_MODULE; v4l2_set_subdevdata(&this->sd, client); - rval = media_entity_init(&this->sd.entity, - this->npads, this->pads, 0); - if (rval) { - dev_err(&client->dev, - "media_entity_init failed\n"); - goto out_nvm_release; - } - - rval = media_entity_create_link(&this->sd.entity, - this->source_pad, - &last->sd.entity, - last->sink_pad, - MEDIA_LNK_FL_ENABLED | - MEDIA_LNK_FL_IMMUTABLE); - if (rval) { - dev_err(&client->dev, - "media_entity_create_link failed\n"); - goto out_nvm_release; - } - - rval = v4l2_device_register_subdev(sensor->src->sd.v4l2_dev, - &this->sd); - if (rval) { - dev_err(&client->dev, - "v4l2_device_register_subdev failed\n"); - goto out_nvm_release; - } - last = this; } + rval = smiapp_register_subdevs(sensor); + if (rval) + goto out_nvm_release; + dev_dbg(&client->dev, "profile %d\n", sensor->minfo.smiapp_profile); sensor->pixel_array->sd.entity.type = MEDIA_ENT_T_V4L2_SUBDEV_SENSOR; |