diff --git a/board/freescale/common/fsl_validate.c b/board/freescale/common/fsl_validate.c
index 37c249d..91e6b50 100644
--- a/board/freescale/common/fsl_validate.c
+++ b/board/freescale/common/fsl_validate.c
@@ -339,27 +339,15 @@ static inline u32 get_key_len(struct fsl_secboot_img_priv *img)
  */
 static void fsl_secboot_header_verification_failure(void)
 {
-	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
-						(CONFIG_SYS_SEC_MON_ADDR);
 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
-	u32 sts = sec_mon_in32(&sec_mon_regs->hp_stat);
 
 	/* 29th bit of OSPR is ITS */
 	u32 its = sfp_in32(&sfp_regs->ospr) >> 2;
 
-	/*
-	 * Read the SEC_MON status register
-	 * Read SSM_ST field
-	 */
-	sts = sec_mon_in32(&sec_mon_regs->hp_stat);
-	if ((sts & HPSR_SSM_ST_MASK) == HPSR_SSM_ST_TRUST) {
-		if (its == 1)
-			change_sec_mon_state(HPSR_SSM_ST_TRUST,
-					     HPSR_SSM_ST_SOFT_FAIL);
-		else
-			change_sec_mon_state(HPSR_SSM_ST_TRUST,
-					     HPSR_SSM_ST_NON_SECURE);
-	}
+	if (its == 1)
+		set_sec_mon_state(HPSR_SSM_ST_SOFT_FAIL);
+	else
+		set_sec_mon_state(HPSR_SSM_ST_NON_SECURE);
 
 	printf("Generating reset request\n");
 	do_reset(NULL, 0, 0, NULL);
@@ -376,32 +364,20 @@ static void fsl_secboot_header_verification_failure(void)
  */
 static void fsl_secboot_image_verification_failure(void)
 {
-	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
-						(CONFIG_SYS_SEC_MON_ADDR);
 	struct ccsr_sfp_regs *sfp_regs = (void *)(CONFIG_SYS_SFP_ADDR);
-	u32 sts = sec_mon_in32(&sec_mon_regs->hp_stat);
 
 	u32 its = (sfp_in32(&sfp_regs->ospr) & ITS_MASK) >> ITS_BIT;
 
-	/*
-	 * Read the SEC_MON status register
-	 * Read SSM_ST field
-	 */
-	sts = sec_mon_in32(&sec_mon_regs->hp_stat);
-	if ((sts & HPSR_SSM_ST_MASK) == HPSR_SSM_ST_TRUST) {
-		if (its == 1) {
-			change_sec_mon_state(HPSR_SSM_ST_TRUST,
-					     HPSR_SSM_ST_SOFT_FAIL);
-
-			printf("Generating reset request\n");
-			do_reset(NULL, 0, 0, NULL);
-			/* If reset doesn't coocur, halt execution */
-			do_esbc_halt(NULL, 0, 0, NULL);
-
-		} else {
-			change_sec_mon_state(HPSR_SSM_ST_TRUST,
-					     HPSR_SSM_ST_NON_SECURE);
-		}
+	if (its == 1) {
+		set_sec_mon_state(HPSR_SSM_ST_SOFT_FAIL);
+
+		printf("Generating reset request\n");
+		do_reset(NULL, 0, 0, NULL);
+		/* If reset doesn't coocur, halt execution */
+		do_esbc_halt(NULL, 0, 0, NULL);
+
+	} else {
+		set_sec_mon_state(HPSR_SSM_ST_NON_SECURE);
 	}
 }
 
diff --git a/drivers/misc/fsl_sec_mon.c b/drivers/misc/fsl_sec_mon.c
index d482a7d..415232e 100644
--- a/drivers/misc/fsl_sec_mon.c
+++ b/drivers/misc/fsl_sec_mon.c
@@ -7,140 +7,158 @@
 #include <common.h>
 #include <fsl_sec_mon.h>
 
-int change_sec_mon_state(u32 initial_state, u32 final_state)
+static u32 get_sec_mon_state(void)
 {
 	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
 						(CONFIG_SYS_SEC_MON_ADDR);
-	u32 sts = sec_mon_in32(&sec_mon_regs->hp_stat);
+	return sec_mon_in32(&sec_mon_regs->hp_stat) & HPSR_SSM_ST_MASK;
+}
+
+static int set_sec_mon_state_non_sec(void)
+{
+	u32 sts;
 	int timeout = 10;
+	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
+						(CONFIG_SYS_SEC_MON_ADDR);
 
-	if ((sts & HPSR_SSM_ST_MASK) != initial_state)
-		return -1;
+	sts = get_sec_mon_state();
 
-	if (initial_state == HPSR_SSM_ST_TRUST) {
-		switch (final_state) {
-		case HPSR_SSM_ST_NON_SECURE:
-			printf("SEC_MON state transitioning to Soft Fail.\n");
-			sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_SV);
-
-			/*
-			 * poll till SEC_MON is in
-			 * Soft Fail state
-			 */
-			while (((sts & HPSR_SSM_ST_MASK) !=
-				HPSR_SSM_ST_SOFT_FAIL)) {
-				while (timeout) {
-					sts = sec_mon_in32
-						(&sec_mon_regs->hp_stat);
-
-					if ((sts & HPSR_SSM_ST_MASK) ==
-						HPSR_SSM_ST_SOFT_FAIL)
-						break;
-
-					udelay(10);
-					timeout--;
-				}
-			}
+	switch (sts) {
+	/*
+	 * If initial state is check or Non-Secure, then set the Software
+	 * Security Violation Bit and transition to Non-Secure State.
+	 */
+	case HPSR_SSM_ST_CHECK:
+		printf("SEC_MON state transitioning to Non Secure.\n");
+		sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_SV);
 
-			if (timeout == 0) {
-				printf("SEC_MON state transition timeout.\n");
-				return -1;
-			}
+		/* polling loop till SEC_MON is in Non Secure state */
+		while (timeout) {
+			sts = get_sec_mon_state();
+
+			if ((sts & HPSR_SSM_ST_MASK) ==
+				HPSR_SSM_ST_NON_SECURE)
+				break;
+
+			udelay(10);
+			timeout--;
+		}
 
-			timeout = 10;
+		if (timeout == 0) {
+			printf("SEC_MON state transition timeout.\n");
+			return -1;
+		}
+		break;
+
+	/*
+	 * If initial state is Trusted, Secure or Soft-Fail, then first set
+	 * the Software Security Violation Bit and transition to Soft-Fail
+	 * State.
+	 */
+	case HPSR_SSM_ST_TRUST:
+	case HPSR_SSM_ST_SECURE:
+	case HPSR_SSM_ST_SOFT_FAIL:
+		printf("SEC_MON state transitioning to Soft Fail.\n");
+		sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_SV);
+
+		/* polling loop till SEC_MON is in Soft-Fail state */
+		while (timeout) {
+			sts = get_sec_mon_state();
+
+			if ((sts & HPSR_SSM_ST_MASK) ==
+				HPSR_SSM_ST_SOFT_FAIL)
+				break;
+
+			udelay(10);
+			timeout--;
+		}
+
+		if (timeout == 0) {
+			printf("SEC_MON state transition timeout.\n");
+			return -1;
+		}
+
+		timeout = 10;
 
+		/*
+		 * If SSM Soft Fail to Non-Secure State Transition
+		 * disable is not set, then set SSM_ST bit and
+		 * transition to Non-Secure State.
+		 */
+		if ((sec_mon_in32(&sec_mon_regs->hp_com) &
+			HPCOMR_SSM_SFNS_DIS) == 0) {
 			printf("SEC_MON state transitioning to Non Secure.\n");
 			sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SSM_ST);
 
-			/*
-			 * poll till SEC_MON is in
-			 * Non Secure state
-			 */
-			while (((sts & HPSR_SSM_ST_MASK) !=
-				HPSR_SSM_ST_NON_SECURE)) {
-				while (timeout) {
-					sts = sec_mon_in32
-						(&sec_mon_regs->hp_stat);
-
-					if ((sts & HPSR_SSM_ST_MASK) ==
-						HPSR_SSM_ST_NON_SECURE)
-						break;
-
-					udelay(10);
-					timeout--;
-				}
-			}
+			/* polling loop till SEC_MON is in Non Secure*/
+			while (timeout) {
+				sts = get_sec_mon_state();
 
-			if (timeout == 0) {
-				printf("SEC_MON state transition timeout.\n");
-				return -1;
-			}
-			break;
-		case HPSR_SSM_ST_SOFT_FAIL:
-			printf("SEC_MON state transitioning to Soft Fail.\n");
-			sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_FSV);
-
-			/*
-			 * polling loop till SEC_MON is in
-			 * Soft Fail state
-			 */
-			while (((sts & HPSR_SSM_ST_MASK) !=
-				HPSR_SSM_ST_SOFT_FAIL)) {
-				while (timeout) {
-					sts = sec_mon_in32
-						(&sec_mon_regs->hp_stat);
-
-					if ((sts & HPSR_SSM_ST_MASK) ==
-						HPSR_SSM_ST_SOFT_FAIL)
-						break;
-
-					udelay(10);
-					timeout--;
-				}
+				if ((sts & HPSR_SSM_ST_MASK) ==
+					HPSR_SSM_ST_NON_SECURE)
+					break;
+
+				udelay(10);
+				timeout--;
 			}
 
 			if (timeout == 0) {
 				printf("SEC_MON state transition timeout.\n");
 				return -1;
 			}
-			break;
-		default:
-			return -1;
 		}
-	} else if (initial_state == HPSR_SSM_ST_NON_SECURE) {
-		switch (final_state) {
-		case HPSR_SSM_ST_SOFT_FAIL:
-			printf("SEC_MON state transitioning to Soft Fail.\n");
-			sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_FSV);
-
-			/*
-			 * polling loop till SEC_MON is in
-			 * Soft Fail state
-			 */
-			while (((sts & HPSR_SSM_ST_MASK) !=
-				HPSR_SSM_ST_SOFT_FAIL)) {
-				while (timeout) {
-					sts = sec_mon_in32
-						(&sec_mon_regs->hp_stat);
-
-					if ((sts & HPSR_SSM_ST_MASK) ==
-						HPSR_SSM_ST_SOFT_FAIL)
-						break;
-
-					udelay(10);
-					timeout--;
-				}
-			}
+		break;
+	default:
+		printf("SEC_MON already in Non Secure state.\n");
+		return 0;
+	}
+	return 0;
+}
 
-			if (timeout == 0) {
-				printf("SEC_MON state transition timeout.\n");
-				return -1;
-			}
+static int set_sec_mon_state_soft_fail(void)
+{
+	u32 sts;
+	int timeout = 10;
+	struct ccsr_sec_mon_regs *sec_mon_regs = (void *)
+						(CONFIG_SYS_SEC_MON_ADDR);
+
+	printf("SEC_MON state transitioning to Soft Fail.\n");
+	sec_mon_setbits32(&sec_mon_regs->hp_com, HPCOMR_SW_FSV);
+
+	/* polling loop till SEC_MON is in Soft-Fail state */
+	while (timeout) {
+		sts = get_sec_mon_state();
+
+		if ((sts & HPSR_SSM_ST_MASK) ==
+			HPSR_SSM_ST_SOFT_FAIL)
 			break;
-		default:
-			return -1;
-		}
+
+		udelay(10);
+		timeout--;
 	}
 
+	if (timeout == 0) {
+		printf("SEC_MON state transition timeout.\n");
+		return -1;
+	}
 	return 0;
 }
+
+int set_sec_mon_state(u32 state)
+{
+	int ret = -1;
+
+	switch (state) {
+	case HPSR_SSM_ST_NON_SECURE:
+		ret = set_sec_mon_state_non_sec();
+		break;
+	case HPSR_SSM_ST_SOFT_FAIL:
+		ret = set_sec_mon_state_soft_fail();
+		break;
+	default:
+		printf("SEC_MON state transition not supported.\n");
+		return 0;
+	}
+
+	return ret;
+}
diff --git a/include/fsl_sec_mon.h b/include/fsl_sec_mon.h
index b6794ce..1f31f88 100644
--- a/include/fsl_sec_mon.h
+++ b/include/fsl_sec_mon.h
@@ -34,13 +34,16 @@ struct ccsr_sec_mon_regs {
 	u32 hp_stat;	/* 0x08 SEC_MON_HP Status Register */
 };
 
-#define HPCOMR_SW_SV 0x100		/* Security Violation bit */
-#define HPCOMR_SW_FSV 0x200		/* Fatal Security Violation bit */
-#define HPCOMR_SSM_ST 0x1		/* SSM_ST field in SEC_MON command */
+#define HPCOMR_SW_SV		0x100	/* Security Violation bit */
+#define HPCOMR_SW_FSV		0x200	/* Fatal Security Violation bit */
+#define HPCOMR_SSM_ST		0x1	/* SSM_ST field in SEC_MON command */
+#define HPCOMR_SSM_ST_DIS	0x2	/* Disable Secure to Trusted State */
+#define HPCOMR_SSM_SFNS_DIS	0x4	/* Disable Soft Fail to Non-Secure */
 #define HPSR_SSM_ST_CHECK	0x900	/* SEC_MON is in check state */
 #define HPSR_SSM_ST_NON_SECURE	0xb00	/* SEC_MON is in non secure state */
 #define HPSR_SSM_ST_TRUST	0xd00	/* SEC_MON is in trusted state */
 #define HPSR_SSM_ST_SOFT_FAIL	0x300	/* SEC_MON is in soft fail state */
+#define HPSR_SSM_ST_SECURE	0xf00	/* SEC_MON is in secure state */
 #define HPSR_SSM_ST_MASK	0xf00	/* Mask for SSM_ST field */
 
 /*
@@ -53,6 +56,7 @@ enum {
 	SEC_MON_SW_SV,
 };
 
-int change_sec_mon_state(uint32_t initial_state, uint32_t final_state);
+/* Transition SEC_MON state */
+int set_sec_mon_state(u32 state);
 
 #endif /* __FSL_SEC_MON_H */
