git.maquefel.me Git - linux.git/blob

1 // SPDX-License-Identifier: GPL-2.0

2 /*

3 * camss-csid.c

4 *

5 * Qualcomm MSM Camera Subsystem - CSID (CSI Decoder) Module

6 *

9 */

10 #include <linux/clk.h>

11 #include <linux/completion.h>

12 #include <linux/interrupt.h>

13 #include <linux/io.h>

14 #include <linux/kernel.h>

15 #include <linux/of.h>

16 #include <linux/platform_device.h>

17 #include <linux/pm_runtime.h>

18 #include <linux/regulator/consumer.h>

19 #include <media/media-entity.h>

20 #include <media/v4l2-device.h>

21 #include <media/v4l2-event.h>

22 #include <media/v4l2-subdev.h>

24 #include "camss-csid.h"

25 #include "camss-csid-gen1.h"

26 #include "camss.h"

28 #define MSM_CSID_NAME "msm_csid"

30 const char * const csid_testgen_modes[] = {

31 "Disabled",

32 "Incrementing",

33 "Alternating 0x55/0xAA",

34 "All Zeros 0x00",

35 "All Ones 0xFF",

36 "Pseudo-random Data",

37 "User Specified",

38 "Complex pattern",

39 "Color box",

40 "Color bars",

41 NULL

42 };

44 u32 csid_find_code(u32 *codes, unsigned int ncodes,

45 unsigned int match_format_idx, u32 match_code)

46 {

47 int i;

49 if (!match_code && (match_format_idx >= ncodes))

50 return 0;

52 for (i = 0; i < ncodes; i++)

53 if (match_code) {

54 if (codes[i] == match_code)

55 return match_code;

56 } else {

57 if (i == match_format_idx)

58 return codes[i];

59 }

61 return codes[0];

62 }

64 const struct csid_format *csid_get_fmt_entry(const struct csid_format *formats,

65 unsigned int nformats,

66 u32 code)

67 {

68 unsigned int i;

70 for (i = 0; i < nformats; i++)

71 if (code == formats[i].code)

72 return &formats[i];

74 WARN(1, "Unknown format\n");

76 return &formats[0];

77 }

79 /*

80 * csid_set_clock_rates - Calculate and set clock rates on CSID module

81 * @csiphy: CSID device

82 */

83 static int csid_set_clock_rates(struct csid_device *csid)

84 {

85 struct device *dev = csid->camss->dev;

86 const struct csid_format *fmt;

87 s64 link_freq;

88 int i, j;

89 int ret;

91 fmt = csid_get_fmt_entry(csid->formats, csid->nformats,

92 csid->fmt[MSM_CSIPHY_PAD_SINK].code);

93 link_freq = camss_get_link_freq(&csid->subdev.entity, fmt->bpp,

94 csid->phy.lane_cnt);

95 if (link_freq < 0)

96 link_freq = 0;

98 for (i = 0; i < csid->nclocks; i++) {

99 struct camss_clock *clock = &csid->clock[i];

100

101 if (!strcmp(clock->name, "csi0") ||

102 !strcmp(clock->name, "csi1") ||

103 !strcmp(clock->name, "csi2") ||

104 !strcmp(clock->name, "csi3")) {

105 u64 min_rate = link_freq / 4;

106 long rate;

107

108 camss_add_clock_margin(&min_rate);

109

110 for (j = 0; j < clock->nfreqs; j++)

111 if (min_rate < clock->freq[j])

112 break;

113

114 if (j == clock->nfreqs) {

115 dev_err(dev,

116 "Pixel clock is too high for CSID\n");

117 return -EINVAL;

118 }

119

120 /* if sensor pixel clock is not available */

121 /* set highest possible CSID clock rate */

122 if (min_rate == 0)

123 j = clock->nfreqs - 1;

124

125 rate = clk_round_rate(clock->clk, clock->freq[j]);

126 if (rate < 0) {

127 dev_err(dev, "clk round rate failed: %ld\n",

128 rate);

129 return -EINVAL;

130 }

131

132 ret = clk_set_rate(clock->clk, rate);

133 if (ret < 0) {

134 dev_err(dev, "clk set rate failed: %d\n", ret);

135 return ret;

136 }

137 } else if (clock->nfreqs) {

138 clk_set_rate(clock->clk, clock->freq[0]);

139 }

140 }

141

142 return 0;

143 }

144

145 /*

146 * csid_set_power - Power on/off CSID module

147 * @sd: CSID V4L2 subdevice

148 * @on: Requested power state

149 *

150 * Return 0 on success or a negative error code otherwise

151 */

152 static int csid_set_power(struct v4l2_subdev *sd, int on)

153 {

154 struct csid_device *csid = v4l2_get_subdevdata(sd);

155 struct device *dev = csid->camss->dev;

156 int ret;

157

158 if (on) {

159 ret = pm_runtime_resume_and_get(dev);

160 if (ret < 0)

161 return ret;

162

163 ret = csid->vdda ? regulator_enable(csid->vdda) : 0;

164 if (ret < 0) {

165 pm_runtime_put_sync(dev);

166 return ret;

167 }

168

169 ret = csid_set_clock_rates(csid);

170 if (ret < 0) {

171 if (csid->vdda)

172 regulator_disable(csid->vdda);

173 pm_runtime_put_sync(dev);

174 return ret;

175 }

176

177 ret = camss_enable_clocks(csid->nclocks, csid->clock, dev);

178 if (ret < 0) {

179 if (csid->vdda)

180 regulator_disable(csid->vdda);

181 pm_runtime_put_sync(dev);

182 return ret;

183 }

184

185 enable_irq(csid->irq);

186

187 ret = csid->ops->reset(csid);

188 if (ret < 0) {

189 disable_irq(csid->irq);

190 camss_disable_clocks(csid->nclocks, csid->clock);

191 if (csid->vdda)

192 regulator_disable(csid->vdda);

193 pm_runtime_put_sync(dev);

194 return ret;

195 }

196

197 csid->ops->hw_version(csid);

198 } else {

199 disable_irq(csid->irq);

200 camss_disable_clocks(csid->nclocks, csid->clock);

201 ret = csid->vdda ? regulator_disable(csid->vdda) : 0;

202 pm_runtime_put_sync(dev);

203 }

204

205 return ret;

206 }

207

208 /*

209 * csid_set_stream - Enable/disable streaming on CSID module

210 * @sd: CSID V4L2 subdevice

211 * @enable: Requested streaming state

212 *

213 * Main configuration of CSID module is also done here.

214 *

215 * Return 0 on success or a negative error code otherwise

216 */

217 static int csid_set_stream(struct v4l2_subdev *sd, int enable)

218 {

219 struct csid_device *csid = v4l2_get_subdevdata(sd);

220 int ret;

221

222 if (enable) {

223 ret = v4l2_ctrl_handler_setup(&csid->ctrls);

224 if (ret < 0) {

225 dev_err(csid->camss->dev,

226 "could not sync v4l2 controls: %d\n", ret);

227 return ret;

228 }

229

230 if (!csid->testgen.enabled &&

231 !media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))

232 return -ENOLINK;

233 }

234

235 csid->ops->configure_stream(csid, enable);

236

237 return 0;

238 }

239

240 /*

241 * __csid_get_format - Get pointer to format structure

242 * @csid: CSID device

243 * @cfg: V4L2 subdev pad configuration

244 * @pad: pad from which format is requested

245 * @which: TRY or ACTIVE format

246 *

247 * Return pointer to TRY or ACTIVE format structure

248 */

249 static struct v4l2_mbus_framefmt *

250 __csid_get_format(struct csid_device *csid,

251 struct v4l2_subdev_state *sd_state,

252 unsigned int pad,

253 enum v4l2_subdev_format_whence which)

254 {

255 if (which == V4L2_SUBDEV_FORMAT_TRY)

256 return v4l2_subdev_get_try_format(&csid->subdev, sd_state,

257 pad);

258

259 return &csid->fmt[pad];

260 }

261

262 /*

263 * csid_try_format - Handle try format by pad subdev method

264 * @csid: CSID device

265 * @cfg: V4L2 subdev pad configuration

266 * @pad: pad on which format is requested

267 * @fmt: pointer to v4l2 format structure

268 * @which: wanted subdev format

269 */

270 static void csid_try_format(struct csid_device *csid,

271 struct v4l2_subdev_state *sd_state,

272 unsigned int pad,

273 struct v4l2_mbus_framefmt *fmt,

274 enum v4l2_subdev_format_whence which)

275 {

276 unsigned int i;

277

278 switch (pad) {

279 case MSM_CSID_PAD_SINK:

280 /* Set format on sink pad */

281

282 for (i = 0; i < csid->nformats; i++)

283 if (fmt->code == csid->formats[i].code)

284 break;

285

286 /* If not found, use UYVY as default */

287 if (i >= csid->nformats)

288 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;

289

290 fmt->width = clamp_t(u32, fmt->width, 1, 8191);

291 fmt->height = clamp_t(u32, fmt->height, 1, 8191);

292

293 fmt->field = V4L2_FIELD_NONE;

294 fmt->colorspace = V4L2_COLORSPACE_SRGB;

295

296 break;

297

298 case MSM_CSID_PAD_SRC:

299 if (csid->testgen_mode->cur.val == 0) {

300 /* Test generator is disabled, */

301 /* keep pad formats in sync */

302 u32 code = fmt->code;

303

304 *fmt = *__csid_get_format(csid, sd_state,

305 MSM_CSID_PAD_SINK, which);

306 fmt->code = csid->ops->src_pad_code(csid, fmt->code, 0, code);

307 } else {

308 /* Test generator is enabled, set format on source */

309 /* pad to allow test generator usage */

310

311 for (i = 0; i < csid->nformats; i++)

312 if (csid->formats[i].code == fmt->code)

313 break;

314

315 /* If not found, use UYVY as default */

316 if (i >= csid->nformats)

317 fmt->code = MEDIA_BUS_FMT_UYVY8_2X8;

318

319 fmt->width = clamp_t(u32, fmt->width, 1, 8191);

320 fmt->height = clamp_t(u32, fmt->height, 1, 8191);

321

322 fmt->field = V4L2_FIELD_NONE;

323 }

324 break;

325 }

326

327 fmt->colorspace = V4L2_COLORSPACE_SRGB;

328 }

329

330 /*

331 * csid_enum_mbus_code - Handle pixel format enumeration

332 * @sd: CSID V4L2 subdevice

333 * @cfg: V4L2 subdev pad configuration

334 * @code: pointer to v4l2_subdev_mbus_code_enum structure

335 * return -EINVAL or zero on success

336 */

337 static int csid_enum_mbus_code(struct v4l2_subdev *sd,

338 struct v4l2_subdev_state *sd_state,

339 struct v4l2_subdev_mbus_code_enum *code)

340 {

341 struct csid_device *csid = v4l2_get_subdevdata(sd);

342

343 if (code->pad == MSM_CSID_PAD_SINK) {

344 if (code->index >= csid->nformats)

345 return -EINVAL;

346

347 code->code = csid->formats[code->index].code;

348 } else {

349 if (csid->testgen_mode->cur.val == 0) {

350 struct v4l2_mbus_framefmt *sink_fmt;

351

352 sink_fmt = __csid_get_format(csid, sd_state,

353 MSM_CSID_PAD_SINK,

354 code->which);

355

356 code->code = csid->ops->src_pad_code(csid, sink_fmt->code,

357 code->index, 0);

358 if (!code->code)

359 return -EINVAL;

360 } else {

361 if (code->index >= csid->nformats)

362 return -EINVAL;

363

364 code->code = csid->formats[code->index].code;

365 }

366 }

367

368 return 0;

369 }

370

371 /*

372 * csid_enum_frame_size - Handle frame size enumeration

373 * @sd: CSID V4L2 subdevice

374 * @cfg: V4L2 subdev pad configuration

375 * @fse: pointer to v4l2_subdev_frame_size_enum structure

376 * return -EINVAL or zero on success

377 */

378 static int csid_enum_frame_size(struct v4l2_subdev *sd,

379 struct v4l2_subdev_state *sd_state,

380 struct v4l2_subdev_frame_size_enum *fse)

381 {

382 struct csid_device *csid = v4l2_get_subdevdata(sd);

383 struct v4l2_mbus_framefmt format;

384

385 if (fse->index != 0)

386 return -EINVAL;

387

388 format.code = fse->code;

389 format.width = 1;

390 format.height = 1;

391 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);

392 fse->min_width = format.width;

393 fse->min_height = format.height;

394

395 if (format.code != fse->code)

396 return -EINVAL;

397

398 format.code = fse->code;

399 format.width = -1;

400 format.height = -1;

401 csid_try_format(csid, sd_state, fse->pad, &format, fse->which);

402 fse->max_width = format.width;

403 fse->max_height = format.height;

404

405 return 0;

406 }

407

408 /*

409 * csid_get_format - Handle get format by pads subdev method

410 * @sd: CSID V4L2 subdevice

411 * @cfg: V4L2 subdev pad configuration

412 * @fmt: pointer to v4l2 subdev format structure

413 *

414 * Return -EINVAL or zero on success

415 */

416 static int csid_get_format(struct v4l2_subdev *sd,

417 struct v4l2_subdev_state *sd_state,

418 struct v4l2_subdev_format *fmt)

419 {

420 struct csid_device *csid = v4l2_get_subdevdata(sd);

421 struct v4l2_mbus_framefmt *format;

422

423 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);

424 if (format == NULL)

425 return -EINVAL;

426

427 fmt->format = *format;

428

429 return 0;

430 }

431

432 /*

433 * csid_set_format - Handle set format by pads subdev method

434 * @sd: CSID V4L2 subdevice

435 * @cfg: V4L2 subdev pad configuration

436 * @fmt: pointer to v4l2 subdev format structure

437 *

438 * Return -EINVAL or zero on success

439 */

440 static int csid_set_format(struct v4l2_subdev *sd,

441 struct v4l2_subdev_state *sd_state,

442 struct v4l2_subdev_format *fmt)

443 {

444 struct csid_device *csid = v4l2_get_subdevdata(sd);

445 struct v4l2_mbus_framefmt *format;

446

447 format = __csid_get_format(csid, sd_state, fmt->pad, fmt->which);

448 if (format == NULL)

449 return -EINVAL;

450

451 csid_try_format(csid, sd_state, fmt->pad, &fmt->format, fmt->which);

452 *format = fmt->format;

453

454 /* Propagate the format from sink to source */

455 if (fmt->pad == MSM_CSID_PAD_SINK) {

456 format = __csid_get_format(csid, sd_state, MSM_CSID_PAD_SRC,

457 fmt->which);

458

459 *format = fmt->format;

460 csid_try_format(csid, sd_state, MSM_CSID_PAD_SRC, format,

461 fmt->which);

462 }

463

464 return 0;

465 }

466

467 /*

468 * csid_init_formats - Initialize formats on all pads

469 * @sd: CSID V4L2 subdevice

470 * @fh: V4L2 subdev file handle

471 *

472 * Initialize all pad formats with default values.

473 *

474 * Return 0 on success or a negative error code otherwise

475 */

476 static int csid_init_formats(struct v4l2_subdev *sd, struct v4l2_subdev_fh *fh)

477 {

478 struct v4l2_subdev_format format = {

479 .pad = MSM_CSID_PAD_SINK,

480 .which = fh ? V4L2_SUBDEV_FORMAT_TRY :

481 V4L2_SUBDEV_FORMAT_ACTIVE,

482 .format = {

483 .code = MEDIA_BUS_FMT_UYVY8_2X8,

484 .width = 1920,

485 .height = 1080

486 }

487 };

488

489 return csid_set_format(sd, fh ? fh->state : NULL, &format);

490 }

491

492 /*

493 * csid_set_test_pattern - Set test generator's pattern mode

494 * @csid: CSID device

495 * @value: desired test pattern mode

496 *

497 * Return 0 on success or a negative error code otherwise

498 */

499 static int csid_set_test_pattern(struct csid_device *csid, s32 value)

500 {

501 struct csid_testgen_config *tg = &csid->testgen;

502

503 /* If CSID is linked to CSIPHY, do not allow to enable test generator */

504 if (value && media_entity_remote_pad(&csid->pads[MSM_CSID_PAD_SINK]))

505 return -EBUSY;

506

507 tg->enabled = !!value;

508

509 return csid->ops->configure_testgen_pattern(csid, value);

510 }

511

512 /*

513 * csid_s_ctrl - Handle set control subdev method

514 * @ctrl: pointer to v4l2 control structure

515 *

516 * Return 0 on success or a negative error code otherwise

517 */

518 static int csid_s_ctrl(struct v4l2_ctrl *ctrl)

519 {

520 struct csid_device *csid = container_of(ctrl->handler,

521 struct csid_device, ctrls);

522 int ret = -EINVAL;

523

524 switch (ctrl->id) {

525 case V4L2_CID_TEST_PATTERN:

526 ret = csid_set_test_pattern(csid, ctrl->val);

527 break;

528 }

529

530 return ret;

531 }

532

533 static const struct v4l2_ctrl_ops csid_ctrl_ops = {

534 .s_ctrl = csid_s_ctrl,

535 };

536

537 /*

538 * msm_csid_subdev_init - Initialize CSID device structure and resources

539 * @csid: CSID device

540 * @res: CSID module resources table

541 * @id: CSID module id

542 *

543 * Return 0 on success or a negative error code otherwise

544 */

545 int msm_csid_subdev_init(struct camss *camss, struct csid_device *csid,

546 const struct resources *res, u8 id)

547 {

548 struct device *dev = camss->dev;

549 struct platform_device *pdev = to_platform_device(dev);

550 struct resource *r;

551 int i, j;

552 int ret;

553

554 csid->camss = camss;

555 csid->id = id;

556

557 if (camss->version == CAMSS_8x16) {

558 csid->ops = &csid_ops_4_1;

559 } else if (camss->version == CAMSS_8x96 ||

560 camss->version == CAMSS_660) {

561 csid->ops = &csid_ops_4_7;

562 } else if (camss->version == CAMSS_845) {

563 csid->ops = &csid_ops_170;

564 } else {

565 return -EINVAL;

566 }

567 csid->ops->subdev_init(csid);

568

569 /* Memory */

570

571 csid->base = devm_platform_ioremap_resource_byname(pdev, res->reg[0]);

572 if (IS_ERR(csid->base))

573 return PTR_ERR(csid->base);

574

575 /* Interrupt */

576

577 r = platform_get_resource_byname(pdev, IORESOURCE_IRQ,

578 res->interrupt[0]);

579 if (!r) {

580 dev_err(dev, "missing IRQ\n");

581 return -EINVAL;

582 }

583

584 csid->irq = r->start;

585 snprintf(csid->irq_name, sizeof(csid->irq_name), "%s_%s%d",

586 dev_name(dev), MSM_CSID_NAME, csid->id);

587 ret = devm_request_irq(dev, csid->irq, csid->ops->isr,

588 IRQF_TRIGGER_RISING | IRQF_NO_AUTOEN,

589 csid->irq_name, csid);

590 if (ret < 0) {

591 dev_err(dev, "request_irq failed: %d\n", ret);

592 return ret;

593 }

594

595 /* Clocks */

596

597 csid->nclocks = 0;

598 while (res->clock[csid->nclocks])

599 csid->nclocks++;

600

601 csid->clock = devm_kcalloc(dev, csid->nclocks, sizeof(*csid->clock),

602 GFP_KERNEL);

603 if (!csid->clock)

604 return -ENOMEM;

605

606 for (i = 0; i < csid->nclocks; i++) {

607 struct camss_clock *clock = &csid->clock[i];

608

609 clock->clk = devm_clk_get(dev, res->clock[i]);

610 if (IS_ERR(clock->clk))

611 return PTR_ERR(clock->clk);

612

613 clock->name = res->clock[i];

614

615 clock->nfreqs = 0;

616 while (res->clock_rate[i][clock->nfreqs])

617 clock->nfreqs++;

618

619 if (!clock->nfreqs) {

620 clock->freq = NULL;

621 continue;

622 }

623

624 clock->freq = devm_kcalloc(dev,

625 clock->nfreqs,

626 sizeof(*clock->freq),

627 GFP_KERNEL);

628 if (!clock->freq)

629 return -ENOMEM;

630

631 for (j = 0; j < clock->nfreqs; j++)

632 clock->freq[j] = res->clock_rate[i][j];

633 }

634

635 /* Regulator */

636

637 csid->vdda = NULL;

638 if (res->regulator[0])

639 csid->vdda = devm_regulator_get(dev, res->regulator[0]);

640 if (IS_ERR(csid->vdda)) {

641 dev_err(dev, "could not get regulator\n");

642 return PTR_ERR(csid->vdda);

643 }

644

645 init_completion(&csid->reset_complete);

646

647 return 0;

648 }

649

650 /*

651 * msm_csid_get_csid_id - Get CSID HW module id

652 * @entity: Pointer to CSID media entity structure

653 * @id: Return CSID HW module id here

654 */

655 void msm_csid_get_csid_id(struct media_entity *entity, u8 *id)

656 {

657 struct v4l2_subdev *sd = media_entity_to_v4l2_subdev(entity);

658 struct csid_device *csid = v4l2_get_subdevdata(sd);

659

660 *id = csid->id;

661 }

662

663 /*

664 * csid_get_lane_assign - Calculate CSI2 lane assign configuration parameter

665 * @lane_cfg - CSI2 lane configuration

666 *

667 * Return lane assign

668 */

669 static u32 csid_get_lane_assign(struct csiphy_lanes_cfg *lane_cfg)

670 {

671 u32 lane_assign = 0;

672 int i;

673

674 for (i = 0; i < lane_cfg->num_data; i++)

675 lane_assign |= lane_cfg->data[i].pos << (i * 4);

676

677 return lane_assign;

678 }

679

680 /*

681 * csid_link_setup - Setup CSID connections

682 * @entity: Pointer to media entity structure

683 * @local: Pointer to local pad

684 * @remote: Pointer to remote pad

685 * @flags: Link flags

686 *

687 * Return 0 on success

688 */

689 static int csid_link_setup(struct media_entity *entity,

690 const struct media_pad *local,

691 const struct media_pad *remote, u32 flags)

692 {

693 if (flags & MEDIA_LNK_FL_ENABLED)

694 if (media_entity_remote_pad(local))

695 return -EBUSY;

696

697 if ((local->flags & MEDIA_PAD_FL_SINK) &&

698 (flags & MEDIA_LNK_FL_ENABLED)) {

699 struct v4l2_subdev *sd;

700 struct csid_device *csid;

701 struct csiphy_device *csiphy;

702 struct csiphy_lanes_cfg *lane_cfg;

703 struct v4l2_subdev_format format = { 0 };

704

705 sd = media_entity_to_v4l2_subdev(entity);

706 csid = v4l2_get_subdevdata(sd);

707

708 /* If test generator is enabled */

709 /* do not allow a link from CSIPHY to CSID */

710 if (csid->testgen_mode->cur.val != 0)

711 return -EBUSY;

712

713 sd = media_entity_to_v4l2_subdev(remote->entity);

714 csiphy = v4l2_get_subdevdata(sd);

715

716 /* If a sensor is not linked to CSIPHY */

717 /* do no allow a link from CSIPHY to CSID */

718 if (!csiphy->cfg.csi2)

719 return -EPERM;

720

721 csid->phy.csiphy_id = csiphy->id;

722

723 lane_cfg = &csiphy->cfg.csi2->lane_cfg;

724 csid->phy.lane_cnt = lane_cfg->num_data;

725 csid->phy.lane_assign = csid_get_lane_assign(lane_cfg);

726

727 /* Reset format on source pad to sink pad format */

728 format.pad = MSM_CSID_PAD_SRC;

729 format.which = V4L2_SUBDEV_FORMAT_ACTIVE;

730 csid_set_format(&csid->subdev, NULL, &format);

731 }

732

733 return 0;

734 }

735

736 static const struct v4l2_subdev_core_ops csid_core_ops = {

737 .s_power = csid_set_power,

738 .subscribe_event = v4l2_ctrl_subdev_subscribe_event,

739 .unsubscribe_event = v4l2_event_subdev_unsubscribe,

740 };

741

742 static const struct v4l2_subdev_video_ops csid_video_ops = {

743 .s_stream = csid_set_stream,

744 };

745

746 static const struct v4l2_subdev_pad_ops csid_pad_ops = {

747 .enum_mbus_code = csid_enum_mbus_code,

748 .enum_frame_size = csid_enum_frame_size,

749 .get_fmt = csid_get_format,

750 .set_fmt = csid_set_format,

751 };

752

753 static const struct v4l2_subdev_ops csid_v4l2_ops = {

754 .core = &csid_core_ops,

755 .video = &csid_video_ops,

756 .pad = &csid_pad_ops,

757 };

758

759 static const struct v4l2_subdev_internal_ops csid_v4l2_internal_ops = {

760 .open = csid_init_formats,

761 };

762

763 static const struct media_entity_operations csid_media_ops = {

764 .link_setup = csid_link_setup,

765 .link_validate = v4l2_subdev_link_validate,

766 };

767

768 /*

769 * msm_csid_register_entity - Register subdev node for CSID module

770 * @csid: CSID device

771 * @v4l2_dev: V4L2 device

772 *

773 * Return 0 on success or a negative error code otherwise

774 */

775 int msm_csid_register_entity(struct csid_device *csid,

776 struct v4l2_device *v4l2_dev)

777 {

778 struct v4l2_subdev *sd = &csid->subdev;

779 struct media_pad *pads = csid->pads;

780 struct device *dev = csid->camss->dev;

781 int ret;

782

783 v4l2_subdev_init(sd, &csid_v4l2_ops);

784 sd->internal_ops = &csid_v4l2_internal_ops;

785 sd->flags |= V4L2_SUBDEV_FL_HAS_DEVNODE |

786 V4L2_SUBDEV_FL_HAS_EVENTS;

787 snprintf(sd->name, ARRAY_SIZE(sd->name), "%s%d",

788 MSM_CSID_NAME, csid->id);

789 v4l2_set_subdevdata(sd, csid);

790

791 ret = v4l2_ctrl_handler_init(&csid->ctrls, 1);

792 if (ret < 0) {

793 dev_err(dev, "Failed to init ctrl handler: %d\n", ret);

794 return ret;

795 }

796

797 csid->testgen_mode = v4l2_ctrl_new_std_menu_items(&csid->ctrls,

798 &csid_ctrl_ops, V4L2_CID_TEST_PATTERN,

799 csid->testgen.nmodes, 0, 0,

800 csid->testgen.modes);

801

802 if (csid->ctrls.error) {

803 dev_err(dev, "Failed to init ctrl: %d\n", csid->ctrls.error);

804 ret = csid->ctrls.error;

805 goto free_ctrl;

806 }

807

808 csid->subdev.ctrl_handler = &csid->ctrls;

809

810 ret = csid_init_formats(sd, NULL);

811 if (ret < 0) {

812 dev_err(dev, "Failed to init format: %d\n", ret);

813 goto free_ctrl;

814 }

815

816 pads[MSM_CSID_PAD_SINK].flags = MEDIA_PAD_FL_SINK;

817 pads[MSM_CSID_PAD_SRC].flags = MEDIA_PAD_FL_SOURCE;

818

819 sd->entity.function = MEDIA_ENT_F_PROC_VIDEO_PIXEL_FORMATTER;

820 sd->entity.ops = &csid_media_ops;

821 ret = media_entity_pads_init(&sd->entity, MSM_CSID_PADS_NUM, pads);

822 if (ret < 0) {

823 dev_err(dev, "Failed to init media entity: %d\n", ret);

824 goto free_ctrl;

825 }

826

827 ret = v4l2_device_register_subdev(v4l2_dev, sd);

828 if (ret < 0) {

829 dev_err(dev, "Failed to register subdev: %d\n", ret);

830 goto media_cleanup;

831 }

832

833 return 0;

834

835 media_cleanup:

836 media_entity_cleanup(&sd->entity);

837 free_ctrl:

838 v4l2_ctrl_handler_free(&csid->ctrls);

839

840 return ret;

841 }

842

843 /*

844 * msm_csid_unregister_entity - Unregister CSID module subdev node

845 * @csid: CSID device

846 */

847 void msm_csid_unregister_entity(struct csid_device *csid)

848 {

849 v4l2_device_unregister_subdev(&csid->subdev);

850 media_entity_cleanup(&csid->subdev.entity);

851 v4l2_ctrl_handler_free(&csid->ctrls);

852 }