+#define JTAG_BP_INSN (1 << 0)
+#define JTAG_BP_TMS (1 << 1)
+#define JTAG_BP_PAYLOAD (1 << 2)
+#define JTAG_BP_2BYTE (1 << 3)
+#define JTAG_BP_4BYTE (1 << 4)
+
+/**
+ * scan_mgr_jtag_io() - Access the JTAG chain
+ * @flags: Control flags, used to configure the action on the JTAG
+ * @iarg: Instruction argument
+ * @parg: Payload argument or data
+ *
+ * Perform I/O on the JTAG chain
+ */
+static void scan_mgr_jtag_io(const u32 flags, const u8 iarg, const u32 parg)
+{
+ u32 data = parg;
+
+ if (flags & JTAG_BP_INSN) { /* JTAG instruction */
+ /*
+ * The SCC JTAG register is LSB first, so make
+ * space for the instruction at the LSB.
+ */
+ data <<= 8;
+ if (flags & JTAG_BP_TMS) {
+ data |= (0 << 7); /* TMS instruction. */
+ data |= iarg & 0x3f; /* TMS arg is 6 bits. */
+ if (flags & JTAG_BP_PAYLOAD)
+ data |= (1 << 6);
+ } else {
+ data |= (1 << 7); /* TDI/TDO instruction. */
+ data |= iarg & 0xf; /* TDI/TDO arg is 4 bits. */
+ if (flags & JTAG_BP_PAYLOAD)
+ data |= (1 << 4);
+ }
+ }
+
+ if (flags & JTAG_BP_4BYTE)
+ writel(data, &scan_manager_base->fifo_quad_byte);
+ else if (flags & JTAG_BP_2BYTE)
+ writel(data & 0xffff, &scan_manager_base->fifo_double_byte);
+ else
+ writel(data & 0xff, &scan_manager_base->fifo_single_byte);
+}
+
+/**
+ * scan_mgr_jtag_insn_data() - Send JTAG instruction and data
+ * @iarg: Instruction argument
+ * @data: Associated data
+ * @dlen: Length of data in bits
+ *
+ * This function is used when programming the IO chains to submit the
+ * instruction followed by variable length payload.
+ */
+static int
+scan_mgr_jtag_insn_data(const u8 iarg, const unsigned long *data,
+ const unsigned int dlen)
+{
+ int i, j;
+
+ scan_mgr_jtag_io(JTAG_BP_INSN | JTAG_BP_2BYTE, iarg, dlen - 1);
+
+ /* 32 bits or more remain */
+ for (i = 0; i < dlen / 32; i++)
+ scan_mgr_jtag_io(JTAG_BP_4BYTE, 0x0, data[i]);
+
+ if ((dlen % 32) > 24) { /* 31...24 bits remain */
+ scan_mgr_jtag_io(JTAG_BP_4BYTE, 0x0, data[i]);
+ } else if (dlen % 32) { /* 24...1 bit remain */
+ for (j = 0; j < dlen % 32; j += 8)
+ scan_mgr_jtag_io(0, 0x0, data[i] >> j);
+ }
+
+ return scan_chain_engine_is_idle(SCANMGR_MAX_DELAY);
+}
+