1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Copyright (C) 2019 Linaro Ltd.
4  *
5  * Author: Stanimir Varbanov <stanimir.varbanov@linaro.org>
6  */
7 #include <linux/clk.h>
8 #include <linux/interconnect.h>
9 #include <linux/iopoll.h>
10 #include <linux/kernel.h>
11 #include <linux/pm_domain.h>
12 #include <linux/pm_opp.h>
13 #include <linux/pm_runtime.h>
14 #include <linux/reset.h>
15 #include <linux/types.h>
16 #include <media/v4l2-mem2mem.h>
17 
18 #include "core.h"
19 #include "hfi_parser.h"
20 #include "hfi_venus_io.h"
21 #include "pm_helpers.h"
22 #include "hfi_platform.h"
23 
24 static bool legacy_binding;
25 
26 static int core_clks_get(struct venus_core *core)
27 {
28 	const struct venus_resources *res = core->res;
29 	struct device *dev = core->dev;
30 	unsigned int i;
31 
32 	for (i = 0; i < res->clks_num; i++) {
33 		core->clks[i] = devm_clk_get(dev, res->clks[i]);
34 		if (IS_ERR(core->clks[i]))
35 			return PTR_ERR(core->clks[i]);
36 	}
37 
38 	return 0;
39 }
40 
41 static int core_clks_enable(struct venus_core *core)
42 {
43 	const struct venus_resources *res = core->res;
44 	const struct freq_tbl *freq_tbl = core->res->freq_tbl;
45 	unsigned int freq_tbl_size = core->res->freq_tbl_size;
46 	unsigned long freq;
47 	unsigned int i;
48 	int ret;
49 
50 	if (!freq_tbl)
51 		return -EINVAL;
52 
53 	freq = freq_tbl[freq_tbl_size - 1].freq;
54 
55 	for (i = 0; i < res->clks_num; i++) {
56 		if (IS_V6(core)) {
57 			ret = clk_set_rate(core->clks[i], freq);
58 			if (ret)
59 				goto err;
60 		}
61 
62 		ret = clk_prepare_enable(core->clks[i]);
63 		if (ret)
64 			goto err;
65 	}
66 
67 	return 0;
68 err:
69 	while (i--)
70 		clk_disable_unprepare(core->clks[i]);
71 
72 	return ret;
73 }
74 
75 static void core_clks_disable(struct venus_core *core)
76 {
77 	const struct venus_resources *res = core->res;
78 	unsigned int i = res->clks_num;
79 
80 	while (i--)
81 		clk_disable_unprepare(core->clks[i]);
82 }
83 
84 static int core_clks_set_rate(struct venus_core *core, unsigned long freq)
85 {
86 	int ret;
87 
88 	ret = dev_pm_opp_set_rate(core->dev, freq);
89 	if (ret)
90 		return ret;
91 
92 	ret = clk_set_rate(core->vcodec0_clks[0], freq);
93 	if (ret)
94 		return ret;
95 
96 	ret = clk_set_rate(core->vcodec1_clks[0], freq);
97 	if (ret)
98 		return ret;
99 
100 	return 0;
101 }
102 
103 static int vcodec_clks_get(struct venus_core *core, struct device *dev,
104 			   struct clk **clks, const char * const *id)
105 {
106 	const struct venus_resources *res = core->res;
107 	unsigned int i;
108 
109 	for (i = 0; i < res->vcodec_clks_num; i++) {
110 		if (!id[i])
111 			continue;
112 		clks[i] = devm_clk_get(dev, id[i]);
113 		if (IS_ERR(clks[i]))
114 			return PTR_ERR(clks[i]);
115 	}
116 
117 	return 0;
118 }
119 
120 static int vcodec_clks_enable(struct venus_core *core, struct clk **clks)
121 {
122 	const struct venus_resources *res = core->res;
123 	unsigned int i;
124 	int ret;
125 
126 	for (i = 0; i < res->vcodec_clks_num; i++) {
127 		ret = clk_prepare_enable(clks[i]);
128 		if (ret)
129 			goto err;
130 	}
131 
132 	return 0;
133 err:
134 	while (i--)
135 		clk_disable_unprepare(clks[i]);
136 
137 	return ret;
138 }
139 
140 static void vcodec_clks_disable(struct venus_core *core, struct clk **clks)
141 {
142 	const struct venus_resources *res = core->res;
143 	unsigned int i = res->vcodec_clks_num;
144 
145 	while (i--)
146 		clk_disable_unprepare(clks[i]);
147 }
148 
149 static u32 load_per_instance(struct venus_inst *inst)
150 {
151 	u32 mbs;
152 
153 	if (!inst || !(inst->state >= INST_INIT && inst->state < INST_STOP))
154 		return 0;
155 
156 	mbs = (ALIGN(inst->width, 16) / 16) * (ALIGN(inst->height, 16) / 16);
157 
158 	return mbs * inst->fps;
159 }
160 
161 static u32 load_per_type(struct venus_core *core, u32 session_type)
162 {
163 	struct venus_inst *inst = NULL;
164 	u32 mbs_per_sec = 0;
165 
166 	list_for_each_entry(inst, &core->instances, list) {
167 		if (inst->session_type != session_type)
168 			continue;
169 
170 		mbs_per_sec += load_per_instance(inst);
171 	}
172 
173 	return mbs_per_sec;
174 }
175 
176 static void mbs_to_bw(struct venus_inst *inst, u32 mbs, u32 *avg, u32 *peak)
177 {
178 	const struct venus_resources *res = inst->core->res;
179 	const struct bw_tbl *bw_tbl;
180 	unsigned int num_rows, i;
181 
182 	*avg = 0;
183 	*peak = 0;
184 
185 	if (mbs == 0)
186 		return;
187 
188 	if (inst->session_type == VIDC_SESSION_TYPE_ENC) {
189 		num_rows = res->bw_tbl_enc_size;
190 		bw_tbl = res->bw_tbl_enc;
191 	} else if (inst->session_type == VIDC_SESSION_TYPE_DEC) {
192 		num_rows = res->bw_tbl_dec_size;
193 		bw_tbl = res->bw_tbl_dec;
194 	} else {
195 		return;
196 	}
197 
198 	if (!bw_tbl || num_rows == 0)
199 		return;
200 
201 	for (i = 0; i < num_rows; i++) {
202 		if (i != 0 && mbs > bw_tbl[i].mbs_per_sec)
203 			break;
204 
205 		if (inst->dpb_fmt & HFI_COLOR_FORMAT_10_BIT_BASE) {
206 			*avg = bw_tbl[i].avg_10bit;
207 			*peak = bw_tbl[i].peak_10bit;
208 		} else {
209 			*avg = bw_tbl[i].avg;
210 			*peak = bw_tbl[i].peak;
211 		}
212 	}
213 }
214 
215 static int load_scale_bw(struct venus_core *core)
216 {
217 	struct venus_inst *inst = NULL;
218 	u32 mbs_per_sec, avg, peak, total_avg = 0, total_peak = 0;
219 
220 	list_for_each_entry(inst, &core->instances, list) {
221 		mbs_per_sec = load_per_instance(inst);
222 		mbs_to_bw(inst, mbs_per_sec, &avg, &peak);
223 		total_avg += avg;
224 		total_peak += peak;
225 	}
226 
227 	/*
228 	 * keep minimum bandwidth vote for "video-mem" path,
229 	 * so that clks can be disabled during vdec_session_release().
230 	 * Actual bandwidth drop will be done during device supend
231 	 * so that device can power down without any warnings.
232 	 */
233 
234 	if (!total_avg && !total_peak)
235 		total_avg = kbps_to_icc(1000);
236 
237 	dev_dbg(core->dev, VDBGL "total: avg_bw: %u, peak_bw: %u\n",
238 		total_avg, total_peak);
239 
240 	return icc_set_bw(core->video_path, total_avg, total_peak);
241 }
242 
243 static int load_scale_v1(struct venus_inst *inst)
244 {
245 	struct venus_core *core = inst->core;
246 	const struct freq_tbl *table = core->res->freq_tbl;
247 	unsigned int num_rows = core->res->freq_tbl_size;
248 	unsigned long freq = table[0].freq;
249 	struct device *dev = core->dev;
250 	u32 mbs_per_sec;
251 	unsigned int i;
252 	int ret = 0;
253 
254 	mutex_lock(&core->lock);
255 	mbs_per_sec = load_per_type(core, VIDC_SESSION_TYPE_ENC) +
256 		      load_per_type(core, VIDC_SESSION_TYPE_DEC);
257 
258 	if (mbs_per_sec > core->res->max_load)
259 		dev_warn(dev, "HW is overloaded, needed: %d max: %d\n",
260 			 mbs_per_sec, core->res->max_load);
261 
262 	if (!mbs_per_sec && num_rows > 1) {
263 		freq = table[num_rows - 1].freq;
264 		goto set_freq;
265 	}
266 
267 	for (i = 0; i < num_rows; i++) {
268 		if (mbs_per_sec > table[i].load)
269 			break;
270 		freq = table[i].freq;
271 	}
272 
273 set_freq:
274 
275 	ret = core_clks_set_rate(core, freq);
276 	if (ret) {
277 		dev_err(dev, "failed to set clock rate %lu (%d)\n",
278 			freq, ret);
279 		goto exit;
280 	}
281 
282 	ret = load_scale_bw(core);
283 	if (ret) {
284 		dev_err(dev, "failed to set bandwidth (%d)\n",
285 			ret);
286 		goto exit;
287 	}
288 
289 exit:
290 	mutex_unlock(&core->lock);
291 	return ret;
292 }
293 
294 static int core_get_v1(struct venus_core *core)
295 {
296 	int ret;
297 
298 	ret = core_clks_get(core);
299 	if (ret)
300 		return ret;
301 
302 	ret = devm_pm_opp_set_clkname(core->dev, "core");
303 	if (ret)
304 		return ret;
305 
306 	return 0;
307 }
308 
309 static void core_put_v1(struct venus_core *core)
310 {
311 }
312 
313 static int core_power_v1(struct venus_core *core, int on)
314 {
315 	int ret = 0;
316 
317 	if (on == POWER_ON)
318 		ret = core_clks_enable(core);
319 	else
320 		core_clks_disable(core);
321 
322 	return ret;
323 }
324 
325 static const struct venus_pm_ops pm_ops_v1 = {
326 	.core_get = core_get_v1,
327 	.core_put = core_put_v1,
328 	.core_power = core_power_v1,
329 	.load_scale = load_scale_v1,
330 };
331 
332 static void
333 vcodec_control_v3(struct venus_core *core, u32 session_type, bool enable)
334 {
335 	void __iomem *ctrl;
336 
337 	if (session_type == VIDC_SESSION_TYPE_DEC)
338 		ctrl = core->wrapper_base + WRAPPER_VDEC_VCODEC_POWER_CONTROL;
339 	else
340 		ctrl = core->wrapper_base + WRAPPER_VENC_VCODEC_POWER_CONTROL;
341 
342 	if (enable)
343 		writel(0, ctrl);
344 	else
345 		writel(1, ctrl);
346 }
347 
348 static int vdec_get_v3(struct device *dev)
349 {
350 	struct venus_core *core = dev_get_drvdata(dev);
351 
352 	return vcodec_clks_get(core, dev, core->vcodec0_clks,
353 			       core->res->vcodec0_clks);
354 }
355 
356 static int vdec_power_v3(struct device *dev, int on)
357 {
358 	struct venus_core *core = dev_get_drvdata(dev);
359 	int ret = 0;
360 
361 	vcodec_control_v3(core, VIDC_SESSION_TYPE_DEC, true);
362 
363 	if (on == POWER_ON)
364 		ret = vcodec_clks_enable(core, core->vcodec0_clks);
365 	else
366 		vcodec_clks_disable(core, core->vcodec0_clks);
367 
368 	vcodec_control_v3(core, VIDC_SESSION_TYPE_DEC, false);
369 
370 	return ret;
371 }
372 
373 static int venc_get_v3(struct device *dev)
374 {
375 	struct venus_core *core = dev_get_drvdata(dev);
376 
377 	return vcodec_clks_get(core, dev, core->vcodec1_clks,
378 			       core->res->vcodec1_clks);
379 }
380 
381 static int venc_power_v3(struct device *dev, int on)
382 {
383 	struct venus_core *core = dev_get_drvdata(dev);
384 	int ret = 0;
385 
386 	vcodec_control_v3(core, VIDC_SESSION_TYPE_ENC, true);
387 
388 	if (on == POWER_ON)
389 		ret = vcodec_clks_enable(core, core->vcodec1_clks);
390 	else
391 		vcodec_clks_disable(core, core->vcodec1_clks);
392 
393 	vcodec_control_v3(core, VIDC_SESSION_TYPE_ENC, false);
394 
395 	return ret;
396 }
397 
398 static const struct venus_pm_ops pm_ops_v3 = {
399 	.core_get = core_get_v1,
400 	.core_put = core_put_v1,
401 	.core_power = core_power_v1,
402 	.vdec_get = vdec_get_v3,
403 	.vdec_power = vdec_power_v3,
404 	.venc_get = venc_get_v3,
405 	.venc_power = venc_power_v3,
406 	.load_scale = load_scale_v1,
407 };
408 
409 static int vcodec_control_v4(struct venus_core *core, u32 coreid, bool enable)
410 {
411 	void __iomem *ctrl, *stat;
412 	u32 val;
413 	int ret;
414 
415 	if (IS_V6(core)) {
416 		ctrl = core->wrapper_base + WRAPPER_CORE_POWER_CONTROL_V6;
417 		stat = core->wrapper_base + WRAPPER_CORE_POWER_STATUS_V6;
418 	} else if (coreid == VIDC_CORE_ID_1) {
419 		ctrl = core->wrapper_base + WRAPPER_VCODEC0_MMCC_POWER_CONTROL;
420 		stat = core->wrapper_base + WRAPPER_VCODEC0_MMCC_POWER_STATUS;
421 	} else {
422 		ctrl = core->wrapper_base + WRAPPER_VCODEC1_MMCC_POWER_CONTROL;
423 		stat = core->wrapper_base + WRAPPER_VCODEC1_MMCC_POWER_STATUS;
424 	}
425 
426 	if (enable) {
427 		writel(0, ctrl);
428 
429 		ret = readl_poll_timeout(stat, val, val & BIT(1), 1, 100);
430 		if (ret)
431 			return ret;
432 	} else {
433 		writel(1, ctrl);
434 
435 		ret = readl_poll_timeout(stat, val, !(val & BIT(1)), 1, 100);
436 		if (ret)
437 			return ret;
438 	}
439 
440 	return 0;
441 }
442 
443 static int poweroff_coreid(struct venus_core *core, unsigned int coreid_mask)
444 {
445 	int ret;
446 
447 	if (coreid_mask & VIDC_CORE_ID_1) {
448 		ret = vcodec_control_v4(core, VIDC_CORE_ID_1, true);
449 		if (ret)
450 			return ret;
451 
452 		vcodec_clks_disable(core, core->vcodec0_clks);
453 
454 		ret = vcodec_control_v4(core, VIDC_CORE_ID_1, false);
455 		if (ret)
456 			return ret;
457 
458 		ret = pm_runtime_put_sync(core->pmdomains[1]);
459 		if (ret < 0)
460 			return ret;
461 	}
462 
463 	if (coreid_mask & VIDC_CORE_ID_2) {
464 		ret = vcodec_control_v4(core, VIDC_CORE_ID_2, true);
465 		if (ret)
466 			return ret;
467 
468 		vcodec_clks_disable(core, core->vcodec1_clks);
469 
470 		ret = vcodec_control_v4(core, VIDC_CORE_ID_2, false);
471 		if (ret)
472 			return ret;
473 
474 		ret = pm_runtime_put_sync(core->pmdomains[2]);
475 		if (ret < 0)
476 			return ret;
477 	}
478 
479 	return 0;
480 }
481 
482 static int poweron_coreid(struct venus_core *core, unsigned int coreid_mask)
483 {
484 	int ret;
485 
486 	if (coreid_mask & VIDC_CORE_ID_1) {
487 		ret = pm_runtime_get_sync(core->pmdomains[1]);
488 		if (ret < 0)
489 			return ret;
490 
491 		ret = vcodec_control_v4(core, VIDC_CORE_ID_1, true);
492 		if (ret)
493 			return ret;
494 
495 		ret = vcodec_clks_enable(core, core->vcodec0_clks);
496 		if (ret)
497 			return ret;
498 
499 		ret = vcodec_control_v4(core, VIDC_CORE_ID_1, false);
500 		if (ret < 0)
501 			return ret;
502 	}
503 
504 	if (coreid_mask & VIDC_CORE_ID_2) {
505 		ret = pm_runtime_get_sync(core->pmdomains[2]);
506 		if (ret < 0)
507 			return ret;
508 
509 		ret = vcodec_control_v4(core, VIDC_CORE_ID_2, true);
510 		if (ret)
511 			return ret;
512 
513 		ret = vcodec_clks_enable(core, core->vcodec1_clks);
514 		if (ret)
515 			return ret;
516 
517 		ret = vcodec_control_v4(core, VIDC_CORE_ID_2, false);
518 		if (ret < 0)
519 			return ret;
520 	}
521 
522 	return 0;
523 }
524 
525 static inline int power_save_mode_enable(struct venus_inst *inst,
526 					 bool enable)
527 {
528 	struct venc_controls *enc_ctr = &inst->controls.enc;
529 	const u32 ptype = HFI_PROPERTY_CONFIG_VENC_PERF_MODE;
530 	u32 venc_mode;
531 	int ret = 0;
532 
533 	if (inst->session_type != VIDC_SESSION_TYPE_ENC)
534 		return 0;
535 
536 	if (enc_ctr->bitrate_mode == V4L2_MPEG_VIDEO_BITRATE_MODE_CQ)
537 		enable = false;
538 
539 	venc_mode = enable ? HFI_VENC_PERFMODE_POWER_SAVE :
540 		HFI_VENC_PERFMODE_MAX_QUALITY;
541 
542 	ret = hfi_session_set_property(inst, ptype, &venc_mode);
543 	if (ret)
544 		return ret;
545 
546 	inst->flags = enable ? inst->flags | VENUS_LOW_POWER :
547 		inst->flags & ~VENUS_LOW_POWER;
548 
549 	return ret;
550 }
551 
552 static int move_core_to_power_save_mode(struct venus_core *core,
553 					u32 core_id)
554 {
555 	struct venus_inst *inst = NULL;
556 
557 	mutex_lock(&core->lock);
558 	list_for_each_entry(inst, &core->instances, list) {
559 		if (inst->clk_data.core_id == core_id &&
560 		    inst->session_type == VIDC_SESSION_TYPE_ENC)
561 			power_save_mode_enable(inst, true);
562 	}
563 	mutex_unlock(&core->lock);
564 	return 0;
565 }
566 
567 static void
568 min_loaded_core(struct venus_inst *inst, u32 *min_coreid, u32 *min_load, bool low_power)
569 {
570 	u32 mbs_per_sec, load, core1_load = 0, core2_load = 0;
571 	u32 cores_max = core_num_max(inst);
572 	struct venus_core *core = inst->core;
573 	struct venus_inst *inst_pos;
574 	unsigned long vpp_freq;
575 	u32 coreid;
576 
577 	mutex_lock(&core->lock);
578 
579 	list_for_each_entry(inst_pos, &core->instances, list) {
580 		if (inst_pos == inst)
581 			continue;
582 
583 		if (inst_pos->state != INST_START)
584 			continue;
585 
586 		if (inst->session_type == VIDC_SESSION_TYPE_DEC)
587 			vpp_freq = inst_pos->clk_data.vpp_freq;
588 		else if (inst->session_type == VIDC_SESSION_TYPE_ENC)
589 			vpp_freq = low_power ? inst_pos->clk_data.low_power_freq :
590 				inst_pos->clk_data.vpp_freq;
591 		else
592 			continue;
593 
594 		coreid = inst_pos->clk_data.core_id;
595 
596 		mbs_per_sec = load_per_instance(inst_pos);
597 		load = mbs_per_sec * vpp_freq;
598 
599 		if ((coreid & VIDC_CORE_ID_3) == VIDC_CORE_ID_3) {
600 			core1_load += load / 2;
601 			core2_load += load / 2;
602 		} else if (coreid & VIDC_CORE_ID_1) {
603 			core1_load += load;
604 		} else if (coreid & VIDC_CORE_ID_2) {
605 			core2_load += load;
606 		}
607 	}
608 
609 	*min_coreid = core1_load <= core2_load ?
610 			VIDC_CORE_ID_1 : VIDC_CORE_ID_2;
611 	*min_load = min(core1_load, core2_load);
612 
613 	if (cores_max < VIDC_CORE_ID_2 || core->res->vcodec_num < 2) {
614 		*min_coreid = VIDC_CORE_ID_1;
615 		*min_load = core1_load;
616 	}
617 
618 	mutex_unlock(&core->lock);
619 }
620 
621 static int decide_core(struct venus_inst *inst)
622 {
623 	const u32 ptype = HFI_PROPERTY_CONFIG_VIDEOCORES_USAGE;
624 	struct venus_core *core = inst->core;
625 	u32 min_coreid, min_load, cur_inst_load;
626 	u32 min_lp_coreid, min_lp_load, cur_inst_lp_load;
627 	struct hfi_videocores_usage_type cu;
628 	unsigned long max_freq;
629 	int ret = 0;
630 
631 	if (legacy_binding) {
632 		if (inst->session_type == VIDC_SESSION_TYPE_DEC)
633 			cu.video_core_enable_mask = VIDC_CORE_ID_1;
634 		else
635 			cu.video_core_enable_mask = VIDC_CORE_ID_2;
636 
637 		goto done;
638 	}
639 
640 	if (inst->clk_data.core_id != VIDC_CORE_ID_DEFAULT)
641 		return 0;
642 
643 	cur_inst_load = load_per_instance(inst);
644 	cur_inst_load *= inst->clk_data.vpp_freq;
645 	/*TODO : divide this inst->load by work_route */
646 
647 	cur_inst_lp_load = load_per_instance(inst);
648 	cur_inst_lp_load *= inst->clk_data.low_power_freq;
649 	/*TODO : divide this inst->load by work_route */
650 
651 	max_freq = core->res->freq_tbl[0].freq;
652 
653 	min_loaded_core(inst, &min_coreid, &min_load, false);
654 	min_loaded_core(inst, &min_lp_coreid, &min_lp_load, true);
655 
656 	if (cur_inst_load + min_load <= max_freq) {
657 		inst->clk_data.core_id = min_coreid;
658 		cu.video_core_enable_mask = min_coreid;
659 	} else if (cur_inst_lp_load + min_load <= max_freq) {
660 		/* Move current instance to LP and return */
661 		inst->clk_data.core_id = min_coreid;
662 		cu.video_core_enable_mask = min_coreid;
663 		power_save_mode_enable(inst, true);
664 	} else if (cur_inst_lp_load + min_lp_load <= max_freq) {
665 		/* Move all instances to LP mode and return */
666 		inst->clk_data.core_id = min_lp_coreid;
667 		cu.video_core_enable_mask = min_lp_coreid;
668 		move_core_to_power_save_mode(core, min_lp_coreid);
669 	} else {
670 		dev_warn(core->dev, "HW can't support this load");
671 		return -EINVAL;
672 	}
673 
674 done:
675 	ret = hfi_session_set_property(inst, ptype, &cu);
676 	if (ret)
677 		return ret;
678 
679 	return ret;
680 }
681 
682 static int acquire_core(struct venus_inst *inst)
683 {
684 	struct venus_core *core = inst->core;
685 	unsigned int coreid_mask = 0;
686 
687 	if (inst->core_acquired)
688 		return 0;
689 
690 	inst->core_acquired = true;
691 
692 	if (inst->clk_data.core_id & VIDC_CORE_ID_1) {
693 		if (core->core0_usage_count++)
694 			return 0;
695 
696 		coreid_mask = VIDC_CORE_ID_1;
697 	}
698 
699 	if (inst->clk_data.core_id & VIDC_CORE_ID_2) {
700 		if (core->core1_usage_count++)
701 			return 0;
702 
703 		coreid_mask |= VIDC_CORE_ID_2;
704 	}
705 
706 	return poweron_coreid(core, coreid_mask);
707 }
708 
709 static int release_core(struct venus_inst *inst)
710 {
711 	struct venus_core *core = inst->core;
712 	unsigned int coreid_mask = 0;
713 	int ret;
714 
715 	if (!inst->core_acquired)
716 		return 0;
717 
718 	if (inst->clk_data.core_id & VIDC_CORE_ID_1) {
719 		if (--core->core0_usage_count)
720 			goto done;
721 
722 		coreid_mask = VIDC_CORE_ID_1;
723 	}
724 
725 	if (inst->clk_data.core_id & VIDC_CORE_ID_2) {
726 		if (--core->core1_usage_count)
727 			goto done;
728 
729 		coreid_mask |= VIDC_CORE_ID_2;
730 	}
731 
732 	ret = poweroff_coreid(core, coreid_mask);
733 	if (ret)
734 		return ret;
735 
736 done:
737 	inst->clk_data.core_id = VIDC_CORE_ID_DEFAULT;
738 	inst->core_acquired = false;
739 	return 0;
740 }
741 
742 static int coreid_power_v4(struct venus_inst *inst, int on)
743 {
744 	struct venus_core *core = inst->core;
745 	int ret;
746 
747 	if (legacy_binding)
748 		return 0;
749 
750 	if (on == POWER_ON) {
751 		ret = decide_core(inst);
752 		if (ret)
753 			return ret;
754 
755 		mutex_lock(&core->lock);
756 		ret = acquire_core(inst);
757 		mutex_unlock(&core->lock);
758 	} else {
759 		mutex_lock(&core->lock);
760 		ret = release_core(inst);
761 		mutex_unlock(&core->lock);
762 	}
763 
764 	return ret;
765 }
766 
767 static int vdec_get_v4(struct device *dev)
768 {
769 	struct venus_core *core = dev_get_drvdata(dev);
770 
771 	if (!legacy_binding)
772 		return 0;
773 
774 	return vcodec_clks_get(core, dev, core->vcodec0_clks,
775 			       core->res->vcodec0_clks);
776 }
777 
778 static void vdec_put_v4(struct device *dev)
779 {
780 	struct venus_core *core = dev_get_drvdata(dev);
781 	unsigned int i;
782 
783 	if (!legacy_binding)
784 		return;
785 
786 	for (i = 0; i < core->res->vcodec_clks_num; i++)
787 		core->vcodec0_clks[i] = NULL;
788 }
789 
790 static int vdec_power_v4(struct device *dev, int on)
791 {
792 	struct venus_core *core = dev_get_drvdata(dev);
793 	int ret;
794 
795 	if (!legacy_binding)
796 		return 0;
797 
798 	ret = vcodec_control_v4(core, VIDC_CORE_ID_1, true);
799 	if (ret)
800 		return ret;
801 
802 	if (on == POWER_ON)
803 		ret = vcodec_clks_enable(core, core->vcodec0_clks);
804 	else
805 		vcodec_clks_disable(core, core->vcodec0_clks);
806 
807 	vcodec_control_v4(core, VIDC_CORE_ID_1, false);
808 
809 	return ret;
810 }
811 
812 static int venc_get_v4(struct device *dev)
813 {
814 	struct venus_core *core = dev_get_drvdata(dev);
815 
816 	if (!legacy_binding)
817 		return 0;
818 
819 	return vcodec_clks_get(core, dev, core->vcodec1_clks,
820 			       core->res->vcodec1_clks);
821 }
822 
823 static void venc_put_v4(struct device *dev)
824 {
825 	struct venus_core *core = dev_get_drvdata(dev);
826 	unsigned int i;
827 
828 	if (!legacy_binding)
829 		return;
830 
831 	for (i = 0; i < core->res->vcodec_clks_num; i++)
832 		core->vcodec1_clks[i] = NULL;
833 }
834 
835 static int venc_power_v4(struct device *dev, int on)
836 {
837 	struct venus_core *core = dev_get_drvdata(dev);
838 	int ret;
839 
840 	if (!legacy_binding)
841 		return 0;
842 
843 	ret = vcodec_control_v4(core, VIDC_CORE_ID_2, true);
844 	if (ret)
845 		return ret;
846 
847 	if (on == POWER_ON)
848 		ret = vcodec_clks_enable(core, core->vcodec1_clks);
849 	else
850 		vcodec_clks_disable(core, core->vcodec1_clks);
851 
852 	vcodec_control_v4(core, VIDC_CORE_ID_2, false);
853 
854 	return ret;
855 }
856 
857 static int vcodec_domains_get(struct venus_core *core)
858 {
859 	int ret;
860 	struct device **opp_virt_dev;
861 	struct device *dev = core->dev;
862 	const struct venus_resources *res = core->res;
863 	struct device *pd;
864 	unsigned int i;
865 
866 	if (!res->vcodec_pmdomains_num)
867 		goto skip_pmdomains;
868 
869 	for (i = 0; i < res->vcodec_pmdomains_num; i++) {
870 		pd = dev_pm_domain_attach_by_name(dev,
871 						  res->vcodec_pmdomains[i]);
872 		if (IS_ERR(pd))
873 			return PTR_ERR(pd);
874 		core->pmdomains[i] = pd;
875 	}
876 
877 skip_pmdomains:
878 	if (!core->has_opp_table)
879 		return 0;
880 
881 	/* Attach the power domain for setting performance state */
882 	ret = devm_pm_opp_attach_genpd(dev, res->opp_pmdomain, &opp_virt_dev);
883 	if (ret)
884 		goto opp_attach_err;
885 
886 	core->opp_pmdomain = *opp_virt_dev;
887 	core->opp_dl_venus = device_link_add(dev, core->opp_pmdomain,
888 					     DL_FLAG_RPM_ACTIVE |
889 					     DL_FLAG_PM_RUNTIME |
890 					     DL_FLAG_STATELESS);
891 	if (!core->opp_dl_venus) {
892 		ret = -ENODEV;
893 		goto opp_attach_err;
894 	}
895 
896 	return 0;
897 
898 opp_attach_err:
899 	for (i = 0; i < res->vcodec_pmdomains_num; i++) {
900 		if (IS_ERR_OR_NULL(core->pmdomains[i]))
901 			continue;
902 		dev_pm_domain_detach(core->pmdomains[i], true);
903 	}
904 
905 	return ret;
906 }
907 
908 static void vcodec_domains_put(struct venus_core *core)
909 {
910 	const struct venus_resources *res = core->res;
911 	unsigned int i;
912 
913 	if (!res->vcodec_pmdomains_num)
914 		goto skip_pmdomains;
915 
916 	for (i = 0; i < res->vcodec_pmdomains_num; i++) {
917 		if (IS_ERR_OR_NULL(core->pmdomains[i]))
918 			continue;
919 		dev_pm_domain_detach(core->pmdomains[i], true);
920 	}
921 
922 skip_pmdomains:
923 	if (!core->has_opp_table)
924 		return;
925 
926 	if (core->opp_dl_venus)
927 		device_link_del(core->opp_dl_venus);
928 }
929 
930 static int core_resets_reset(struct venus_core *core)
931 {
932 	const struct venus_resources *res = core->res;
933 	unsigned int i;
934 	int ret;
935 
936 	if (!res->resets_num)
937 		return 0;
938 
939 	for (i = 0; i < res->resets_num; i++) {
940 		ret = reset_control_assert(core->resets[i]);
941 		if (ret)
942 			goto err;
943 
944 		usleep_range(150, 250);
945 		ret = reset_control_deassert(core->resets[i]);
946 		if (ret)
947 			goto err;
948 	}
949 
950 err:
951 	return ret;
952 }
953 
954 static int core_resets_get(struct venus_core *core)
955 {
956 	struct device *dev = core->dev;
957 	const struct venus_resources *res = core->res;
958 	unsigned int i;
959 	int ret;
960 
961 	if (!res->resets_num)
962 		return 0;
963 
964 	for (i = 0; i < res->resets_num; i++) {
965 		core->resets[i] =
966 			devm_reset_control_get_exclusive(dev, res->resets[i]);
967 		if (IS_ERR(core->resets[i])) {
968 			ret = PTR_ERR(core->resets[i]);
969 			return ret;
970 		}
971 	}
972 
973 	return 0;
974 }
975 
976 static int core_get_v4(struct venus_core *core)
977 {
978 	struct device *dev = core->dev;
979 	const struct venus_resources *res = core->res;
980 	int ret;
981 
982 	ret = core_clks_get(core);
983 	if (ret)
984 		return ret;
985 
986 	if (!res->vcodec_pmdomains_num)
987 		legacy_binding = true;
988 
989 	dev_info(dev, "%s legacy binding\n", legacy_binding ? "" : "non");
990 
991 	ret = vcodec_clks_get(core, dev, core->vcodec0_clks, res->vcodec0_clks);
992 	if (ret)
993 		return ret;
994 
995 	ret = vcodec_clks_get(core, dev, core->vcodec1_clks, res->vcodec1_clks);
996 	if (ret)
997 		return ret;
998 
999 	ret = core_resets_get(core);
1000 	if (ret)
1001 		return ret;
1002 
1003 	if (legacy_binding)
1004 		return 0;
1005 
1006 	ret = devm_pm_opp_set_clkname(dev, "core");
1007 	if (ret)
1008 		return ret;
1009 
1010 	if (core->res->opp_pmdomain) {
1011 		ret = devm_pm_opp_of_add_table(dev);
1012 		if (!ret) {
1013 			core->has_opp_table = true;
1014 		} else if (ret != -ENODEV) {
1015 			dev_err(dev, "invalid OPP table in device tree\n");
1016 			return ret;
1017 		}
1018 	}
1019 
1020 	ret = vcodec_domains_get(core);
1021 	if (ret)
1022 		return ret;
1023 
1024 	return 0;
1025 }
1026 
1027 static void core_put_v4(struct venus_core *core)
1028 {
1029 	if (legacy_binding)
1030 		return;
1031 
1032 	vcodec_domains_put(core);
1033 }
1034 
1035 static int core_power_v4(struct venus_core *core, int on)
1036 {
1037 	struct device *dev = core->dev;
1038 	struct device *pmctrl = core->pmdomains[0];
1039 	int ret = 0;
1040 
1041 	if (on == POWER_ON) {
1042 		if (pmctrl) {
1043 			ret = pm_runtime_resume_and_get(pmctrl);
1044 			if (ret < 0) {
1045 				return ret;
1046 			}
1047 		}
1048 
1049 		ret = core_resets_reset(core);
1050 		if (ret) {
1051 			if (pmctrl)
1052 				pm_runtime_put_sync(pmctrl);
1053 			return ret;
1054 		}
1055 
1056 		ret = core_clks_enable(core);
1057 		if (ret < 0 && pmctrl)
1058 			pm_runtime_put_sync(pmctrl);
1059 	} else {
1060 		/* Drop the performance state vote */
1061 		if (core->opp_pmdomain)
1062 			dev_pm_opp_set_rate(dev, 0);
1063 
1064 		core_clks_disable(core);
1065 
1066 		ret = core_resets_reset(core);
1067 
1068 		if (pmctrl)
1069 			pm_runtime_put_sync(pmctrl);
1070 	}
1071 
1072 	return ret;
1073 }
1074 
1075 static unsigned long calculate_inst_freq(struct venus_inst *inst,
1076 					 unsigned long filled_len)
1077 {
1078 	unsigned long vpp_freq_per_mb = 0, vpp_freq = 0, vsp_freq = 0;
1079 	u32 fps = (u32)inst->fps;
1080 	u32 mbs_per_sec;
1081 
1082 	mbs_per_sec = load_per_instance(inst);
1083 
1084 	if (inst->state != INST_START)
1085 		return 0;
1086 
1087 	if (inst->session_type == VIDC_SESSION_TYPE_ENC) {
1088 		vpp_freq_per_mb = inst->flags & VENUS_LOW_POWER ?
1089 			inst->clk_data.low_power_freq :
1090 			inst->clk_data.vpp_freq;
1091 
1092 		vpp_freq = mbs_per_sec * vpp_freq_per_mb;
1093 	} else {
1094 		vpp_freq = mbs_per_sec * inst->clk_data.vpp_freq;
1095 	}
1096 
1097 	/* 21 / 20 is overhead factor */
1098 	vpp_freq += vpp_freq / 20;
1099 	vsp_freq = mbs_per_sec * inst->clk_data.vsp_freq;
1100 
1101 	/* 10 / 7 is overhead factor */
1102 	if (inst->session_type == VIDC_SESSION_TYPE_ENC)
1103 		vsp_freq += (inst->controls.enc.bitrate * 10) / 7;
1104 	else
1105 		vsp_freq += ((fps * filled_len * 8) * 10) / 7;
1106 
1107 	return max(vpp_freq, vsp_freq);
1108 }
1109 
1110 static int load_scale_v4(struct venus_inst *inst)
1111 {
1112 	struct venus_core *core = inst->core;
1113 	const struct freq_tbl *table = core->res->freq_tbl;
1114 	unsigned int num_rows = core->res->freq_tbl_size;
1115 	struct device *dev = core->dev;
1116 	unsigned long freq = 0, freq_core1 = 0, freq_core2 = 0;
1117 	unsigned long filled_len = 0;
1118 	int i, ret = 0;
1119 
1120 	for (i = 0; i < inst->num_input_bufs; i++)
1121 		filled_len = max(filled_len, inst->payloads[i]);
1122 
1123 	if (inst->session_type == VIDC_SESSION_TYPE_DEC && !filled_len)
1124 		return ret;
1125 
1126 	freq = calculate_inst_freq(inst, filled_len);
1127 	inst->clk_data.freq = freq;
1128 
1129 	mutex_lock(&core->lock);
1130 	list_for_each_entry(inst, &core->instances, list) {
1131 		if (inst->clk_data.core_id == VIDC_CORE_ID_1) {
1132 			freq_core1 += inst->clk_data.freq;
1133 		} else if (inst->clk_data.core_id == VIDC_CORE_ID_2) {
1134 			freq_core2 += inst->clk_data.freq;
1135 		} else if (inst->clk_data.core_id == VIDC_CORE_ID_3) {
1136 			freq_core1 += inst->clk_data.freq;
1137 			freq_core2 += inst->clk_data.freq;
1138 		}
1139 	}
1140 
1141 	freq = max(freq_core1, freq_core2);
1142 
1143 	if (freq > table[0].freq) {
1144 		dev_dbg(dev, VDBGL "requested clock rate: %lu scaling clock rate : %lu\n",
1145 			freq, table[0].freq);
1146 
1147 		freq = table[0].freq;
1148 		goto set_freq;
1149 	}
1150 
1151 	for (i = num_rows - 1 ; i >= 0; i--) {
1152 		if (freq <= table[i].freq) {
1153 			freq = table[i].freq;
1154 			break;
1155 		}
1156 	}
1157 
1158 set_freq:
1159 
1160 	ret = core_clks_set_rate(core, freq);
1161 	if (ret) {
1162 		dev_err(dev, "failed to set clock rate %lu (%d)\n",
1163 			freq, ret);
1164 		goto exit;
1165 	}
1166 
1167 	ret = load_scale_bw(core);
1168 	if (ret) {
1169 		dev_err(dev, "failed to set bandwidth (%d)\n",
1170 			ret);
1171 		goto exit;
1172 	}
1173 
1174 exit:
1175 	mutex_unlock(&core->lock);
1176 	return ret;
1177 }
1178 
1179 static const struct venus_pm_ops pm_ops_v4 = {
1180 	.core_get = core_get_v4,
1181 	.core_put = core_put_v4,
1182 	.core_power = core_power_v4,
1183 	.vdec_get = vdec_get_v4,
1184 	.vdec_put = vdec_put_v4,
1185 	.vdec_power = vdec_power_v4,
1186 	.venc_get = venc_get_v4,
1187 	.venc_put = venc_put_v4,
1188 	.venc_power = venc_power_v4,
1189 	.coreid_power = coreid_power_v4,
1190 	.load_scale = load_scale_v4,
1191 };
1192 
1193 const struct venus_pm_ops *venus_pm_get(enum hfi_version version)
1194 {
1195 	switch (version) {
1196 	case HFI_VERSION_1XX:
1197 	default:
1198 		return &pm_ops_v1;
1199 	case HFI_VERSION_3XX:
1200 		return &pm_ops_v3;
1201 	case HFI_VERSION_4XX:
1202 	case HFI_VERSION_6XX:
1203 		return &pm_ops_v4;
1204 	}
1205 
1206 	return NULL;
1207 }
1208