km/common: check test_bank and testpin for testboot

If the testpin is asserted, first check if a test_bank
variable is defined before trying to boot this
test_bank.

Signed-off-by: Thomas Herzmann <thomas.herzmann@keymile.com>
Signed-off-by: Holger Brunck <holger.brunck@keymile.com>
Signed-off-by: Kim Phillips <kim.phillips@freescale.com>
diff --git a/board/keymile/common/common.c b/board/keymile/common/common.c
index f262306..1013f42 100644
--- a/board/keymile/common/common.c
+++ b/board/keymile/common/common.c
@@ -34,6 +34,9 @@
 #include <asm/io.h>
 #include <linux/ctype.h>
 
+#if defined(CONFIG_POST)
+#include "post.h"
+#endif
 #include "common.h"
 #if defined(CONFIG_HARD_I2C) || defined(CONFIG_SOFT_I2C)
 #include <i2c.h>
@@ -390,3 +393,36 @@
 		"\"boardIdListHex\" against stored boardid and hwkey "\
 		"from the IVM\n    v: verbose output"
 );
+
+/*
+ * command km_checktestboot
+ *  if the testpin of the board is asserted, return 1
+ *  *	else return 0
+ */
+int do_checktestboot(cmd_tbl_t *cmdtp, int flag, int argc,
+			char *const argv[])
+{
+	int testpin = 0;
+	char *s = NULL;
+	int testboot = 0;
+	int verbose = argc > 1 && *argv[1] == 'v';
+
+#if defined(CONFIG_POST)
+	testpin = post_hotkeys_pressed();
+	s = getenv("test_bank");
+#endif
+	/* when test_bank is not set, act as if testpin is not asserted */
+	testboot = (testpin != 0) && (s);
+	if (verbose) {
+		printf("testpin   = %d\n", testpin);
+		printf("test_bank = %s\n", s ? s : "not set");
+		printf("boot test app : %s\n", (testboot) ? "yes" : "no");
+	}
+	/* return 0 means: testboot, therefore we need the inversion */
+	return !testboot;
+}
+
+U_BOOT_CMD(km_checktestboot, 2, 0, do_checktestboot,
+		"check if testpin is asserted",
+		"[v]\n  v - verbose output"
+);