video: driver: refine core state machine

introduced core error state and added changes
to refine core state machine.

Change-Id: Ib3b94fd3798e902b7a6cfc5de45820558c89806e
Signed-off-by: Govindaraj Rajagopal <quic_grajagop@quicinc.com>

Kbuild

@@ -79,6 +79,7 @@ msm_video-objs += driver/vidc/src/msm_vidc_v4l2.o \
                   driver/vidc/src/msm_vdec.o \
                   driver/vidc/src/msm_venc.o \
                   driver/vidc/src/msm_vidc_driver.o \
+                  driver/vidc/src/msm_vidc_state.o \
                   driver/vidc/src/msm_vidc_control.o \
                   driver/vidc/src/msm_vidc_control_ext.o \
                   driver/vidc/src/msm_vidc_buffer.o \

driver/variant/common/src/msm_vidc_variant.c

@@ -9,6 +9,7 @@
 
 #include "msm_vidc_core.h"
 #include "msm_vidc_driver.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_variant.h"
 #include "msm_vidc_platform.h"

driver/variant/iris2/src/msm_vidc_iris2.c

@@ -12,6 +12,7 @@
 #include "msm_vidc_platform.h"
 #include "msm_vidc_internal.h"
 #include "msm_vidc_buffer.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_variant.h"
 

driver/variant/iris3/src/msm_vidc_iris3.c

@@ -14,6 +14,7 @@
 #include "msm_vidc_platform.h"
 #include "msm_vidc_internal.h"
 #include "msm_vidc_buffer.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_variant.h"
 

driver/variant/iris33/src/msm_vidc_iris33.c

@@ -14,6 +14,7 @@
 #include "msm_vidc_platform.h"
 #include "msm_vidc_internal.h"
 #include "msm_vidc_buffer.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_variant.h"
 

driver/vidc/inc/msm_vidc_core.h

@@ -10,6 +10,7 @@
 #include <linux/platform_device.h>
 
 #include "msm_vidc_internal.h"
+#include "msm_vidc_state.h"
 #include "venus_hfi_queue.h"
 #include "resources.h"
 
@@ -17,13 +18,6 @@ struct msm_vidc_core;
 
 #define MAX_EVENTS 30
 
-#define FOREACH_CORE_STATE(CORE_STATE) {               \
-	CORE_STATE(CORE_DEINIT)                        \
-	CORE_STATE(CORE_INIT_WAIT)                     \
-	CORE_STATE(CORE_INIT)                          \
-	CORE_STATE(CORE_ERROR)                         \
-}
-
 #define call_venus_op(d, op, ...)			\
 	(((d) && (d)->venus_ops && (d)->venus_ops->op) ? \
 	((d)->venus_ops->op(__VA_ARGS__)):0)
@@ -64,20 +58,6 @@ struct msm_vidc_core_power {
 	u64 bw_llcc;
 };
 
-enum msm_vidc_core_state FOREACH_CORE_STATE(GENERATE_MSM_VIDC_ENUM);
-
-enum msm_vidc_core_sub_state {
-	CORE_SUBSTATE_NONE                   = 0x0,
-	CORE_SUBSTATE_POWER_ENABLE           = BIT(0),
-	CORE_SUBSTATE_GDSC_HANDOFF           = BIT(1),
-	CORE_SUBSTATE_PM_SUSPEND             = BIT(2),
-	CORE_SUBSTATE_FW_PWR_CTRL            = BIT(3),
-	CORE_SUBSTATE_PAGE_FAULT             = BIT(4),
-	CORE_SUBSTATE_CPU_WATCHDOG           = BIT(5),
-	CORE_SUBSTATE_VIDEO_UNRESPONSIVE     = BIT(6),
-	CORE_SUBSTATE_MAX                    = BIT(7),
-};
-
 struct msm_vidc_core {
 	struct platform_device                *pdev;
 	struct msm_video_device                vdev[2];
@@ -89,6 +69,9 @@ struct msm_vidc_core {
 	struct dentry                         *debugfs_root;
 	char                                   fw_version[MAX_NAME_LENGTH];
 	enum msm_vidc_core_state               state;
+	int                                  (*state_handle)(struct msm_vidc_core *core,
+					       enum msm_vidc_core_event_type type,
+					       struct msm_vidc_event_data *data);
 	enum msm_vidc_core_sub_state           sub_state;
 	char                                   sub_state_name[MAX_NAME_LENGTH];
 	struct mutex                           lock;
@@ -132,10 +115,4 @@ struct msm_vidc_core {
 	u32                                    sys_init_id;
 };
 
-static inline bool core_in_valid_state(struct msm_vidc_core *core)
-{
-	return (core->state == MSM_VIDC_CORE_INIT ||
-		core->state == MSM_VIDC_CORE_INIT_WAIT);
-}
-
 #endif // _MSM_VIDC_CORE_H_

driver/vidc/inc/msm_vidc_driver.h

@@ -370,17 +370,6 @@ static inline bool is_sub_state(struct msm_vidc_inst *inst,
 	return (inst->sub_state & sub_state);
 }
 
-static inline bool is_core_state(struct msm_vidc_core *core, enum msm_vidc_core_state state)
-{
-	return core->state == state;
-}
-
-static inline bool is_core_sub_state(struct msm_vidc_core *core,
-	enum msm_vidc_core_sub_state sub_state)
-{
-	return !!(core->sub_state & sub_state);
-}
-
 const char *cap_name(enum msm_vidc_inst_capability_type cap_id);
 const char *v4l2_pixelfmt_name(struct msm_vidc_inst *inst, u32 pixelfmt);
 const char *v4l2_type_name(u32 port);
@@ -412,7 +401,6 @@ int v4l2_type_to_driver_port(struct msm_vidc_inst *inst, u32 type,
 	const char *func);
 const char *allow_name(enum msm_vidc_allow allow);
 const char *state_name(enum msm_vidc_state state);
-const char *core_state_name(enum msm_vidc_core_state state);
 int msm_vidc_change_state(struct msm_vidc_inst *inst,
 	enum msm_vidc_state request_state, const char *func);
 int msm_vidc_change_sub_state(struct msm_vidc_inst *inst,

driver/vidc/inc/msm_vidc_internal.h

@@ -636,6 +636,17 @@ struct msm_vidc_inst_cap_entry {
 	enum msm_vidc_inst_capability_type cap_id;
 };
 
+struct msm_vidc_event_data {
+	union {
+		bool                         bval;
+		u32                          uval;
+		u64                          uval64;
+		s32                          val;
+		s64                          val64;
+		void                        *ptr;
+	} edata;
+};
+
 struct debug_buf_count {
 	u64 etb;
 	u64 ftb;

driver/vidc/inc/msm_vidc_state.h

@@ -0,0 +1,59 @@
+/* SPDX-License-Identifier: GPL-2.0-only */
+/*
+ * Copyright (c) 2020-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#ifndef _MSM_VIDC_STATE_H_
+#define _MSM_VIDC_STATE_H_
+
+#include "msm_vidc_internal.h"
+
+struct msm_vidc_core;
+
+#define FOREACH_CORE_STATE(CORE_STATE) {               \
+	CORE_STATE(CORE_DEINIT)                        \
+	CORE_STATE(CORE_INIT_WAIT)                     \
+	CORE_STATE(CORE_INIT)                          \
+	CORE_STATE(CORE_ERROR)                         \
+}
+
+enum msm_vidc_core_state FOREACH_CORE_STATE(GENERATE_MSM_VIDC_ENUM);
+
+enum msm_vidc_core_sub_state {
+	CORE_SUBSTATE_NONE                   = 0x0,
+	CORE_SUBSTATE_POWER_ENABLE           = BIT(0),
+	CORE_SUBSTATE_GDSC_HANDOFF           = BIT(1),
+	CORE_SUBSTATE_PM_SUSPEND             = BIT(2),
+	CORE_SUBSTATE_FW_PWR_CTRL            = BIT(3),
+	CORE_SUBSTATE_PAGE_FAULT             = BIT(4),
+	CORE_SUBSTATE_CPU_WATCHDOG           = BIT(5),
+	CORE_SUBSTATE_VIDEO_UNRESPONSIVE     = BIT(6),
+	CORE_SUBSTATE_MAX                    = BIT(7),
+};
+
+enum msm_vidc_core_event_type {
+	CORE_EVENT_NONE                      = BIT(0),
+	CORE_EVENT_UPDATE_SUB_STATE          = BIT(1),
+};
+
+struct msm_vidc_core_state_handle {
+	enum msm_vidc_core_state   state;
+	int                      (*handle)(struct msm_vidc_core *core,
+				   enum msm_vidc_core_event_type type,
+				   struct msm_vidc_event_data *data);
+};
+
+enum msm_vidc_allow msm_vidc_allow_core_state_change(
+	struct msm_vidc_core *core,
+	enum msm_vidc_core_state req_state);
+int msm_vidc_update_core_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_state request_state, const char *func);
+bool core_in_valid_state(struct msm_vidc_core *core);
+bool is_core_state(struct msm_vidc_core *core, enum msm_vidc_core_state state);
+bool is_core_sub_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_sub_state sub_state);
+const char *core_state_name(enum msm_vidc_core_state state);
+const char *core_sub_state_name(enum msm_vidc_core_sub_state sub_state);
+
+#endif // _MSM_VIDC_STATE_H_

driver/vidc/src/msm_vidc_driver.c

@@ -13,6 +13,7 @@
 #include "msm_vidc_internal.h"
 #include "msm_vidc_control.h"
 #include "msm_vidc_memory.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_power.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_power.h"
@@ -130,39 +131,6 @@ const char *sub_state_name(enum msm_vidc_sub_state sub_state)
 	return "SUB_STATE_NONE";
 }
 
-static const char * const core_state_name_arr[] =
-	FOREACH_CORE_STATE(GENERATE_STRING);
-
-const char *core_state_name(enum msm_vidc_core_state state)
-{
-	const char *name = "UNKNOWN STATE";
-
-	if (state >= ARRAY_SIZE(core_state_name_arr))
-		goto exit;
-
-	name = core_state_name_arr[state];
-
-exit:
-	return name;
-}
-
-const char *core_sub_state_name(enum msm_vidc_core_sub_state sub_state)
-{
-	switch (sub_state) {
-	case CORE_SUBSTATE_NONE:                 return "NONE ";
-	case CORE_SUBSTATE_GDSC_HANDOFF:         return "GDSC_HANDOFF ";
-	case CORE_SUBSTATE_PM_SUSPEND:           return "PM_SUSPEND ";
-	case CORE_SUBSTATE_FW_PWR_CTRL:          return "FW_PWR_CTRL ";
-	case CORE_SUBSTATE_POWER_ENABLE:         return "POWER_ENABLE ";
-	case CORE_SUBSTATE_PAGE_FAULT:           return "PAGE_FAULT ";
-	case CORE_SUBSTATE_CPU_WATCHDOG:         return "CPU_WATCHDOG ";
-	case CORE_SUBSTATE_VIDEO_UNRESPONSIVE:   return "VIDEO_UNRESPONSIVE ";
-	case CORE_SUBSTATE_MAX:                  return "MAX ";
-	}
-
-	return "UNKNOWN ";
-}
-
 const char *v4l2_type_name(u32 port)
 {
 	switch (port) {
@@ -1029,77 +997,6 @@ int signal_session_msg_receipt(struct msm_vidc_inst *inst,
 	return 0;
 }
 
-int msm_vidc_change_core_state(struct msm_vidc_core *core,
-	enum msm_vidc_core_state request_state, const char *func)
-{
-	int rc = 0;
-
-	if (!core) {
-		d_vpr_e("%s: invalid params\n", __func__);
-		return -EINVAL;
-	}
-
-	/* core must be locked */
-	rc = __strict_check(core, func);
-	if (rc) {
-		d_vpr_e("%s(): core was not locked\n", func);
-		return rc;
-	}
-
-	d_vpr_h("%s: core state changed to %s from %s\n",
-		func, core_state_name(request_state),
-		core_state_name(core->state));
-	core->state = request_state;
-	return 0;
-}
-
-int msm_vidc_change_core_sub_state(struct msm_vidc_core *core,
-		enum msm_vidc_core_sub_state clear_sub_state,
-		enum msm_vidc_core_sub_state set_sub_state, const char *func)
-{
-	int i = 0;
-	enum msm_vidc_core_sub_state prev_sub_state;
-
-	if (!core) {
-		d_vpr_e("%s: invalid params\n", __func__);
-		return -EINVAL;
-	}
-
-	/* final value will not change */
-	if (clear_sub_state == set_sub_state)
-		return 0;
-
-	/* sanitize clear & set value */
-	if (set_sub_state > CORE_SUBSTATE_MAX ||
-		clear_sub_state > CORE_SUBSTATE_MAX) {
-		d_vpr_e("%s: invalid sub states. clear %#x or set %#x\n",
-			func, clear_sub_state, set_sub_state);
-		return -EINVAL;
-	}
-
-	prev_sub_state = core->sub_state;
-	core->sub_state |= set_sub_state;
-	core->sub_state &= ~clear_sub_state;
-
-	/* print substates only when there is a change */
-	if (core->sub_state != prev_sub_state) {
-		strscpy(core->sub_state_name, "\0", sizeof(core->sub_state_name));
-		for (i = 0; BIT(i) < CORE_SUBSTATE_MAX; i++) {
-			if (core->sub_state == CORE_SUBSTATE_NONE) {
-				strscpy(core->sub_state_name, "CORE_SUBSTATE_NONE",
-					sizeof(core->sub_state_name));
-				break;
-			}
-			if (core->sub_state & BIT(i))
-				strlcat(core->sub_state_name, core_sub_state_name(BIT(i)),
-					sizeof(core->sub_state_name));
-		}
-		d_vpr_h("%s: core sub state changed to %s\n", func, core->sub_state_name);
-	}
-
-	return 0;
-}
-
 int msm_vidc_change_state(struct msm_vidc_inst *inst,
 		enum msm_vidc_state request_state, const char *func)
 {
@@ -4722,6 +4619,7 @@ int msm_vidc_core_deinit_locked(struct msm_vidc_core *core, bool force)
 {
 	int rc = 0;
 	struct msm_vidc_inst *inst, *dummy;
+	enum msm_vidc_allow allow;
 
 	if (!core) {
 		d_vpr_e("%s: invalid params\n", __func__);
@@ -4737,6 +4635,13 @@ int msm_vidc_core_deinit_locked(struct msm_vidc_core *core, bool force)
 	if (is_core_state(core, MSM_VIDC_CORE_DEINIT))
 		return 0;
 
+	/* print error for state change not allowed case */
+	allow = msm_vidc_allow_core_state_change(core, MSM_VIDC_CORE_DEINIT);
+	if (allow != MSM_VIDC_ALLOW)
+		d_vpr_e("%s: %s core state change %s -> %s\n", __func__,
+			allow_name(allow), core_state_name(core->state),
+			core_state_name(MSM_VIDC_CORE_DEINIT));
+
 	if (force) {
 		d_vpr_e("%s(): force deinit core\n", __func__);
 	} else {
@@ -4840,6 +4745,7 @@ unlock:
 
 int msm_vidc_core_init(struct msm_vidc_core *core)
 {
+	enum msm_vidc_allow allow;
 	int rc = 0;
 
 	if (!core || !core->capabilities) {
@@ -4857,6 +4763,13 @@ int msm_vidc_core_init(struct msm_vidc_core *core)
 		goto unlock;
 	}
 
+	/* print error for state change not allowed case */
+	allow = msm_vidc_allow_core_state_change(core, MSM_VIDC_CORE_INIT_WAIT);
+	if (allow != MSM_VIDC_ALLOW)
+		d_vpr_e("%s: %s core state change %s -> %s\n", __func__,
+			allow_name(allow), core_state_name(core->state),
+			core_state_name(MSM_VIDC_CORE_INIT_WAIT));
+
 	msm_vidc_change_core_state(core, MSM_VIDC_CORE_INIT_WAIT, __func__);
 	/* clear PM suspend from core sub_state */
 	msm_vidc_change_core_sub_state(core, CORE_SUBSTATE_PM_SUSPEND, 0, __func__);

driver/vidc/src/msm_vidc_probe.c

@@ -19,6 +19,7 @@
 #include "msm_vidc_internal.h"
 #include "msm_vidc_debug.h"
 #include "msm_vidc_driver.h"
+#include "msm_vidc_state.h"
 #include "msm_vidc_platform.h"
 #include "msm_vidc_core.h"
 #include "msm_vidc_memory.h"
@@ -274,7 +275,7 @@ static int msm_vidc_deinitialize_core(struct msm_vidc_core *core)
 	d_vpr_h("%s()\n", __func__);
 
 	mutex_destroy(&core->lock);
-	msm_vidc_change_core_state(core, MSM_VIDC_CORE_DEINIT, __func__);
+	msm_vidc_update_core_state(core, MSM_VIDC_CORE_DEINIT, __func__);
 
 	msm_vidc_vmem_free((void **)&core->response_packet);
 	msm_vidc_vmem_free((void **)&core->packet);
@@ -303,7 +304,7 @@ static int msm_vidc_initialize_core(struct msm_vidc_core *core)
 	}
 	d_vpr_h("%s()\n", __func__);
 
-	msm_vidc_change_core_state(core, MSM_VIDC_CORE_DEINIT, __func__);
+	msm_vidc_update_core_state(core, MSM_VIDC_CORE_DEINIT, __func__);
 
 	core->pm_workq = create_singlethread_workqueue("pm_workq");
 	if (!core->pm_workq) {

driver/vidc/src/msm_vidc_state.c

@@ -0,0 +1,458 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2020-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ */
+
+#include "msm_vidc_driver.h"
+#include "msm_vidc_state.h"
+#include "msm_vidc_debug.h"
+#include "msm_vidc_core.h"
+
+bool core_in_valid_state(struct msm_vidc_core *core)
+{
+	return (core->state == MSM_VIDC_CORE_INIT ||
+		core->state == MSM_VIDC_CORE_INIT_WAIT);
+}
+
+bool is_core_state(struct msm_vidc_core *core, enum msm_vidc_core_state state)
+{
+	return core->state == state;
+}
+
+static const char * const core_state_name_arr[] =
+	FOREACH_CORE_STATE(GENERATE_STRING);
+
+const char *core_state_name(enum msm_vidc_core_state state)
+{
+	const char *name = "UNKNOWN STATE";
+
+	if (state >= ARRAY_SIZE(core_state_name_arr))
+		goto exit;
+
+	name = core_state_name_arr[state];
+
+exit:
+	return name;
+}
+
+static int __strict_check(struct msm_vidc_core *core, const char *function)
+{
+	bool fatal = !mutex_is_locked(&core->lock);
+
+	WARN_ON(fatal);
+
+	if (fatal)
+		d_vpr_e("%s: strict check failed\n", function);
+
+	return fatal ? -EINVAL : 0;
+}
+
+static int msm_vidc_core_deinit_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_event_type type,
+	struct msm_vidc_event_data *data)
+{
+	if (!core || !data) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	d_vpr_e("%s: unexpected core event type %u\n", __func__, type);
+	return -EINVAL;
+}
+
+static int msm_vidc_core_init_wait_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_event_type type,
+	struct msm_vidc_event_data *data)
+{
+	int rc = 0;
+
+	if (!core || !data) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	switch (type) {
+	case CORE_EVENT_UPDATE_SUB_STATE:
+	{
+		u32 req_sub_state;
+		u32 allow_mask = -1;
+
+		req_sub_state = data->edata.uval;
+
+		/* none of the requested substate supported */
+		if (!(req_sub_state & allow_mask)) {
+			d_vpr_e("%s: invalid substate update request %#x\n",
+				__func__, req_sub_state);
+			return -EINVAL;
+		}
+
+		/* update core substate */
+		core->sub_state |= req_sub_state & allow_mask;
+		return rc;
+	}
+	default: {
+		d_vpr_e("%s: unexpected core event type %u\n",
+			__func__, type);
+		return -EINVAL;
+	}
+	}
+
+	return rc;
+}
+
+static int msm_vidc_core_init_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_event_type type,
+	struct msm_vidc_event_data *data)
+{
+	int rc = 0;
+
+	if (!core || !data) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	switch (type) {
+	case CORE_EVENT_UPDATE_SUB_STATE:
+	{
+		u32 req_sub_state;
+		u32 allow_mask = -1;
+
+		req_sub_state = data->edata.uval;
+
+		/* none of the requested substate supported */
+		if (!(req_sub_state & allow_mask)) {
+			d_vpr_e("%s: invalid substate update request %#x\n",
+				__func__, req_sub_state);
+			return -EINVAL;
+		}
+
+		/* update core substate */
+		core->sub_state |= req_sub_state & allow_mask;
+		return rc;
+	}
+	default: {
+		d_vpr_e("%s: unexpected core event type %u\n",
+			__func__, type);
+		return -EINVAL;
+	}
+	}
+
+	return rc;
+}
+
+static int msm_vidc_core_error_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_event_type type,
+	struct msm_vidc_event_data *data)
+{
+	int rc = 0;
+
+	if (!core || !data) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	switch (type) {
+	case CORE_EVENT_UPDATE_SUB_STATE:
+	{
+		u32 req_sub_state;
+		u32 allow_mask = -1;
+
+		req_sub_state = data->edata.uval;
+
+		/* none of the requested substate supported */
+		if (!(req_sub_state & allow_mask)) {
+			d_vpr_e("%s: invalid substate update request %#x\n",
+				__func__, req_sub_state);
+			return -EINVAL;
+		}
+
+		/* update core substate */
+		core->sub_state |= req_sub_state & allow_mask;
+		return rc;
+	}
+	default: {
+		d_vpr_e("%s: unexpected core event type %u\n",
+			__func__, type);
+		return -EINVAL;
+	}
+	}
+
+	return rc;
+}
+
+struct msm_vidc_core_state_handle *msm_vidc_get_core_state_handle(
+	enum msm_vidc_core_state req_state)
+{
+	int cnt;
+	struct msm_vidc_core_state_handle *core_state_handle = NULL;
+	static struct msm_vidc_core_state_handle state_handle[] = {
+		{MSM_VIDC_CORE_DEINIT,      msm_vidc_core_deinit_state      },
+		{MSM_VIDC_CORE_INIT_WAIT,   msm_vidc_core_init_wait_state   },
+		{MSM_VIDC_CORE_INIT,        msm_vidc_core_init_state        },
+		{MSM_VIDC_CORE_ERROR,       msm_vidc_core_error_state       },
+	};
+
+	for (cnt = 0; cnt < ARRAY_SIZE(state_handle); cnt++) {
+		if (state_handle[cnt].state == req_state) {
+			core_state_handle = &state_handle[cnt];
+			break;
+		}
+	}
+
+	/* if req_state does not exist in the table */
+	if (cnt == ARRAY_SIZE(state_handle)) {
+		d_vpr_e("%s: invalid core state \"%s\" requested\n",
+			__func__, core_state_name(req_state));
+		return core_state_handle;
+	}
+
+	return core_state_handle;
+}
+
+int msm_vidc_update_core_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_state request_state, const char *func)
+{
+	struct msm_vidc_core_state_handle *state_handle = NULL;
+	int rc = 0;
+
+	/* get core state handler for requested state */
+	state_handle = msm_vidc_get_core_state_handle(request_state);
+	if (!state_handle)
+		return -EINVAL;
+
+	d_vpr_h("%s: core state changed to %s from %s\n", func,
+		core_state_name(state_handle->state), core_state_name(core->state));
+
+	/* finally update core state and handler */
+	core->state = state_handle->state;
+	core->state_handle = state_handle->handle;
+
+	return rc;
+}
+
+struct msm_vidc_core_state_allow {
+	enum msm_vidc_core_state   from;
+	enum msm_vidc_core_state   to;
+	enum msm_vidc_allow        allow;
+};
+
+enum msm_vidc_allow msm_vidc_allow_core_state_change(
+	struct msm_vidc_core *core,
+	enum msm_vidc_core_state req_state)
+{
+	int cnt;
+	enum msm_vidc_allow allow = MSM_VIDC_DISALLOW;
+	static struct msm_vidc_core_state_allow state[] = {
+		/* from, to, allow */
+		{MSM_VIDC_CORE_DEINIT,      MSM_VIDC_CORE_DEINIT,      MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_DEINIT,      MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_DEINIT,      MSM_VIDC_CORE_INIT,        MSM_VIDC_DISALLOW  },
+		{MSM_VIDC_CORE_DEINIT,      MSM_VIDC_CORE_ERROR,       MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_CORE_DEINIT,      MSM_VIDC_DISALLOW  },
+		{MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_CORE_INIT,        MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_CORE_ERROR,       MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_INIT,        MSM_VIDC_CORE_DEINIT,      MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_INIT,        MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_DISALLOW  },
+		{MSM_VIDC_CORE_INIT,        MSM_VIDC_CORE_INIT,        MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_INIT,        MSM_VIDC_CORE_ERROR,       MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_ERROR,       MSM_VIDC_CORE_DEINIT,      MSM_VIDC_ALLOW     },
+		{MSM_VIDC_CORE_ERROR,       MSM_VIDC_CORE_INIT_WAIT,   MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_ERROR,       MSM_VIDC_CORE_INIT,        MSM_VIDC_IGNORE    },
+		{MSM_VIDC_CORE_ERROR,       MSM_VIDC_CORE_ERROR,       MSM_VIDC_IGNORE    },
+	};
+
+	for (cnt = 0; cnt < ARRAY_SIZE(state); cnt++) {
+		if (state[cnt].from == core->state && state[cnt].to == req_state) {
+			allow = state[cnt].allow;
+			break;
+		}
+	}
+
+	return allow;
+}
+
+int msm_vidc_change_core_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_state request_state, const char *func)
+{
+	enum msm_vidc_allow allow;
+	int rc = 0;
+
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	/* core must be locked */
+	rc = __strict_check(core, func);
+	if (rc) {
+		d_vpr_e("%s(): core was not locked\n", func);
+		return rc;
+	}
+
+	/* current and requested state is same */
+	if (core->state == request_state)
+		return 0;
+
+	/* check if requested state movement is allowed */
+	allow = msm_vidc_allow_core_state_change(core, request_state);
+	if (allow == MSM_VIDC_IGNORE) {
+		d_vpr_h("%s: %s core state change %s -> %s\n", func,
+			allow_name(allow), core_state_name(core->state),
+			core_state_name(request_state));
+		return 0;
+	} else if (allow == MSM_VIDC_DISALLOW) {
+		d_vpr_e("%s: %s core state change %s -> %s\n", func,
+			allow_name(allow), core_state_name(core->state),
+			core_state_name(request_state));
+		return -EINVAL;
+	}
+
+	/* go ahead and update core state */
+	rc = msm_vidc_update_core_state(core, request_state, func);
+	if (rc)
+		return rc;
+
+	return rc;
+}
+
+bool is_core_sub_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_sub_state sub_state)
+{
+	return !!(core->sub_state & sub_state);
+}
+
+const char *core_sub_state_name(enum msm_vidc_core_sub_state sub_state)
+{
+	switch (sub_state) {
+	case CORE_SUBSTATE_NONE:                 return "NONE ";
+	case CORE_SUBSTATE_GDSC_HANDOFF:         return "GDSC_HANDOFF ";
+	case CORE_SUBSTATE_PM_SUSPEND:           return "PM_SUSPEND ";
+	case CORE_SUBSTATE_FW_PWR_CTRL:          return "FW_PWR_CTRL ";
+	case CORE_SUBSTATE_POWER_ENABLE:         return "POWER_ENABLE ";
+	case CORE_SUBSTATE_PAGE_FAULT:           return "PAGE_FAULT ";
+	case CORE_SUBSTATE_CPU_WATCHDOG:         return "CPU_WATCHDOG ";
+	case CORE_SUBSTATE_VIDEO_UNRESPONSIVE:   return "VIDEO_UNRESPONSIVE ";
+	case CORE_SUBSTATE_MAX:                  return "MAX ";
+	}
+
+	return "UNKNOWN ";
+}
+
+static int prepare_core_sub_state_name(enum msm_vidc_core_sub_state sub_state,
+	char *buf, u32 size)
+{
+	int i = 0;
+
+	if (!buf || !size)
+		return -EINVAL;
+
+	strscpy(buf, "\0", size);
+	if (sub_state == CORE_SUBSTATE_NONE) {
+		strscpy(buf, "CORE_SUBSTATE_NONE", size);
+		return 0;
+	}
+
+	for (i = 0; BIT(i) < CORE_SUBSTATE_MAX; i++) {
+		if (sub_state & BIT(i))
+			strlcat(buf, core_sub_state_name(BIT(i)), size);
+	}
+
+	return 0;
+}
+
+static int msm_vidc_update_core_sub_state(struct msm_vidc_core *core,
+	enum msm_vidc_core_sub_state sub_state, const char *func)
+{
+	struct msm_vidc_event_data data;
+	char sub_state_name[MAX_NAME_LENGTH];
+	int ret = 0, rc = 0;
+
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	/* no substate update */
+	if (!sub_state)
+		return 0;
+
+	/* invoke update core substate event */
+	memset(&data, 0, sizeof(struct msm_vidc_event_data));
+	data.edata.uval = sub_state;
+	rc = core->state_handle(core, CORE_EVENT_UPDATE_SUB_STATE, &data);
+	if (rc) {
+		ret = prepare_core_sub_state_name(sub_state,
+			 sub_state_name, sizeof(sub_state_name) - 1);
+		if (!ret)
+			d_vpr_e("%s: state %s, requested invalid core substate %s\n",
+				func, core_state_name(core->state), sub_state_name);
+		return rc;
+	}
+
+	return rc;
+}
+
+int msm_vidc_change_core_sub_state(struct msm_vidc_core *core,
+		enum msm_vidc_core_sub_state clear_sub_state,
+		enum msm_vidc_core_sub_state set_sub_state, const char *func)
+{
+	int rc = 0;
+	enum msm_vidc_core_sub_state prev_sub_state;
+
+	if (!core) {
+		d_vpr_e("%s: invalid params\n", __func__);
+		return -EINVAL;
+	}
+
+	/* core must be locked */
+	rc = __strict_check(core, func);
+	if (rc) {
+		d_vpr_e("%s(): core was not locked\n", func);
+		return rc;
+	}
+
+	/* sanitize core state handler */
+	if (!core->state_handle) {
+		d_vpr_e("%s: invalid core state handle\n", __func__);
+		return -EINVAL;
+	}
+
+	/* final value will not change */
+	if (clear_sub_state == set_sub_state)
+		return 0;
+
+	/* sanitize clear & set value */
+	if (set_sub_state > CORE_SUBSTATE_MAX ||
+		clear_sub_state > CORE_SUBSTATE_MAX) {
+		d_vpr_e("%s: invalid sub states. clear %#x or set %#x\n",
+			func, clear_sub_state, set_sub_state);
+		return -EINVAL;
+	}
+
+	prev_sub_state = core->sub_state;
+
+	/* set sub state */
+	rc = msm_vidc_update_core_sub_state(core, set_sub_state, func);
+	if (rc)
+		return rc;
+
+	/* check if all core substates updated */
+	if ((core->sub_state & set_sub_state) != set_sub_state)
+		d_vpr_e("%s: all substates not updated %#x, expected %#x\n",
+			func, core->sub_state & set_sub_state, set_sub_state);
+
+	/* clear sub state */
+	core->sub_state &= ~clear_sub_state;
+
+	/* print substates only when there is a change */
+	if (core->sub_state != prev_sub_state) {
+		rc = prepare_core_sub_state_name(core->sub_state, core->sub_state_name,
+			sizeof(core->sub_state_name) - 1);
+		if (!rc)
+			d_vpr_h("%s: core sub state changed to %s\n", func, core->sub_state_name);
+	}
+
+	return 0;
+}

driver/vidc/src/venus_hfi.c

@@ -27,6 +27,7 @@
 #include "venus_hfi_response.h"
 #include "venus_hfi_queue.h"
 #include "msm_vidc_events.h"
+#include "msm_vidc_state.h"
 #include "firmware.h"
 
 #define update_offset(offset, val)		((offset) += (val))

driver/vidc/src/venus_hfi_response.c

@@ -492,24 +492,23 @@ int handle_system_error(struct msm_vidc_core *core,
 static int handle_system_init(struct msm_vidc_core *core,
 	struct hfi_packet *pkt)
 {
-	if (pkt->flags & HFI_FW_FLAGS_SUCCESS) {
+	if (!(pkt->flags & HFI_FW_FLAGS_SUCCESS)) {
-		core_lock(core, __func__);
-		if (is_core_state(core, MSM_VIDC_CORE_INIT_WAIT) &&
-				pkt->packet_id == core->sys_init_id) {
-			msm_vidc_change_core_state(core, MSM_VIDC_CORE_INIT, __func__);
-			d_vpr_h("%s: successful\n", __func__);
-		} else if (core->state != MSM_VIDC_CORE_INIT_WAIT) {
-			d_vpr_e("%s: invalid core state %s\n", __func__,
-				core_state_name(core->state));
-		} else if (pkt->packet_id != core->sys_init_id) {
-			d_vpr_e("%s: invalid pkt id %u, expected %u\n", __func__,
-				pkt->packet_id, core->sys_init_id);
-		}
-		core_unlock(core, __func__);
-	} else {
 		d_vpr_h("%s: unhandled. flags=%d\n", __func__, pkt->flags);
+		return 0;
 	}
 
+	core_lock(core, __func__);
+	if (pkt->packet_id != core->sys_init_id) {
+		d_vpr_e("%s: invalid pkt id %u, expected %u\n", __func__,
+			pkt->packet_id, core->sys_init_id);
+		goto unlock;
+	}
+
+	msm_vidc_change_core_state(core, MSM_VIDC_CORE_INIT, __func__);
+	d_vpr_h("%s: successful\n", __func__);
+
+unlock:
+	core_unlock(core, __func__);
 	return 0;
 }