diff --git a/drivers/gpu/drm/bridge/sii902x.c b/drivers/gpu/drm/bridge/sii902x.c index 33fd33f953ec..f78c17f49887 100644 --- a/drivers/gpu/drm/bridge/sii902x.c +++ b/drivers/gpu/drm/bridge/sii902x.c @@ -954,13 +954,73 @@ static const struct drm_bridge_timings default_sii902x_timings = { | DRM_BUS_FLAG_DE_HIGH, }; +static int sii902x_init(struct sii902x *sii902x) +{ + struct device *dev = &sii902x->i2c->dev; + unsigned int status = 0; + u8 chipid[4]; + int ret; + + sii902x_reset(sii902x); + + ret = regmap_write(sii902x->regmap, SII902X_REG_TPI_RQB, 0x0); + if (ret) + return ret; + + ret = regmap_bulk_read(sii902x->regmap, SII902X_REG_CHIPID(0), + &chipid, 4); + if (ret) { + dev_err(dev, "regmap_read failed %d\n", ret); + return ret; + } + + if (chipid[0] != 0xb0) { + dev_err(dev, "Invalid chipid: %02x (expecting 0xb0)\n", + chipid[0]); + return -EINVAL; + } + + /* Clear all pending interrupts */ + regmap_read(sii902x->regmap, SII902X_INT_STATUS, &status); + regmap_write(sii902x->regmap, SII902X_INT_STATUS, status); + + if (sii902x->i2c->irq > 0) { + regmap_write(sii902x->regmap, SII902X_INT_ENABLE, + SII902X_HOTPLUG_EVENT); + + ret = devm_request_threaded_irq(dev, sii902x->i2c->irq, NULL, + sii902x_interrupt, + IRQF_ONESHOT, dev_name(dev), + sii902x); + if (ret) + return ret; + } + + sii902x->bridge.funcs = &sii902x_bridge_funcs; + sii902x->bridge.of_node = dev->of_node; + sii902x->bridge.timings = &default_sii902x_timings; + drm_bridge_add(&sii902x->bridge); + + sii902x_audio_codec_init(sii902x, dev); + + i2c_set_clientdata(sii902x->i2c, sii902x); + + sii902x->i2cmux = i2c_mux_alloc(sii902x->i2c->adapter, dev, + 1, 0, I2C_MUX_GATE, + sii902x_i2c_bypass_select, + sii902x_i2c_bypass_deselect); + if (!sii902x->i2cmux) + return -ENOMEM; + + sii902x->i2cmux->priv = sii902x; + return i2c_mux_add_adapter(sii902x->i2cmux, 0, 0, 0); +} + static int sii902x_probe(struct i2c_client *client, const struct i2c_device_id *id) { struct device *dev = &client->dev; - unsigned int status = 0; struct sii902x *sii902x; - u8 chipid[4]; int ret; ret = i2c_check_functionality(client->adapter, @@ -989,59 +1049,8 @@ static int sii902x_probe(struct i2c_client *client, mutex_init(&sii902x->mutex); - sii902x_reset(sii902x); - - ret = regmap_write(sii902x->regmap, SII902X_REG_TPI_RQB, 0x0); - if (ret) - return ret; - - ret = regmap_bulk_read(sii902x->regmap, SII902X_REG_CHIPID(0), - &chipid, 4); - if (ret) { - dev_err(dev, "regmap_read failed %d\n", ret); - return ret; - } - - if (chipid[0] != 0xb0) { - dev_err(dev, "Invalid chipid: %02x (expecting 0xb0)\n", - chipid[0]); - return -EINVAL; - } - - /* Clear all pending interrupts */ - regmap_read(sii902x->regmap, SII902X_INT_STATUS, &status); - regmap_write(sii902x->regmap, SII902X_INT_STATUS, status); - - if (client->irq > 0) { - regmap_write(sii902x->regmap, SII902X_INT_ENABLE, - SII902X_HOTPLUG_EVENT); - - ret = devm_request_threaded_irq(dev, client->irq, NULL, - sii902x_interrupt, - IRQF_ONESHOT, dev_name(dev), - sii902x); - if (ret) - return ret; - } - - sii902x->bridge.funcs = &sii902x_bridge_funcs; - sii902x->bridge.of_node = dev->of_node; - sii902x->bridge.timings = &default_sii902x_timings; - drm_bridge_add(&sii902x->bridge); - - sii902x_audio_codec_init(sii902x, dev); - - i2c_set_clientdata(client, sii902x); - - sii902x->i2cmux = i2c_mux_alloc(client->adapter, dev, - 1, 0, I2C_MUX_GATE, - sii902x_i2c_bypass_select, - sii902x_i2c_bypass_deselect); - if (!sii902x->i2cmux) - return -ENOMEM; - - sii902x->i2cmux->priv = sii902x; - return i2c_mux_add_adapter(sii902x->i2cmux, 0, 0, 0); + ret = sii902x_init(sii902x); + return ret; } static int sii902x_remove(struct i2c_client *client)