1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2020 Rockchip Electronics Co. Ltd.
4 *
5 * Author: Wyon Bi <bivvy.bi@rock-chips.com>
6 */
7
8 #include <linux/module.h>
9 #include <linux/clk.h>
10 #include <linux/platform_device.h>
11 #include <linux/of.h>
12 #include <linux/regmap.h>
13 #include <linux/reset.h>
14 #include <linux/mfd/rk628.h>
15 #include <video/of_display_timing.h>
16 #include <video/videomode.h>
17
18 #include <drm/drm_of.h>
19
20 enum rk628_mode_sync_pol {
21 MODE_FLAG_NSYNC,
22 MODE_FLAG_PSYNC,
23 };
24
25 struct rk628_post_process {
26 struct drm_bridge base;
27 struct drm_bridge *bridge;
28 struct drm_display_mode src_mode;
29 struct drm_display_mode dst_mode;
30 struct device *dev;
31 struct regmap *grf;
32 struct clk *sclk_vop;
33 struct clk *clk_rx_read;
34 struct reset_control *rstc_decoder;
35 struct reset_control *rstc_clk_rx;
36 struct reset_control *rstc_vop;
37 struct rk628 *parent;
38 int sync_pol;
39 };
40
bridge_to_pp(struct drm_bridge * bridge)41 static inline struct rk628_post_process *bridge_to_pp(struct drm_bridge *bridge)
42 {
43 return container_of(bridge, struct rk628_post_process, base);
44 }
45
calc_dsp_frm_hst_vst(const struct videomode * src,const struct videomode * dst,u32 * dsp_frame_hst,u32 * dsp_frame_vst)46 static void calc_dsp_frm_hst_vst(const struct videomode *src,
47 const struct videomode *dst,
48 u32 *dsp_frame_hst, u32 *dsp_frame_vst)
49 {
50 u32 bp_in, bp_out;
51 u32 v_scale_ratio;
52 u64 t_frm_st;
53 u64 t_bp_in, t_bp_out, t_delta, tin;
54 u32 src_pixclock, dst_pixclock;
55 u32 dsp_htotal, src_htotal, src_vtotal;
56
57 src_pixclock = div_u64(1000000000000llu, src->pixelclock);
58 dst_pixclock = div_u64(1000000000000llu, dst->pixelclock);
59
60 src_htotal = src->hsync_len + src->hback_porch + src->hactive +
61 src->hfront_porch;
62 src_vtotal = src->vsync_len + src->vback_porch + src->vactive +
63 src->vfront_porch;
64 dsp_htotal = dst->hsync_len + dst->hback_porch + dst->hactive +
65 dst->hfront_porch;
66
67 bp_in = (src->vback_porch + src->vsync_len) * src_htotal +
68 src->hsync_len + src->hback_porch;
69 bp_out = (dst->vback_porch + dst->vsync_len) * dsp_htotal +
70 dst->hsync_len + dst->hback_porch;
71
72 t_bp_in = bp_in * src_pixclock;
73 t_bp_out = bp_out * dst_pixclock;
74 tin = src_vtotal * src_htotal * src_pixclock;
75
76 v_scale_ratio = src->vactive / dst->vactive;
77 if (v_scale_ratio <= 2)
78 t_delta = 5 * src_htotal * src_pixclock;
79 else
80 t_delta = 12 * src_htotal * src_pixclock;
81
82 if (t_bp_in + t_delta > t_bp_out)
83 t_frm_st = (t_bp_in + t_delta - t_bp_out);
84 else
85 t_frm_st = tin - (t_bp_out - (t_bp_in + t_delta));
86
87 do_div(t_frm_st, src_pixclock);
88 *dsp_frame_hst = do_div(t_frm_st, src_htotal);
89 *dsp_frame_vst = t_frm_st;
90 }
91
rk628_post_process_scaler_init(struct rk628_post_process * pp,const struct drm_display_mode * s,const struct drm_display_mode * d)92 static void rk628_post_process_scaler_init(struct rk628_post_process *pp,
93 const struct drm_display_mode *s,
94 const struct drm_display_mode *d)
95 {
96 struct videomode src, dst;
97 u32 dsp_frame_hst, dsp_frame_vst;
98 u32 scl_hor_mode, scl_ver_mode;
99 u32 scl_v_factor, scl_h_factor;
100 u32 dsp_htotal, dsp_hs_end, dsp_hact_st, dsp_hact_end;
101 u32 dsp_vtotal, dsp_vs_end, dsp_vact_st, dsp_vact_end;
102 u32 dsp_hbor_end, dsp_hbor_st, dsp_vbor_end, dsp_vbor_st;
103 u16 bor_right = 0, bor_left = 0, bor_up = 0, bor_down = 0;
104 u8 hor_down_mode = 0, ver_down_mode = 0;
105
106 drm_display_mode_to_videomode(s, &src);
107 drm_display_mode_to_videomode(d, &dst);
108
109 dsp_htotal = dst.hsync_len + dst.hback_porch + dst.hactive +
110 dst.hfront_porch;
111 dsp_vtotal = dst.vsync_len + dst.vback_porch + dst.vactive +
112 dst.vfront_porch;
113 dsp_hs_end = dst.hsync_len;
114 dsp_vs_end = dst.vsync_len;
115 dsp_hbor_end = dst.hsync_len + dst.hback_porch + dst.hactive;
116 dsp_hbor_st = dst.hsync_len + dst.hback_porch;
117 dsp_vbor_end = dst.vsync_len + dst.vback_porch + dst.vactive;
118 dsp_vbor_st = dst.vsync_len + dst.vback_porch;
119 dsp_hact_st = dsp_hbor_st + bor_left;
120 dsp_hact_end = dsp_hbor_end - bor_right;
121 dsp_vact_st = dsp_vbor_st + bor_up;
122 dsp_vact_end = dsp_vbor_end - bor_down;
123
124 calc_dsp_frm_hst_vst(&src, &dst, &dsp_frame_hst, &dsp_frame_vst);
125 dev_dbg(pp->dev, "dsp_frame_vst=%d, dsp_frame_hst=%d\n",
126 dsp_frame_vst, dsp_frame_hst);
127
128 if (src.hactive > dst.hactive) {
129 scl_hor_mode = 2;
130
131 if (hor_down_mode == 0) {
132 if ((src.hactive - 1) / (dst.hactive - 1) > 2)
133 scl_h_factor = ((src.hactive - 1) << 14) /
134 (dst.hactive - 1);
135 else
136 scl_h_factor = ((src.hactive - 2) << 14) /
137 (dst.hactive - 1);
138 } else {
139 scl_h_factor = (dst.hactive << 16) /
140 (src.hactive - 1);
141 }
142
143 dev_dbg(pp->dev, "horizontal scale down\n");
144 } else if (src.hactive == dst.hactive) {
145 scl_hor_mode = 0;
146 scl_h_factor = 0;
147
148 dev_dbg(pp->dev, "horizontal no scale\n");
149 } else {
150 scl_hor_mode = 1;
151 scl_h_factor = ((src.hactive - 1) << 16) / (dst.hactive - 1);
152
153 dev_dbg(pp->dev, "horizontal scale up\n");
154 }
155
156 if (src.vactive > dst.vactive) {
157 scl_ver_mode = 2;
158
159 if (ver_down_mode == 0) {
160 if ((src.vactive - 1) / (dst.vactive - 1) > 2)
161 scl_v_factor = ((src.vactive - 1) << 14) /
162 (dst.vactive - 1);
163 else
164 scl_v_factor = ((src.vactive - 2) << 14) /
165 (dst.vactive - 1);
166 } else {
167 scl_v_factor = (dst.vactive << 16) /
168 (src.vactive - 1);
169 }
170
171 dev_dbg(pp->dev, "vertical scale down\n");
172 } else if (src.vactive == dst.vactive) {
173 scl_ver_mode = 0;
174 scl_v_factor = 0;
175
176 dev_dbg(pp->dev, "vertical no scale\n");
177 } else {
178 scl_ver_mode = 1;
179 scl_v_factor = ((src.vactive - 1) << 16) / (dst.vactive - 1);
180
181 dev_dbg(pp->dev, "vertical scale up\n");
182 }
183
184 regmap_update_bits(pp->grf, GRF_RGB_DEC_CON0,
185 SW_HRES_MASK, SW_HRES(src.hactive));
186 regmap_write(pp->grf, GRF_SCALER_CON0,
187 SCL_VER_DOWN_MODE(ver_down_mode) |
188 SCL_HOR_DOWN_MODE(hor_down_mode) |
189 SCL_VER_MODE(scl_ver_mode) | SCL_HOR_MODE(scl_hor_mode));
190 regmap_write(pp->grf, GRF_SCALER_CON1,
191 SCL_V_FACTOR(scl_v_factor) | SCL_H_FACTOR(scl_h_factor));
192 regmap_write(pp->grf, GRF_SCALER_CON2,
193 DSP_FRAME_VST(dsp_frame_vst) |
194 DSP_FRAME_HST(dsp_frame_hst));
195 regmap_write(pp->grf, GRF_SCALER_CON3,
196 DSP_HS_END(dsp_hs_end) | DSP_HTOTAL(dsp_htotal));
197 regmap_write(pp->grf, GRF_SCALER_CON4,
198 DSP_HACT_END(dsp_hact_end) | DSP_HACT_ST(dsp_hact_st));
199 regmap_write(pp->grf, GRF_SCALER_CON5,
200 DSP_VS_END(dsp_vs_end) | DSP_VTOTAL(dsp_vtotal));
201 regmap_write(pp->grf, GRF_SCALER_CON6,
202 DSP_VACT_END(dsp_vact_end) | DSP_VACT_ST(dsp_vact_st));
203 regmap_write(pp->grf, GRF_SCALER_CON7,
204 DSP_HBOR_END(dsp_hbor_end) | DSP_HBOR_ST(dsp_hbor_st));
205 regmap_write(pp->grf, GRF_SCALER_CON8,
206 DSP_VBOR_END(dsp_vbor_end) | DSP_VBOR_ST(dsp_vbor_st));
207 }
208
rk628_post_process_bridge_pre_enable(struct drm_bridge * bridge)209 static void rk628_post_process_bridge_pre_enable(struct drm_bridge *bridge)
210 {
211 struct rk628_post_process *pp = bridge_to_pp(bridge);
212 struct drm_display_mode *src = &pp->src_mode;
213 struct drm_display_mode *dst = &pp->dst_mode;
214 u64 dst_rate, src_rate;
215
216 reset_control_assert(pp->rstc_decoder);
217 udelay(10);
218 reset_control_deassert(pp->rstc_decoder);
219 udelay(10);
220
221 clk_set_rate(pp->clk_rx_read, src->clock * 1000);
222 clk_prepare_enable(pp->clk_rx_read);
223 reset_control_assert(pp->rstc_clk_rx);
224 udelay(10);
225 reset_control_deassert(pp->rstc_clk_rx);
226 udelay(10);
227
228 src_rate = src->clock * 1000;
229 dst_rate = src_rate * dst->vdisplay * dst->htotal;
230 do_div(dst_rate, src->vdisplay * src->htotal);
231 do_div(dst_rate, 1000);
232 dst->clock = dst_rate;
233
234 clk_set_rate(pp->sclk_vop, dst->clock * 1000);
235 clk_prepare_enable(pp->sclk_vop);
236 reset_control_assert(pp->rstc_vop);
237 udelay(10);
238 reset_control_deassert(pp->rstc_vop);
239 udelay(10);
240
241 regmap_update_bits(pp->grf, GRF_SYSTEM_CON0, SW_VSYNC_POL_MASK,
242 SW_VSYNC_POL(pp->sync_pol));
243 regmap_update_bits(pp->grf, GRF_SYSTEM_CON0, SW_HSYNC_POL_MASK,
244 SW_HSYNC_POL(pp->sync_pol));
245
246 rk628_post_process_scaler_init(pp, src, dst);
247 }
248
rk628_post_process_bridge_post_disable(struct drm_bridge * bridge)249 static void rk628_post_process_bridge_post_disable(struct drm_bridge *bridge)
250 {
251
252 }
253
rk628_post_process_bridge_enable(struct drm_bridge * bridge)254 static void rk628_post_process_bridge_enable(struct drm_bridge *bridge)
255 {
256 struct rk628_post_process *pp = bridge_to_pp(bridge);
257
258 regmap_write(pp->grf, GRF_SCALER_CON0, SCL_EN(1));
259 }
260
rk628_post_process_bridge_disable(struct drm_bridge * bridge)261 static void rk628_post_process_bridge_disable(struct drm_bridge *bridge)
262 {
263 struct rk628_post_process *pp = bridge_to_pp(bridge);
264
265 regmap_write(pp->grf, GRF_SCALER_CON0, SCL_EN(0));
266
267 clk_disable_unprepare(pp->sclk_vop);
268 clk_disable_unprepare(pp->clk_rx_read);
269 }
270
rk628_post_process_bridge_mode_set(struct drm_bridge * bridge,const struct drm_display_mode * mode,const struct drm_display_mode * adj)271 static void rk628_post_process_bridge_mode_set(struct drm_bridge *bridge,
272 const struct drm_display_mode *mode,
273 const struct drm_display_mode *adj)
274 {
275 struct rk628_post_process *pp = bridge_to_pp(bridge);
276 struct rk628 *rk628 = pp->parent;
277
278 drm_mode_copy(&pp->src_mode, adj);
279
280 if (rk628->dst_mode_valid)
281 drm_mode_copy(&pp->dst_mode, &rk628->dst_mode);
282 else
283 drm_mode_copy(&pp->dst_mode, &pp->src_mode);
284
285 /* hdmirx 4k-60Hz mode only support yuv420 */
286 if (pp->src_mode.clock == 594000)
287 regmap_write(pp->grf, GRF_CSC_CTRL_CON, SW_Y2R_EN(1));
288 }
289
rk628_post_process_bridge_attach(struct drm_bridge * bridge,enum drm_bridge_attach_flags flags)290 static int rk628_post_process_bridge_attach(struct drm_bridge *bridge,
291 enum drm_bridge_attach_flags flags)
292 {
293 struct rk628_post_process *pp = bridge_to_pp(bridge);
294 struct device *dev = pp->dev;
295 int ret;
296
297 ret = drm_of_find_panel_or_bridge(dev->of_node, 1, -1,
298 NULL, &pp->bridge);
299 if (ret)
300 return ret;
301
302 ret = drm_bridge_attach(bridge->encoder, pp->bridge, bridge, flags);
303 if (ret) {
304 dev_err(dev, "failed to attach bridge\n");
305 return ret;
306 }
307
308 return 0;
309 }
310
311 static bool
rk628_post_process_bridge_mode_fixup(struct drm_bridge * bridge,const struct drm_display_mode * mode,struct drm_display_mode * adj)312 rk628_post_process_bridge_mode_fixup(struct drm_bridge *bridge,
313 const struct drm_display_mode *mode,
314 struct drm_display_mode *adj)
315 {
316 struct rk628_post_process *pp = bridge_to_pp(bridge);
317
318 if (pp->sync_pol == MODE_FLAG_NSYNC) {
319 adj->flags &= ~(DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC);
320 adj->flags |= (DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC);
321 } else {
322 adj->flags &= ~(DRM_MODE_FLAG_NHSYNC | DRM_MODE_FLAG_NVSYNC);
323 adj->flags |= (DRM_MODE_FLAG_PHSYNC | DRM_MODE_FLAG_PVSYNC);
324 }
325
326 return true;
327 }
328
329 static const struct drm_bridge_funcs rk628_post_process_bridge_funcs = {
330 .pre_enable = rk628_post_process_bridge_pre_enable,
331 .post_disable = rk628_post_process_bridge_post_disable,
332 .enable = rk628_post_process_bridge_enable,
333 .disable = rk628_post_process_bridge_disable,
334 .mode_set = rk628_post_process_bridge_mode_set,
335 .mode_fixup = rk628_post_process_bridge_mode_fixup,
336 .attach = rk628_post_process_bridge_attach,
337 };
338
339
340 /**
341 * rk628_scaler_add_src_mode - add source mode for scaler
342 * @rk628: parent device
343 * @connector: DRM connector
344 * If need scale, call the function at last of get_modes.
345 */
rk628_scaler_add_src_mode(struct rk628 * rk628,struct drm_connector * connector)346 int rk628_scaler_add_src_mode(struct rk628 *rk628,
347 struct drm_connector *connector)
348 {
349 struct drm_display_mode *pmode;
350 struct drm_display_mode *dst;
351
352 if (!rk628 || !connector)
353 return 0;
354
355 if (drm_mode_validate_driver(connector->dev, &rk628->src_mode) !=
356 MODE_OK)
357 return 0;
358
359 list_for_each_entry(pmode, &connector->probed_modes, head) {
360 if (pmode->type & DRM_MODE_TYPE_PREFERRED) {
361 drm_mode_copy(&rk628->dst_mode, pmode);
362 drm_mode_copy(pmode, &rk628->src_mode);
363 pmode->type |= DRM_MODE_TYPE_PREFERRED;
364 rk628->dst_mode_valid = true;
365 break;
366 }
367 }
368 if (rk628->dst_mode_valid) {
369 dst = drm_mode_duplicate(connector->dev, &rk628->dst_mode);
370 dst->type &= ~DRM_MODE_TYPE_PREFERRED;
371 drm_mode_probed_add(connector, dst);
372 return 1;
373 }
374
375 return 0;
376 }
377 EXPORT_SYMBOL(rk628_scaler_add_src_mode);
378
379 /**
380 * rk628_mode_copy - rk628 mode copy
381 * @rk628: parent device
382 * @dst: dst mode
383 * @src: src mode
384 * Call the function at mode_set, replace drm_mode_copy.
385 */
rk628_mode_copy(struct rk628 * rk628,struct drm_display_mode * dst,const struct drm_display_mode * src)386 void rk628_mode_copy(struct rk628 *rk628, struct drm_display_mode *dst,
387 const struct drm_display_mode *src)
388 {
389 if (rk628->dst_mode_valid)
390 drm_mode_copy(dst, &rk628->dst_mode);
391 else
392 drm_mode_copy(dst, src);
393 }
394 EXPORT_SYMBOL(rk628_mode_copy);
395
rk628_post_process_probe(struct platform_device * pdev)396 static int rk628_post_process_probe(struct platform_device *pdev)
397 {
398 struct rk628 *rk628 = dev_get_drvdata(pdev->dev.parent);
399 struct device *dev = &pdev->dev;
400 struct rk628_post_process *pp;
401 u32 bus_flags;
402 u32 val;
403 int ret;
404
405 if (!of_device_is_available(dev->of_node))
406 return -ENODEV;
407
408 pp = devm_kzalloc(dev, sizeof(*pp), GFP_KERNEL);
409 if (!pp)
410 return -ENOMEM;
411
412 pp->dev = dev;
413 pp->grf = rk628->grf;
414 platform_set_drvdata(pdev, pp);
415 pp->parent = rk628;
416
417 pp->sclk_vop = devm_clk_get(dev, "sclk_vop");
418 if (IS_ERR(pp->sclk_vop)) {
419 ret = PTR_ERR(pp->sclk_vop);
420 dev_err(dev, "failed to get sclk: %d\n", ret);
421 return ret;
422 }
423
424 pp->clk_rx_read = devm_clk_get(dev, "rx_read");
425 if (IS_ERR(pp->clk_rx_read)) {
426 ret = PTR_ERR(pp->clk_rx_read);
427 dev_err(dev, "failed to get clk_rx_read: %d\n", ret);
428 return ret;
429 }
430
431 pp->rstc_decoder = of_reset_control_get(dev->of_node, "decoder");
432 if (IS_ERR(pp->rstc_decoder)) {
433 ret = PTR_ERR(pp->rstc_decoder);
434 dev_err(dev, "failed to get decoder reset: %d\n", ret);
435 return ret;
436 }
437
438 pp->rstc_clk_rx = of_reset_control_get(dev->of_node, "clk_rx");
439 if (IS_ERR(pp->rstc_clk_rx)) {
440 ret = PTR_ERR(pp->rstc_clk_rx);
441 dev_err(dev, "failed to get clk_rx reset: %d\n", ret);
442 return ret;
443 }
444
445 pp->rstc_vop = of_reset_control_get(dev->of_node, "vop");
446 if (IS_ERR(pp->rstc_vop)) {
447 ret = PTR_ERR(pp->rstc_vop);
448 dev_err(dev, "failed to get vop reset: %d\n", ret);
449 return ret;
450 }
451
452 ret = of_property_read_u32(dev->of_node, "mode-sync-pol", &val);
453 if (ret < 0)
454 pp->sync_pol = MODE_FLAG_PSYNC;
455 else
456 pp->sync_pol = (!val ? MODE_FLAG_NSYNC : MODE_FLAG_PSYNC);
457
458 pp->base.funcs = &rk628_post_process_bridge_funcs;
459 pp->base.of_node = dev->of_node;
460 drm_bridge_add(&pp->base);
461
462 of_get_drm_display_mode(dev->of_node, &rk628->src_mode, &bus_flags,
463 OF_USE_NATIVE_MODE);
464
465 return 0;
466 }
467
rk628_post_process_remove(struct platform_device * pdev)468 static int rk628_post_process_remove(struct platform_device *pdev)
469 {
470 struct rk628_post_process *pp = platform_get_drvdata(pdev);
471
472 drm_bridge_remove(&pp->base);
473
474 return 0;
475 }
476
477 static const struct of_device_id rk628_post_process_of_match[] = {
478 { .compatible = "rockchip,rk628-post-process", },
479 {},
480 };
481 MODULE_DEVICE_TABLE(of, rk628_post_process_of_match);
482
483 static struct platform_driver rk628_post_process_driver = {
484 .driver = {
485 .name = "rk628-post-process",
486 .of_match_table = of_match_ptr(rk628_post_process_of_match),
487 },
488 .probe = rk628_post_process_probe,
489 .remove = rk628_post_process_remove,
490 };
491 module_platform_driver(rk628_post_process_driver);
492
493 MODULE_AUTHOR("Wyon Bi <bivvy.bi@rock-chips.com>");
494 MODULE_DESCRIPTION("Rockchip RK628 Post Process driver");
495 MODULE_LICENSE("GPL v2");
496