diff --git a/drivers/jtaglib.c b/drivers/jtaglib.c
index 8f7bb14..ba2b4b2 100644
--- a/drivers/jtaglib.c
+++ b/drivers/jtaglib.c
@@ -297,6 +297,7 @@ static const struct jtaglib_funcs* get_jlf(struct jtdev *p)
 	} else {
 		if (p->jtag_id == 0) {
 			printc_err("jtaglib: ERROR: no JTAG ID set!\n");
+			__builtin_trap();
 			return NULL;
 		} else if (p->jtag_id == JTAG_ID_CPU16) {
 			return &jlf_cpu16;
@@ -452,6 +453,12 @@ int jtag_get_config_fuses(struct jtdev *p)
 
 /* ------------------------------------------------------------------------- */
 
+static uint8_t jtag_get_id(struct jtdev *p)
+{
+	jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+	return jtag_ir_shift(p, IR_CNTRL_SIG_16BIT);
+}
+
 /* Take target device under JTAG control.
  * Disable the target watchdog.
  * return: 0 - fuse is blown
@@ -459,7 +466,7 @@ int jtag_get_config_fuses(struct jtdev *p)
  */
 unsigned int jtag_init(struct jtdev *p)
 {
-	unsigned int jtag_id;
+	unsigned int jtag_id, jtag_id_2;
 
 	jtag_init_dap(p);
 
@@ -470,13 +477,22 @@ unsigned int jtag_init(struct jtdev *p)
 		return 0;
 	}
 
+	jtag_id = jtag_get_id(p);
+	p->jtag_id = jtag_id;
+
 	/* Set device into JTAG mode */
-	jtag_id = jtag_get_device(p);
-	if (jtag_id == 0) {
+	jtag_id_2 = jtag_get_device(p);
+	if (jtag_id_2 == 0) {
 		printc_err("jtag_init: invalid jtag_id: 0x%02x\n", jtag_id);
 		p->failed = 1;
 		return 0;
 	}
+	if (jtag_id != jtag_id_2) {
+		printc_err("jtag_init: inconsistent jtag_id: 0x%02x vs 0x%02x\n",
+				jtag_id, jtag_id_2);
+		p->failed = 1;
+		return 0;
+	}
 
 	/* Perform PUC, includes target watchdog disable */
 	if (jtag_execute_puc(p) != jtag_id) {