msm: vidc: Add support of timestamps based frame rate

Add timestamp based frame rate detection.

Change-Id: Id0c522e01769ea6cf214c7b84c11faae6049e2ef
Signed-off-by: Vikash Garodia <vgarodia@codeaurora.org>
Signed-off-by: Govindaraj Rajagopal <grajagop@codeaurora.org>

driver/variant/iris2/src/msm_vidc_power_iris2.c

@@ -23,6 +23,7 @@ u64 msm_vidc_calc_freq_iris2(struct msm_vidc_inst *inst, u32 data_size)
 	u32 base_cycles = 0;
 	u32 fps;
 	u32 prio_val;
+	u32 ts_fps;
 
 	if (!inst || !inst->core || !inst->capabilities) {
 		d_vpr_e("%s: invalid params\n", __func__);
@@ -47,6 +48,10 @@ u64 msm_vidc_calc_freq_iris2(struct msm_vidc_inst *inst, u32 data_size)
 	mbs_per_second = msm_vidc_get_inst_load(inst);
 	fps = msm_vidc_get_fps(inst);
 
+	ts_fps = msm_vidc_calc_framerate(inst);
+	if (ts_fps > fps)
+		i_vpr_l(inst, "%s: ts_rate %d set rate %d\n", __func__, ts_fps, fps);
+
 	/*
 	 * Calculate vpp, vsp, fw cycles separately for encoder and decoder.
 	 * Even though, most part is common now, in future it may change

driver/vidc/inc/msm_vidc_driver.h

@@ -266,12 +266,15 @@ int msm_vidc_vb2_buffer_done(struct msm_vidc_inst *inst,
 struct msm_vidc_buffer *msm_vidc_get_vidc_buffer(struct msm_vidc_inst *inst);
 struct msm_vidc_alloc *msm_vidc_get_alloc_buffer(struct msm_vidc_inst *inst);
 struct msm_vidc_map *msm_vidc_get_map_buffer(struct msm_vidc_inst *inst);
+struct msm_vidc_timestamp *msm_vidc_get_ts(struct msm_vidc_inst *inst);
 int msm_vidc_put_vidc_buffer(struct msm_vidc_inst *inst, struct msm_vidc_buffer *buf);
 int msm_vidc_put_alloc_buffer(struct msm_vidc_inst *inst, struct msm_vidc_alloc *alloc);
 int msm_vidc_put_map_buffer(struct msm_vidc_inst *inst, struct msm_vidc_map *map);
+int msm_vidc_put_ts(struct msm_vidc_inst *inst, struct msm_vidc_timestamp *ts);
 int msm_vidc_destroy_vidc_buffer(struct msm_vidc_inst *inst);
 int msm_vidc_destroy_alloc_buffer(struct msm_vidc_inst *inst);
 int msm_vidc_destroy_map_buffer(struct msm_vidc_inst *inst);
+int msm_vidc_destroy_ts(struct msm_vidc_inst *inst);
 int msm_vidc_remove_session(struct msm_vidc_inst *inst);
 int msm_vidc_add_session(struct msm_vidc_inst *inst);
 int msm_vidc_session_open(struct msm_vidc_inst *inst);
@@ -386,6 +389,9 @@ bool msm_vidc_allow_decode_batch(struct msm_vidc_inst *inst);
 int msm_vidc_check_session_supported(struct msm_vidc_inst *inst);
 int msm_vidc_check_mbps_supported(struct msm_vidc_inst *inst);
 int msm_vidc_check_scaling_supported(struct msm_vidc_inst *inst);
+int msm_vidc_update_timestamp(struct msm_vidc_inst *inst, u64 timestamp);
+int msm_vidc_calc_framerate(struct msm_vidc_inst *inst);
+int msm_vidc_flush_ts(struct msm_vidc_inst *inst);
 const char *buf_name(enum msm_vidc_buffer_type type);
 void msm_vidc_free_capabililty_list(struct msm_vidc_inst *inst,
 	enum msm_vidc_ctrl_list_type list_type);

driver/vidc/inc/msm_vidc_inst.h

@@ -31,6 +31,7 @@ struct msm_vidc_pool_info {
 	struct msm_vidc_pool            allocations;
 	struct msm_vidc_pool            mappings;
 	struct msm_vidc_pool            buffers;
+	struct msm_vidc_pool            timestamps;
 };
 
 struct msm_vidc_allocations_info {
@@ -122,6 +123,7 @@ struct msm_vidc_inst {
 	struct msm_vidc_buffers_info       buffers;
 	struct msm_vidc_mappings_info      mappings;
 	struct msm_vidc_allocations_info   allocations;
+	struct msm_vidc_timestamps         timestamps;
 	bool                               subscribed_input_psc;
 	bool                               subscribed_output_psc;
 	bool                               subscribed_input_prop;

driver/vidc/inc/msm_vidc_internal.h

@@ -662,13 +662,6 @@ struct msm_vidc_input_cr_data {
 	u32                    input_cr;
 };
 
-struct msm_vidc_timestamps {
-	struct list_head       list;
-	u64                    timestamp_us;
-	u32                    framerate;
-	bool                   is_valid;
-};
-
 struct msm_vidc_session_idle {
 	bool                   idle;
 	u64                    last_activity_time_ns;
@@ -821,6 +814,22 @@ struct msm_vidc_buffers {
 	bool                   reuse;
 };
 
+struct msm_vidc_sort {
+	struct list_head       list;
+	u64                    val;
+};
+
+struct msm_vidc_timestamp {
+	struct msm_vidc_sort   sort;
+	u64                    rank;
+};
+
+struct msm_vidc_timestamps {
+	struct list_head       list;
+	u32                    count;
+	u32                    rank;
+};
+
 struct msm_vidc_pool {
 	struct list_head       list;
 	u32                    count;

driver/vidc/src/msm_vdec.c

@@ -1422,6 +1422,8 @@ int msm_vdec_streamoff_input(struct msm_vidc_inst *inst)
 	if (rc)
 		return rc;
 
+	msm_vidc_flush_ts(inst);
+
 	return 0;
 }
 
@@ -1493,6 +1495,10 @@ int msm_vdec_streamon_input(struct msm_vidc_inst *inst)
 	if (rc)
 		goto error;
 
+	rc = msm_vidc_flush_ts(inst);
+	if (rc)
+		goto error;
+
 	return 0;
 
 error:
@@ -1983,6 +1989,7 @@ int msm_vdec_process_cmd(struct msm_vidc_inst *inst, u32 cmd)
 	}
 
 	if (cmd == V4L2_DEC_CMD_STOP) {
+		i_vpr_h(inst, "received cmd: drain\n");
 		allow = msm_vidc_allow_stop(inst);
 		if (allow == MSM_VIDC_DISALLOW)
 			return -EBUSY;
@@ -2002,6 +2009,7 @@ int msm_vdec_process_cmd(struct msm_vidc_inst *inst, u32 cmd)
 		if (rc)
 			return rc;
 	} else if (cmd == V4L2_DEC_CMD_START) {
+		i_vpr_h(inst, "received cmd: resume\n");
 		if (!msm_vidc_allow_start(inst))
 			return -EBUSY;
 		port = (inst->state == MSM_VIDC_DRAIN_LAST_FLAG) ? INPUT_PORT : OUTPUT_PORT;

driver/vidc/src/msm_venc.c

@@ -942,6 +942,7 @@ int msm_venc_process_cmd(struct msm_vidc_inst *inst, u32 cmd)
 	}
 
 	if (cmd == V4L2_ENC_CMD_STOP) {
+		i_vpr_h(inst, "received cmd: drain\n");
 		allow = msm_vidc_allow_stop(inst);
 		if (allow == MSM_VIDC_DISALLOW)
 			return -EBUSY;
@@ -961,6 +962,7 @@ int msm_venc_process_cmd(struct msm_vidc_inst *inst, u32 cmd)
 		if (rc)
 			return rc;
 	} else if (cmd == V4L2_ENC_CMD_START) {
+		i_vpr_h(inst, "received cmd: resume\n");
 		if (!msm_vidc_allow_start(inst))
 			return -EBUSY;
 		vb2_clear_last_buffer_dequeued(&inst->vb2q[OUTPUT_META_PORT]);

driver/vidc/src/msm_vidc.c

@@ -594,6 +594,8 @@ int msm_vidc_cmd(void *instance, union msm_v4l2_cmd *cmd)
 		enc = (struct v4l2_encoder_cmd *)cmd;
 		rc = msm_venc_process_cmd(inst, enc->cmd);
 	}
+	if (rc)
+		return rc;
 
 	return 0;
 }
@@ -819,6 +821,8 @@ void *msm_vidc_open(void *vidc_core, u32 session_type)
 	INIT_LIST_HEAD(&inst->pool.buffers.list);
 	INIT_LIST_HEAD(&inst->pool.mappings.list);
 	INIT_LIST_HEAD(&inst->pool.allocations.list);
+	INIT_LIST_HEAD(&inst->pool.timestamps.list);
+	INIT_LIST_HEAD(&inst->timestamps.list);
 	INIT_LIST_HEAD(&inst->buffers.input.list);
 	INIT_LIST_HEAD(&inst->buffers.input_meta.list);
 	INIT_LIST_HEAD(&inst->buffers.output.list);

driver/vidc/src/msm_vidc_driver.c

@@ -1914,6 +1914,210 @@ int msm_vidc_memory_unmap_completely(struct msm_vidc_inst *inst,
 	return rc;
 }
 
+int msm_vidc_calc_framerate(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_timestamp *ts;
+	struct msm_vidc_timestamp *prev = NULL;
+	u32 counter = 0;
+	u64 ts_ms = 0;
+
+	if (!inst) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	list_for_each_entry(ts, &inst->timestamps.list, sort.list) {
+		if (prev) {
+			if (ts->sort.val == prev->sort.val)
+				continue;
+
+			ts_ms += div_u64(ts->sort.val - prev->sort.val, 1000000);
+			counter++;
+		}
+		prev = ts;
+	}
+
+	return ts_ms ? (1000 * counter) / ts_ms : 0;
+}
+
+static int msm_vidc_insert_sort(struct list_head *head,
+	struct msm_vidc_sort *entry)
+{
+	struct msm_vidc_sort *first, *node;
+	struct msm_vidc_sort *prev = NULL;
+	bool is_inserted = false;
+
+	if (!head || !entry) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	if (list_empty(head)) {
+		list_add(&entry->list, head);
+		return 0;
+	}
+
+	first = list_first_entry(head, struct msm_vidc_sort, list);
+	if (entry->val < first->val) {
+		list_add(&entry->list, head);
+		return 0;
+	}
+
+	list_for_each_entry(node, head, list) {
+		if (prev &&
+			entry->val >= prev->val && entry->val <= node->val) {
+			list_add(&entry->list, &prev->list);
+			is_inserted = true;
+			break;
+		}
+		prev = node;
+	}
+
+	if (!is_inserted)
+		list_add(&entry->list, &prev->list);
+
+	return 0;
+}
+
+static struct msm_vidc_timestamp *msm_vidc_get_least_rank_ts(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_timestamp *ts, *final = NULL;
+	u64 least_rank = INT_MAX;
+
+	if (!inst) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return NULL;
+	}
+
+	list_for_each_entry(ts, &inst->timestamps.list, sort.list) {
+		if (ts->rank < least_rank) {
+			least_rank = ts->rank;
+			final = ts;
+		}
+	}
+
+	return final;
+}
+
+int msm_vidc_flush_ts(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_timestamp *temp, *ts = NULL;
+
+	if (!inst) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	list_for_each_entry_safe(ts, temp, &inst->timestamps.list, sort.list) {
+		i_vpr_l(inst, "%s: flushing ts: val %lld, rank %%lld\n",
+			__func__, ts->sort.val, ts->rank);
+		list_del(&ts->sort.list);
+		msm_vidc_put_ts(inst, ts);
+	}
+	inst->timestamps.count = 0;
+
+	return 0;
+}
+
+int msm_vidc_update_timestamp(struct msm_vidc_inst *inst, u64 timestamp)
+{
+	struct msm_vidc_timestamp *ts;
+	int rc = 0;
+
+	if (!inst) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	ts = msm_vidc_get_ts(inst);
+	if (!ts) {
+		i_vpr_e(inst, "%s: ts alloc failed\n", __func__);
+		return -ENOMEM;
+	}
+
+	INIT_LIST_HEAD(&ts->sort.list);
+	ts->sort.val = timestamp;
+	ts->rank = inst->timestamps.rank++;
+	rc = msm_vidc_insert_sort(&inst->timestamps.list, &ts->sort);
+	if (rc)
+		return rc;
+	inst->timestamps.count++;
+
+	/* keep sliding window of 10 ts nodes */
+	if (inst->timestamps.count > 10) {
+		ts = msm_vidc_get_least_rank_ts(inst);
+		if (!ts) {
+			i_vpr_e(inst, "%s: least rank ts is NULL\n", __func__);
+			return -EINVAL;
+		}
+		inst->timestamps.count--;
+		list_del(&ts->sort.list);
+		msm_vidc_put_ts(inst, ts);
+	}
+
+	return 0;
+}
+
+struct msm_vidc_timestamp *msm_vidc_get_ts(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_timestamp *ts = NULL;
+
+	if (!inst) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return NULL;
+	}
+
+	if (!list_empty(&inst->pool.timestamps.list)) {
+		ts = list_first_entry(&inst->pool.timestamps.list,
+			struct msm_vidc_timestamp, sort.list);
+		inst->pool.timestamps.count--;
+		list_del(&ts->sort.list);
+		memset(ts, 0, sizeof(struct msm_vidc_timestamp));
+		return ts;
+	}
+
+	ts = kzalloc(sizeof(struct msm_vidc_timestamp), GFP_KERNEL);
+	if (!ts) {
+		i_vpr_e(inst, "%s: ts failed\n", __func__);
+		return NULL;
+	}
+
+	return ts;
+}
+
+int msm_vidc_put_ts(struct msm_vidc_inst *inst, struct msm_vidc_timestamp *ts)
+{
+	if (!inst || !ts) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	inst->pool.timestamps.count++;
+	list_add_tail(&ts->sort.list, &inst->pool.timestamps.list);
+
+	return 0;
+}
+
+int msm_vidc_destroy_ts(struct msm_vidc_inst *inst)
+{
+	struct msm_vidc_timestamp *ts, *temp;
+
+	if (!inst) {
+		d_vpr_e("%s: Invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	i_vpr_h(inst, "%s: pool: ts count %u\n", __func__, inst->pool.timestamps.count);
+
+	/* free all timestamps from pool */
+	list_for_each_entry_safe(ts, temp, &inst->pool.timestamps.list, sort.list) {
+		list_del(&ts->sort.list);
+		kfree(ts);
+	}
+
+	return 0;
+}
+
 struct msm_vidc_buffer *msm_vidc_get_vidc_buffer(struct msm_vidc_inst *inst)
 {
 	struct msm_vidc_buffer *buf = NULL;
@@ -3801,7 +4005,7 @@ static void __strict_check(struct msm_vidc_core *core)
 
 static int msm_vidc_core_init_wait(struct msm_vidc_core *core)
 {
-	static const int interval = 40;
+	const int interval = 40;
 	int max_tries, count = 0, rc = 0;
 
 	if (!core || !core->capabilities) {
@@ -4084,7 +4288,7 @@ void msm_vidc_ssr_handler(struct work_struct *work)
 	ssr = &core->ssr;
 
 	core_lock(core, __func__);
-	if (core->state != MSM_VIDC_CORE_DEINIT) {
+	if (core->state == MSM_VIDC_CORE_INIT) {
 		/*
 		 * In current implementation, user-initiated SSR triggers
 		 * a fatal error from hardware. However, there is no way
@@ -4278,6 +4482,7 @@ void msm_vidc_destroy_buffers(struct msm_vidc_inst *inst)
 {
 	struct msm_vidc_buffers *buffers;
 	struct msm_vidc_buffer *buf, *dummy;
+	struct msm_vidc_timestamp *ts, *dummy_ts;
 	static const enum msm_vidc_buffer_type ext_buf_types[] = {
 		MSM_VIDC_BUF_INPUT,
 		MSM_VIDC_BUF_OUTPUT,
@@ -4339,11 +4544,18 @@ void msm_vidc_destroy_buffers(struct msm_vidc_inst *inst)
 		msm_vidc_put_vidc_buffer(inst, buf);
 	}
 
+	list_for_each_entry_safe(ts, dummy_ts, &inst->timestamps.list, sort.list) {
+		i_vpr_e(inst, "%s: removing ts: val %lld, rank %%lld\n",
+			__func__, ts->sort.val, ts->rank);
+		list_del(&ts->sort.list);
+		msm_vidc_put_ts(inst, ts);
+	}
+
 	/* destroy buffers from pool */
 	msm_vidc_destroy_vidc_buffer(inst);
 	msm_vidc_destroy_alloc_buffer(inst);
 	msm_vidc_destroy_map_buffer(inst);
-
+	msm_vidc_destroy_ts(inst);
 }
 
 static void msm_vidc_close_helper(struct kref *kref)

driver/vidc/src/venus_hfi.c

@@ -3317,6 +3317,7 @@ int venus_hfi_queue_super_buffer(struct msm_vidc_inst *inst,
 		/* Create yuv packet */
 		update_offset(hfi_buffer.addr_offset, (cnt ? frame_size : 0u));
 		update_timestamp(hfi_buffer.timestamp, (cnt ? ts_delta_us : 0u));
+		msm_vidc_update_timestamp(inst, hfi_buffer.timestamp);
 		rc = hfi_create_packet(inst->packet,
 				inst->packet_size,
 				HFI_CMD_BUFFER,
@@ -3384,6 +3385,9 @@ int venus_hfi_queue_buffer(struct msm_vidc_inst *inst,
 	if (rc)
 		goto unlock;
 
+	if (is_encode_session(inst) && is_input_buffer(buffer->type))
+		msm_vidc_update_timestamp(inst, hfi_buffer.timestamp);
+
 	rc = hfi_create_header(inst->packet, inst->packet_size,
 			   inst->session_id, core->header_id++);
 	if (rc)

driver/vidc/src/venus_hfi_response.c

@@ -779,6 +779,9 @@ static int handle_output_buffer(struct msm_vidc_inst *inst,
 		inst->power.fw_cr = inst->hfi_frame_info.cr;
 	}
 
+	if (is_decode_session(inst) && buf->data_size)
+		msm_vidc_update_timestamp(inst, buf->timestamp);
+
 	print_vidc_buffer(VIDC_HIGH, "high", "dqbuf", inst, buf);
 	msm_vidc_debugfs_update(inst, MSM_VIDC_DEBUGFS_EVENT_FBD);
 
@@ -1278,6 +1281,7 @@ static int handle_session_property(struct msm_vidc_inst *inst,
 				__func__, pkt->port, pkt->type);
 			break;
 		}
+		i_vpr_e(inst, "received prop_no_output attached input\n");
 		inst->hfi_frame_info.no_output = 1;
 		break;
 	case HFI_PROP_WORST_COMPRESSION_RATIO: