Merge remote-tracking branch 'origin/riscv' into HEAD

84 files changed, 3488 insertions, 879 deletions

diff --git a/Doxyfile.in b/Doxyfile.in
index 13b0c24..6e68b4e 100644
--- a/Doxyfile.in
+++ b/Doxyfile.in
@@ -216,7 +216,7 @@ OPTIMIZE_OUTPUT_VHDL   = NO
 # .inc files as Fortran files (default is PHP), and .f files as C (default is Fortran), 
 # use: inc=Fortran f=C
 
-EXTENSION_MAPPING      = 
+EXTENSION_MAPPING      = no_extension=C
 
 # If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
 # to include (a tag file for) the STL sources as input, then you should
diff --git a/contrib/loaders/flash/fpga/xilinx_bscan_spi.py b/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
index fa4ec2a..4246aa2 100755
--- a/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
+++ b/contrib/loaders/flash/fpga/xilinx_bscan_spi.py
@@ -13,8 +13,11 @@
 #  GNU General Public License for more details.
 #
 
-from migen import *
-from migen.build.generic_platform import *
+import unittest
+
+import migen as mg
+import migen.build.generic_platform as mb
+from migen.genlib import io
 from migen.build import xilinx
 
 
@@ -25,59 +28,256 @@ behind FPGAs.
 Bitstream binaries built with this script are available at:
 https://github.com/jordens/bscan_spi_bitstreams
 
-JTAG signalling is connected directly to SPI signalling. CS_N is
-asserted when the JTAG IR contains the USER1 instruction and the state is
-SHIFT-DR. Xilinx bscan cells sample TDO on falling TCK and forward it.
-MISO requires sampling on rising CLK and leads to one cycle of latency.
+A JTAG2SPI transfer consists of:
+
+1. an arbitrary number of 0 bits (from BYPASS registers in front of the
+   JTAG2SPI DR)
+2. a marker bit (1) indicating the start of the JTAG2SPI transaction
+3. 32 bits (big endian) describing the length of the SPI transaction
+4. a number of SPI clock cycles (corresponding to 3.) with CS_N asserted
+5. an arbitrary number of cycles (to shift MISO/TDO data through subsequent
+   BYPASS registers)
+
+Notes:
+
+* The JTAG2SPI DR is 1 bit long (due to different sampling edges of
+  {MISO,MOSI}/{TDO,TDI}).
+* MOSI is TDI with half a cycle delay.
+* TDO is MISO with half a cycle delay.
+* CAPTURE-DR needs to be performed before SHIFT-DR on the BYPASSed TAPs in
+  JTAG chain to clear the BYPASS registers to 0.
 
 https://github.com/m-labs/migen
 """
 
 
-class Spartan3(Module):
+class JTAG2SPI(mg.Module):
+    def __init__(self, spi=None, bits=32):
+        self.jtag = mg.Record([
+            ("sel", 1),
+            ("shift", 1),
+            ("capture", 1),
+            ("tck", 1),
+            ("tdi", 1),
+            ("tdo", 1),
+        ])
+        self.cs_n = mg.TSTriple()
+        self.clk = mg.TSTriple()
+        self.mosi = mg.TSTriple()
+        self.miso = mg.TSTriple()
+
+        # # #
+
+        self.cs_n.o.reset = mg.Constant(1)
+        self.mosi.o.reset_less = True
+        bits = mg.Signal(bits, reset_less=True)
+        head = mg.Signal(max=len(bits), reset=len(bits) - 1)
+        self.clock_domains.cd_sys = mg.ClockDomain()
+        self.submodules.fsm = mg.FSM("IDLE")
+        if spi is not None:
+            self.specials += [
+                    self.cs_n.get_tristate(spi.cs_n),
+                    self.mosi.get_tristate(spi.mosi),
+                    self.miso.get_tristate(spi.miso),
+            ]
+            if hasattr(spi, "clk"):  # 7 Series drive it fixed
+                self.specials += self.clk.get_tristate(spi.clk)
+                # self.specials += io.DDROutput(1, 0, spi.clk, self.clk.o)
+        self.comb += [
+                self.cd_sys.rst.eq(self.jtag.sel & self.jtag.capture),
+                self.cd_sys.clk.eq(self.jtag.tck),
+                self.cs_n.oe.eq(self.jtag.sel),
+                self.clk.oe.eq(self.jtag.sel),
+                self.mosi.oe.eq(self.jtag.sel),
+                self.miso.oe.eq(0),
+                # Do not suppress CLK toggles outside CS_N asserted.
+                # Xilinx USRCCLK0 requires three dummy cycles to do anything
+                # https://www.xilinx.com/support/answers/52626.html
+                # This is fine since CS_N changes only on falling CLK.
+                self.clk.o.eq(~self.jtag.tck),
+                self.jtag.tdo.eq(self.miso.i),
+        ]
+        # Latency calculation (in half cycles):
+        # 0 (falling TCK, rising CLK):
+        #   JTAG adapter: set TDI
+        # 1 (rising TCK, falling CLK):
+        #   JTAG2SPI: sample TDI -> set MOSI
+        #   SPI: set MISO
+        # 2 (falling TCK, rising CLK):
+        #   SPI: sample MOSI
+        #   JTAG2SPI (BSCAN primitive): sample MISO -> set TDO
+        # 3 (rising TCK, falling CLK):
+        #   JTAG adapter: sample TDO
+        self.fsm.act("IDLE",
+                mg.If(self.jtag.tdi & self.jtag.sel & self.jtag.shift,
+                    mg.NextState("HEAD")
+                )
+        )
+        self.fsm.act("HEAD",
+                mg.If(head == 0,
+                    mg.NextState("XFER")
+                )
+        )
+        self.fsm.act("XFER",
+                mg.If(bits == 0,
+                    mg.NextState("IDLE")
+                ),
+        )
+        self.sync += [
+                self.mosi.o.eq(self.jtag.tdi),
+                self.cs_n.o.eq(~self.fsm.ongoing("XFER")),
+                mg.If(self.fsm.ongoing("HEAD"),
+                    bits.eq(mg.Cat(self.jtag.tdi, bits)),
+                    head.eq(head - 1)
+                ),
+                mg.If(self.fsm.ongoing("XFER"),
+                    bits.eq(bits - 1)
+                )
+        ]
+
+
+class JTAG2SPITest(unittest.TestCase):
+    def setUp(self):
+        self.bits = 8
+        self.dut = JTAG2SPI(bits=self.bits)
+
+    def test_instantiate(self):
+        pass
+
+    def test_initial_conditions(self):
+        def check():
+            yield
+            self.assertEqual((yield self.dut.cs_n.oe), 0)
+            self.assertEqual((yield self.dut.mosi.oe), 0)
+            self.assertEqual((yield self.dut.miso.oe), 0)
+            self.assertEqual((yield self.dut.clk.oe), 0)
+        mg.run_simulation(self.dut, check())
+
+    def test_enable(self):
+        def check():
+            yield self.dut.jtag.sel.eq(1)
+            yield self.dut.jtag.shift.eq(1)
+            yield
+            self.assertEqual((yield self.dut.cs_n.oe), 1)
+            self.assertEqual((yield self.dut.mosi.oe), 1)
+            self.assertEqual((yield self.dut.miso.oe), 0)
+            self.assertEqual((yield self.dut.clk.oe), 1)
+        mg.run_simulation(self.dut, check())
+
+    def run_seq(self, tdi, tdo, spi=None):
+        yield self.dut.jtag.sel.eq(1)
+        yield
+        yield self.dut.jtag.shift.eq(1)
+        for di in tdi:
+            yield self.dut.jtag.tdi.eq(di)
+            yield
+            tdo.append((yield self.dut.jtag.tdo))
+            if spi is not None:
+                v = []
+                for k in "cs_n clk mosi miso".split():
+                    t = getattr(self.dut, k)
+                    v.append("{}>".format((yield t.o)) if (yield t.oe)
+                            else "<{}".format((yield t.i)))
+                spi.append(" ".join(v))
+        yield self.dut.jtag.sel.eq(0)
+        yield
+        yield self.dut.jtag.shift.eq(0)
+        yield
+
+    def test_shift(self):
+        bits = 8
+        data = 0x81
+        tdi = [0, 0, 1]  # dummy from BYPASS TAPs and marker
+        tdi += [((bits - 1) >> j) & 1 for j in range(self.bits - 1, -1, -1)]
+        tdi += [(data >> j) & 1 for j in range(bits)]
+        tdi += [0, 0, 0, 0]  # dummy from BYPASS TAPs
+        tdo = []
+        spi = []
+        mg.run_simulation(self.dut, self.run_seq(tdi, tdo, spi))
+        # print(tdo)
+        for l in spi:
+            print(l)
+
+
+class Spartan3(mg.Module):
     macro = "BSCAN_SPARTAN3"
     toolchain = "ise"
 
     def __init__(self, platform):
         platform.toolchain.bitgen_opt += " -g compress -g UnusedPin:Pullup"
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        shift = Signal()
-        tdo = Signal()
-        sel1 = Signal()
-        self.comb += [
-            self.cd_jtag.clk.eq(spi.clk),
-            spi.cs_n.eq(~shift | ~sel1),
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        self.specials += [
+                mg.Instance(
+                    self.macro,
+                    o_SHIFT=j2s.jtag.shift, o_SEL1=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK1=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO1=j2s.jtag.tdo,
+                    i_TDO2=0),
         ]
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance(self.macro,
-                                  o_DRCK1=spi.clk, o_SHIFT=shift,
-                                  o_TDI=spi.mosi, i_TDO1=tdo, i_TDO2=0,
-                                  o_SEL1=sel1)
+        platform.add_period_constraint(j2s.jtag.tck, 6)
 
 
 class Spartan3A(Spartan3):
     macro = "BSCAN_SPARTAN3A"
 
 
-class Spartan6(Module):
+class Spartan6(mg.Module):
     toolchain = "ise"
 
     def __init__(self, platform):
         platform.toolchain.bitgen_opt += " -g compress -g UnusedPin:Pullup"
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        shift = Signal()
-        tdo = Signal()
-        sel = Signal()
-        self.comb += self.cd_jtag.clk.eq(spi.clk), spi.cs_n.eq(~shift | ~sel)
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance("BSCAN_SPARTAN6", p_JTAG_CHAIN=1,
-                                  o_TCK=spi.clk, o_SHIFT=shift, o_SEL=sel,
-                                  o_TDI=spi.mosi, i_TDO=tdo)
-
-
-class Series7(Module):
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        # clk = mg.Signal()
+        self.specials += [
+                mg.Instance(
+                    "BSCAN_SPARTAN6", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s.jtag.shift, o_SEL=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO=j2s.jtag.tdo),
+                # mg.Instance("BUFG", i_I=clk, o_O=j2s.jtag.tck)
+        ]
+        platform.add_period_constraint(j2s.jtag.tck, 6)
+
+
+class Series7(mg.Module):
+    toolchain = "vivado"
+
+    def __init__(self, platform):
+        platform.toolchain.bitstream_commands.extend([
+            "set_property BITSTREAM.GENERAL.COMPRESS True [current_design]",
+            "set_property BITSTREAM.CONFIG.UNUSEDPIN Pullnone [current_design]"
+        ])
+        self.submodules.j2s = j2s = JTAG2SPI(platform.request("spiflash"))
+        # clk = mg.Signal()
+        self.specials += [
+                mg.Instance(
+                    "BSCANE2", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s.jtag.shift, o_SEL=j2s.jtag.sel,
+                    o_CAPTURE=j2s.jtag.capture,
+                    o_DRCK=j2s.jtag.tck,
+                    o_TDI=j2s.jtag.tdi, i_TDO=j2s.jtag.tdo),
+                mg.Instance(
+                    "STARTUPE2", i_CLK=0, i_GSR=0, i_GTS=0,
+                    i_KEYCLEARB=0, i_PACK=1,
+                    i_USRCCLKO=j2s.clk.o, i_USRCCLKTS=~j2s.clk.oe,
+                    i_USRDONEO=1, i_USRDONETS=1),
+                # mg.Instance("BUFG", i_I=clk, o_O=j2s.jtag.tck)
+        ]
+        platform.add_period_constraint(j2s.jtag.tck, 6)
+        try:
+            self.comb += [
+                    platform.request("user_sma_gpio_p").eq(j2s.cs_n.i),
+                    platform.request("user_sma_gpio_n").eq(j2s.clk.o),
+                    platform.request("user_sma_clock_p").eq(j2s.mosi.o),
+                    platform.request("user_sma_clock_n").eq(j2s.miso.i),
+            ]
+        except mb.ConstraintError:
+            pass
+
+
+class Ultrascale(mg.Module):
     toolchain = "vivado"
 
     def __init__(self, platform):
@@ -85,20 +285,33 @@ class Series7(Module):
             "set_property BITSTREAM.GENERAL.COMPRESS True [current_design]",
             "set_property BITSTREAM.CONFIG.UNUSEDPIN Pullnone [current_design]",
         ])
-        self.clock_domains.cd_jtag = ClockDomain(reset_less=True)
-        spi = platform.request("spiflash")
-        clk = Signal()
-        shift = Signal()
-        tdo = Signal()
-        sel = Signal()
-        self.comb += self.cd_jtag.clk.eq(clk), spi.cs_n.eq(~shift | ~sel)
-        self.sync.jtag += tdo.eq(spi.miso)
-        self.specials += Instance("BSCANE2", p_JTAG_CHAIN=1,
-                                  o_SHIFT=shift, o_TCK=clk, o_SEL=sel,
-                                  o_TDI=spi.mosi, i_TDO=tdo)
-        self.specials += Instance("STARTUPE2", i_CLK=0, i_GSR=0, i_GTS=0,
-                                  i_KEYCLEARB=0, i_PACK=1, i_USRCCLKO=clk,
-                                  i_USRCCLKTS=0, i_USRDONEO=1, i_USRDONETS=1)
+        self.submodules.j2s0 = j2s0 = JTAG2SPI()
+        self.submodules.j2s1 = j2s1 = JTAG2SPI(platform.request("spiflash"))
+        di = mg.Signal(4)
+        self.comb += mg.Cat(j2s0.mosi.i, j2s0.miso.i).eq(di)
+        self.specials += [
+                mg.Instance("BSCANE2", p_JTAG_CHAIN=1,
+                    o_SHIFT=j2s0.jtag.shift, o_SEL=j2s0.jtag.sel,
+                    o_CAPTURE=j2s0.jtag.capture,
+                    o_DRCK=j2s0.jtag.tck,
+                    o_TDI=j2s0.jtag.tdi, i_TDO=j2s0.jtag.tdo),
+                mg.Instance("BSCANE2", p_JTAG_CHAIN=2,
+                    o_SHIFT=j2s1.jtag.shift, o_SEL=j2s1.jtag.sel,
+                    o_CAPTURE=j2s1.jtag.capture,
+                    o_DRCK=j2s1.jtag.tck,
+                    o_TDI=j2s1.jtag.tdi, i_TDO=j2s1.jtag.tdo),
+                mg.Instance("STARTUPE3", i_GSR=0, i_GTS=0,
+                    i_KEYCLEARB=0, i_PACK=1,
+                    i_USRDONEO=1, i_USRDONETS=1,
+                    i_USRCCLKO=mg.Mux(j2s0.clk.oe, j2s0.clk.o, j2s1.clk.o),
+                    i_USRCCLKTS=~(j2s0.clk.oe | j2s1.clk.oe),
+                    i_FCSBO=j2s0.cs_n.o, i_FCSBTS=~j2s0.cs_n.oe,
+                    o_DI=di,
+                    i_DO=mg.Cat(j2s0.mosi.o, j2s0.miso.o, 0, 0),
+                    i_DTS=mg.Cat(~j2s0.mosi.oe, ~j2s0.miso.oe, 1, 1))
+        ]
+        platform.add_period_constraint(j2s0.jtag.tck, 6)
+        platform.add_period_constraint(j2s1.jtag.tck, 6)
 
 
 class XilinxBscanSpi(xilinx.XilinxPlatform):
@@ -124,6 +337,7 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         ("fbg484-1", 2): ["L16", None, "H18", "H19", "G18", "F19"],
         ("fbg676-1", 1): ["C23", None, "B24", "A25", "B22", "A22"],
         ("ffg901-1", 1): ["V26", None, "R30", "T30", "R28", "T28"],
+        ("ffg900-1", 1): ["U19", None, "P24", "R25", "R20", "R21"],
         ("ffg1156-1", 1): ["V30", None, "AA33", "AA34", "Y33", "Y34"],
         ("ffg1157-1", 1): ["AL33", None, "AN33", "AN34", "AK34", "AL34"],
         ("ffg1158-1", 1): ["C24", None, "A23", "A24", "B26", "A26"],
@@ -132,6 +346,9 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         ("flg1155-1", 1): ["AL28", None, "AE28", "AF28", "AJ29", "AJ30"],
         ("flg1932-1", 1): ["V32", None, "T33", "R33", "U31", "T31"],
         ("flg1926-1", 1): ["AK33", None, "AN34", "AN35", "AJ34", "AK34"],
+
+        ("ffva1156-2-e", 1): ["G26", None, "M20", "L20", "R21", "R22"],
+        ("ffva1156-2-e", "sayma"): ["K21", None, "M20", "L20", "R21", "R22"],
     }
 
     pinouts = {
@@ -181,6 +398,7 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         "xc7a75t": ("csg324-1", 1, "LVCMOS25", Series7),
         "xc7k160t": ("fbg484-1", 2, "LVCMOS25", Series7),
         "xc7k325t": ("fbg676-1", 1, "LVCMOS25", Series7),
+        "xc7k325t-debug": ("ffg900-1", 1, "LVCMOS25", Series7),
         "xc7k355t": ("ffg901-1", 1, "LVCMOS25", Series7),
         "xc7k410t": ("fbg676-1", 1, "LVCMOS25", Series7),
         "xc7k420t": ("ffg1156-1", 1, "LVCMOS25", Series7),
@@ -197,35 +415,53 @@ class XilinxBscanSpi(xilinx.XilinxPlatform):
         "xc7vx550t": ("ffg1158-1", 1, "LVCMOS18", Series7),
         "xc7vx690t": ("ffg1157-1", 1, "LVCMOS18", Series7),
         "xc7vx980t": ("ffg1926-1", 1, "LVCMOS18", Series7),
+
+        "xcku040": ("ffva1156-2-e", 1, "LVCMOS18", Ultrascale),
+        "xcku040-sayma": ("ffva1156-2-e", "sayma", "LVCMOS18", Ultrascale),
     }
 
     def __init__(self, device, pins, std, toolchain="ise"):
+        ios = [self.make_spi(0, pins, std, toolchain)]
+        if device == "xc7k325t-ffg900-1":  # debug
+            ios += [
+                ("user_sma_clock_p", 0, mb.Pins("L25"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_clock_n", 0, mb.Pins("K25"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_gpio_p", 0, mb.Pins("Y23"), mb.IOStandard("LVCMOS25")),
+                ("user_sma_gpio_n", 0, mb.Pins("Y24"), mb.IOStandard("LVCMOS25")),
+            ]
+        xilinx.XilinxPlatform.__init__(self, device, ios, toolchain=toolchain)
+
+    @staticmethod
+    def make_spi(i, pins, std, toolchain):
+        pu = "PULLUP" if toolchain == "ise" else "PULLUP TRUE"
+        pd = "PULLDOWN" if toolchain == "ise" else "PULLDOWN TRUE"
         cs_n, clk, mosi, miso = pins[:4]
-        io = ["spiflash", 0,
-              Subsignal("cs_n", Pins(cs_n)),
-              Subsignal("mosi", Pins(mosi)),
-              Subsignal("miso", Pins(miso), Misc("PULLUP")),
-              IOStandard(std),
-              ]
+        io = ["spiflash", i,
+            mb.Subsignal("cs_n", mb.Pins(cs_n), mb.Misc(pu)),
+            mb.Subsignal("mosi", mb.Pins(mosi), mb.Misc(pu)),
+            mb.Subsignal("miso", mb.Pins(miso), mb.Misc(pu)),
+            mb.IOStandard(std),
+            ]
         if clk:
-            io.append(Subsignal("clk", Pins(clk)))
+            io.append(mb.Subsignal("clk", mb.Pins(clk), mb.Misc(pd)))
         for i, p in enumerate(pins[4:]):
-            io.append(Subsignal("pullup{}".format(i), Pins(p), Misc("PULLUP")))
-        xilinx.XilinxPlatform.__init__(self, device, [io], toolchain=toolchain)
+            io.append(mb.Subsignal("pullup{}".format(i), mb.Pins(p),
+                                mb.Misc(pu)))
+        return io
 
     @classmethod
-    def make(cls, device, errors=False):
-        pkg, id, std, Top = cls.pinouts[device]
+    def make(cls, target, errors=False):
+        pkg, id, std, Top = cls.pinouts[target]
         pins = cls.packages[(pkg, id)]
+        device = target.split("-", 1)[0]
         platform = cls("{}-{}".format(device, pkg), pins, std, Top.toolchain)
         top = Top(platform)
-        name = "bscan_spi_{}".format(device)
-        dir = "build_{}".format(device)
+        name = "bscan_spi_{}".format(target)
         try:
-            platform.build(top, build_name=name, build_dir=dir)
+            platform.build(top, build_name=name)
         except Exception as e:
             print(("ERROR: xilinx_bscan_spi build failed "
-                  "for {}: {}").format(device, e))
+                  "for {}: {}").format(target, e))
             if errors:
                 raise
 
diff --git a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
index e59a1bd..6641307 100644
--- a/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
+++ b/contrib/remote_bitbang/remote_bitbang_sysfsgpio.c
@@ -302,7 +302,7 @@ static void process_remote_protocol(void)
 			break;
 		else if (c == 'b' || c == 'B') /* Blink */
 			continue;
-		else if (c >= 'r' && c <= 'r' + 2) { /* Reset */
+		else if (c >= 'r' && c <= 'r' + 3) { /* Reset */
 			char d = c - 'r';
 			sysfsgpio_reset(!!(d & 2),
 					(d & 1));
diff --git a/doc/manual/style.txt b/doc/manual/style.txt
index 2ff2a29..0bfae35 100644
--- a/doc/manual/style.txt
+++ b/doc/manual/style.txt
@@ -176,10 +176,10 @@ comments.
  * @returns The value(s) returned, or possible error conditions.
  */
 @endverbatim
-  -# The block should start on the line following the opening @c /**.
-  -# The end of the block, \f$*/\f$, should also be on its own line.
+  -# The block should start on the line following the opening @c /\**.
+  -# The end of the block, @c *&zwj;/, should also be on its own line.
   -# Every line in the block should have a @c '*' in-line with its start:
-    - A leading space is required to align the @c '*' with the @c /** line.
+    - A leading space is required to align the @c '*' with the @c /\** line.
     - A single "empty" line should separate the function documentation
       from the block of parameter and return value descriptions.
     - Except to separate paragraphs of documentation, other extra
diff --git a/doc/openocd.texi b/doc/openocd.texi
index ebd03c4..898ffb9 100644
--- a/doc/openocd.texi
+++ b/doc/openocd.texi
@@ -4913,19 +4913,62 @@ functionality is available through the @command{flash write_bank},
 @item @var{ir} ... is loaded into the JTAG IR to map the flash as the JTAG DR.
 For the bitstreams generated from @file{xilinx_bscan_spi.py} this is the
 @var{USER1} instruction.
-@item @var{dr_length} ... is the length of the DR register. This will be 1 for
-@file{xilinx_bscan_spi.py} bitstreams and most other cases.
 @end itemize
 
 @example
 target create $_TARGETNAME testee -chain-position $_CHIPNAME.fpga
 set _XILINX_USER1 0x02
-set _DR_LENGTH 1
 flash bank $_FLASHNAME spi 0x0 0 0 0 \
-           $_TARGETNAME $_XILINX_USER1 $_DR_LENGTH
+           $_TARGETNAME $_XILINX_USER1
 @end example
 @end deffn
 
+@deffn {Flash Driver} xcf
+@cindex Xilinx Platform flash driver
+@cindex xcf
+Xilinx FPGAs can be configured from specialized flash ICs named Platform Flash.
+It is (almost) regular NOR flash with erase sectors, program pages, etc. The
+only difference is special registers controlling its FPGA specific behavior.
+They must be properly configured for successful FPGA loading using
+additional @var{xcf} driver command:
+
+@deffn Command {xcf ccb} <bank_id>
+command accepts additional parameters:
+@itemize
+@item @var{external|internal} ... selects clock source.
+@item @var{serial|parallel} ... selects serial or parallel data bus mode.
+@item @var{slave|master} ... selects slave of master mode for flash device.
+@item @var{40|20} ... selects clock frequency in MHz for internal clock
+in master mode.
+@end itemize
+@example
+xcf ccb 0 external parallel slave 40
+@end example
+All of them must be specified even if clock frequency is pointless
+in slave mode. If only bank id specified than command prints current
+CCB register value. Note: there is no need to write this register
+every time you erase/program data sectors because it stores in
+dedicated sector.
+@end deffn
+
+@deffn Command {xcf configure} <bank_id>
+Initiates FPGA loading procedure. Useful if your board has no "configure"
+button.
+@example
+xcf configure 0
+@end example
+@end deffn
+
+Additional driver notes:
+@itemize
+@item Only single revision supported.
+@item Driver automatically detects need of bit reverse, but
+only "bin" (raw binary, do not confuse it with "bit") and "mcs"
+(Intel hex) file types supported.
+@item For additional info check xapp972.pdf and ug380.pdf.
+@end itemize
+@end deffn
+
 @deffn {Flash Driver} lpcspifi
 @cindex NXP SPI Flash Interface
 @cindex SPIFI
@@ -5358,7 +5401,7 @@ from NXP (former Freescale) include
 internal flash and use ARM Cortex-M0+ or M4 cores. The driver automatically
 recognizes flash size and a number of flash banks (1-4) using the chip
 identification register, and autoconfigures itself.
-Use kinetis_ke driver for KE0x devices.
+Use kinetis_ke driver for KE0x and KEAx devices.
 
 The @var{kinetis} driver defines option:
 @itemize
@@ -5451,7 +5494,7 @@ Command disables watchdog timer.
 
 @deffn {Flash Driver} kinetis_ke
 @cindex kinetis_ke
-KE0x members of the Kinetis microcontroller family from Freescale include
+KE0x and KEAx members of the Kinetis microcontroller family from NXP include
 internal flash and use ARM Cortex-M0+. The driver automatically recognizes
 the KE0x sub-family using the chip identification register, and
 autoconfigures itself.
@@ -8375,6 +8418,11 @@ halting or resuming of all cores in the group. The command @code{target smp} def
 group. With SMP handling disabled, all targets need to be treated individually.
 @end deffn
 
+@deffn Command {aarch64 maskisr} [@option{on}|@option{off}]
+Selects whether interrupts will be processed when single stepping. The default configuration is
+@option{on}.
+@end deffn
+
 @section Intel Architecture
 
 Intel Quark X10xx is the first product in the Quark family of SoCs. It is an IA-32
@@ -8779,11 +8827,27 @@ way to represent JTAG test patterns in text files.
 In a debug session using JTAG for its transport protocol,
 OpenOCD supports running such test files.
 
-@deffn Command {svf} filename [@option{quiet}]
+@deffn Command {svf} @file{filename} [@option{-tap @var{tapname}}] [@option{[-]quiet}] @
+                     [@option{[-]nil}] [@option{[-]progress}] [@option{[-]ignore_error}]
 This issues a JTAG reset (Test-Logic-Reset) and then
 runs the SVF script from @file{filename}.
-Unless the @option{quiet} option is specified,
-each command is logged before it is executed.
+
+Arguments can be specified in any order; the optional dash doesn't
+affect their semantics.
+
+Command options:
+@itemize @minus
+@item @option{-tap @var{tapname}} ignore IR and DR headers and footers
+specified by the SVF file with HIR, TIR, HDR and TDR commands;
+instead, calculate them automatically according to the current JTAG
+chain configuration, targetting @var{tapname};
+@item @option{[-]quiet} do not log every command before execution;
+@item @option{[-]nil} ``dry run'', i.e., do not perform any operations
+on the real interface;
+@item @option{[-]progress} enable progress indication;
+@item @option{[-]ignore_error} continue execution despite TDO check
+errors.
+@end itemize
 @end deffn
 
 @section XSVF: Xilinx Serial Vector Format
diff --git a/src/flash/nor/Makefile.am b/src/flash/nor/Makefile.am
index 8a57f4f..cc72088 100644
--- a/src/flash/nor/Makefile.am
+++ b/src/flash/nor/Makefile.am
@@ -56,6 +56,7 @@ NOR_DRIVERS = \
 	%D%/str9xpec.c \
 	%D%/tms470.c \
 	%D%/virtual.c \
+	%D%/xcf.c \
 	%D%/xmc1xxx.c \
 	%D%/xmc4xxx.c
 
diff --git a/src/flash/nor/drivers.c b/src/flash/nor/drivers.c
index 3f0c3c7..8168011 100644
--- a/src/flash/nor/drivers.c
+++ b/src/flash/nor/drivers.c
@@ -68,6 +68,7 @@ extern struct flash_driver str9x_flash;
 extern struct flash_driver str9xpec_flash;
 extern struct flash_driver tms470_flash;
 extern struct flash_driver virtual_flash;
+extern struct flash_driver xcf_flash;
 extern struct flash_driver xmc1xxx_flash;
 extern struct flash_driver xmc4xxx_flash;
 
@@ -124,6 +125,7 @@ static struct flash_driver *flash_drivers[] = {
 	&str9xpec_flash,
 	&tms470_flash,
 	&virtual_flash,
+	&xcf_flash,
 	&xmc1xxx_flash,
 	&xmc4xxx_flash,
 	NULL,
diff --git a/src/flash/nor/jtagspi.c b/src/flash/nor/jtagspi.c
index a995fc7..a73812d 100644
--- a/src/flash/nor/jtagspi.c
+++ b/src/flash/nor/jtagspi.c
@@ -32,14 +32,13 @@ struct jtagspi_flash_bank {
 	const struct flash_device *dev;
 	int probed;
 	uint32_t ir;
-	uint32_t dr_len;
 };
 
 FLASH_BANK_COMMAND_HANDLER(jtagspi_flash_bank_command)
 {
 	struct jtagspi_flash_bank *info;
 
-	if (CMD_ARGC < 8)
+	if (CMD_ARGC < 7)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
 	info = malloc(sizeof(struct jtagspi_flash_bank));
@@ -52,7 +51,6 @@ FLASH_BANK_COMMAND_HANDLER(jtagspi_flash_bank_command)
 	info->tap = NULL;
 	info->probed = 0;
 	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[6], info->ir);
-	COMMAND_PARSE_NUMBER(u32, CMD_ARGV[7], info->dr_len);
 
 	return ERROR_OK;
 }
@@ -63,9 +61,6 @@ static void jtagspi_set_ir(struct flash_bank *bank)
 	struct scan_field field;
 	uint8_t buf[4];
 
-	if (buf_get_u32(info->tap->cur_instr, 0, info->tap->ir_length) == info->ir)
-		return;
-
 	LOG_DEBUG("loading jtagspi ir");
 	buf_set_u32(buf, 0, info->tap->ir_length, info->ir);
 	field.num_bits = info->tap->ir_length;
@@ -84,28 +79,53 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 		uint32_t *addr, uint8_t *data, int len)
 {
 	struct jtagspi_flash_bank *info = bank->driver_priv;
-	struct scan_field fields[3];
-	uint8_t cmd_buf[4];
+	struct scan_field fields[6];
+	uint8_t marker = 1;
+	uint8_t xfer_bits_buf[4];
+	uint8_t addr_buf[3];
 	uint8_t *data_buf;
+	uint32_t xfer_bits;
 	int is_read, lenb, n;
 
 	/* LOG_DEBUG("cmd=0x%02x len=%i", cmd, len); */
 
+	is_read = (len < 0);
+	if (is_read)
+		len = -len;
+
 	n = 0;
+
+	fields[n].num_bits = 1;
+	fields[n].out_value = &marker;
+	fields[n].in_value = NULL;
+	n++;
+
+	xfer_bits = 8 + len - 1;
+	/* cmd + read/write - 1 due to the counter implementation */
+	if (addr)
+		xfer_bits += 24;
+	h_u32_to_be(xfer_bits_buf, xfer_bits);
+	flip_u8(xfer_bits_buf, xfer_bits_buf, 4);
+	fields[n].num_bits = 32;
+	fields[n].out_value = xfer_bits_buf;
+	fields[n].in_value = NULL;
+	n++;
+
+	cmd = flip_u32(cmd, 8);
 	fields[n].num_bits = 8;
-	cmd_buf[0] = cmd;
-	if (addr) {
-		h_u24_to_be(cmd_buf + 1, *addr);
-		fields[n].num_bits += 24;
-	}
-	flip_u8(cmd_buf, cmd_buf, 4);
-	fields[n].out_value = cmd_buf;
+	fields[n].out_value = &cmd;
 	fields[n].in_value = NULL;
 	n++;
 
-	is_read = (len < 0);
-	if (is_read)
-		len = -len;
+	if (addr) {
+		h_u24_to_be(addr_buf, *addr);
+		flip_u8(addr_buf, addr_buf, 3);
+		fields[n].num_bits = 24;
+		fields[n].out_value = addr_buf;
+		fields[n].in_value = NULL;
+		n++;
+	}
+
 	lenb = DIV_ROUND_UP(len, 8);
 	data_buf = malloc(lenb);
 	if (lenb > 0) {
@@ -114,10 +134,11 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 			return ERROR_FAIL;
 		}
 		if (is_read) {
-			fields[n].num_bits = info->dr_len;
+			fields[n].num_bits = jtag_tap_count_enabled();
 			fields[n].out_value = NULL;
 			fields[n].in_value = NULL;
 			n++;
+
 			fields[n].out_value = NULL;
 			fields[n].in_value = data_buf;
 		} else {
@@ -130,6 +151,7 @@ static int jtagspi_cmd(struct flash_bank *bank, uint8_t cmd,
 	}
 
 	jtagspi_set_ir(bank);
+	/* passing from an IR scan to SHIFT-DR clears BYPASS registers */
 	jtag_add_dr_scan(info->tap, n, fields, TAP_IDLE);
 	jtag_execute_queue();
 
@@ -202,9 +224,10 @@ static int jtagspi_probe(struct flash_bank *bank)
 static void jtagspi_read_status(struct flash_bank *bank, uint32_t *status)
 {
 	uint8_t buf;
-	jtagspi_cmd(bank, SPIFLASH_READ_STATUS, NULL, &buf, -8);
-	*status = buf;
-	/* LOG_DEBUG("status=0x%08" PRIx32, *status); */
+	if (jtagspi_cmd(bank, SPIFLASH_READ_STATUS, NULL, &buf, -8) == ERROR_OK) {
+		*status = buf;
+		/* LOG_DEBUG("status=0x%08" PRIx32, *status); */
+	}
 }
 
 static int jtagspi_wait(struct flash_bank *bank, int timeout_ms)
diff --git a/src/flash/nor/kinetis.c b/src/flash/nor/kinetis.c
index 5c0ffbd..48a5de4 100644
--- a/src/flash/nor/kinetis.c
+++ b/src/flash/nor/kinetis.c
@@ -287,6 +287,7 @@ struct kinetis_chip {
 
 		FS_NO_CMD_BLOCKSTAT = 0x40,
 		FS_WIDTH_256BIT = 0x80,
+		FS_ECC = 0x100,
 	} flash_support;
 
 	enum {
@@ -388,6 +389,7 @@ static const struct kinetis_type kinetis_types_old[] = {
 
 static bool allow_fcf_writes;
 static uint8_t fcf_fopt = 0xff;
+static bool fcf_fopt_configured;
 static bool create_banks;
 
 
@@ -1881,9 +1883,13 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 {
 	int result;
 	bool set_fcf = false;
+	bool fcf_in_data_valid = false;
 	int sect = 0;
 	struct kinetis_flash_bank *k_bank = bank->driver_priv;
 	struct kinetis_chip *k_chip = k_bank->k_chip;
+	uint8_t fcf_buffer[FCF_SIZE];
+	uint8_t fcf_current[FCF_SIZE];
+	uint8_t fcf_in_data[FCF_SIZE];
 
 	result = kinetis_check_run_mode(k_chip);
 	if (result != ERROR_OK)
@@ -1904,11 +1910,41 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 	}
 
 	if (set_fcf) {
-		uint8_t fcf_buffer[FCF_SIZE];
-		uint8_t fcf_current[FCF_SIZE];
-
 		kinetis_fill_fcf(bank, fcf_buffer);
 
+		fcf_in_data_valid = offset <= FCF_ADDRESS
+					 && offset + count >= FCF_ADDRESS + FCF_SIZE;
+		if (fcf_in_data_valid) {
+			memcpy(fcf_in_data, buffer + FCF_ADDRESS - offset, FCF_SIZE);
+			if (memcmp(fcf_in_data + FCF_FPROT, fcf_buffer, 4)) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Flash protection requested in programmed file differs from current setting.");
+			}
+			if (fcf_in_data[FCF_FDPROT] != fcf_buffer[FCF_FDPROT]) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Data flash protection requested in programmed file differs from current setting.");
+			}
+			if ((fcf_in_data[FCF_FSEC] & 3) != 2) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Device security requested in programmed file!");
+			} else if (k_chip->flash_support & FS_ECC
+			    && fcf_in_data[FCF_FSEC] != fcf_buffer[FCF_FSEC]) {
+				fcf_in_data_valid = false;
+				LOG_INFO("Strange unsecure mode 0x%02" PRIx8
+					 "requested in programmed file!",
+					 fcf_in_data[FCF_FSEC]);
+			}
+			if ((k_chip->flash_support & FS_ECC || fcf_fopt_configured)
+			    && fcf_in_data[FCF_FOPT] != fcf_fopt) {
+				fcf_in_data_valid = false;
+				LOG_INFO("FOPT requested in programmed file differs from current setting.");
+			}
+			if (!fcf_in_data_valid)
+				LOG_INFO("Expect verify errors at FCF (0x408-0x40f).");
+		}
+	}
+
+	if (set_fcf && !fcf_in_data_valid) {
 		if (offset < FCF_ADDRESS) {
 			/* write part preceding FCF */
 			result = kinetis_write_inner(bank, buffer, offset, FCF_ADDRESS - offset);
@@ -1937,9 +1973,10 @@ static int kinetis_write(struct flash_bank *bank, const uint8_t *buffer,
 		}
 		return result;
 
-	} else
+	} else {
 		/* no FCF fiddling, normal write */
 		return kinetis_write_inner(bank, buffer, offset, count);
+	}
 }
 
 
@@ -2146,10 +2183,21 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 				k_chip->nvm_sector_size = 4<<10;
 				k_chip->max_flash_prog_size = 1<<10;
 				num_blocks = 4;
-				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_ECC;
 				cpu_mhz = 180;
 				break;
 
+			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX7:
+				/* K27FN2M0 */
+			case KINETIS_SDID_FAMILYID_K2X | KINETIS_SDID_SUBFAMID_KX8:
+				/* K28FN2M0 */
+				k_chip->pflash_sector_size = 4<<10;
+				k_chip->max_flash_prog_size = 1<<10;
+				num_blocks = 4;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_ECC;
+				cpu_mhz = 150;
+				break;
+
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX0:
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX1:
 			case KINETIS_SDID_FAMILYID_K8X | KINETIS_SDID_SUBFAMID_KX2:
@@ -2300,7 +2348,7 @@ static int kinetis_probe_chip(struct kinetis_chip *k_chip)
 				k_chip->max_flash_prog_size = 1<<10;
 				num_blocks = 1;
 				maxaddr_shift = 14;
-				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_WIDTH_256BIT;
+				k_chip->flash_support = FS_PROGRAM_PHRASE | FS_PROGRAM_SECTOR | FS_WIDTH_256BIT | FS_ECC;
 				k_chip->pflash_base = 0x10000000;
 				k_chip->progr_accel_ram = 0x18000000;
 				cpu_mhz = 240;
@@ -2959,10 +3007,12 @@ COMMAND_HANDLER(kinetis_fopt_handler)
 	if (CMD_ARGC > 1)
 		return ERROR_COMMAND_SYNTAX_ERROR;
 
-	if (CMD_ARGC == 1)
+	if (CMD_ARGC == 1) {
 		fcf_fopt = (uint8_t)strtoul(CMD_ARGV[0], NULL, 0);
-	else
+		fcf_fopt_configured = true;
+	} else {
 		command_print(CMD_CTX, "FCF_FOPT 0x%02" PRIx8, fcf_fopt);
+	}
 
 	return ERROR_OK;
 }
diff --git a/src/flash/nor/nrf5.c b/src/flash/nor/nrf5.c
index 11e5729..4fa62e3 100644
--- a/src/flash/nor/nrf5.c
+++ b/src/flash/nor/nrf5.c
@@ -168,6 +168,7 @@ static const struct nrf5_device_spec nrf5_known_devices_table[] = {
 
 	/* nRF51822 Devices (IC rev 3). */
 	NRF5_DEVICE_DEF(0x0072, "51822", "QFAA", "H0",    256),
+	NRF5_DEVICE_DEF(0x00D1, "51822", "QFAA", "H2",    256),
 	NRF5_DEVICE_DEF(0x007B, "51822", "QFAB", "C0",    128),
 	NRF5_DEVICE_DEF(0x0083, "51822", "QFAC", "A0",    256),
 	NRF5_DEVICE_DEF(0x0084, "51822", "QFAC", "A1",    256),
diff --git a/src/flash/nor/stm32l4x.c b/src/flash/nor/stm32l4x.c
index fa0c48b..6a1fa07 100644
--- a/src/flash/nor/stm32l4x.c
+++ b/src/flash/nor/stm32l4x.c
@@ -652,6 +652,9 @@ static int stm32l4_probe(struct flash_bank *bank)
 	/* get options to for DUAL BANK. */
 	retval = target_read_u32(target, STM32_FLASH_OPTR, &options);
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* only devices with < 1024 kiB may be set to single bank dual banks */
 	if ((flash_size_in_kb == 1024) || !(options & OPT_DUALBANK))
 		stm32l4_info->option_bytes.bank_b_start = 256;
diff --git a/src/flash/nor/xcf.c b/src/flash/nor/xcf.c
new file mode 100644
index 0000000..035791e
--- /dev/null
+++ b/src/flash/nor/xcf.c
@@ -0,0 +1,897 @@
+/***************************************************************************
+ *   Copyright (C) 2016 by Uladzimir Pylinski aka barthess                 *
+ *   barthess@yandex.ru                                                    *
+ *                                                                         *
+ *   This program is free software; you can redistribute it and/or modify  *
+ *   it under the terms of the GNU General Public License as published by  *
+ *   the Free Software Foundation; either version 2 of the License, or     *
+ *   (at your option) any later version.                                   *
+ *                                                                         *
+ *   This program is distributed in the hope that it will be useful,       *
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
+ *   GNU General Public License for more details.                          *
+ *                                                                         *
+ *   You should have received a copy of the GNU General Public License     *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
+ ***************************************************************************/
+
+#ifdef HAVE_CONFIG_H
+#include "config.h"
+#endif
+
+#include <string.h>
+
+#include "imp.h"
+#include <jtag/jtag.h>
+#include <helper/time_support.h>
+
+/*
+ ******************************************************************************
+ * DEFINES
+ ******************************************************************************
+ */
+
+#define SECTOR_ERASE_TIMEOUT_MS         (35 * 1000)
+
+#define XCF_PAGE_SIZE                   32
+#define XCF_DATA_SECTOR_SIZE            (1024 * 1024)
+
+#define ID_XCF01S                       0x05044093
+#define ID_XCF02S                       0x05045093
+#define ID_XCF04S                       0x05046093
+#define ID_XCF08P                       0x05057093
+#define ID_XCF16P                       0x05058093
+#define ID_XCF32P                       0x05059093
+#define ID_MEANINGFUL_MASK              0x0FFFFFFF
+
+const char *xcf_name_list[] = {
+	"XCF08P",
+	"XCF16P",
+	"XCF32P",
+	"unknown"
+};
+
+struct xcf_priv {
+	bool probed;
+};
+
+struct xcf_status {
+	bool isc_error;		/* false == OK, true == error */
+	bool prog_error;	/* false == OK, true == error */
+	bool prog_busy;		/* false == idle, true == busy */
+	bool isc_mode;		/* false == normal mode, true == ISC mode */
+};
+
+/*
+ ******************************************************************************
+ * GLOBAL VARIABLES
+ ******************************************************************************
+ */
+static const uint8_t CMD_BYPASS[2]              = {0xFF, 0xFF};
+
+static const uint8_t CMD_ISC_ADDRESS_SHIFT[2]   = {0xEB, 0x00};
+static const uint8_t CMD_ISC_DATA_SHIFT[2]      = {0xED, 0x00};
+static const uint8_t CMD_ISC_DISABLE[2]         = {0xF0, 0x00};
+static const uint8_t CMD_ISC_ENABLE[2]          = {0xE8, 0x00};
+static const uint8_t CMD_ISC_ERASE[2]           = {0xEC, 0x00};
+static const uint8_t CMD_ISC_PROGRAM[2]         = {0xEA, 0x00};
+
+static const uint8_t CMD_XSC_BLANK_CHECK[2]     = {0x0D, 0x00};
+static const uint8_t CMD_XSC_CONFIG[2]          = {0xEE, 0x00};
+static const uint8_t CMD_XSC_DATA_BTC[2]        = {0xF2, 0x00};
+static const uint8_t CMD_XSC_DATA_CCB[2]        = {0x0C, 0x00};
+static const uint8_t CMD_XSC_DATA_DONE[2]       = {0x09, 0x00};
+static const uint8_t CMD_XSC_DATA_SUCR[2]       = {0x0E, 0x00};
+static const uint8_t CMD_XSC_DATA_WRPT[2]       = {0xF7, 0x00};
+static const uint8_t CMD_XSC_OP_STATUS[2]       = {0xE3, 0x00};
+static const uint8_t CMD_XSC_READ[2]            = {0xEF, 0x00};
+static const uint8_t CMD_XSC_UNLOCK[2]          = {0x55, 0xAA};
+
+/*
+ ******************************************************************************
+ * LOCAL FUNCTIONS
+ ******************************************************************************
+ */
+
+static const char *product_name(const struct flash_bank *bank)
+{
+
+	switch (bank->target->tap->idcode & ID_MEANINGFUL_MASK) {
+		case ID_XCF08P:
+			return xcf_name_list[0];
+		case ID_XCF16P:
+			return xcf_name_list[1];
+		case ID_XCF32P:
+			return xcf_name_list[2];
+		default:
+			return xcf_name_list[3];
+	}
+}
+
+static void fill_sector_table(struct flash_bank *bank)
+{
+	/* Note: is_erased and is_protected fields must be set here to an unknown
+	 * state, they will be correctly filled from other API calls. */
+
+	int i = 0;
+
+	for (i = 0; i < bank->num_sectors; i++) {
+		bank->sectors[i].is_erased              = -1;
+		bank->sectors[i].is_protected   = -1;
+	}
+	for (i = 0; i < bank->num_sectors; i++) {
+		bank->sectors[i].size   = XCF_DATA_SECTOR_SIZE;
+		bank->sectors[i].offset = i * XCF_DATA_SECTOR_SIZE;
+	}
+
+	bank->size = bank->num_sectors * XCF_DATA_SECTOR_SIZE;
+}
+
+static struct xcf_status read_status(struct flash_bank *bank)
+{
+	struct xcf_status ret;
+	uint8_t irdata[2];
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_BYPASS;
+	scan.in_value = irdata;
+
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	ret.isc_error   = ((irdata[0] >> 7) & 3) == 0b01;
+	ret.prog_error  = ((irdata[0] >> 5) & 3) == 0b01;
+	ret.prog_busy   = ((irdata[0] >> 4) & 1) == 0;
+	ret.isc_mode    = ((irdata[0] >> 3) & 1) == 1;
+
+	return ret;
+}
+
+static int isc_enter(struct flash_bank *bank)
+{
+
+	struct xcf_status status = read_status(bank);
+
+	if (true == status.isc_mode)
+		return ERROR_OK;
+	else {
+		struct scan_field scan;
+
+		scan.check_mask = NULL;
+		scan.check_value = NULL;
+		scan.num_bits = 16;
+		scan.out_value = CMD_ISC_ENABLE;
+		scan.in_value = NULL;
+
+		jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+		jtag_execute_queue();
+
+		status = read_status(bank);
+		if (false == status.isc_mode) {
+			LOG_ERROR("*** XCF: FAILED to enter ISC mode");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		return ERROR_OK;
+	}
+}
+
+static int isc_leave(struct flash_bank *bank)
+{
+
+	struct xcf_status status = read_status(bank);
+
+	if (false == status.isc_mode)
+		return ERROR_OK;
+	else {
+		struct scan_field scan;
+
+		scan.check_mask = NULL;
+		scan.check_value = NULL;
+		scan.num_bits = 16;
+		scan.out_value = CMD_ISC_DISABLE;
+		scan.in_value = NULL;
+
+		jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+		jtag_execute_queue();
+		alive_sleep(1);	/* device needs 50 uS to leave ISC mode */
+
+		status = read_status(bank);
+		if (true == status.isc_mode) {
+			LOG_ERROR("*** XCF: FAILED to leave ISC mode");
+			return ERROR_FLASH_OPERATION_FAILED;
+		}
+
+		return ERROR_OK;
+	}
+}
+
+static int sector_state(uint8_t wrpt, int sector)
+{
+	if (((wrpt >> sector) & 1) == 1)
+		return 0;
+	else
+		return 1;
+}
+
+static uint8_t fill_select_block(int first, int last)
+{
+	uint8_t ret = 0;
+	for (int i = first; i <= last; i++)
+		ret |= 1 << i;
+	return ret;
+}
+
+static int isc_read_register(struct flash_bank *bank, const uint8_t *cmd,
+	uint8_t *data_buf, int num_bits)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	scan.num_bits = 16;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.out_value = NULL;
+	scan.in_value = data_buf;
+	scan.num_bits = num_bits;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	return jtag_execute_queue();
+}
+
+static int isc_wait_erase_program(struct flash_bank *bank, int64_t timeout_ms)
+{
+
+	uint8_t isc_default;
+	int64_t t0 = timeval_ms();
+	int64_t dt;
+
+	do {
+		isc_read_register(bank, CMD_XSC_OP_STATUS, &isc_default, 8);
+		if (((isc_default >> 2) & 1) == 1)
+			return ERROR_OK;
+		dt = timeval_ms() - t0;
+	} while (dt <= timeout_ms);
+	return ERROR_FLASH_OPERATION_FAILED;
+}
+
+/*
+ * helper function for procedures without program jtag command at the end
+ */
+static int isc_set_register(struct flash_bank *bank, const uint8_t *cmd,
+	const uint8_t *data_buf, int num_bits, int64_t timeout_ms)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.num_bits = num_bits;
+	scan.out_value = data_buf;
+	scan.in_value = NULL;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	if (0 == timeout_ms)
+		return jtag_execute_queue();
+	else
+		return isc_wait_erase_program(bank, timeout_ms);
+}
+
+/*
+ * helper function for procedures required program jtag command at the end
+ */
+static int isc_program_register(struct flash_bank *bank, const uint8_t *cmd,
+	const uint8_t *data_buf, int num_bits, int64_t timeout_ms)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = cmd;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_DRSHIFT);
+
+	scan.num_bits = num_bits;
+	scan.out_value = data_buf;
+	scan.in_value = NULL;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IRSHIFT);
+
+	scan.num_bits = 16;
+	scan.out_value = CMD_ISC_PROGRAM;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+
+	if (0 == timeout_ms)
+		return jtag_execute_queue();
+	else
+		return isc_wait_erase_program(bank, timeout_ms);
+}
+
+static int isc_clear_protect(struct flash_bank *bank, int first, int last)
+{
+	uint8_t select_block[3] = {0x0, 0x0, 0x0};
+	select_block[0] = fill_select_block(first, last);
+	return isc_set_register(bank, CMD_XSC_UNLOCK, select_block, 24, 0);
+}
+
+static int isc_set_protect(struct flash_bank *bank, int first, int last)
+{
+	uint8_t wrpt[2] = {0xFF, 0xFF};
+	for (int i = first; i <= last; i++)
+		wrpt[0] &= ~(1 << i);
+
+	return isc_program_register(bank, CMD_XSC_DATA_WRPT, wrpt, 16, 0);
+}
+
+static int isc_erase_sectors(struct flash_bank *bank, int first, int last)
+{
+	uint8_t select_block[3] = {0, 0, 0};
+	select_block[0] = fill_select_block(first, last);
+	int64_t timeout = SECTOR_ERASE_TIMEOUT_MS * (last - first + 1);
+	return isc_set_register(bank, CMD_ISC_ERASE, select_block, 24, timeout);
+}
+
+static int isc_adr_shift(struct flash_bank *bank, int adr)
+{
+	uint8_t adr_buf[3];
+	h_u24_to_le(adr_buf, adr);
+	return isc_set_register(bank, CMD_ISC_ADDRESS_SHIFT, adr_buf, 24, 0);
+}
+
+static int isc_program_data_page(struct flash_bank *bank, const uint8_t *page_buf)
+{
+	return isc_program_register(bank, CMD_ISC_DATA_SHIFT, page_buf, 8 * XCF_PAGE_SIZE, 100);
+}
+
+static void isc_data_read_out(struct flash_bank *bank, uint8_t *buffer, uint32_t count)
+{
+
+	struct scan_field scan;
+
+	/* Do not change this code with isc_read_register() call because it needs
+	 * transition to IDLE state before data retrieving. */
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_READ;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+
+	scan.num_bits = 8 * count;
+	scan.out_value = NULL;
+	scan.in_value = buffer;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+
+	jtag_execute_queue();
+}
+
+static int isc_set_data_done(struct flash_bank *bank, int sector)
+{
+	uint8_t done = 0xFF;
+	done &= ~(1 << sector);
+	return isc_program_register(bank, CMD_XSC_DATA_DONE, &done, 8, 100);
+}
+
+static void flip_u8(uint8_t *out, const uint8_t *in, int len)
+{
+	for (int i = 0; i < len; i++)
+		out[i] = flip_u32(in[i], 8);
+}
+
+/*
+ * Xilinx bin file contains simple fixed header for automatic bus width detection:
+ * 16 bytes of 0xFF
+ * 4 byte sync word 0xAA995566 or (bit reversed) 0x5599AA66 in MSC file
+ *
+ * Function presumes need of bit reversing if it can not exactly detects
+ * the opposite.
+ */
+bool need_bit_reverse(const uint8_t *buffer)
+{
+	const size_t L = 20;
+	uint8_t reference[L];
+	memset(reference, 0xFF, 16);
+	reference[16] = 0x55;
+	reference[17] = 0x99;
+	reference[18] = 0xAA;
+	reference[19] = 0x66;
+
+	if (0 == memcmp(reference, buffer, L))
+		return false;
+	else
+		return true;
+}
+
+/*
+ * The page address to be programmed is determined by loading the
+ * internal ADDRESS Register using an ISC_ADDRESS_SHIFT instruction sequence.
+ * The page address automatically increments to the next 256-bit
+ * page address after each programming sequence until the last address
+ * in the 8 Mb block is reached. To continue programming the next block,
+ * the next 8 Mb block's starting address must be loaded into the
+ * internal ADDRESS register.
+ */
+static int read_write_data(struct flash_bank *bank, const uint8_t *w_buffer,
+	uint8_t *r_buffer, bool write_flag, uint32_t offset, uint32_t count)
+{
+	int dbg_count = count;
+	int dbg_written = 0;
+	int ret = ERROR_OK;
+	uint8_t *page_buf = malloc(XCF_PAGE_SIZE);
+	bool revbit = true;
+	isc_enter(bank);
+
+	if (offset % XCF_PAGE_SIZE != 0) {
+		ret = ERROR_FLASH_DST_BREAKS_ALIGNMENT;
+		goto EXIT;
+	}
+
+	if ((offset + count) > (uint32_t)(bank->num_sectors * XCF_DATA_SECTOR_SIZE)) {
+		ret = ERROR_FLASH_DST_OUT_OF_BANK;
+		goto EXIT;
+	}
+
+	if ((write_flag) && (0 == offset) && (count >= XCF_PAGE_SIZE))
+		revbit = need_bit_reverse(w_buffer);
+
+	while (count > 0) {
+		uint32_t sector_num = offset / XCF_DATA_SECTOR_SIZE;
+		uint32_t sector_offset = offset - sector_num * XCF_DATA_SECTOR_SIZE;
+		uint32_t sector_bytes = XCF_DATA_SECTOR_SIZE - sector_offset;
+		if (count < sector_bytes)
+			sector_bytes = count;
+		isc_adr_shift(bank, offset);
+		offset += sector_bytes;
+		count -= sector_bytes;
+
+		if (write_flag) {
+			while (sector_bytes > 0) {
+				int len;
+
+				if (sector_bytes < XCF_PAGE_SIZE) {
+					len = sector_bytes;
+					memset(page_buf, 0xFF, XCF_PAGE_SIZE);
+				} else
+					len = XCF_PAGE_SIZE;
+
+				if (revbit)
+					flip_u8(page_buf, w_buffer, len);
+				else
+					memcpy(page_buf, w_buffer, len);
+
+				w_buffer += len;
+				sector_bytes -= len;
+				ret = isc_program_data_page(bank, page_buf);
+				if (ERROR_OK != ret)
+					goto EXIT;
+				else {
+					LOG_DEBUG("written %d bytes from %d", dbg_written, dbg_count);
+					dbg_written += len;
+				}
+			}
+		} else {
+			isc_data_read_out(bank, r_buffer, sector_bytes);
+			flip_u8(r_buffer, r_buffer, sector_bytes);
+			r_buffer += sector_bytes;
+		}
+	}
+
+	/* Set 'done' flags for all data sectors because driver supports
+	 * only single revision. */
+	if (write_flag) {
+		for (int i = 0; i < bank->num_sectors; i++) {
+			ret = isc_set_data_done(bank, i);
+			if (ERROR_OK != ret)
+				goto EXIT;
+		}
+	}
+
+EXIT:
+	free(page_buf);
+	isc_leave(bank);
+	return ret;
+}
+
+static uint16_t isc_read_ccb(struct flash_bank *bank)
+{
+	uint8_t ccb[2];
+	isc_read_register(bank, CMD_XSC_DATA_CCB, ccb, 16);
+	return le_to_h_u16(ccb);
+}
+
+static int gucr_num(const struct flash_bank *bank)
+{
+	return bank->num_sectors;
+}
+
+static int sucr_num(const struct flash_bank *bank)
+{
+	return bank->num_sectors + 1;
+}
+
+static int isc_program_ccb(struct flash_bank *bank, uint16_t ccb)
+{
+	uint8_t buf[2];
+	h_u16_to_le(buf, ccb);
+	return isc_program_register(bank, CMD_XSC_DATA_CCB, buf, 16, 100);
+}
+
+static int isc_program_singe_revision_sucr(struct flash_bank *bank)
+{
+	uint8_t sucr[2] = {0xFC, 0xFF};
+	return isc_program_register(bank, CMD_XSC_DATA_SUCR, sucr, 16, 100);
+}
+
+static int isc_program_single_revision_btc(struct flash_bank *bank)
+{
+	uint8_t buf[4];
+	uint32_t btc = 0xFFFFFFFF;
+	btc &= ~0b1111;
+	btc |= ((bank->num_sectors - 1) << 2);
+	btc &= ~(1 << 4);
+	h_u32_to_le(buf, btc);
+	return isc_program_register(bank, CMD_XSC_DATA_BTC, buf, 32, 100);
+}
+
+static int fpga_configure(struct flash_bank *bank)
+{
+	struct scan_field scan;
+
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_CONFIG;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	return ERROR_OK;
+}
+
+/*
+ ******************************************************************************
+ * EXPORTED FUNCTIONS
+ ******************************************************************************
+ */
+
+FLASH_BANK_COMMAND_HANDLER(xcf_flash_bank_command)
+{
+	struct xcf_priv *priv;
+
+	priv = malloc(sizeof(struct xcf_priv));
+	if (priv == NULL) {
+		LOG_ERROR("no memory for flash bank info");
+		return ERROR_FAIL;
+	}
+	bank->driver_priv = priv;
+	priv->probed = false;
+	return ERROR_OK;
+}
+
+static int xcf_info(struct flash_bank *bank, char *buf, int buf_size)
+{
+	const struct xcf_priv *priv = bank->driver_priv;
+
+	if (false == priv->probed) {
+		snprintf(buf, buf_size, "\nXCF flash bank not probed yet\n");
+		return ERROR_OK;
+	}
+	snprintf(buf, buf_size, "%s", product_name(bank));
+	return ERROR_OK;
+}
+
+static int xcf_probe(struct flash_bank *bank)
+{
+	struct xcf_priv *priv = bank->driver_priv;
+	uint32_t id;
+
+	if (true == priv->probed)
+		free(bank->sectors);
+	priv->probed = false;
+
+	if (bank->target->tap == NULL) {
+		LOG_ERROR("Target has no JTAG tap");
+		return ERROR_FAIL;
+	}
+
+	/* check idcode and alloc memory for sector table */
+	if (!bank->target->tap->hasidcode)
+		return ERROR_FLASH_OPERATION_FAILED;
+
+	/* guess number of blocks using chip ID */
+	id = bank->target->tap->idcode;
+	switch (id & ID_MEANINGFUL_MASK) {
+		case ID_XCF08P:
+			bank->num_sectors = 1;
+			break;
+		case ID_XCF16P:
+			bank->num_sectors = 2;
+			break;
+		case ID_XCF32P:
+			bank->num_sectors = 4;
+			break;
+		default:
+			LOG_ERROR("Unknown flash device ID 0x%X", id);
+			return ERROR_FAIL;
+			break;
+	}
+
+	bank->sectors = malloc(bank->num_sectors * sizeof(struct flash_sector));
+	if (NULL == bank->sectors) {
+		LOG_ERROR("No memory for sector table");
+		return ERROR_FAIL;
+	}
+	fill_sector_table(bank);
+
+	priv->probed = true;
+	bank->driver_priv = priv;
+
+	LOG_INFO("product name: %s", product_name(bank));
+	LOG_INFO("device id = 0x%X ", bank->target->tap->idcode);
+	LOG_INFO("flash size = %d configuration bits",
+		bank->num_sectors * XCF_DATA_SECTOR_SIZE * 8);
+	LOG_INFO("number of sectors = %d", bank->num_sectors);
+
+	return ERROR_OK;
+}
+
+static int xcf_auto_probe(struct flash_bank *bank)
+{
+	struct xcf_priv *priv = bank->driver_priv;
+
+	if (true == priv->probed)
+		return ERROR_OK;
+	else
+		return xcf_probe(bank);
+}
+
+static int xcf_protect_check(struct flash_bank *bank)
+{
+	uint8_t wrpt[2];
+
+	isc_enter(bank);
+	isc_read_register(bank, CMD_XSC_DATA_WRPT, wrpt, 16);
+	isc_leave(bank);
+
+	for (int i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_protected = sector_state(wrpt[0], i);
+
+	return ERROR_OK;
+}
+
+static int xcf_erase_check(struct flash_bank *bank)
+{
+	uint8_t blankreg;
+	struct scan_field scan;
+
+	isc_enter(bank);
+
+	/* Do not change this code with isc_read_register() call because it needs
+	 * transition to IDLE state and pause before data retrieving. */
+	scan.check_mask = NULL;
+	scan.check_value = NULL;
+	scan.num_bits = 16;
+	scan.out_value = CMD_XSC_BLANK_CHECK;
+	scan.in_value = NULL;
+	jtag_add_ir_scan(bank->target->tap, &scan, TAP_IDLE);
+	jtag_execute_queue();
+	alive_sleep(500);	/* device needs at least 0.5s to self check */
+
+	scan.num_bits = 8;
+	scan.in_value = &blankreg;
+	jtag_add_dr_scan(bank->target->tap, 1, &scan, TAP_IDLE);
+	jtag_execute_queue();
+
+	isc_leave(bank);
+
+	for (int i = 0; i < bank->num_sectors; i++)
+		bank->sectors[i].is_erased = sector_state(blankreg, i);
+
+	return ERROR_OK;
+}
+
+static int xcf_erase(struct flash_bank *bank, int first, int last)
+{
+	if ((first >= bank->num_sectors)
+		|| (last >= bank->num_sectors)
+		|| (last < first))
+		return ERROR_FLASH_SECTOR_INVALID;
+	else {
+		isc_enter(bank);
+		isc_clear_protect(bank, first, last);
+		int ret = isc_erase_sectors(bank, first, last);
+		isc_leave(bank);
+		return ret;
+	}
+}
+
+static int xcf_read(struct flash_bank *bank, uint8_t *buffer, uint32_t offset, uint32_t count)
+{
+	return read_write_data(bank, NULL, buffer, false, offset, count);
+}
+
+static int xcf_write(struct flash_bank *bank, const uint8_t *buffer, uint32_t offset,
+	uint32_t count)
+{
+	return read_write_data(bank, buffer, NULL, true, offset, count);
+}
+
+static int xcf_protect(struct flash_bank *bank, int set, int first, int last)
+{
+	int ret;
+
+	isc_enter(bank);
+	if (set)
+		ret = isc_set_protect(bank, first, last);
+	else {
+		/* write protection may be removed only with following erase */
+		isc_clear_protect(bank, first, last);
+		ret = isc_erase_sectors(bank, first, last);
+	}
+	isc_leave(bank);
+
+	return ret;
+}
+
+COMMAND_HANDLER(xcf_handle_ccb_command) {
+
+	if (!((CMD_ARGC == 1) || (CMD_ARGC == 5)))
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	uint16_t ccb = 0xFFFF;
+	isc_enter(bank);
+	uint16_t old_ccb = isc_read_ccb(bank);
+	isc_leave(bank);
+
+	if (CMD_ARGC == 1) {
+		LOG_INFO("current CCB = 0x%X", old_ccb);
+		return ERROR_OK;
+	} else {
+		/* skip over flash bank */
+		CMD_ARGC--;
+		CMD_ARGV++;
+		while (CMD_ARGC) {
+			if (strcmp("external", CMD_ARGV[0]) == 0)
+				ccb |= (1 << 0);
+			else if (strcmp("internal", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 0);
+			else if (strcmp("serial", CMD_ARGV[0]) == 0)
+				ccb |= (3 << 1);
+			else if (strcmp("parallel", CMD_ARGV[0]) == 0)
+				ccb &= ~(3 << 1);
+			else if (strcmp("slave", CMD_ARGV[0]) == 0)
+				ccb |= (1 << 3);
+			else if (strcmp("master", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 3);
+			else if (strcmp("40", CMD_ARGV[0]) == 0)
+				ccb |= (3 << 4);
+			else if (strcmp("20", CMD_ARGV[0]) == 0)
+				ccb &= ~(1 << 5);
+			else
+				return ERROR_COMMAND_SYNTAX_ERROR;
+			CMD_ARGC--;
+			CMD_ARGV++;
+		}
+
+		isc_enter(bank);
+		int sector;
+
+		/* GUCR sector */
+		sector = gucr_num(bank);
+		isc_clear_protect(bank, sector, sector);
+		int ret = isc_erase_sectors(bank, sector, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_ccb(bank, ccb);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_single_revision_btc(bank);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_set_data_done(bank, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+
+		/* SUCR sector */
+		sector = sucr_num(bank);
+		isc_clear_protect(bank, sector, sector);
+		ret = isc_erase_sectors(bank, sector, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_program_singe_revision_sucr(bank);
+		if (ERROR_OK != ret)
+			goto EXIT;
+		ret = isc_set_data_done(bank, sector);
+		if (ERROR_OK != ret)
+			goto EXIT;
+
+EXIT:
+		isc_leave(bank);
+		return ret;
+	}
+}
+
+COMMAND_HANDLER(xcf_handle_configure_command) {
+
+	if (CMD_ARGC != 1)
+		return ERROR_COMMAND_SYNTAX_ERROR;
+
+	struct flash_bank *bank;
+	int retval = CALL_COMMAND_HANDLER(flash_command_get_bank, 0, &bank);
+	if (ERROR_OK != retval)
+		return retval;
+
+	return fpga_configure(bank);
+}
+
+static const struct command_registration xcf_exec_command_handlers[] = {
+	{
+		.name = "configure",
+		.handler = xcf_handle_configure_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id",
+		.help = "Initiate FPGA loading procedure."
+	},
+	{
+		.name = "ccb",
+		.handler = xcf_handle_ccb_command,
+		.mode = COMMAND_EXEC,
+		.usage = "bank_id [('external'|'internal') "
+			"('serial'|'parallel') "
+			"('slave'|'master') "
+			"('40'|'20')]",
+		.help = "Write CCB register with supplied options and (silently) BTC "
+			"register with single revision options. Display current "
+			"CCB value when only bank_id supplied. "
+			"Following options available: "
+			"1) external or internal clock source; "
+			"2) serial or parallel bus mode; "
+			"3) slave or master mode; "
+			"4) clock frequency in MHz for internal clock in master mode;"
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+static const struct command_registration xcf_command_handlers[] = {
+	{
+		.name = "xcf",
+		.mode = COMMAND_ANY,
+		.help = "Xilinx platform flash command group",
+		.usage = "",
+		.chain = xcf_exec_command_handlers
+	},
+	COMMAND_REGISTRATION_DONE
+};
+
+struct flash_driver xcf_flash = {
+	.name               = "xcf",
+	.usage              = NULL,
+	.commands           = xcf_command_handlers,
+	.flash_bank_command = xcf_flash_bank_command,
+	.erase              = xcf_erase,
+	.protect            = xcf_protect,
+	.write              = xcf_write,
+	.read               = xcf_read,
+	.probe              = xcf_probe,
+	.auto_probe         = xcf_auto_probe,
+	.erase_check        = xcf_erase_check,
+	.protect_check      = xcf_protect_check,
+	.info               = xcf_info
+};
diff --git a/src/flash/startup.tcl b/src/flash/startup.tcl
index fbb8d8e..ff053ae 100644
--- a/src/flash/startup.tcl
+++ b/src/flash/startup.tcl
@@ -42,6 +42,7 @@ proc program {filename args} {
 
 	# start programming phase
 	echo "** Programming Started **"
+	set filename \{$filename\}
 	if {[info exists address]} {
 		set flash_args "$filename $address"
 	} else {
@@ -62,8 +63,10 @@ proc program {filename args} {
 
 		if {[info exists reset]} {
 			# reset target if requested
-			# also disable target polling, we are shutting down anyway
-			poll off
+			if {$exit == 1} {
+				# also disable target polling, we are shutting down anyway
+				poll off
+			}
 			echo "** Resetting Target **"
 			reset run
 		}
diff --git a/src/helper/command.c b/src/helper/command.c
index 40e8b05..cbd52fb 100644
--- a/src/helper/command.c
+++ b/src/helper/command.c
@@ -1339,6 +1339,15 @@ struct command_context *command_init(const char *startup_tcl, Jim_Interp *interp
 	return context;
 }
 
+void command_exit(struct command_context *context)
+{
+	if (!context)
+		return;
+
+	Jim_FreeInterp(context->interp);
+	command_done(context);
+}
+
 int command_context_mode(struct command_context *cmd_ctx, enum command_mode mode)
 {
 	if (!cmd_ctx)
diff --git a/src/helper/command.h b/src/helper/command.h
index bd24156..f696ab8 100644
--- a/src/helper/command.h
+++ b/src/helper/command.h
@@ -308,6 +308,14 @@ struct command_context *current_command_context(Jim_Interp *interp);
  */
 struct command_context *command_init(const char *startup_tcl, Jim_Interp *interp);
 /**
+ * Shutdown a command context.
+ *
+ * Free the command context and the associated Jim interpreter.
+ *
+ * @param context The command_context that will be destroyed.
+ */
+void command_exit(struct command_context *context);
+/**
  * Creates a copy of an existing command context.  This does not create
  * a deep copy of the command list, so modifications in one context will
  * affect all shared contexts.  The caller must track reference counting
diff --git a/src/helper/time_support.c b/src/helper/time_support.c
index 8337e73..05eaf0a 100644
--- a/src/helper/time_support.c
+++ b/src/helper/time_support.c
@@ -62,6 +62,21 @@ int timeval_add_time(struct timeval *result, long sec, long usec)
 	return 0;
 }
 
+/* compare two timevals and return -1/0/+1 accordingly */
+int timeval_compare(const struct timeval *x, const struct timeval *y)
+{
+	if (x->tv_sec < y->tv_sec)
+		return -1;
+	else if (x->tv_sec > y->tv_sec)
+		return 1;
+	else if (x->tv_usec < y->tv_usec)
+		return -1;
+	else if (x->tv_usec > y->tv_usec)
+		return 1;
+	else
+		return 0;
+}
+
 int duration_start(struct duration *duration)
 {
 	return gettimeofday(&duration->start, NULL);
diff --git a/src/helper/time_support.h b/src/helper/time_support.h
index 58c8c48..7abbdb2 100644
--- a/src/helper/time_support.h
+++ b/src/helper/time_support.h
@@ -38,6 +38,7 @@
 
 int timeval_subtract(struct timeval *result, struct timeval *x, struct timeval *y);
 int timeval_add_time(struct timeval *result, long sec, long usec);
+int timeval_compare(const struct timeval *x, const struct timeval *y);
 
 /** @returns gettimeofday() timeval as 64-bit in ms */
 int64_t timeval_ms(void);
diff --git a/src/jtag/drivers/at91rm9200.c b/src/jtag/drivers/at91rm9200.c
index 8f65413..0015da0 100644
--- a/src/jtag/drivers/at91rm9200.c
+++ b/src/jtag/drivers/at91rm9200.c
@@ -109,9 +109,9 @@ static uint32_t *pio_base;
 
 /* low level command set
  */
-static int at91rm9200_read(void);
-static void at91rm9200_write(int tck, int tms, int tdi);
-static void at91rm9200_reset(int trst, int srst);
+static bb_value_t at91rm9200_read(void);
+static int at91rm9200_write(int tck, int tms, int tdi);
+static int at91rm9200_reset(int trst, int srst);
 
 static int at91rm9200_init(void);
 static int at91rm9200_quit(void);
@@ -123,12 +123,12 @@ static struct bitbang_interface at91rm9200_bitbang = {
 	.blink = 0
 };
 
-static int at91rm9200_read(void)
+static bb_value_t at91rm9200_read(void)
 {
-	return (pio_base[device->TDO_PIO + PIO_PDSR] & device->TDO_MASK) != 0;
+	return (pio_base[device->TDO_PIO + PIO_PDSR] & device->TDO_MASK) ? BB_HIGH : BB_LOW;
 }
 
-static void at91rm9200_write(int tck, int tms, int tdi)
+static int at91rm9200_write(int tck, int tms, int tdi)
 {
 	if (tck)
 		pio_base[device->TCK_PIO + PIO_SODR] = device->TCK_MASK;
@@ -144,10 +144,12 @@ static void at91rm9200_write(int tck, int tms, int tdi)
 		pio_base[device->TDI_PIO + PIO_SODR] = device->TDI_MASK;
 	else
 		pio_base[device->TDI_PIO + PIO_CODR] = device->TDI_MASK;
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void at91rm9200_reset(int trst, int srst)
+static int at91rm9200_reset(int trst, int srst)
 {
 	if (trst == 0)
 		pio_base[device->TRST_PIO + PIO_SODR] = device->TRST_MASK;
@@ -158,6 +160,8 @@ static void at91rm9200_reset(int trst, int srst)
 		pio_base[device->SRST_PIO + PIO_SODR] = device->SRST_MASK;
 	else if (srst == 1)
 		pio_base[device->SRST_PIO + PIO_CODR] = device->SRST_MASK;
+
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(at91rm9200_handle_device_command)
diff --git a/src/jtag/drivers/bcm2835gpio.c b/src/jtag/drivers/bcm2835gpio.c
index a41caf0..38ef163 100644
--- a/src/jtag/drivers/bcm2835gpio.c
+++ b/src/jtag/drivers/bcm2835gpio.c
@@ -49,9 +49,9 @@ uint32_t bcm2835_peri_base = 0x20000000;
 static int dev_mem_fd;
 static volatile uint32_t *pio_base;
 
-static int bcm2835gpio_read(void);
-static void bcm2835gpio_write(int tck, int tms, int tdi);
-static void bcm2835gpio_reset(int trst, int srst);
+static bb_value_t bcm2835gpio_read(void);
+static int bcm2835gpio_write(int tck, int tms, int tdi);
+static int bcm2835gpio_reset(int trst, int srst);
 
 static int bcm2835_swdio_read(void);
 static void bcm2835_swdio_drive(bool is_output);
@@ -91,12 +91,12 @@ static int speed_coeff = 113714;
 static int speed_offset = 28;
 static unsigned int jtag_delay;
 
-static int bcm2835gpio_read(void)
+static bb_value_t bcm2835gpio_read(void)
 {
-	return !!(GPIO_LEV & 1<<tdo_gpio);
+	return (GPIO_LEV & 1<<tdo_gpio) ? BB_HIGH : BB_LOW;
 }
 
-static void bcm2835gpio_write(int tck, int tms, int tdi)
+static int bcm2835gpio_write(int tck, int tms, int tdi)
 {
 	uint32_t set = tck<<tck_gpio | tms<<tms_gpio | tdi<<tdi_gpio;
 	uint32_t clear = !tck<<tck_gpio | !tms<<tms_gpio | !tdi<<tdi_gpio;
@@ -106,9 +106,11 @@ static void bcm2835gpio_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
-static void bcm2835gpio_swd_write(int tck, int tms, int tdi)
+static int bcm2835gpio_swd_write(int tck, int tms, int tdi)
 {
 	uint32_t set = tck<<swclk_gpio | tdi<<swdio_gpio;
 	uint32_t clear = !tck<<swclk_gpio | !tdi<<swdio_gpio;
@@ -118,10 +120,12 @@ static void bcm2835gpio_swd_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void bcm2835gpio_reset(int trst, int srst)
+static int bcm2835gpio_reset(int trst, int srst)
 {
 	uint32_t set = 0;
 	uint32_t clear = 0;
@@ -138,6 +142,8 @@ static void bcm2835gpio_reset(int trst, int srst)
 
 	GPIO_SET = set;
 	GPIO_CLR = clear;
+
+	return ERROR_OK;
 }
 
 static void bcm2835_swdio_drive(bool is_output)
@@ -432,10 +438,6 @@ static int bcm2835gpio_init(void)
 			LOG_INFO("JTAG and SWD modes enabled");
 		else
 			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
-		if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
-			LOG_ERROR("Require at least one of trst or srst gpios to be specified");
-			return ERROR_JTAG_INIT_FAILED;
-		}
 	} else if (bcm2835gpio_swd_mode_possible()) {
 		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
 	} else {
diff --git a/src/jtag/drivers/bitbang.c b/src/jtag/drivers/bitbang.c
index 4fdca79..722a5f2 100644
--- a/src/jtag/drivers/bitbang.c
+++ b/src/jtag/drivers/bitbang.c
@@ -41,7 +41,7 @@ extern struct jtag_interface *jtag_interface;
  * this function checks the current stable state to decide on the value of TMS
  * to use.
  */
-static void bitbang_stableclocks(int num_cycles);
+static int bitbang_stableclocks(int num_cycles);
 
 static void bitbang_swd_write_reg(uint8_t cmd, uint32_t value, uint32_t ap_delay_clk);
 
@@ -70,15 +70,11 @@ struct bitbang_interface *bitbang_interface;
 /* The bitbang driver leaves the TCK 0 when in idle */
 static void bitbang_end_state(tap_state_t state)
 {
-	if (tap_is_state_stable(state))
-		tap_set_end_state(state);
-	else {
-		LOG_ERROR("BUG: %i is not a valid end state", state);
-		exit(-1);
-	}
+	assert(tap_is_state_stable(state));
+	tap_set_end_state(state);
 }
 
-static void bitbang_state_move(int skip)
+static int bitbang_state_move(int skip)
 {
 	int i = 0, tms = 0;
 	uint8_t tms_scan = tap_get_tms_path(tap_get_state(), tap_get_end_state());
@@ -86,12 +82,16 @@ static void bitbang_state_move(int skip)
 
 	for (i = skip; i < tms_count; i++) {
 		tms = (tms_scan >> i) & 1;
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	tap_set_state(tap_get_end_state());
+	return ERROR_OK;
 }
 
 /**
@@ -108,15 +108,18 @@ static int bitbang_execute_tms(struct jtag_command *cmd)
 	int tms = 0;
 	for (unsigned i = 0; i < num_bits; i++) {
 		tms = ((bits[i/8] >> (i % 8)) & 1);
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	return ERROR_OK;
 }
 
-static void bitbang_path_move(struct pathmove_command *cmd)
+static int bitbang_path_move(struct pathmove_command *cmd)
 {
 	int num_states = cmd->num_states;
 	int state_count;
@@ -135,20 +138,24 @@ static void bitbang_path_move(struct pathmove_command *cmd)
 			exit(-1);
 		}
 
-		bitbang_interface->write(0, tms, 0);
-		bitbang_interface->write(1, tms, 0);
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 
 		tap_set_state(cmd->path[state_count]);
 		state_count++;
 		num_states--;
 	}
 
-	bitbang_interface->write(CLOCK_IDLE(), tms, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), tms, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	tap_set_end_state(tap_get_state());
+	return ERROR_OK;
 }
 
-static void bitbang_runtest(int num_cycles)
+static int bitbang_runtest(int num_cycles)
 {
 	int i;
 
@@ -157,35 +164,46 @@ static void bitbang_runtest(int num_cycles)
 	/* only do a state_move when we're not already in IDLE */
 	if (tap_get_state() != TAP_IDLE) {
 		bitbang_end_state(TAP_IDLE);
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
 
 	/* execute num_cycles */
 	for (i = 0; i < num_cycles; i++) {
-		bitbang_interface->write(0, 0, 0);
-		bitbang_interface->write(1, 0, 0);
+		if (bitbang_interface->write(0, 0, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(1, 0, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
-	bitbang_interface->write(CLOCK_IDLE(), 0, 0);
+	if (bitbang_interface->write(CLOCK_IDLE(), 0, 0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	/* finish in end_state */
 	bitbang_end_state(saved_end_state);
 	if (tap_get_state() != tap_get_end_state())
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
+
+	return ERROR_OK;
 }
 
-static void bitbang_stableclocks(int num_cycles)
+static int bitbang_stableclocks(int num_cycles)
 {
 	int tms = (tap_get_state() == TAP_RESET ? 1 : 0);
 	int i;
 
 	/* send num_cycles clocks onto the cable */
 	for (i = 0; i < num_cycles; i++) {
-		bitbang_interface->write(1, tms, 0);
-		bitbang_interface->write(0, tms, 0);
+		if (bitbang_interface->write(1, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
+		if (bitbang_interface->write(0, tms, 0) != ERROR_OK)
+			return ERROR_FAIL;
 	}
+
+	return ERROR_OK;
 }
 
-static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
+static int bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 		unsigned scan_size)
 {
 	tap_state_t saved_end_state = tap_get_end_state();
@@ -199,7 +217,8 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 		else
 			bitbang_end_state(TAP_DRSHIFT);
 
-		bitbang_state_move(0);
+		if (bitbang_state_move(0) != ERROR_OK)
+			return ERROR_FAIL;
 		bitbang_end_state(saved_end_state);
 	}
 
@@ -218,31 +237,45 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 		if ((type != SCAN_IN) && (buffer[bytec] & bcval))
 			tdi = 1;
 
-		bitbang_interface->write(0, tms, tdi);
+		if (bitbang_interface->write(0, tms, tdi) != ERROR_OK)
+			return ERROR_FAIL;
 
 		if (type != SCAN_OUT) {
 			if (bitbang_interface->buf_size) {
-				bitbang_interface->sample();
+				if (bitbang_interface->sample() != ERROR_OK)
+					return ERROR_FAIL;
 				buffered++;
 			} else {
-				int val = bitbang_interface->read();
-				if (val)
-					buffer[bytec] |= bcval;
-				else
-					buffer[bytec] &= ~bcval;
+				switch (bitbang_interface->read()) {
+					case BB_LOW:
+						buffer[bytec] &= ~bcval;
+						break;
+					case BB_HIGH:
+						buffer[bytec] |= bcval;
+						break;
+					default:
+						return ERROR_FAIL;
+				}
 			}
 		}
 
-		bitbang_interface->write(1, tms, tdi);
+		if (bitbang_interface->write(1, tms, tdi) != ERROR_OK)
+			return ERROR_FAIL;
 
 		if (type != SCAN_OUT && bitbang_interface->buf_size &&
 				(buffered == bitbang_interface->buf_size ||
 				 bit_cnt == scan_size - 1)) {
 			for (unsigned i = bit_cnt + 1 - buffered; i <= bit_cnt; i++) {
-				if (bitbang_interface->read_sample())
-					buffer[i/8] |= 1 << (i % 8);
-				else
-					buffer[i/8] &= ~(1 << (i % 8));
+				switch (bitbang_interface->read_sample()) {
+					case BB_LOW:
+						buffer[i/8] &= ~(1 << (i % 8));
+						break;
+					case BB_HIGH:
+						buffer[i/8] |= 1 << (i % 8);
+						break;
+					default:
+						return ERROR_FAIL;
+				}
 			}
 			buffered = 0;
 		}
@@ -253,8 +286,10 @@ static void bitbang_scan(bool ir_scan, enum scan_type type, uint8_t *buffer,
 		 * the shift state, so we skip the first state
 		 * and move directly to the end state.
 		 */
-		bitbang_state_move(1);
+		if (bitbang_state_move(1) != ERROR_OK)
+			return ERROR_FAIL;
 	}
+	return ERROR_OK;
 }
 
 int bitbang_execute_queue(void)
@@ -275,8 +310,10 @@ int bitbang_execute_queue(void)
 	 */
 	retval = ERROR_OK;
 
-	if (bitbang_interface->blink)
-		bitbang_interface->blink(1);
+	if (bitbang_interface->blink) {
+		if (bitbang_interface->blink(1) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
 	while (cmd) {
 		switch (cmd->type) {
@@ -289,7 +326,9 @@ int bitbang_execute_queue(void)
 				if ((cmd->cmd.reset->trst == 1) ||
 						(cmd->cmd.reset->srst && (jtag_get_reset_config() & RESET_SRST_PULLS_TRST)))
 					tap_set_state(TAP_RESET);
-				bitbang_interface->reset(cmd->cmd.reset->trst, cmd->cmd.reset->srst);
+				if (bitbang_interface->reset(cmd->cmd.reset->trst,
+							cmd->cmd.reset->srst) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_RUNTEST:
 #ifdef _DEBUG_JTAG_IO_
@@ -298,14 +337,16 @@ int bitbang_execute_queue(void)
 						tap_state_name(cmd->cmd.runtest->end_state));
 #endif
 				bitbang_end_state(cmd->cmd.runtest->end_state);
-				bitbang_runtest(cmd->cmd.runtest->num_cycles);
+				if (bitbang_runtest(cmd->cmd.runtest->num_cycles) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 
 			case JTAG_STABLECLOCKS:
 				/* this is only allowed while in a stable state.  A check for a stable
 				 * state was done in jtag_add_clocks()
 				 */
-				bitbang_stableclocks(cmd->cmd.stableclocks->num_cycles);
+				if (bitbang_stableclocks(cmd->cmd.stableclocks->num_cycles) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 
 			case JTAG_TLR_RESET:
@@ -314,7 +355,8 @@ int bitbang_execute_queue(void)
 						tap_state_name(cmd->cmd.statemove->end_state));
 #endif
 				bitbang_end_state(cmd->cmd.statemove->end_state);
-				bitbang_state_move(0);
+				if (bitbang_state_move(0) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_PATHMOVE:
 #ifdef _DEBUG_JTAG_IO_
@@ -322,7 +364,8 @@ int bitbang_execute_queue(void)
 						cmd->cmd.pathmove->num_states,
 						tap_state_name(cmd->cmd.pathmove->path[cmd->cmd.pathmove->num_states - 1]));
 #endif
-				bitbang_path_move(cmd->cmd.pathmove);
+				if (bitbang_path_move(cmd->cmd.pathmove) != ERROR_OK)
+					return ERROR_FAIL;
 				break;
 			case JTAG_SCAN:
 				bitbang_end_state(cmd->cmd.scan->end_state);
@@ -334,7 +377,9 @@ int bitbang_execute_queue(void)
 					tap_state_name(cmd->cmd.scan->end_state));
 #endif
 				type = jtag_scan_type(cmd->cmd.scan);
-				bitbang_scan(cmd->cmd.scan->ir_scan, type, buffer, scan_size);
+				if (bitbang_scan(cmd->cmd.scan->ir_scan, type, buffer,
+							scan_size) != ERROR_OK)
+					return ERROR_FAIL;
 				if (jtag_read_buffer(buffer, cmd->cmd.scan) != ERROR_OK)
 					retval = ERROR_JTAG_QUEUE_FAILED;
 				if (buffer)
@@ -355,8 +400,10 @@ int bitbang_execute_queue(void)
 		}
 		cmd = cmd->next;
 	}
-	if (bitbang_interface->blink)
-		bitbang_interface->blink(0);
+	if (bitbang_interface->blink) {
+		if (bitbang_interface->blink(0) != ERROR_OK)
+			return ERROR_FAIL;
+	}
 
 	return retval;
 }
diff --git a/src/jtag/drivers/bitbang.h b/src/jtag/drivers/bitbang.h
index f0b9263..577717e 100644
--- a/src/jtag/drivers/bitbang.h
+++ b/src/jtag/drivers/bitbang.h
@@ -24,30 +24,35 @@
 
 #include <jtag/swd.h>
 
-struct bitbang_interface {
-	/* low level callbacks (for bitbang)
-	 */
-
-	/* Either read() or sample()/read_sample() must be implemented. */
+typedef enum {
+	BB_LOW,
+	BB_HIGH,
+	BB_ERROR
+} bb_value_t;
 
-	/* Sample TDO and return 0 or 1. */
-	int (*read)(void);
+/** Low level callbacks (for bitbang).
+ *
+ * Either read(), or sample() and read_sample() must be implemented.
+ *
+ * The sample functions allow an interface to batch a number of writes and
+ * sample requests together. Not waiting for a value to come back can greatly
+ * increase throughput. */
+struct bitbang_interface {
+	/** Sample TDO. */
+	bb_value_t (*read)(void);
 
-	/* The sample functions allow an interface to batch a number of writes and
-	 * sample requests together. Not waiting for a value to come back can
-	 * greatly increase throughput. */
-	/* The number of TDO samples that can be buffered up before the caller has
+	/** The number of TDO samples that can be buffered up before the caller has
 	 * to call read_sample. */
 	size_t buf_size;
-	/* Sample TDO and put the result in a buffer. */
-	void (*sample)(void);
-	/* Return the next unread value from the buffer. */
-	int (*read_sample)(void);
+	/** Sample TDO and put the result in a buffer. */
+	int (*sample)(void);
+	/** Return the next unread value from the buffer. */
+	bb_value_t (*read_sample)(void);
 
-	/* Set TCK, TMS, and TDI to the given values. */
-	void (*write)(int tck, int tms, int tdi);
-	void (*reset)(int trst, int srst);
-	void (*blink)(int on);
+	/** Set TCK, TMS, and TDI to the given values. */
+	int (*write)(int tck, int tms, int tdi);
+	int (*reset)(int trst, int srst);
+	int (*blink)(int on);
 	int (*swdio_read)(void);
 	void (*swdio_drive)(bool on);
 };
diff --git a/src/jtag/drivers/cmsis_dap_usb.c b/src/jtag/drivers/cmsis_dap_usb.c
index 345c1fd..b8181d6 100644
--- a/src/jtag/drivers/cmsis_dap_usb.c
+++ b/src/jtag/drivers/cmsis_dap_usb.c
@@ -206,6 +206,8 @@ static uint8_t queued_seq_buf[1024]; /* TODO: make dynamic / move into cmsis obj
 
 static int queued_retval;
 
+static uint8_t output_pins = SWJ_PIN_SRST | SWJ_PIN_TRST;
+
 static struct cmsis_dap *cmsis_dap_handle;
 
 static int cmsis_dap_usb_open(void)
@@ -790,15 +792,21 @@ static int cmsis_dap_swd_switch_seq(enum swd_special_seq seq)
 	unsigned int s_len;
 	int retval;
 
-	/* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
-	cmsis_dap_cmd_DAP_Disconnect();
+	if ((output_pins & (SWJ_PIN_SRST | SWJ_PIN_TRST)) == (SWJ_PIN_SRST | SWJ_PIN_TRST)) {
+		/* Following workaround deasserts reset on most adapters.
+		 * Do not reconnect if a reset line is active!
+		 * Reconnecting would break connecting under reset. */
 
-	/* When we are reconnecting, DAP_Connect needs to be rerun, at
-	 * least on Keil ULINK-ME */
-	retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
+		/* First disconnect before connecting, Atmel EDBG needs it for SAMD/R/L/C */
+		cmsis_dap_cmd_DAP_Disconnect();
+
+		/* When we are reconnecting, DAP_Connect needs to be rerun, at
+		 * least on Keil ULINK-ME */
+		retval = cmsis_dap_cmd_DAP_Connect(seq == LINE_RESET || seq == JTAG_TO_SWD ?
 					   CONNECT_SWD : CONNECT_JTAG);
-	if (retval != ERROR_OK)
-		return retval;
+		if (retval != ERROR_OK)
+			return retval;
+	}
 
 	switch (seq) {
 	case LINE_RESET:
@@ -1010,14 +1018,14 @@ static void cmsis_dap_execute_reset(struct jtag_command *cmd)
 {
 	/* Set both TRST and SRST even if they're not enabled as
 	 * there's no way to tristate them */
-	uint8_t pins = 0;
 
+	output_pins = 0;
 	if (!cmd->cmd.reset->srst)
-		pins |= SWJ_PIN_SRST;
+		output_pins |= SWJ_PIN_SRST;
 	if (!cmd->cmd.reset->trst)
-		pins |= SWJ_PIN_TRST;
+		output_pins |= SWJ_PIN_TRST;
 
-	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(pins,
+	int retval = cmsis_dap_cmd_DAP_SWJ_Pins(output_pins,
 			SWJ_PIN_TRST | SWJ_PIN_SRST, 0, NULL);
 	if (retval != ERROR_OK)
 		LOG_ERROR("CMSIS-DAP: Interface reset failed");
diff --git a/src/jtag/drivers/dummy.c b/src/jtag/drivers/dummy.c
index 0f7c12d..db1ba13 100644
--- a/src/jtag/drivers/dummy.c
+++ b/src/jtag/drivers/dummy.c
@@ -33,14 +33,14 @@ static int clock_count;		/* count clocks in any stable state, only stable states
 
 static uint32_t dummy_data;
 
-static int dummy_read(void)
+static bb_value_t dummy_read(void)
 {
 	int data = 1 & dummy_data;
 	dummy_data = (dummy_data >> 1) | (1 << 31);
-	return data;
+	return data ? BB_HIGH : BB_LOW;
 }
 
-static void dummy_write(int tck, int tms, int tdi)
+static int dummy_write(int tck, int tms, int tdi)
 {
 	/* TAP standard: "state transitions occur on rising edge of clock" */
 	if (tck != dummy_clock) {
@@ -69,9 +69,10 @@ static void dummy_write(int tck, int tms, int tdi)
 		}
 		dummy_clock = tck;
 	}
+	return ERROR_OK;
 }
 
-static void dummy_reset(int trst, int srst)
+static int dummy_reset(int trst, int srst)
 {
 	dummy_clock = 0;
 
@@ -79,10 +80,12 @@ static void dummy_reset(int trst, int srst)
 		dummy_state = TAP_RESET;
 
 	LOG_DEBUG("reset to: %s", tap_state_name(dummy_state));
+	return ERROR_OK;
 }
 
-static void dummy_led(int on)
+static int dummy_led(int on)
 {
+	return ERROR_OK;
 }
 
 static struct bitbang_interface dummy_bitbang = {
diff --git a/src/jtag/drivers/ep93xx.c b/src/jtag/drivers/ep93xx.c
index ccd9795..36fc777 100644
--- a/src/jtag/drivers/ep93xx.c
+++ b/src/jtag/drivers/ep93xx.c
@@ -41,9 +41,9 @@ static volatile uint8_t *gpio_data_direction_register;
 
 /* low level command set
  */
-static int ep93xx_read(void);
-static void ep93xx_write(int tck, int tms, int tdi);
-static void ep93xx_reset(int trst, int srst);
+static bb_value_t ep93xx_read(void);
+static int ep93xx_write(int tck, int tms, int tdi);
+static int ep93xx_reset(int trst, int srst);
 
 static int ep93xx_init(void);
 static int ep93xx_quit(void);
@@ -67,12 +67,12 @@ static struct bitbang_interface ep93xx_bitbang = {
 	.blink = 0,
 };
 
-static int ep93xx_read(void)
+static bb_value_t ep93xx_read(void)
 {
-	return !!(*gpio_data_register & TDO_BIT);
+	return (*gpio_data_register & TDO_BIT) ? BB_HIGH : BB_LOW;
 }
 
-static void ep93xx_write(int tck, int tms, int tdi)
+static int ep93xx_write(int tck, int tms, int tdi)
 {
 	if (tck)
 		output_value |= TCK_BIT;
@@ -91,10 +91,12 @@ static void ep93xx_write(int tck, int tms, int tdi)
 
 	*gpio_data_register = output_value;
 	nanosleep(&ep93xx_zzzz, NULL);
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void ep93xx_reset(int trst, int srst)
+static int ep93xx_reset(int trst, int srst)
 {
 	if (trst == 0)
 		output_value |= TRST_BIT;
@@ -108,6 +110,8 @@ static void ep93xx_reset(int trst, int srst)
 
 	*gpio_data_register = output_value;
 	nanosleep(&ep93xx_zzzz, NULL);
+
+	return ERROR_OK;
 }
 
 static int set_gonk_mode(void)
diff --git a/src/jtag/drivers/ftdi.c b/src/jtag/drivers/ftdi.c
index 32876ba..e69707e 100644
--- a/src/jtag/drivers/ftdi.c
+++ b/src/jtag/drivers/ftdi.c
@@ -1064,8 +1064,19 @@ static int ftdi_swd_init(void)
 static void ftdi_swd_swdio_en(bool enable)
 {
 	struct signal *oe = find_signal_by_name("SWDIO_OE");
-	if (oe)
-		ftdi_set_signal(oe, enable ? '1' : '0');
+	if (oe) {
+		if (oe->data_mask)
+			ftdi_set_signal(oe, enable ? '1' : '0');
+		else {
+			/* Sets TDI/DO pin (pin 2) to input during rx when both pins are connected
+			   to SWDIO */
+			if (enable)
+				direction |= jtag_direction_init & 0x0002U;
+			else
+				direction &= ~0x0002U;
+			mpsse_set_data_bits_low_byte(mpsse_ctx, output & 0xff, direction & 0xff);
+		}
+	}
 }
 
 /**
diff --git a/src/jtag/drivers/imx_gpio.c b/src/jtag/drivers/imx_gpio.c
index f33d109..2a822af 100644
--- a/src/jtag/drivers/imx_gpio.c
+++ b/src/jtag/drivers/imx_gpio.c
@@ -82,9 +82,9 @@ static inline bool gpio_level(int g)
 	return pio_base[g / 32].dr >> (g & 0x1F) & 1;
 }
 
-static int imx_gpio_read(void);
-static void imx_gpio_write(int tck, int tms, int tdi);
-static void imx_gpio_reset(int trst, int srst);
+static bb_value_t imx_gpio_read(void);
+static int imx_gpio_write(int tck, int tms, int tdi);
+static int imx_gpio_reset(int trst, int srst);
 
 static int imx_gpio_swdio_read(void);
 static void imx_gpio_swdio_drive(bool is_output);
@@ -128,12 +128,12 @@ static int speed_coeff = 50000;
 static int speed_offset = 100;
 static unsigned int jtag_delay;
 
-static int imx_gpio_read(void)
+static bb_value_t imx_gpio_read(void)
 {
-	return gpio_level(tdo_gpio);
+	return gpio_level(tdo_gpio) ? BB_HIGH : BB_LOW;
 }
 
-static void imx_gpio_write(int tck, int tms, int tdi)
+static int imx_gpio_write(int tck, int tms, int tdi)
 {
 	tms ? gpio_set(tms_gpio) : gpio_clear(tms_gpio);
 	tdi ? gpio_set(tdi_gpio) : gpio_clear(tdi_gpio);
@@ -141,25 +141,31 @@ static void imx_gpio_write(int tck, int tms, int tdi)
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
-static void imx_gpio_swd_write(int tck, int tms, int tdi)
+static int imx_gpio_swd_write(int tck, int tms, int tdi)
 {
 	tdi ? gpio_set(swdio_gpio) : gpio_clear(swdio_gpio);
 	tck ? gpio_set(swclk_gpio) : gpio_clear(swclk_gpio);
 
 	for (unsigned int i = 0; i < jtag_delay; i++)
 		asm volatile ("");
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void imx_gpio_reset(int trst, int srst)
+static int imx_gpio_reset(int trst, int srst)
 {
 	if (trst_gpio != -1)
 		trst ? gpio_set(trst_gpio) : gpio_clear(trst_gpio);
 
 	if (srst_gpio != -1)
 		srst ? gpio_set(srst_gpio) : gpio_clear(srst_gpio);
+
+	return ERROR_OK;
 }
 
 static void imx_gpio_swdio_drive(bool is_output)
@@ -469,7 +475,7 @@ static int imx_gpio_init(void)
 
 
 	LOG_INFO("imx_gpio mmap: pagesize: %u, regionsize: %u",
-			sysconf(_SC_PAGE_SIZE), IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE);
+			(unsigned int) sysconf(_SC_PAGE_SIZE), IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE);
 	pio_base = mmap(NULL, IMX_GPIO_REGS_COUNT * IMX_GPIO_SIZE,
 				PROT_READ | PROT_WRITE,
 				MAP_SHARED, dev_mem_fd, imx_gpio_peri_base);
diff --git a/src/jtag/drivers/kitprog.c b/src/jtag/drivers/kitprog.c
index 584da8c..db5b62e 100644
--- a/src/jtag/drivers/kitprog.c
+++ b/src/jtag/drivers/kitprog.c
@@ -681,7 +681,7 @@ static int kitprog_swd_run_queue(void)
 	uint8_t *buffer = kitprog_handle->packet_buffer;
 
 	do {
-		LOG_DEBUG("Executing %d queued transactions", pending_transfer_count);
+		LOG_DEBUG_IO("Executing %d queued transactions", pending_transfer_count);
 
 		if (queued_retval != ERROR_OK) {
 			LOG_DEBUG("Skipping due to previous errors: %d", queued_retval);
@@ -714,12 +714,10 @@ static int kitprog_swd_run_queue(void)
 				data &= ~CORUNDETECT;
 			}
 
-#if 0
-			LOG_DEBUG("%s %s reg %x %"PRIx32,
+			LOG_DEBUG_IO("%s %s reg %x %"PRIx32,
 					cmd & SWD_CMD_APnDP ? "AP" : "DP",
 					cmd & SWD_CMD_RnW ? "read" : "write",
 				  (cmd & SWD_CMD_A32) >> 1, data);
-#endif
 
 			buffer[write_count++] = (cmd | SWD_CMD_START | SWD_CMD_PARK) & ~SWD_CMD_STOP;
 			read_count++;
@@ -764,9 +762,7 @@ static int kitprog_swd_run_queue(void)
 			if (pending_transfers[i].cmd & SWD_CMD_RnW) {
 				uint32_t data = le_to_h_u32(&buffer[read_index]);
 
-#if 0
-				LOG_DEBUG("Read result: %"PRIx32, data);
-#endif
+				LOG_DEBUG_IO("Read result: %"PRIx32, data);
 
 				if (pending_transfers[i].buffer)
 					*(uint32_t *)pending_transfers[i].buffer = data;
diff --git a/src/jtag/drivers/parport.c b/src/jtag/drivers/parport.c
index c9e3316..14fa9df 100644
--- a/src/jtag/drivers/parport.c
+++ b/src/jtag/drivers/parport.c
@@ -116,7 +116,7 @@ static unsigned long dataport;
 static unsigned long statusport;
 #endif
 
-static int parport_read(void)
+static bb_value_t parport_read(void)
 {
 	int data = 0;
 
@@ -127,9 +127,9 @@ static int parport_read(void)
 #endif
 
 	if ((data ^ cable->INPUT_INVERT) & cable->TDO_MASK)
-		return 1;
+		return BB_HIGH;
 	else
-		return 0;
+		return BB_LOW;
 }
 
 static inline void parport_write_data(void)
@@ -148,7 +148,7 @@ static inline void parport_write_data(void)
 #endif
 }
 
-static void parport_write(int tck, int tms, int tdi)
+static int parport_write(int tck, int tms, int tdi)
 {
 	int i = wait_states + 1;
 
@@ -169,10 +169,12 @@ static void parport_write(int tck, int tms, int tdi)
 
 	while (i-- > 0)
 		parport_write_data();
+
+	return ERROR_OK;
 }
 
 /* (1) assert or (0) deassert reset lines */
-static void parport_reset(int trst, int srst)
+static int parport_reset(int trst, int srst)
 {
 	LOG_DEBUG("trst: %i, srst: %i", trst, srst);
 
@@ -187,10 +189,12 @@ static void parport_reset(int trst, int srst)
 		dataport_value &= ~cable->SRST_MASK;
 
 	parport_write_data();
+
+	return ERROR_OK;
 }
 
 /* turn LED on parport adapter on (1) or off (0) */
-static void parport_led(int on)
+static int parport_led(int on)
 {
 	if (on)
 		dataport_value |= cable->LED_MASK;
@@ -198,6 +202,8 @@ static void parport_led(int on)
 		dataport_value &= ~cable->LED_MASK;
 
 	parport_write_data();
+
+	return ERROR_OK;
 }
 
 static int parport_speed(int speed)
@@ -365,9 +371,12 @@ static int parport_init(void)
 
 #endif /* PARPORT_USE_PPDEV */
 
-	parport_reset(0, 0);
-	parport_write(0, 0, 0);
-	parport_led(1);
+	if (parport_reset(0, 0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (parport_write(0, 0, 0) != ERROR_OK)
+		return ERROR_FAIL;
+	if (parport_led(1) != ERROR_OK)
+		return ERROR_FAIL;
 
 	bitbang_interface = &parport_bitbang;
 
@@ -376,7 +385,8 @@ static int parport_init(void)
 
 static int parport_quit(void)
 {
-	parport_led(0);
+	if (parport_led(0) != ERROR_OK)
+		return ERROR_FAIL;
 
 	if (parport_exit) {
 		dataport_value = cable->PORT_EXIT;
diff --git a/src/jtag/drivers/presto.c b/src/jtag/drivers/presto.c
index 49caa67..29bc811 100644
--- a/src/jtag/drivers/presto.c
+++ b/src/jtag/drivers/presto.c
@@ -117,8 +117,7 @@ static int presto_read(uint8_t *buf, uint32_t size)
 		ftbytes += presto->retval;
 
 		gettimeofday(&now, NULL);
-		if ((now.tv_sec > timeout.tv_sec) ||
-				((now.tv_sec == timeout.tv_sec) && (now.tv_usec > timeout.tv_usec)))
+		if (timeval_compare(&now, &timeout) > 0)
 			break;
 	}
 
diff --git a/src/jtag/drivers/remote_bitbang.c b/src/jtag/drivers/remote_bitbang.c
index 669e025..1f8fc1a 100644
--- a/src/jtag/drivers/remote_bitbang.c
+++ b/src/jtag/drivers/remote_bitbang.c
@@ -30,18 +30,10 @@
 /* arbitrary limit on host name length: */
 #define REMOTE_BITBANG_HOST_MAX 255
 
-#define REMOTE_BITBANG_RAISE_ERROR(expr ...) \
-	do { \
-		LOG_ERROR(expr); \
-		LOG_ERROR("Terminating openocd."); \
-		exit(-1); \
-	} while (0)
-
 static char *remote_bitbang_host;
 static char *remote_bitbang_port;
 
-static FILE *remote_bitbang_in;
-static FILE *remote_bitbang_out;
+static FILE *remote_bitbang_file;
 static int remote_bitbang_fd;
 
 /* Circular buffer. When start == end, the buffer is empty. */
@@ -57,7 +49,7 @@ static int remote_bitbang_buf_full(void)
 }
 
 /* Read any incoming data, placing it into the buffer. */
-static void remote_bitbang_fill_buf(void)
+static int remote_bitbang_fill_buf(void)
 {
 	socket_nonblock(remote_bitbang_fd);
 	while (!remote_bitbang_buf_full()) {
@@ -79,39 +71,45 @@ static void remote_bitbang_fill_buf(void)
 			if (remote_bitbang_end == sizeof(remote_bitbang_buf))
 				remote_bitbang_end = 0;
 		} else if (count == 0) {
-			return;
+			return ERROR_OK;
 		} else if (count < 0) {
 			if (errno == EAGAIN) {
-				return;
+				return ERROR_OK;
 			} else {
-				REMOTE_BITBANG_RAISE_ERROR("remote_bitbang_fill_buf: %s (%d)",
+				LOG_ERROR("remote_bitbang_fill_buf: %s (%d)",
 						strerror(errno), errno);
+				return ERROR_FAIL;
 			}
 		}
 	}
+
+	return ERROR_OK;
 }
 
-static void remote_bitbang_putc(int c)
+static int remote_bitbang_putc(int c)
 {
-	if (EOF == fputc(c, remote_bitbang_out))
-		REMOTE_BITBANG_RAISE_ERROR("remote_bitbang_putc: %s", strerror(errno));
+	if (EOF == fputc(c, remote_bitbang_file)) {
+		LOG_ERROR("remote_bitbang_putc: %s", strerror(errno));
+		return ERROR_FAIL;
+	}
+	return ERROR_OK;
 }
 
 static int remote_bitbang_quit(void)
 {
-	if (EOF == fputc('Q', remote_bitbang_out)) {
+	if (EOF == fputc('Q', remote_bitbang_file)) {
 		LOG_ERROR("fputs: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
 
-	if (EOF == fflush(remote_bitbang_out)) {
+	if (EOF == fflush(remote_bitbang_file)) {
 		LOG_ERROR("fflush: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
 
 	/* We only need to close one of the FILE*s, because they both use the same */
 	/* underlying file descriptor. */
-	if (EOF == fclose(remote_bitbang_out)) {
+	if (EOF == fclose(remote_bitbang_file)) {
 		LOG_ERROR("fclose: %s", strerror(errno));
 		return ERROR_FAIL;
 	}
@@ -123,26 +121,27 @@ static int remote_bitbang_quit(void)
 	return ERROR_OK;
 }
 
-static int char_to_int(int c)
+static bb_value_t char_to_int(int c)
 {
 	switch (c) {
 		case '0':
-			return 0;
+			return BB_LOW;
 		case '1':
-			return 1;
+			return BB_HIGH;
 		default:
 			remote_bitbang_quit();
-			REMOTE_BITBANG_RAISE_ERROR(
-					"remote_bitbang: invalid read response: %c(%i)", c, c);
+			LOG_ERROR("remote_bitbang: invalid read response: %c(%i)", c, c);
+			return BB_ERROR;
 	}
 }
 
 /* Get the next read response. */
-static int remote_bitbang_rread(void)
+static bb_value_t remote_bitbang_rread(void)
 {
-	if (EOF == fflush(remote_bitbang_out)) {
+	if (EOF == fflush(remote_bitbang_file)) {
 		remote_bitbang_quit();
-		REMOTE_BITBANG_RAISE_ERROR("fflush: %s", strerror(errno));
+		LOG_ERROR("fflush: %s", strerror(errno));
+		return BB_ERROR;
 	}
 
 	/* Enable blocking access. */
@@ -153,19 +152,20 @@ static int remote_bitbang_rread(void)
 		return char_to_int(c);
 	} else {
 		remote_bitbang_quit();
-		REMOTE_BITBANG_RAISE_ERROR("read: count=%d, error=%s", (int) count,
-				strerror(errno));
+		LOG_ERROR("read: count=%d, error=%s", (int) count, strerror(errno));
+		return BB_ERROR;
 	}
 }
 
-static void remote_bitbang_sample(void)
+static int remote_bitbang_sample(void)
 {
-	remote_bitbang_fill_buf();
+	if (remote_bitbang_fill_buf() != ERROR_OK)
+		return ERROR_FAIL;
 	assert(!remote_bitbang_buf_full());
-	remote_bitbang_putc('R');
+	return remote_bitbang_putc('R');
 }
 
-static int remote_bitbang_read_sample(void)
+static bb_value_t remote_bitbang_read_sample(void)
 {
 	if (remote_bitbang_start != remote_bitbang_end) {
 		int c = remote_bitbang_buf[remote_bitbang_start];
@@ -176,22 +176,22 @@ static int remote_bitbang_read_sample(void)
 	return remote_bitbang_rread();
 }
 
-static void remote_bitbang_write(int tck, int tms, int tdi)
+static int remote_bitbang_write(int tck, int tms, int tdi)
 {
 	char c = '0' + ((tck ? 0x4 : 0x0) | (tms ? 0x2 : 0x0) | (tdi ? 0x1 : 0x0));
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
-static void remote_bitbang_reset(int trst, int srst)
+static int remote_bitbang_reset(int trst, int srst)
 {
 	char c = 'r' + ((trst ? 0x2 : 0x0) | (srst ? 0x1 : 0x0));
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
-static void remote_bitbang_blink(int on)
+static int remote_bitbang_blink(int on)
 {
 	char c = on ? 'B' : 'b';
-	remote_bitbang_putc(c);
+	return remote_bitbang_putc(c);
 }
 
 static struct bitbang_interface remote_bitbang_bitbang = {
@@ -289,17 +289,10 @@ static int remote_bitbang_init(void)
 	if (remote_bitbang_fd < 0)
 		return remote_bitbang_fd;
 
-	remote_bitbang_in = fdopen(remote_bitbang_fd, "r");
-	if (remote_bitbang_in == NULL) {
-		LOG_ERROR("fdopen: failed to open read stream");
-		close(remote_bitbang_fd);
-		return ERROR_FAIL;
-	}
-
-	remote_bitbang_out = fdopen(remote_bitbang_fd, "w");
-	if (remote_bitbang_out == NULL) {
+	remote_bitbang_file = fdopen(remote_bitbang_fd, "w+");
+	if (remote_bitbang_file == NULL) {
 		LOG_ERROR("fdopen: failed to open write stream");
-		fclose(remote_bitbang_in);
+		close(remote_bitbang_fd);
 		return ERROR_FAIL;
 	}
 
diff --git a/src/jtag/drivers/stlink_usb.c b/src/jtag/drivers/stlink_usb.c
index 64868ea..6f720b8 100644
--- a/src/jtag/drivers/stlink_usb.c
+++ b/src/jtag/drivers/stlink_usb.c
@@ -1,4 +1,8 @@
 /***************************************************************************
+ *   SWIM contributions by Ake Rehnman                                     *
+ *   Copyright (C) 2017  Ake Rehnman                                       *
+ *   ake.rehnman(at)gmail.com                                              *
+ *                                                                         *
  *   Copyright (C) 2011-2012 by Mathias Kuester                            *
  *   Mathias Kuester <kesmtp@freenet.de>                                   *
  *                                                                         *
@@ -130,6 +134,8 @@ struct stlink_usb_handle_s {
 	bool reconnect_pending;
 };
 
+#define STLINK_SWIM_ERR_OK             0x00
+#define STLINK_SWIM_BUSY               0x01
 #define STLINK_DEBUG_ERR_OK            0x80
 #define STLINK_DEBUG_ERR_FAULT         0x81
 #define STLINK_SWD_AP_WAIT             0x10
@@ -167,8 +173,36 @@ struct stlink_usb_handle_s {
 
 #define STLINK_DFU_EXIT                0x07
 
-#define STLINK_SWIM_ENTER              0x00
-#define STLINK_SWIM_EXIT               0x01
+/*
+	STLINK_SWIM_ENTER_SEQ
+	1.3ms low then 750Hz then 1.5kHz
+
+	STLINK_SWIM_GEN_RST
+	STM8 DM pulls reset pin low 50us
+
+	STLINK_SWIM_SPEED
+	uint8_t (0=low|1=high)
+
+	STLINK_SWIM_WRITEMEM
+	uint16_t length
+	uint32_t address
+
+	STLINK_SWIM_RESET
+	send syncronization seq (16us low, response 64 clocks low)
+*/
+#define STLINK_SWIM_ENTER                  0x00
+#define STLINK_SWIM_EXIT                   0x01
+#define STLINK_SWIM_READ_CAP               0x02
+#define STLINK_SWIM_SPEED                  0x03
+#define STLINK_SWIM_ENTER_SEQ              0x04
+#define STLINK_SWIM_GEN_RST                0x05
+#define STLINK_SWIM_RESET                  0x06
+#define STLINK_SWIM_ASSERT_RESET           0x07
+#define STLINK_SWIM_DEASSERT_RESET         0x08
+#define STLINK_SWIM_READSTATUS             0x09
+#define STLINK_SWIM_WRITEMEM               0x0a
+#define STLINK_SWIM_READMEM                0x0b
+#define STLINK_SWIM_READBUF                0x0c
 
 #define STLINK_DEBUG_ENTER_JTAG            0x00
 #define STLINK_DEBUG_GETSTATUS             0x01
@@ -252,6 +286,7 @@ static const struct {
 };
 
 static void stlink_usb_init_buffer(void *handle, uint8_t direction, uint32_t size);
+static int stlink_swim_status(void *handle);
 
 /** */
 static int stlink_usb_xfer_v1_get_status(void *handle)
@@ -342,7 +377,11 @@ static int stlink_usb_xfer_v1_get_sense(void *handle)
 	return ERROR_OK;
 }
 
-/** */
+/*
+	transfers block in cmdbuf
+	<size> indicates number of bytes in the following
+	data phase.
+*/
 static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 {
 	int err, cmdsize = STLINK_CMD_SIZE_V2;
@@ -350,8 +389,11 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 
 	assert(handle != NULL);
 
-	if (h->version.stlink == 1)
+	if (h->version.stlink == 1) {
 		cmdsize = STLINK_SG_SIZE;
+		/* put length in bCBWCBLength */
+		h->cmdbuf[14] = h->cmdidx-15;
+	}
 
 	err = stlink_usb_xfer_rw(handle, cmdsize, buf, size);
 
@@ -373,7 +415,6 @@ static int stlink_usb_xfer(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
-
 /**
     Converts an STLINK status code held in the first byte of a response
     to an openocd error, logs any error/wait status as debug output.
@@ -384,6 +425,18 @@ static int stlink_usb_error_check(void *handle)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		switch (h->databuf[0]) {
+			case STLINK_SWIM_ERR_OK:
+				return ERROR_OK;
+			case STLINK_SWIM_BUSY:
+				return ERROR_WAIT;
+			default:
+				LOG_DEBUG("unknown/unexpected STLINK status code 0x%x", h->databuf[0]);
+				return ERROR_FAIL;
+		}
+	}
+
 	/* TODO: no error checking yet on api V1 */
 	if (h->jtag_api == STLINK_JTAG_API_V1)
 		h->databuf[0] = STLINK_DEBUG_ERR_OK;
@@ -448,7 +501,7 @@ static int stlink_usb_error_check(void *handle)
 /** Issue an STLINK command via USB transfer, with retries on any wait status responses.
 
     Works for commands where the STLINK_DEBUG status is returned in the first
-    byte of the response packet.
+    byte of the response packet. For SWIM a SWIM_READSTATUS is requested instead.
 
     Returns an openocd result code.
 */
@@ -456,10 +509,21 @@ static int stlink_cmd_allow_retry(void *handle, const uint8_t *buf, int size)
 {
 	int retries = 0;
 	int res;
+	struct stlink_usb_handle_s *h = handle;
+
 	while (1) {
-		res = stlink_usb_xfer(handle, buf, size);
-		if (res != ERROR_OK)
-			return res;
+		if ((h->transport != HL_TRANSPORT_SWIM) || !retries) {
+			res = stlink_usb_xfer(handle, buf, size);
+			if (res != ERROR_OK)
+				return res;
+		}
+
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			res = stlink_swim_status(handle);
+			if (res != ERROR_OK)
+				return res;
+		}
+
 		res = stlink_usb_error_check(handle);
 		if (res == ERROR_WAIT && retries < MAX_WAIT_RETRIES) {
 			usleep((1<<retries++) * 1000);
@@ -487,7 +551,17 @@ static int stlink_usb_read_trace(void *handle, const uint8_t *buf, int size)
 	return ERROR_OK;
 }
 
-/** */
+/*
+	this function writes transfer length in
+	the right place in the cb
+*/
+static void stlink_usb_set_cbw_transfer_datalength(void *handle, uint32_t size)
+{
+	struct stlink_usb_handle_s *h = handle;
+
+	buf_set_u32(h->cmdbuf+8, 0, 32, size);
+}
+
 static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint32_t size)
 {
 	struct stlink_usb_handle_s *h = handle;
@@ -496,12 +570,16 @@ static void stlink_usb_xfer_v1_create_cmd(void *handle, uint8_t direction, uint3
 	strcpy((char *)h->cmdbuf, "USBC");
 	h->cmdidx += 4;
 	/* csw tag not used */
+	buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, 0);
 	h->cmdidx += 4;
+	/* cbw data transfer length (in the following data phase in or out) */
 	buf_set_u32(h->cmdbuf+h->cmdidx, 0, 32, size);
 	h->cmdidx += 4;
+	/* cbw flags */
 	h->cmdbuf[h->cmdidx++] = (direction == h->rx_ep ? ENDPOINT_IN : ENDPOINT_OUT);
 	h->cmdbuf[h->cmdidx++] = 0; /* lun */
-	h->cmdbuf[h->cmdidx++] = STLINK_CMD_SIZE_V1;
+	/* cdb clength (is filled in at xfer) */
+	h->cmdbuf[h->cmdidx++] = 0;
 }
 
 /** */
@@ -681,6 +759,8 @@ static int stlink_usb_mode_enter(void *handle, enum stlink_mode type)
 		case STLINK_MODE_DEBUG_SWIM:
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
 			h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER;
+			/* no answer for this function... */
+			rx_size = 0;
 			break;
 		case STLINK_MODE_DFU:
 		case STLINK_MODE_MASS:
@@ -824,23 +904,228 @@ static int stlink_usb_init_mode(void *handle, bool connect_under_reset)
 		return ERROR_FAIL;
 	}
 
+	/* preliminary SRST assert:
+	 * We want SRST is asserted before activating debug signals (mode_enter).
+	 * As the required mode has not been set, the adapter may not know what pin to use.
+	 * Tested firmware STLINK v2 JTAG v29 API v2 SWIM v0 uses T_NRST pin by default
+	 * Tested firmware STLINK v2 JTAG v27 API v2 SWIM v6 uses T_NRST pin by default
+	 * after power on, SWIM_RST stays unchanged */
+	if (connect_under_reset && emode != STLINK_MODE_DEBUG_SWIM)
+		stlink_usb_assert_srst(handle, 0);
+		/* do not check the return status here, we will
+		   proceed and enter the desired mode below
+		   and try asserting srst again. */
+
+	res = stlink_usb_mode_enter(handle, emode);
+	if (res != ERROR_OK)
+		return res;
+
+	/* assert SRST again: a little bit late but now the adapter knows for sure what pin to use */
 	if (connect_under_reset) {
 		res = stlink_usb_assert_srst(handle, 0);
 		if (res != ERROR_OK)
 			return res;
 	}
 
-	res = stlink_usb_mode_enter(handle, emode);
+	res = stlink_usb_current_mode(handle, &mode);
 
 	if (res != ERROR_OK)
 		return res;
 
-	res = stlink_usb_current_mode(handle, &mode);
+	LOG_DEBUG("MODE: 0x%02X", mode);
+
+	return ERROR_OK;
+}
 
+/* request status from last swim request */
+static int stlink_swim_status(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 4);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READSTATUS;
+	res = stlink_usb_xfer(handle, h->databuf, 4);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+/*
+	the purpose of this function is unknown...
+	capabilites? anyway for swim v6 it returns
+	0001020600000000
+*/
+__attribute__((unused))
+static int stlink_swim_cap(void *handle, uint8_t *cap)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 8);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READ_CAP;
+	h->cmdbuf[h->cmdidx++] = 0x01;
+	res = stlink_usb_xfer(handle, h->databuf, 8);
 	if (res != ERROR_OK)
 		return res;
+	memcpy(cap, h->databuf, 8);
+	return ERROR_OK;
+}
 
-	LOG_DEBUG("MODE: 0x%02X", mode);
+/*	debug dongle assert/deassert sreset line */
+static int stlink_swim_assert_reset(void *handle, int reset)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	if (!reset)
+		h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ASSERT_RESET;
+	else
+		h->cmdbuf[h->cmdidx++] = STLINK_SWIM_DEASSERT_RESET;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	send swim enter seq
+	1.3ms low then 750Hz then 1.5kHz
+*/
+static int stlink_swim_enter(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_ENTER_SEQ;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*	switch high/low speed swim */
+static int stlink_swim_speed(void *handle, int speed)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_SPEED;
+	if (speed)
+		h->cmdbuf[h->cmdidx++] = 1;
+	else
+		h->cmdbuf[h->cmdidx++] = 0;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	initiate srst from swim.
+	nrst is pulled low for 50us.
+*/
+static int stlink_swim_generate_rst(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_GEN_RST;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+/*
+	send resyncronize sequence
+	swim is pulled low for 16us
+	reply is 64 clks low
+*/
+static int stlink_swim_resync(void *handle)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_RESET;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+static int stlink_swim_writebytes(void *handle, uint32_t addr, uint32_t len, const uint8_t *data)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+	unsigned int i;
+	unsigned int datalen = 0;
+	int cmdsize = STLINK_CMD_SIZE_V2;
+
+	if (len > STLINK_DATA_SIZE)
+		return ERROR_FAIL;
+
+	if (h->version.stlink == 1)
+		cmdsize = STLINK_SG_SIZE;
+
+	stlink_usb_init_buffer(handle, h->tx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_WRITEMEM;
+	h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	for (i = 0; i < len; i++) {
+		if (h->cmdidx == cmdsize)
+			h->databuf[datalen++] = *(data++);
+		else
+			h->cmdbuf[h->cmdidx++] = *(data++);
+	}
+	if (h->version.stlink == 1)
+		stlink_usb_set_cbw_transfer_datalength(handle, datalen);
+
+	res = stlink_cmd_allow_retry(handle, h->databuf, datalen);
+	if (res != ERROR_OK)
+		return res;
+	return ERROR_OK;
+}
+
+static int stlink_swim_readbytes(void *handle, uint32_t addr, uint32_t len, uint8_t *data)
+{
+	struct stlink_usb_handle_s *h = handle;
+	int res;
+
+	if (len > STLINK_DATA_SIZE)
+		return ERROR_FAIL;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, 0);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READMEM;
+	h_u16_to_be(h->cmdbuf+h->cmdidx, len);
+	h->cmdidx += 2;
+	h_u32_to_be(h->cmdbuf+h->cmdidx, addr);
+	h->cmdidx += 4;
+	res = stlink_cmd_allow_retry(handle, h->databuf, 0);
+	if (res != ERROR_OK)
+		return res;
+
+	stlink_usb_init_buffer(handle, h->rx_ep, len);
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_COMMAND;
+	h->cmdbuf[h->cmdidx++] = STLINK_SWIM_READBUF;
+	res = stlink_usb_xfer(handle, data, len);
+	if (res != ERROR_OK)
+		return res;
 
 	return ERROR_OK;
 }
@@ -853,6 +1138,12 @@ static int stlink_usb_idcode(void *handle, uint32_t *idcode)
 
 	assert(handle != NULL);
 
+	/* there is no swim read core id cmd */
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		*idcode = 0;
+		return ERROR_OK;
+	}
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 4);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -971,6 +1262,18 @@ static enum target_state stlink_usb_state(void *handle)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
+		if (res != ERROR_OK)
+			return TARGET_UNKNOWN;
+
+		res = stlink_swim_resync(handle);
+		if (res != ERROR_OK)
+			return TARGET_UNKNOWN;
+
+		return ERROR_OK;
+	}
+
 	if (h->reconnect_pending) {
 		LOG_INFO("Previous state query failed, trying to reconnect");
 		res = stlink_usb_mode_enter(handle, stlink_get_mode(h->transport));
@@ -1014,6 +1317,9 @@ static int stlink_usb_assert_srst(void *handle, int srst)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM)
+		return stlink_swim_assert_reset(handle, srst);
+
 	if (h->version.stlink == 1)
 		return ERROR_COMMAND_NOTFOUND;
 
@@ -1088,6 +1394,9 @@ static int stlink_usb_reset(void *handle)
 
 	assert(handle != NULL);
 
+	if (h->transport == HL_TRANSPORT_SWIM)
+		return stlink_swim_generate_rst(handle);
+
 	stlink_usb_init_buffer(handle, h->rx_ep, 2);
 
 	h->cmdbuf[h->cmdidx++] = STLINK_DEBUG_COMMAND;
@@ -1440,6 +1749,11 @@ static int stlink_usb_read_mem(void *handle, uint32_t addr, uint32_t size,
 		if (count < bytes_remaining)
 			bytes_remaining = count;
 
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			retval = stlink_swim_readbytes(handle, addr, bytes_remaining, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+		} else
 		/* the stlink only supports 8/32bit memory read/writes
 		 * honour 32bit, all others will be handled as 8bit access */
 		if (size == 4) {
@@ -1510,6 +1824,11 @@ static int stlink_usb_write_mem(void *handle, uint32_t addr, uint32_t size,
 		if (count < bytes_remaining)
 			bytes_remaining = count;
 
+		if (h->transport == HL_TRANSPORT_SWIM) {
+			retval = stlink_swim_writebytes(handle, addr, bytes_remaining, buffer);
+			if (retval != ERROR_OK)
+				return retval;
+		} else
 		/* the stlink only supports 8/32bit memory read/writes
 		 * honour 32bit, all others will be handled as 8bit access */
 		if (size == 4) {
@@ -1574,6 +1893,20 @@ static int stlink_speed(void *handle, int khz, bool query)
 	int speed_diff = INT_MAX;
 	struct stlink_usb_handle_s *h = handle;
 
+	if (h && (h->transport == HL_TRANSPORT_SWIM)) {
+		/*
+			we dont care what the khz rate is
+			we only have low and high speed...
+			before changing speed the SWIM_CSR HS bit
+			must be updated
+		 */
+		if (khz == 0)
+			stlink_swim_speed(handle, 0);
+		else
+			stlink_swim_speed(handle, 1);
+		return khz;
+	}
+
 	/* only supported by stlink/v2 and for firmware >= 22 */
 	if (h && (h->version.stlink == 1 || h->version.jtag < 22))
 		return khz;
@@ -1622,9 +1955,44 @@ static int stlink_speed(void *handle, int khz, bool query)
 /** */
 static int stlink_usb_close(void *handle)
 {
+	int res;
+	uint8_t mode;
+	enum stlink_mode emode;
 	struct stlink_usb_handle_s *h = handle;
 
 	if (h && h->fd)
+		res = stlink_usb_current_mode(handle, &mode);
+	else
+		res = ERROR_FAIL;
+	/* do not exit if return code != ERROR_OK,
+	   it prevents us from closing jtag_libusb */
+
+	if (res == ERROR_OK) {
+		/* try to exit current mode */
+		switch (mode) {
+			case STLINK_DEV_DFU_MODE:
+				emode = STLINK_MODE_DFU;
+				break;
+			case STLINK_DEV_DEBUG_MODE:
+				emode = STLINK_MODE_DEBUG_SWD;
+				break;
+			case STLINK_DEV_SWIM_MODE:
+				emode = STLINK_MODE_DEBUG_SWIM;
+				break;
+			case STLINK_DEV_BOOTLOADER_MODE:
+			case STLINK_DEV_MASS_MODE:
+			default:
+				emode = STLINK_MODE_UNKNOWN;
+				break;
+		}
+
+		if (emode != STLINK_MODE_UNKNOWN)
+			stlink_usb_mode_leave(handle, emode);
+			/* do not check return code, it prevent
+			us from closing jtag_libusb */
+	}
+
+	if (h && h->fd)
 		jtag_libusb_close(h->fd);
 
 	free(h);
@@ -1778,6 +2146,17 @@ static int stlink_usb_open(struct hl_interface_param_s *param, void **fd)
 		goto error_open;
 	}
 
+	if (h->transport == HL_TRANSPORT_SWIM) {
+		err = stlink_swim_enter(h);
+		if (err != ERROR_OK) {
+			LOG_ERROR("stlink_swim_enter_failed (unable to connect to the target)");
+			goto error_open;
+		}
+		*fd = h;
+		h->max_mem_packet = STLINK_DATA_SIZE;
+		return ERROR_OK;
+	}
+
 	/* clock speed only supported by stlink/v2 and for firmware >= 22 */
 	if (h->version.stlink >= 2 && h->version.jtag >= 22) {
 		LOG_DEBUG("Supported clock speeds are:");
diff --git a/src/jtag/drivers/sysfsgpio.c b/src/jtag/drivers/sysfsgpio.c
index 77b727c..5a4651d 100644
--- a/src/jtag/drivers/sysfsgpio.c
+++ b/src/jtag/drivers/sysfsgpio.c
@@ -244,7 +244,7 @@ static void sysfsgpio_swdio_write(int swclk, int swdio)
  * The sysfs value will read back either '0' or '1'. The trick here is to call
  * lseek to bypass buffering in the sysfs kernel driver.
  */
-static int sysfsgpio_read(void)
+static bb_value_t sysfsgpio_read(void)
 {
 	char buf[1];
 
@@ -257,7 +257,7 @@ static int sysfsgpio_read(void)
 		return 0;
 	}
 
-	return buf[0] != '0';
+	return buf[0] == '0' ? BB_LOW : BB_HIGH;
 }
 
 /*
@@ -266,11 +266,11 @@ static int sysfsgpio_read(void)
  * Seeing as this is the only function where the outputs are changed,
  * we can cache the old value to avoid needlessly writing it.
  */
-static void sysfsgpio_write(int tck, int tms, int tdi)
+static int sysfsgpio_write(int tck, int tms, int tdi)
 {
 	if (swd_mode) {
 		sysfsgpio_swdio_write(tck, tdi);
-		return;
+		return ERROR_OK;
 	}
 
 	const char one[] = "1";
@@ -312,6 +312,8 @@ static void sysfsgpio_write(int tck, int tms, int tdi)
 	last_tdi = tdi;
 	last_tms = tms;
 	last_tck = tck;
+
+	return ERROR_OK;
 }
 
 /*
@@ -319,7 +321,7 @@ static void sysfsgpio_write(int tck, int tms, int tdi)
  *
  * (1) assert or (0) deassert reset lines
  */
-static void sysfsgpio_reset(int trst, int srst)
+static int sysfsgpio_reset(int trst, int srst)
 {
 	LOG_DEBUG("sysfsgpio_reset");
 	const char one[] = "1";
@@ -339,6 +341,8 @@ static void sysfsgpio_reset(int trst, int srst)
 		if (bytes_written != 1)
 			LOG_WARNING("writing trst failed");
 	}
+
+	return ERROR_OK;
 }
 
 COMMAND_HANDLER(sysfsgpio_handle_jtag_gpionums)
@@ -592,10 +596,6 @@ static int sysfsgpio_init(void)
 			LOG_INFO("JTAG and SWD modes enabled");
 		else
 			LOG_INFO("JTAG only mode enabled (specify swclk and swdio gpio to add SWD mode)");
-		if (!is_gpio_valid(trst_gpio) && !is_gpio_valid(srst_gpio)) {
-			LOG_ERROR("Require at least one of trst or srst gpios to be specified");
-			return ERROR_JTAG_INIT_FAILED;
-		}
 	} else if (sysfsgpio_swd_mode_possible()) {
 		LOG_INFO("SWD only mode enabled (specify tck, tms, tdi and tdo gpios to add JTAG mode)");
 	} else {
diff --git a/src/jtag/drivers/usb_blaster/usb_blaster.c b/src/jtag/drivers/usb_blaster/usb_blaster.c
index a975bd1..df9f2a1 100644
--- a/src/jtag/drivers/usb_blaster/usb_blaster.c
+++ b/src/jtag/drivers/usb_blaster/usb_blaster.c
@@ -839,26 +839,30 @@ static int ublast_init(void)
 {
 	int ret, i;
 
-	if (info.lowlevel_name) {
-		for (i = 0; lowlevel_drivers_map[i].name; i++)
-			if (!strcmp(lowlevel_drivers_map[i].name, info.lowlevel_name))
+	for (i = 0; lowlevel_drivers_map[i].name; i++) {
+		if (info.lowlevel_name) {
+			if (!strcmp(lowlevel_drivers_map[i].name, info.lowlevel_name)) {
+				info.drv = lowlevel_drivers_map[i].drv_register();
+				if (!info.drv) {
+					LOG_ERROR("Error registering lowlevel driver \"%s\"",
+						  info.lowlevel_name);
+					return ERROR_JTAG_DEVICE_ERROR;
+				}
 				break;
-		if (lowlevel_drivers_map[i].name)
+			}
+		} else {
 			info.drv = lowlevel_drivers_map[i].drv_register();
-		if (!info.drv) {
-			LOG_ERROR("no lowlevel driver found for %s or lowlevel driver opening error",
-				  info.lowlevel_name);
-			return ERROR_JTAG_DEVICE_ERROR;
-		}
-	} else {
-		LOG_INFO("No lowlevel driver configured, will try them all");
-		for (i = 0; !info.drv && lowlevel_drivers_map[i].name; i++)
-			info.drv = lowlevel_drivers_map[i].drv_register();
-		if (!info.drv) {
-			LOG_ERROR("no lowlevel driver found");
-			return ERROR_JTAG_DEVICE_ERROR;
+			if (info.drv) {
+				info.lowlevel_name = strdup(lowlevel_drivers_map[i].name);
+				LOG_INFO("No lowlevel driver configured, using %s", info.lowlevel_name);
+				break;
+			}
 		}
-		info.lowlevel_name = strdup(lowlevel_drivers_map[i-1].name);
+	}
+
+	if (!info.drv) {
+		LOG_ERROR("No lowlevel driver available");
+		return ERROR_JTAG_DEVICE_ERROR;
 	}
 
 	/*
diff --git a/src/openocd.c b/src/openocd.c
index 83329b5..831bd17 100644
--- a/src/openocd.c
+++ b/src/openocd.c
@@ -344,8 +344,8 @@ int openocd_main(int argc, char *argv[])
 
 	unregister_all_commands(cmd_ctx, NULL);
 
-	/* free commandline interface */
-	command_done(cmd_ctx);
+	/* Shutdown commandline interface */
+	command_exit(cmd_ctx);
 
 	adapter_quit();
 
diff --git a/src/server/gdb_server.c b/src/server/gdb_server.c
index ed17f50..b59b9f1 100644
--- a/src/server/gdb_server.c
+++ b/src/server/gdb_server.c
@@ -991,7 +991,9 @@ static int gdb_new_connection(struct connection *connection)
 	}
 
 	gdb_actual_connections++;
-	LOG_DEBUG("New GDB Connection: %d, Target %s, state: %s",
+	log_printf_lf(all_targets->next != NULL ? LOG_LVL_INFO : LOG_LVL_DEBUG,
+			__FILE__, __LINE__, __func__,
+			"New GDB Connection: %d, Target %s, state: %s",
 			gdb_actual_connections,
 			target_name(target),
 			target_state_name(target));
@@ -3122,7 +3124,7 @@ static int gdb_target_add_one(struct target *target)
 				} else {
 					/* Don't increment if gdb_port is 0, since we're just
 					 * trying to allocate an unused port. */
-					gdb_port_next = alloc_printf("0");
+					gdb_port_next = strdup("0");
 				}
 			}
 		}
diff --git a/src/server/server.c b/src/server/server.c
index f3ae34e..b00db6a 100644
--- a/src/server/server.c
+++ b/src/server/server.c
@@ -301,10 +301,11 @@ int add_service(char *name,
 		}
 
 		struct sockaddr_in addr_in;
+		addr_in.sin_port = 0;
 		socklen_t addr_in_size = sizeof(addr_in);
-		getsockname(c->fd, (struct sockaddr *)&addr_in, &addr_in_size);
-		LOG_INFO("Listening on port %d for %s connections",
-				ntohs(addr_in.sin_port), name);
+		if (getsockname(c->fd, (struct sockaddr *)&addr_in, &addr_in_size) == 0)
+			LOG_INFO("Listening on port %hu for %s connections",
+				 ntohs(addr_in.sin_port), name);
 	} else if (c->type == CONNECTION_STDINOUT) {
 		c->fd = fileno(stdin);
 
diff --git a/src/server/telnet_server.c b/src/server/telnet_server.c
index 7507afe..9077b6c 100644
--- a/src/server/telnet_server.c
+++ b/src/server/telnet_server.c
@@ -54,7 +54,7 @@ static int telnet_write(struct connection *connection, const void *data,
 
 	if (connection_write(connection, data, len) == len)
 		return ERROR_OK;
-	t_con->closed = 1;
+	t_con->closed = true;
 	return ERROR_SERVER_REMOTE_CLOSED;
 }
 
@@ -101,29 +101,36 @@ static void telnet_log_callback(void *priv, const char *file, unsigned line,
 {
 	struct connection *connection = priv;
 	struct telnet_connection *t_con = connection->priv;
-	int i;
+	size_t i;
+	size_t tmp;
 
-	/* if there is no prompt, simply output the message */
-	if (t_con->line_cursor < 0) {
+	/* If the prompt is not visible, simply output the message. */
+	if (!t_con->prompt_visible) {
 		telnet_outputline(connection, string);
 		return;
 	}
 
-	/* clear the command line */
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "                ", i > 16 ? 16 : i);
-	for (i = strlen(t_con->prompt) + t_con->line_size; i > 0; i -= 16)
-		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b", i > 16 ? 16 : i);
+	/* Clear the command line. */
+	tmp = strlen(t_con->prompt) + t_con->line_size;
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+			MIN(tmp - i, 16));
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "                ", MIN(tmp - i, 16));
+
+	for (i = 0; i < tmp; i += 16)
+		telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+			MIN(tmp - i, 16));
 
-	/* output the message */
 	telnet_outputline(connection, string);
 
-	/* put the command line to its previous state */
+	/* Put the command line to its previous state. */
 	telnet_prompt(connection);
 	telnet_write(connection, t_con->line, t_con->line_size);
-	for (i = t_con->line_size; i > t_con->line_cursor; i--)
+
+	for (i = t_con->line_cursor; i < t_con->line_size; i++)
 		telnet_write(connection, "\b", 1);
 }
 
@@ -219,10 +226,11 @@ static int telnet_new_connection(struct connection *connection)
 	connection->priv = telnet_connection;
 
 	/* initialize telnet connection information */
-	telnet_connection->closed = 0;
+	telnet_connection->closed = false;
 	telnet_connection->line_size = 0;
 	telnet_connection->line_cursor = 0;
 	telnet_connection->prompt = strdup("> ");
+	telnet_connection->prompt_visible = true;
 	telnet_connection->state = TELNET_STATE_DATA;
 
 	/* output goes through telnet connection */
@@ -289,7 +297,7 @@ static void telnet_history_up(struct connection *connection)
 {
 	struct telnet_connection *t_con = connection->priv;
 
-	int last_history = (t_con->current_history > 0) ?
+	size_t last_history = (t_con->current_history > 0) ?
 				t_con->current_history - 1 :
 				TELNET_LINE_HISTORY_SIZE-1;
 	telnet_history_go(connection, last_history);
@@ -298,11 +306,36 @@ static void telnet_history_up(struct connection *connection)
 static void telnet_history_down(struct connection *connection)
 {
 	struct telnet_connection *t_con = connection->priv;
+	size_t next_history;
 
-	int next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
+	next_history = (t_con->current_history + 1) % TELNET_LINE_HISTORY_SIZE;
 	telnet_history_go(connection, next_history);
 }
 
+static void telnet_move_cursor(struct connection *connection, size_t pos)
+{
+	struct telnet_connection *tc;
+	size_t tmp;
+
+	tc = connection->priv;
+
+	if (pos < tc->line_cursor) {
+		tmp = tc->line_cursor - pos;
+
+		for (size_t i = 0; i < tmp; i += 16)
+			telnet_write(connection, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b",
+				MIN(tmp - i, 16));
+	} else {
+		tmp = pos - tc->line_cursor;
+
+		for (size_t i = 0; i < tmp; i += 16)
+			telnet_write(connection, tc->line + tc->line_cursor + i,
+				MIN(tmp - i, 16));
+	}
+
+	tc->line_cursor = pos;
+}
+
 static int telnet_input(struct connection *connection)
 {
 	int bytes_read;
@@ -339,7 +372,7 @@ static int telnet_input(struct connection *connection)
 							t_con->line[t_con->line_size++] = *buf_p;
 							t_con->line_cursor++;
 						} else {
-							int i;
+							size_t i;
 							memmove(t_con->line + t_con->line_cursor + 1,
 									t_con->line + t_con->line_cursor,
 									t_con->line_size - t_con->line_cursor);
@@ -374,7 +407,7 @@ static int telnet_input(struct connection *connection)
 							telnet_write(connection, "\r\n\x00", 3);
 
 							if (strcmp(t_con->line, "history") == 0) {
-								int i;
+								size_t i;
 								for (i = 1; i < TELNET_LINE_HISTORY_SIZE; i++) {
 									/* the t_con->next_history line contains empty string
 									 * (unless NULL), thus it is not printed */
@@ -420,7 +453,7 @@ static int telnet_input(struct connection *connection)
 							t_con->line_size = 0;
 
 							/* to suppress prompt in log callback during command execution */
-							t_con->line_cursor = -1;
+							t_con->prompt_visible = false;
 
 							if (strcmp(t_con->line, "shutdown") == 0)
 								telnet_save_history(t_con);
@@ -428,6 +461,7 @@ static int telnet_input(struct connection *connection)
 							retval = command_run_line(command_context, t_con->line);
 
 							t_con->line_cursor = 0;
+							t_con->prompt_visible = true;
 
 							if (retval == ERROR_COMMAND_CLOSE_CONNECTION)
 								return ERROR_SERVER_REMOTE_CLOSED;
@@ -442,7 +476,7 @@ static int telnet_input(struct connection *connection)
 						} else if ((*buf_p == 0x7f) || (*buf_p == 0x8)) {	/* delete character */
 							if (t_con->line_cursor > 0) {
 								if (t_con->line_cursor != t_con->line_size) {
-									int i;
+									size_t i;
 									telnet_write(connection, "\b", 1);
 									t_con->line_cursor--;
 									t_con->line_size--;
@@ -482,6 +516,10 @@ static int telnet_input(struct connection *connection)
 							telnet_history_up(connection);
 						else if (*buf_p == CTRL('N'))		/* cursor down */
 							telnet_history_down(connection);
+						else if (*buf_p == CTRL('A'))
+							telnet_move_cursor(connection, 0);
+						else if (*buf_p == CTRL('E'))
+							telnet_move_cursor(connection, t_con->line_size);
 						else
 							LOG_DEBUG("unhandled nonprintable: %2.2x", *buf_p);
 					}
@@ -538,7 +576,7 @@ static int telnet_input(struct connection *connection)
 					/* Remove character */
 					if (*buf_p == '~') {
 						if (t_con->line_cursor < t_con->line_size) {
-							int i;
+							size_t i;
 							t_con->line_size--;
 							/* remove char from line buffer */
 							memmove(t_con->line + t_con->line_cursor,
diff --git a/src/server/telnet_server.h b/src/server/telnet_server.h
index f8fb826..5e238f4 100644
--- a/src/server/telnet_server.h
+++ b/src/server/telnet_server.h
@@ -46,15 +46,16 @@ enum telnet_states {
 
 struct telnet_connection {
 	char *prompt;
+	bool prompt_visible;
 	enum telnet_states state;
 	char line[TELNET_LINE_MAX_SIZE];
-	int line_size;
-	int line_cursor;
+	size_t line_size;
+	size_t line_cursor;
 	char last_escape;
 	char *history[TELNET_LINE_HISTORY_SIZE];
-	int next_history;
-	int current_history;
-	int closed;
+	size_t next_history;
+	size_t current_history;
+	bool closed;
 };
 
 struct telnet_service {
diff --git a/src/target/aarch64.c b/src/target/aarch64.c
index 5e5d3fc..14a2da6 100644
--- a/src/target/aarch64.c
+++ b/src/target/aarch64.c
@@ -54,7 +54,7 @@ static int aarch64_unset_breakpoint(struct target *target,
 static int aarch64_mmu(struct target *target, int *enabled);
 static int aarch64_virt2phys(struct target *target,
 	target_addr_t virt, target_addr_t *phys);
-static int aarch64_read_apb_ap_memory(struct target *target,
+static int aarch64_read_cpu_memory(struct target *target,
 	uint64_t address, uint32_t size, uint32_t count, uint8_t *buffer);
 
 #define foreach_smp_target(pos, head) \
@@ -161,8 +161,16 @@ static int aarch64_mmu_modify(struct target *target, int enable)
 	case ARMV8_64_EL3T:
 		instr = ARMV8_MSR_GP(SYSTEM_SCTLR_EL3, 0);
 		break;
+
+	case ARM_MODE_SVC:
+	case ARM_MODE_ABT:
+	case ARM_MODE_FIQ:
+	case ARM_MODE_IRQ:
+		instr = ARMV4_5_MCR(15, 0, 0, 1, 0, 0);
+		break;
+
 	default:
-		LOG_DEBUG("unknown cpu state 0x%x" PRIx32, armv8->arm.core_state);
+		LOG_DEBUG("unknown cpu state 0x%" PRIx32, armv8->arm.core_mode);
 		break;
 	}
 
@@ -180,7 +188,7 @@ static int aarch64_init_debug_access(struct target *target)
 	int retval;
 	uint32_t dummy;
 
-	LOG_DEBUG(" ");
+	LOG_DEBUG("%s", target_name(target));
 
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
@@ -634,9 +642,11 @@ static int aarch64_prepare_restart_one(struct target *target)
 				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
 	}
 
-	/* clear sticky bits in PRSR, SDR is now 0 */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
+	if (retval == ERROR_OK) {
+		/* clear sticky bits in PRSR, SDR is now 0 */
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_PRSR, &tmp);
+	}
 
 	return retval;
 }
@@ -770,6 +780,9 @@ static int aarch64_step_restart_smp(struct target *target)
 			if (curr == target)
 				continue;
 
+			if (!target_was_examined(curr))
+				continue;
+
 			retval = aarch64_check_state_one(curr,
 					PRSR_SDR, PRSR_SDR, &resumed, &prsr);
 			if (retval != ERROR_OK || (!resumed && (prsr & PRSR_HALT))) {
@@ -1046,6 +1059,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 	int handle_breakpoints)
 {
 	struct armv8_common *armv8 = target_to_armv8(target);
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
 	int saved_retval = ERROR_OK;
 	int retval;
 	uint32_t edecr;
@@ -1066,7 +1080,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 				armv8->debug_base + CPUV8_DBG_EDECR, (edecr|0x4));
 	}
 	/* disable interrupts while stepping */
-	if (retval == ERROR_OK)
+	if (retval == ERROR_OK && aarch64->isrmasking_mode == AARCH64_ISRMASK_ON)
 		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0x3 << 22);
 	/* bail out if stepping setup has failed */
 	if (retval != ERROR_OK)
@@ -1110,7 +1124,7 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		if (retval != ERROR_OK || stepped)
 			break;
 
-		if (timeval_ms() > then + 1000) {
+		if (timeval_ms() > then + 100) {
 			LOG_ERROR("timeout waiting for target %s halt after step",
 					target_name(target));
 			retval = ERROR_TARGET_TIMEOUT;
@@ -1118,8 +1132,14 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		}
 	}
 
+	/*
+	 * At least on one SoC (Renesas R8A7795) stepping over a WFI instruction
+	 * causes a timeout. The core takes the step but doesn't complete it and so
+	 * debug state is never entered. However, you can manually halt the core
+	 * as an external debug even is also a WFI wakeup event.
+	 */
 	if (retval == ERROR_TARGET_TIMEOUT)
-		saved_retval = retval;
+		saved_retval = aarch64_halt_one(target, HALT_SYNC);
 
 	/* restore EDECR */
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
@@ -1128,9 +1148,11 @@ static int aarch64_step(struct target *target, int current, target_addr_t addres
 		return retval;
 
 	/* restore interrupts */
-	retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
-	if (retval != ERROR_OK)
-		return ERROR_OK;
+	if (aarch64->isrmasking_mode == AARCH64_ISRMASK_ON) {
+		retval = aarch64_set_dscr_bits(target, 0x3 << 22, 0);
+		if (retval != ERROR_OK)
+			return ERROR_OK;
+	}
 
 	if (saved_retval != ERROR_OK)
 		return saved_retval;
@@ -1668,7 +1690,99 @@ static int aarch64_deassert_reset(struct target *target)
 	return aarch64_init_debug_access(target);
 }
 
-static int aarch64_write_apb_ap_memory(struct target *target,
+static int aarch64_write_cpu_memory_slow(struct target *target,
+	uint32_t size, uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* change DCC to normal mode if necessary */
+	if (*dscr & DSCR_MA) {
+		*dscr &= ~DSCR_MA;
+		retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	while (count) {
+		uint32_t data, opcode;
+
+		/* write the data to store into DTRRX */
+		if (size == 1)
+			data = *buffer;
+		else if (size == 2)
+			data = target_buffer_get_u16(target, buffer);
+		else
+			data = target_buffer_get_u32(target, buffer);
+		retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRRX, data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (arm->core_state == ARM_STATE_AARCH64)
+			retval = dpm->instr_execute(dpm, ARMV8_MRS(SYSTEM_DBG_DTRRX_EL0, 1));
+		else
+			retval = dpm->instr_execute(dpm, ARMV4_5_MRC(14, 0, 1, 0, 5, 0));
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (size == 1)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRB_IP);
+		else if (size == 2)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRH_IP);
+		else
+			opcode = armv8_opcode(armv8, ARMV8_OPC_STRW_IP);
+		retval = dpm->instr_execute(dpm, opcode);
+		if (retval != ERROR_OK)
+			return retval;
+
+		/* Advance */
+		buffer += size;
+		--count;
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_write_cpu_memory_fast(struct target *target,
+	uint32_t count, const uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* Step 1.d   - Change DCC to memory mode */
+	*dscr |= DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+
+	/* Step 2.a   - Do the write */
+	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
+					buffer, 4, count, armv8->debug_base + CPUV8_DBG_DTRRX);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 3.a   - Switch DTR mode back to Normal mode */
+	*dscr &= ~DSCR_MA;
+	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	return ERROR_OK;
+}
+
+static int aarch64_write_cpu_memory(struct target *target,
 	uint64_t address, uint32_t size,
 	uint32_t count, const uint8_t *buffer)
 {
@@ -1677,144 +1791,213 @@ static int aarch64_write_apb_ap_memory(struct target *target,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = &armv8->dpm;
 	struct arm *arm = &armv8->arm;
-	int total_bytes = count * size;
-	int total_u32;
-	int start_byte = address & 0x3;
-	int end_byte   = (address + total_bytes) & 0x3;
-	struct reg *reg;
 	uint32_t dscr;
-	uint8_t *tmp_buff = NULL;
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
-
-	/* Mark register R0 as dirty, as it will be used
+	/* Mark register X0 as dirty, as it will be used
 	 * for transferring the data.
 	 * It will be restored automatically when exiting
 	 * debug mode
 	 */
-	reg = armv8_reg_current(arm, 1);
-	reg->dirty = true;
-
-	reg = armv8_reg_current(arm, 0);
-	reg->dirty = true;
+	armv8_reg_current(arm, 0)->dirty = true;
 
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
-	/* The algorithm only copies 32 bit words, so the buffer
-	 * should be expanded to include the words at either end.
-	 * The first and last words will be read first to avoid
-	 * corruption if needed.
-	 */
-	tmp_buff = malloc(total_u32 * 4);
-
-	if ((start_byte != 0) && (total_u32 > 1)) {
-		/* First bytes not aligned - read the 32 bit word to avoid corrupting
-		 * the other bytes in the word.
-		 */
-		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3), 4, 1, tmp_buff);
-		if (retval != ERROR_OK)
-			goto error_free_buff_w;
-	}
-
-	/* If end of write is not aligned, or the write is less than 4 bytes */
-	if ((end_byte != 0) ||
-		((total_u32 == 1) && (total_bytes != 4))) {
-
-		/* Read the last word to avoid corruption during 32 bit write */
-		int mem_offset = (total_u32-1) * 4;
-		retval = aarch64_read_apb_ap_memory(target, (address & ~0x3) + mem_offset, 4, 1, &tmp_buff[mem_offset]);
-		if (retval != ERROR_OK)
-			goto error_free_buff_w;
-	}
-
-	/* Copy the write buffer over the top of the temporary buffer */
-	memcpy(&tmp_buff[start_byte], buffer, total_bytes);
-
-	/* We now have a 32 bit aligned buffer that can be written */
-
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_w;
+		return retval;
 
 	/* Set Normal access mode  */
 	dscr = (dscr & ~DSCR_MA);
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
 		retval = dpm->instr_write_data_dcc_64(dpm,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRX */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		dpm->instr_write_data_dcc(dpm,
-				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
-
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
-	/* Step 1.d   - Change DCC to memory mode */
-	dscr = dscr | DSCR_MA;
-	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
 
-
-	/* Step 2.a   - Do the write */
-	retval = mem_ap_write_buf_noincr(armv8->debug_ap,
-					tmp_buff, 4, total_u32, armv8->debug_base + CPUV8_DBG_DTRRX);
 	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
+		return retval;
 
-	/* Step 3.a   - Switch DTR mode back to Normal mode */
-	dscr = (dscr & ~DSCR_MA);
-	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_w;
+	if (size == 4 && (address % 4) == 0)
+		retval = aarch64_write_cpu_memory_fast(target, count, buffer, &dscr);
+	else
+		retval = aarch64_write_cpu_memory_slow(target, size, count, buffer, &dscr);
+
+	if (retval != ERROR_OK) {
+		/* Unset DTR mode */
+		mem_ap_read_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+		dscr &= ~DSCR_MA;
+		mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	}
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_w;
+		return retval;
 
 	dpm->dscr = dscr;
 	if (dscr & (DSCR_ERR | DSCR_SYS_ERROR_PEND)) {
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		armv8_dpm_handle_exception(dpm);
-		goto error_free_buff_w;
+		return ERROR_FAIL;
 	}
 
 	/* Done */
-	free(tmp_buff);
 	return ERROR_OK;
+}
 
-error_unset_dtr_w:
-	/* Unset DTR mode */
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	dscr = (dscr & ~DSCR_MA);
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-error_free_buff_w:
-	LOG_ERROR("error");
-	free(tmp_buff);
-	return ERROR_FAIL;
+static int aarch64_read_cpu_memory_slow(struct target *target,
+	uint32_t size, uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	/* change DCC to normal mode (if necessary) */
+	if (*dscr & DSCR_MA) {
+		*dscr &= DSCR_MA;
+		retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	while (count) {
+		uint32_t opcode, data;
+
+		if (size == 1)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRB_IP);
+		else if (size == 2)
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRH_IP);
+		else
+			opcode = armv8_opcode(armv8, ARMV8_OPC_LDRW_IP);
+		retval = dpm->instr_execute(dpm, opcode);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (arm->core_state == ARM_STATE_AARCH64)
+			retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DTRTX_EL0, 1));
+		else
+			retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 1, 0, 5, 0));
+		if (retval != ERROR_OK)
+			return retval;
+
+		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+				armv8->debug_base + CPUV8_DBG_DTRTX, &data);
+		if (retval != ERROR_OK)
+			return retval;
+
+		if (size == 1)
+			*buffer = (uint8_t)data;
+		else if (size == 2)
+			target_buffer_set_u16(target, buffer, (uint16_t)data);
+		else
+			target_buffer_set_u32(target, buffer, data);
+
+		/* Advance */
+		buffer += size;
+		--count;
+	}
+
+	return ERROR_OK;
+}
+
+static int aarch64_read_cpu_memory_fast(struct target *target,
+	uint32_t count, uint8_t *buffer, uint32_t *dscr)
+{
+	struct armv8_common *armv8 = target_to_armv8(target);
+	struct arm_dpm *dpm = &armv8->dpm;
+	struct arm *arm = &armv8->arm;
+	int retval;
+	uint32_t value;
+
+	/* Mark X1 as dirty */
+	armv8_reg_current(arm, 1)->dirty = true;
+
+	if (arm->core_state == ARM_STATE_AARCH64) {
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval = dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
+	} else {
+		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
+		retval = dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
+	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 1.e - Change DCC to memory mode */
+	*dscr |= DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 1.f - read DBGDTRTX and discard the value */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	count--;
+	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
+	 * Abort flags are sticky, so can be read at end of transactions
+	 *
+	 * This data is read in aligned to 32 bit boundary.
+	 */
+
+	if (count) {
+		/* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
+		 * increments X0 by 4. */
+		retval = mem_ap_read_buf_noincr(armv8->debug_ap, buffer, 4, count,
+									armv8->debug_base + CPUV8_DBG_DTRTX);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* Step 3.a - set DTR access mode back to Normal mode	*/
+	*dscr &= ~DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+					armv8->debug_base + CPUV8_DBG_DSCR, *dscr);
+	if (retval != ERROR_OK)
+		return retval;
+
+	/* Step 3.b - read DBGDTRTX for the final value */
+	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+	if (retval != ERROR_OK)
+		return retval;
+
+	target_buffer_set_u32(target, buffer + count * 4, value);
+	return retval;
 }
 
-static int aarch64_read_apb_ap_memory(struct target *target,
+static int aarch64_read_cpu_memory(struct target *target,
 	target_addr_t address, uint32_t size,
 	uint32_t count, uint8_t *buffer)
 {
@@ -1823,126 +2006,74 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 	struct armv8_common *armv8 = target_to_armv8(target);
 	struct arm_dpm *dpm = &armv8->dpm;
 	struct arm *arm = &armv8->arm;
-	int total_bytes = count * size;
-	int total_u32;
-	int start_byte = address & 0x3;
-	int end_byte   = (address + total_bytes) & 0x3;
-	struct reg *reg;
 	uint32_t dscr;
-	uint8_t *tmp_buff = NULL;
-	uint8_t *u8buf_ptr;
-	uint32_t value;
+
+	LOG_DEBUG("Reading CPU memory address 0x%016" PRIx64 " size %" PRIu32 " count %" PRIu32,
+			address, size, count);
 
 	if (target->state != TARGET_HALTED) {
 		LOG_WARNING("target not halted");
 		return ERROR_TARGET_NOT_HALTED;
 	}
 
-	total_u32 = DIV_ROUND_UP((address & 3) + total_bytes, 4);
-	/* Mark register X0, X1 as dirty, as it will be used
+	/* Mark register X0 as dirty, as it will be used
 	 * for transferring the data.
 	 * It will be restored automatically when exiting
 	 * debug mode
 	 */
-	reg = armv8_reg_current(arm, 1);
-	reg->dirty = true;
-
-	reg = armv8_reg_current(arm, 0);
-	reg->dirty = true;
+	armv8_reg_current(arm, 0)->dirty = true;
 
 	/* Read DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* This algorithm comes from DDI0487A.g, chapter J9.1 */
 
 	/* Set Normal access mode  */
-	dscr = (dscr & ~DSCR_MA);
-	retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
+	dscr &= ~DSCR_MA;
+	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_DSCR, dscr);
+	if (retval != ERROR_OK)
+		return retval;
 
 	if (arm->core_state == ARM_STATE_AARCH64) {
 		/* Write X0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTR_EL0 */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrs DBGDTR_EL0, x0 */
-		retval += dpm->instr_write_data_dcc_64(dpm,
-				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address & ~0x3ULL);
-		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-		retval += dpm->instr_execute(dpm, ARMV8_MSR_GP(SYSTEM_DBG_DBGDTR_EL0, 0));
-		/* Step 1.e - Change DCC to memory mode */
-		dscr = dscr | DSCR_MA;
-		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-		/* Step 1.f - read DBGDTRTX and discard the value */
-		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
+		retval = dpm->instr_write_data_dcc_64(dpm,
+				ARMV8_MRS(SYSTEM_DBG_DBGDTR_EL0, 0), address);
 	} else {
 		/* Write R0 with value 'address' using write procedure */
 		/* Step 1.a+b - Write the address for read access into DBGDTRRXint */
 		/* Step 1.c   - Copy value from DTR to R0 using instruction mrc DBGDTRTXint, r0 */
-		retval += dpm->instr_write_data_dcc(dpm,
-				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address & ~0x3ULL);
-		/* Step 1.d - Dummy operation to ensure EDSCR.Txfull == 1 */
-		retval += dpm->instr_execute(dpm, ARMV4_5_MCR(14, 0, 0, 0, 5, 0));
-		/* Step 1.e - Change DCC to memory mode */
-		dscr = dscr | DSCR_MA;
-		retval +=  mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-		/* Step 1.f - read DBGDTRTX and discard the value */
-		retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DTRTX, &value);
-
+		retval = dpm->instr_write_data_dcc(dpm,
+				ARMV4_5_MRC(14, 0, 0, 0, 5, 0), address);
 	}
-	if (retval != ERROR_OK)
-		goto error_unset_dtr_r;
-
-	/* Optimize the read as much as we can, either way we read in a single pass  */
-	if ((start_byte) || (end_byte)) {
-		/* The algorithm only copies 32 bit words, so the buffer
-		 * should be expanded to include the words at either end.
-		 * The first and last words will be read into a temp buffer
-		 * to avoid corruption
-		 */
-		tmp_buff = malloc(total_u32 * 4);
-		if (!tmp_buff)
-			goto error_unset_dtr_r;
-
-		/* use the tmp buffer to read the entire data */
-		u8buf_ptr = tmp_buff;
-	} else
-		/* address and read length are aligned so read directly into the passed buffer */
-		u8buf_ptr = buffer;
-
-	/* Read the data - Each read of the DTRTX register causes the instruction to be reissued
-	 * Abort flags are sticky, so can be read at end of transactions
-	 *
-	 * This data is read in aligned to 32 bit boundary.
-	 */
 
-	/* Step 2.a - Loop n-1 times, each read of DBGDTRTX reads the data from [X0] and
-	 * increments X0 by 4. */
-	retval = mem_ap_read_buf_noincr(armv8->debug_ap, u8buf_ptr, 4, total_u32-1,
-									armv8->debug_base + CPUV8_DBG_DTRTX);
 	if (retval != ERROR_OK)
-			goto error_unset_dtr_r;
+		return retval;
 
-	/* Step 3.a - set DTR access mode back to Normal mode	*/
-	dscr = (dscr & ~DSCR_MA);
-	retval =  mem_ap_write_atomic_u32(armv8->debug_ap,
+	if (size == 4 && (address % 4) == 0)
+		retval = aarch64_read_cpu_memory_fast(target, count, buffer, &dscr);
+	else
+		retval = aarch64_read_cpu_memory_slow(target, size, count, buffer, &dscr);
+
+	if (dscr & DSCR_MA) {
+		dscr &= ~DSCR_MA;
+		mem_ap_write_atomic_u32(armv8->debug_ap,
 					armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-	if (retval != ERROR_OK)
-		goto error_free_buff_r;
+	}
 
-	/* Step 3.b - read DBGDTRTX for the final value */
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DTRTX, &value);
-	memcpy(u8buf_ptr + (total_u32-1) * 4, &value, 4);
+	if (retval != ERROR_OK)
+		return retval;
 
 	/* Check for sticky abort flags in the DSCR */
 	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
 				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
 	if (retval != ERROR_OK)
-		goto error_free_buff_r;
+		return retval;
 
 	dpm->dscr = dscr;
 
@@ -1950,29 +2081,11 @@ static int aarch64_read_apb_ap_memory(struct target *target,
 		/* Abort occurred - clear it and exit */
 		LOG_ERROR("abort occurred - dscr = 0x%08" PRIx32, dscr);
 		armv8_dpm_handle_exception(dpm);
-		goto error_free_buff_r;
-	}
-
-	/* check if we need to copy aligned data by applying any shift necessary */
-	if (tmp_buff) {
-		memcpy(buffer, tmp_buff + start_byte, total_bytes);
-		free(tmp_buff);
+		return ERROR_FAIL;
 	}
 
 	/* Done */
 	return ERROR_OK;
-
-error_unset_dtr_r:
-	/* Unset DTR mode */
-	mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, &dscr);
-	dscr = (dscr & ~DSCR_MA);
-	mem_ap_write_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_DSCR, dscr);
-error_free_buff_r:
-	LOG_ERROR("error");
-	free(tmp_buff);
-	return ERROR_FAIL;
 }
 
 static int aarch64_read_phys_memory(struct target *target,
@@ -1986,7 +2099,7 @@ static int aarch64_read_phys_memory(struct target *target,
 		retval = aarch64_mmu_modify(target, 0);
 		if (retval != ERROR_OK)
 			return retval;
-		retval = aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+		retval = aarch64_read_cpu_memory(target, address, size, count, buffer);
 	}
 	return retval;
 }
@@ -2008,7 +2121,7 @@ static int aarch64_read_memory(struct target *target, target_addr_t address,
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	return aarch64_read_apb_ap_memory(target, address, size, count, buffer);
+	return aarch64_read_cpu_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_write_phys_memory(struct target *target,
@@ -2022,7 +2135,7 @@ static int aarch64_write_phys_memory(struct target *target,
 		retval = aarch64_mmu_modify(target, 0);
 		if (retval != ERROR_OK)
 			return retval;
-		return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+		return aarch64_write_cpu_memory(target, address, size, count, buffer);
 	}
 
 	return retval;
@@ -2045,7 +2158,7 @@ static int aarch64_write_memory(struct target *target, target_addr_t address,
 		if (retval != ERROR_OK)
 			return retval;
 	}
-	return aarch64_write_apb_ap_memory(target, address, size, count, buffer);
+	return aarch64_write_cpu_memory(target, address, size, count, buffer);
 }
 
 static int aarch64_handle_target_request(void *priv)
@@ -2090,7 +2203,7 @@ static int aarch64_examine_first(struct target *target)
 	int retval = ERROR_OK;
 	uint64_t debug, ttypr;
 	uint32_t cpuid;
-	uint32_t tmp0, tmp1;
+	uint32_t tmp0, tmp1, tmp2, tmp3;
 	debug = ttypr = cpuid = 0;
 
 	retval = dap_dp_init(swjdp);
@@ -2130,32 +2243,6 @@ static int aarch64_examine_first(struct target *target)
 	} else
 		armv8->debug_base = target->dbgbase;
 
-	uint32_t prsr;
-	int64_t then = timeval_ms();
-	do {
-		retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-				armv8->debug_base + CPUV8_DBG_PRSR, &prsr);
-		if (retval == ERROR_OK) {
-			retval = mem_ap_write_atomic_u32(armv8->debug_ap,
-					armv8->debug_base + CPUV8_DBG_PRCR, PRCR_COREPURQ|PRCR_CORENPDRQ);
-			if (retval != ERROR_OK) {
-				LOG_DEBUG("write to PRCR failed");
-				break;
-			}
-		}
-
-		if (timeval_ms() > then + 1000) {
-			retval = ERROR_TARGET_TIMEOUT;
-			break;
-		}
-
-	} while ((prsr & PRSR_PU) == 0);
-
-	if (retval != ERROR_OK) {
-		LOG_ERROR("target %s: failed to set power state of the core.", target_name(target));
-		return retval;
-	}
-
 	retval = mem_ap_write_atomic_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_OSLAR, 0);
 	if (retval != ERROR_OK) {
@@ -2163,34 +2250,40 @@ static int aarch64_examine_first(struct target *target)
 		return retval;
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval = mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MAINID0, &cpuid);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "CPUID");
 		return retval;
 	}
 
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval = mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MEMFEATURE0, &tmp0);
-	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
+	retval += mem_ap_read_u32(armv8->debug_ap,
 			armv8->debug_base + CPUV8_DBG_MEMFEATURE0 + 4, &tmp1);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "Memory Model Type");
 		return retval;
 	}
-	ttypr |= tmp1;
-	ttypr = (ttypr << 32) | tmp0;
-
-	retval = mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp0);
-	retval += mem_ap_read_atomic_u32(armv8->debug_ap,
-			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp1);
+	retval = mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0, &tmp2);
+	retval += mem_ap_read_u32(armv8->debug_ap,
+			armv8->debug_base + CPUV8_DBG_DBGFEATURE0 + 4, &tmp3);
 	if (retval != ERROR_OK) {
 		LOG_DEBUG("Examine %s failed", "ID_AA64DFR0_EL1");
 		return retval;
 	}
-	debug |= tmp1;
-	debug = (debug << 32) | tmp0;
+
+	retval = dap_run(armv8->debug_ap->dap);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s: examination failed\n", target_name(target));
+		return retval;
+	}
+
+	ttypr |= tmp1;
+	ttypr = (ttypr << 32) | tmp0;
+	debug |= tmp3;
+	debug = (debug << 32) | tmp2;
 
 	LOG_DEBUG("cpuid = 0x%08" PRIx32, cpuid);
 	LOG_DEBUG("ttypr = 0x%08" PRIx64, ttypr);
@@ -2229,9 +2322,9 @@ static int aarch64_examine_first(struct target *target)
 
 	LOG_DEBUG("Configured %i hw breakpoints", aarch64->brp_num);
 
-	target->state = TARGET_RUNNING;
+	target->state = TARGET_UNKNOWN;
 	target->debug_reason = DBG_REASON_NOTHALTED;
-
+	aarch64->isrmasking_mode = AARCH64_ISRMASK_ON;
 	target_set_examined(target);
 	return ERROR_OK;
 }
@@ -2369,6 +2462,34 @@ COMMAND_HANDLER(aarch64_handle_smp_on_command)
 	return ERROR_OK;
 }
 
+COMMAND_HANDLER(aarch64_mask_interrupts_command)
+{
+	struct target *target = get_current_target(CMD_CTX);
+	struct aarch64_common *aarch64 = target_to_aarch64(target);
+
+	static const Jim_Nvp nvp_maskisr_modes[] = {
+		{ .name = "off", .value = AARCH64_ISRMASK_OFF },
+		{ .name = "on", .value = AARCH64_ISRMASK_ON },
+		{ .name = NULL, .value = -1 },
+	};
+	const Jim_Nvp *n;
+
+	if (CMD_ARGC > 0) {
+		n = Jim_Nvp_name2value_simple(nvp_maskisr_modes, CMD_ARGV[0]);
+		if (n->name == NULL) {
+			LOG_ERROR("Unknown parameter: %s - should be off or on", CMD_ARGV[0]);
+			return ERROR_COMMAND_SYNTAX_ERROR;
+		}
+
+		aarch64->isrmasking_mode = n->value;
+	}
+
+	n = Jim_Nvp_value2name_simple(nvp_maskisr_modes, aarch64->isrmasking_mode);
+	command_print(CMD_CTX, "aarch64 interrupt mask %s", n->name);
+
+	return ERROR_OK;
+}
+
 static const struct command_registration aarch64_exec_command_handlers[] = {
 	{
 		.name = "cache_info",
@@ -2397,6 +2518,13 @@ static const struct command_registration aarch64_exec_command_handlers[] = {
 		.help = "Restart smp handling",
 		.usage = "",
 	},
+	{
+		.name = "maskisr",
+		.handler = aarch64_mask_interrupts_command,
+		.mode = COMMAND_ANY,
+		.help = "mask aarch64 interrupts during single-step",
+		.usage = "['on'|'off']",
+	},
 
 	COMMAND_REGISTRATION_DONE
 };
diff --git a/src/target/aarch64.h b/src/target/aarch64.h
index c9ec02d..d7886a3 100644
--- a/src/target/aarch64.h
+++ b/src/target/aarch64.h
@@ -36,6 +36,11 @@
 
 #define AARCH64_PADDRDBG_CPU_SHIFT 13
 
+enum aarch64_isrmasking_mode {
+	AARCH64_ISRMASK_OFF,
+	AARCH64_ISRMASK_ON,
+};
+
 struct aarch64_brp {
 	int used;
 	int type;
@@ -58,6 +63,8 @@ struct aarch64_common {
 	struct aarch64_brp *brp_list;
 
 	struct armv8_common armv8_common;
+
+	enum aarch64_isrmasking_mode isrmasking_mode;
 };
 
 static inline struct aarch64_common *
diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index a6aada3..c503f09 100644
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -124,7 +124,7 @@ static int swd_connect(struct adiv5_dap *dap)
 
 	/* Clear link state, including the SELECT cache. */
 	dap->do_reconnect = false;
-	dap->select = DP_SELECT_INVALID;
+	dap_invalidate_cache(dap);
 
 	swd_queue_dp_read(dap, DP_DPIDR, &dpidr);
 
diff --git a/src/target/arm.h b/src/target/arm.h
index f89aa68..eb4a51f 100644
--- a/src/target/arm.h
+++ b/src/target/arm.h
@@ -66,14 +66,13 @@ enum arm_mode {
 	ARM_MODE_USER_THREAD = 1,
 	ARM_MODE_HANDLER = 2,
 
-	/* shift left 4 bits for armv8 64 */
-	ARMV8_64_EL0T = 0x0F,
-	ARMV8_64_EL1T = 0x4F,
-	ARMV8_64_EL1H = 0x5F,
-	ARMV8_64_EL2T = 0x8F,
-	ARMV8_64_EL2H = 0x9F,
-	ARMV8_64_EL3T = 0xCF,
-	ARMV8_64_EL3H = 0xDF,
+	ARMV8_64_EL0T = 0x0,
+	ARMV8_64_EL1T = 0x4,
+	ARMV8_64_EL1H = 0x5,
+	ARMV8_64_EL2T = 0x8,
+	ARMV8_64_EL2H = 0x9,
+	ARMV8_64_EL3T = 0xC,
+	ARMV8_64_EL3H = 0xD,
 
 	ARM_MODE_ANY = -1
 };
diff --git a/src/target/arm_adi_v5.c b/src/target/arm_adi_v5.c
index 2006290..dfbc5ad 100644
--- a/src/target/arm_adi_v5.c
+++ b/src/target/arm_adi_v5.c
@@ -111,17 +111,68 @@ static int mem_ap_setup_csw(struct adiv5_ap *ap, uint32_t csw)
 
 static int mem_ap_setup_tar(struct adiv5_ap *ap, uint32_t tar)
 {
-	if (tar != ap->tar_value ||
-			(ap->csw_value & CSW_ADDRINC_MASK)) {
+	if (!ap->tar_valid || tar != ap->tar_value) {
 		/* LOG_DEBUG("DAP: Set TAR %x",tar); */
 		int retval = dap_queue_ap_write(ap, MEM_AP_REG_TAR, tar);
 		if (retval != ERROR_OK)
 			return retval;
 		ap->tar_value = tar;
+		ap->tar_valid = true;
 	}
 	return ERROR_OK;
 }
 
+static int mem_ap_read_tar(struct adiv5_ap *ap, uint32_t *tar)
+{
+	int retval = dap_queue_ap_read(ap, MEM_AP_REG_TAR, tar);
+	if (retval != ERROR_OK) {
+		ap->tar_valid = false;
+		return retval;
+	}
+
+	retval = dap_run(ap->dap);
+	if (retval != ERROR_OK) {
+		ap->tar_valid = false;
+		return retval;
+	}
+
+	ap->tar_value = *tar;
+	ap->tar_valid = true;
+	return ERROR_OK;
+}
+
+static uint32_t mem_ap_get_tar_increment(struct adiv5_ap *ap)
+{
+	switch (ap->csw_value & CSW_ADDRINC_MASK) {
+	case CSW_ADDRINC_SINGLE:
+		switch (ap->csw_value & CSW_SIZE_MASK) {
+		case CSW_8BIT:
+			return 1;
+		case CSW_16BIT:
+			return 2;
+		case CSW_32BIT:
+			return 4;
+		}
+	case CSW_ADDRINC_PACKED:
+		return 4;
+	}
+	return 0;
+}
+
+/* mem_ap_update_tar_cache is called after an access to MEM_AP_REG_DRW
+ */
+static void mem_ap_update_tar_cache(struct adiv5_ap *ap)
+{
+	if (!ap->tar_valid)
+		return;
+
+	uint32_t inc = mem_ap_get_tar_increment(ap);
+	if (inc >= max_tar_block_size(ap->tar_autoincr_block, ap->tar_value))
+		ap->tar_valid = false;
+	else
+		ap->tar_value += inc;
+}
+
 /**
  * Queue transactions setting up transfer parameters for the
  * currently selected MEM-AP.
@@ -170,7 +221,8 @@ int mem_ap_read_u32(struct adiv5_ap *ap, uint32_t address,
 	/* Use banked addressing (REG_BDx) to avoid some link traffic
 	 * (updating TAR) when reading several consecutive addresses.
 	 */
-	retval = mem_ap_setup_transfer(ap, CSW_32BIT | CSW_ADDRINC_OFF,
+	retval = mem_ap_setup_transfer(ap,
+			CSW_32BIT | (ap->csw_value & CSW_ADDRINC_MASK),
 			address & 0xFFFFFFF0);
 	if (retval != ERROR_OK)
 		return retval;
@@ -221,7 +273,8 @@ int mem_ap_write_u32(struct adiv5_ap *ap, uint32_t address,
 	/* Use banked addressing (REG_BDx) to avoid some link traffic
 	 * (updating TAR) when writing several consecutive addresses.
 	 */
-	retval = mem_ap_setup_transfer(ap, CSW_32BIT | CSW_ADDRINC_OFF,
+	retval = mem_ap_setup_transfer(ap,
+			CSW_32BIT | (ap->csw_value & CSW_ADDRINC_MASK),
 			address & 0xFFFFFFF0);
 	if (retval != ERROR_OK)
 		return retval;
@@ -272,7 +325,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
 	uint32_t csw_size;
 	uint32_t addr_xor;
-	int retval;
+	int retval = ERROR_OK;
 
 	/* TI BE-32 Quirks mode:
 	 * Writes on big-endian TMS570 behave very strangely. Observed behavior:
@@ -303,10 +356,6 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 	if (ap->unaligned_access_bad && (address % size != 0))
 		return ERROR_TARGET_UNALIGNED_ACCESS;
 
-	retval = mem_ap_setup_tar(ap, address ^ addr_xor);
-	if (retval != ERROR_OK)
-		return retval;
-
 	while (nbytes > 0) {
 		uint32_t this_size = size;
 
@@ -322,36 +371,41 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 		if (retval != ERROR_OK)
 			break;
 
+		retval = mem_ap_setup_tar(ap, address ^ addr_xor);
+		if (retval != ERROR_OK)
+			return retval;
+
 		/* How many source bytes each transfer will consume, and their location in the DRW,
 		 * depends on the type of transfer and alignment. See ARM document IHI0031C. */
 		uint32_t outvalue = 0;
+		uint32_t drw_byte_idx = address;
 		if (dap->ti_be_32_quirks) {
 			switch (this_size) {
 			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (3 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
-				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (1 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (address++ & 3) ^ addr_xor);
+				outvalue |= (uint32_t)*buffer++ << 8 * (0 ^ (drw_byte_idx & 3) ^ addr_xor);
 				break;
 			}
 		} else {
 			switch (this_size) {
 			case 4:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
 				/* fallthrough */
 			case 2:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx++ & 3);
 				/* fallthrough */
 			case 1:
-				outvalue |= (uint32_t)*buffer++ << 8 * (address++ & 3);
+				outvalue |= (uint32_t)*buffer++ << 8 * (drw_byte_idx & 3);
 			}
 		}
 
@@ -361,12 +415,9 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 		if (retval != ERROR_OK)
 			break;
 
-		/* Rewrite TAR if it wrapped or we're xoring addresses */
-		if (addrinc && (addr_xor || (address % ap->tar_autoincr_block < size && nbytes > 0))) {
-			retval = mem_ap_setup_tar(ap, address ^ addr_xor);
-			if (retval != ERROR_OK)
-				break;
-		}
+		mem_ap_update_tar_cache(ap);
+		if (addrinc)
+			address += this_size;
 	}
 
 	/* REVISIT: Might want to have a queued version of this function that does not run. */
@@ -375,8 +426,7 @@ static int mem_ap_write(struct adiv5_ap *ap, const uint8_t *buffer, uint32_t siz
 
 	if (retval != ERROR_OK) {
 		uint32_t tar;
-		if (dap_queue_ap_read(ap, MEM_AP_REG_TAR, &tar) == ERROR_OK
-				&& dap_run(dap) == ERROR_OK)
+		if (mem_ap_read_tar(ap, &tar) == ERROR_OK)
 			LOG_ERROR("Failed to write memory at 0x%08"PRIx32, tar);
 		else
 			LOG_ERROR("Failed to write memory and, additionally, failed to find out where");
@@ -405,7 +455,7 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	const uint32_t csw_addrincr = addrinc ? CSW_ADDRINC_SINGLE : CSW_ADDRINC_OFF;
 	uint32_t csw_size;
 	uint32_t address = adr;
-	int retval;
+	int retval = ERROR_OK;
 
 	/* TI BE-32 Quirks mode:
 	 * Reads on big-endian TMS570 behave strangely differently than writes.
@@ -429,19 +479,14 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	/* Allocate buffer to hold the sequence of DRW reads that will be made. This is a significant
 	 * over-allocation if packed transfers are going to be used, but determining the real need at
 	 * this point would be messy. */
-	uint32_t *read_buf = malloc(count * sizeof(uint32_t));
+	uint32_t *read_buf = calloc(count, sizeof(uint32_t));
+	/* Multiplication count * sizeof(uint32_t) may overflow, calloc() is safe */
 	uint32_t *read_ptr = read_buf;
 	if (read_buf == NULL) {
 		LOG_ERROR("Failed to allocate read buffer");
 		return ERROR_FAIL;
 	}
 
-	retval = mem_ap_setup_tar(ap, address);
-	if (retval != ERROR_OK) {
-		free(read_buf);
-		return retval;
-	}
-
 	/* Queue up all reads. Each read will store the entire DRW word in the read buffer. How many
 	 * useful bytes it contains, and their location in the word, depends on the type of transfer
 	 * and alignment. */
@@ -459,19 +504,19 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 		if (retval != ERROR_OK)
 			break;
 
+		retval = mem_ap_setup_tar(ap, address);
+		if (retval != ERROR_OK)
+			break;
+
 		retval = dap_queue_ap_read(ap, MEM_AP_REG_DRW, read_ptr++);
 		if (retval != ERROR_OK)
 			break;
 
 		nbytes -= this_size;
-		address += this_size;
+		if (addrinc)
+			address += this_size;
 
-		/* Rewrite TAR if it wrapped */
-		if (addrinc && address % ap->tar_autoincr_block < size && nbytes > 0) {
-			retval = mem_ap_setup_tar(ap, address);
-			if (retval != ERROR_OK)
-				break;
-		}
+		mem_ap_update_tar_cache(ap);
 	}
 
 	if (retval == ERROR_OK)
@@ -486,8 +531,8 @@ static int mem_ap_read(struct adiv5_ap *ap, uint8_t *buffer, uint32_t size, uint
 	 * at least give the caller what we have. */
 	if (retval != ERROR_OK) {
 		uint32_t tar;
-		if (dap_queue_ap_read(ap, MEM_AP_REG_TAR, &tar) == ERROR_OK
-				&& dap_run(dap) == ERROR_OK) {
+		if (mem_ap_read_tar(ap, &tar) == ERROR_OK) {
+			/* TAR is incremented after failed transfer on some devices (eg Cortex-M4) */
 			LOG_ERROR("Failed to read memory at 0x%08"PRIx32, tar);
 			if (nbytes > tar - address)
 				nbytes = tar - address;
@@ -597,6 +642,22 @@ struct adiv5_dap *dap_init(void)
 }
 
 /**
+ * Invalidate cached DP select and cached TAR and CSW of all APs
+ */
+void dap_invalidate_cache(struct adiv5_dap *dap)
+{
+	dap->select = DP_SELECT_INVALID;
+	dap->last_read = NULL;
+
+	int i;
+	for (i = 0; i <= 255; i++) {
+		/* force csw and tar write on the next mem-ap access */
+		dap->ap[i].tar_valid = false;
+		dap->ap[i].csw_value = 0;
+	}
+}
+
+/**
  * Initialize a DAP.  This sets up the power domains, prepares the DP
  * for further use and activates overrun checking.
  *
@@ -615,8 +676,7 @@ int dap_dp_init(struct adiv5_dap *dap)
 	if (!dap->ops)
 		dap->ops = &jtag_dp_ops;
 
-	dap->select = DP_SELECT_INVALID;
-	dap->last_read = NULL;
+	dap_invalidate_cache(dap);
 
 	for (size_t i = 0; i < 30; i++) {
 		/* DP initialization */
@@ -688,6 +748,8 @@ int mem_ap_init(struct adiv5_ap *ap)
 	int retval;
 	struct adiv5_dap *dap = ap->dap;
 
+	ap->tar_valid = false;
+	ap->csw_value = 0;      /* force csw and tar write */
 	retval = mem_ap_setup_transfer(ap, CSW_8BIT | CSW_ADDRINC_PACKED, 0);
 	if (retval != ERROR_OK)
 		return retval;
@@ -979,12 +1041,14 @@ static const struct {
 	{ ARM_ID, 0x4a2, "Cortex-A57 ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4a3, "Cortex-A53 ROM",             "(v7 Memory Map ROM Table)", },
 	{ ARM_ID, 0x4a4, "Cortex-A72 ROM",             "(ROM Table)", },
+	{ ARM_ID, 0x4a9, "Cortex-A9 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4af, "Cortex-A15 ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4c0, "Cortex-M0+ ROM",             "(ROM Table)", },
 	{ ARM_ID, 0x4c3, "Cortex-M3 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4c4, "Cortex-M4 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x4c7, "Cortex-M7 PPB ROM",          "(Private Peripheral Bus ROM Table)", },
 	{ ARM_ID, 0x4c8, "Cortex-M7 ROM",              "(ROM Table)", },
+	{ ARM_ID, 0x4b5, "Cortex-R5 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x470, "Cortex-M1 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x471, "Cortex-M0 ROM",              "(ROM Table)", },
 	{ ARM_ID, 0x906, "CoreSight CTI",              "(Cross Trigger)", },
@@ -1025,7 +1089,7 @@ static const struct {
 	{ ARM_ID, 0x9a9, "Cortex-M7 TPIU",             "(Trace Port Interface Unit)", },
 	{ ARM_ID, 0x9ae, "Cortex-A17 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9af, "Cortex-A15 PMU",             "(Performance Monitor Unit)", },
-	{ ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitoring Unit)", },
+	{ ARM_ID, 0x9b7, "Cortex-R7 PMU",              "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d3, "Cortex-A53 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d7, "Cortex-A57 PMU",             "(Performance Monitor Unit)", },
 	{ ARM_ID, 0x9d8, "Cortex-A72 PMU",             "(Performance Monitor Unit)", },
@@ -1048,6 +1112,11 @@ static const struct {
 	{ 0x0c1,  0x1ed, "XMC1000 ROM",                "(ROM Table)" },
 	{ 0x0E5,  0x000, "SHARC+/Blackfin+",           "", },
 	{ 0x0F0,  0x440, "Qualcomm QDSS Component v1", "(Qualcomm Designed CoreSight Component v1)", },
+	{ 0x3eb,  0x181, "Tegra 186 ROM",              "(ROM Table)", },
+	{ 0x3eb,  0x211, "Tegra 210 ROM",              "(ROM Table)", },
+	{ 0x3eb,  0x202, "Denver ETM",                 "(Denver Embedded Trace)", },
+	{ 0x3eb,  0x302, "Denver Debug",               "(Debug Unit)", },
+	{ 0x3eb,  0x402, "Denver PMU",                 "(Performance Monitor Unit)", },
 	/* legacy comment: 0x113: what? */
 	{ ANY_ID, 0x120, "TI SDTI",                    "(System Debug Trace Interface)", }, /* from OMAP3 memmap */
 	{ ANY_ID, 0x343, "TI DAPCTL",                  "", }, /* from OMAP3 memmap */
diff --git a/src/target/arm_adi_v5.h b/src/target/arm_adi_v5.h
index bf9cb5c..657427b 100644
--- a/src/target/arm_adi_v5.h
+++ b/src/target/arm_adi_v5.h
@@ -102,6 +102,7 @@
 #define AP_REG_IDR			0xFC		/* RO: Identification Register */
 
 /* Fields of the MEM-AP's CSW register */
+#define CSW_SIZE_MASK		7
 #define CSW_8BIT		0
 #define CSW_16BIT		1
 #define CSW_32BIT		2
@@ -180,6 +181,9 @@ struct adiv5_ap {
 
 	/* true if unaligned memory access is not supported by the MEM-AP */
 	bool unaligned_access_bad;
+
+	/* true if tar_value is in sync with TAR register */
+	bool tar_valid;
 };
 
 
@@ -476,6 +480,9 @@ struct adiv5_dap *dap_init(void);
 int dap_dp_init(struct adiv5_dap *dap);
 int mem_ap_init(struct adiv5_ap *ap);
 
+/* Invalidate cached DP select and cached TAR and CSW of all APs */
+void dap_invalidate_cache(struct adiv5_dap *dap);
+
 /* Probe the AP for ROM Table location */
 int dap_get_debugbase(struct adiv5_ap *ap,
 			uint32_t *dbgbase, uint32_t *apid);
diff --git a/src/target/arm_disassembler.c b/src/target/arm_disassembler.c
index 3f1daca..ef69a20 100644
--- a/src/target/arm_disassembler.c
+++ b/src/target/arm_disassembler.c
@@ -129,6 +129,59 @@ static int evaluate_pld(uint32_t opcode,
 
 		return ERROR_OK;
 	}
+	/* DSB */
+	if ((opcode & 0x07f000f0) == 0x05700040) {
+		instruction->type = ARM_DSB;
+
+		char *opt;
+		switch (opcode & 0x0000000f) {
+		case 0xf:
+			opt = "SY";
+			break;
+		case 0xe:
+			opt = "ST";
+			break;
+		case 0xb:
+			opt = "ISH";
+			break;
+		case 0xa:
+			opt = "ISHST";
+			break;
+		case 0x7:
+			opt = "NSH";
+			break;
+		case 0x6:
+			opt = "NSHST";
+			break;
+		case 0x3:
+			opt = "OSH";
+			break;
+		case 0x2:
+			opt = "OSHST";
+			break;
+		default:
+			opt = "UNK";
+		}
+
+		snprintf(instruction->text,
+				128,
+				"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tDSB %s",
+				address, opcode, opt);
+
+		return ERROR_OK;
+	}
+	/* ISB */
+	if ((opcode & 0x07f000f0) == 0x05700060) {
+		instruction->type = ARM_ISB;
+
+		snprintf(instruction->text,
+				128,
+				"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tISB %s",
+				address, opcode,
+				((opcode & 0x0000000f) == 0xf) ? "SY" : "UNK");
+
+		return ERROR_OK;
+	}
 	return evaluate_unknown(opcode, address, instruction);
 }
 
@@ -1562,6 +1615,33 @@ static int evaluate_misc_instr(uint32_t opcode,
 	return ERROR_OK;
 }
 
+static int evaluate_mov_imm(uint32_t opcode,
+			      uint32_t address, struct arm_instruction *instruction)
+{
+	uint16_t immediate;
+	uint8_t Rd;
+	bool T;
+
+	Rd = (opcode & 0xf000) >> 12;
+	T = opcode & 0x00400000;
+	immediate = (opcode & 0xf0000) >> 4 | (opcode & 0xfff);
+
+	instruction->type = ARM_MOV;
+	instruction->info.data_proc.Rd = Rd;
+
+	snprintf(instruction->text,
+		 128,
+		 "0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tMOV%s%s r%i, #0x%" PRIx16,
+		 address,
+		 opcode,
+		 T ? "T" : "W",
+		 COND(opcode),
+		 Rd,
+		 immediate);
+
+	return ERROR_OK;
+}
+
 static int evaluate_data_proc(uint32_t opcode,
 			      uint32_t address, struct arm_instruction *instruction)
 {
@@ -1838,16 +1918,9 @@ int arm_evaluate_opcode(uint32_t opcode, uint32_t address,
 
 	/* catch opcodes with [27:25] = b001 */
 	if ((opcode & 0x0e000000) == 0x02000000) {
-		/* Undefined instruction */
-		if ((opcode & 0x0fb00000) == 0x03000000) {
-			instruction->type = ARM_UNDEFINED_INSTRUCTION;
-			snprintf(instruction->text,
-					128,
-					"0x%8.8" PRIx32 "\t0x%8.8" PRIx32 "\tUNDEFINED INSTRUCTION",
-					address,
-					opcode);
-			return ERROR_OK;
-		}
+		/* 16-bit immediate load */
+		if ((opcode & 0x0fb00000) == 0x03000000)
+			return evaluate_mov_imm(opcode, address, instruction);
 
 		/* Move immediate to status register */
 		if ((opcode & 0x0fb00000) == 0x03200000)
@@ -2896,12 +2969,26 @@ static int t2ev_b_bl(uint32_t opcode, uint32_t address,
 	address += 4;
 	address += offset << 1;
 
-	instruction->type = (opcode & (1 << 14)) ? ARM_BL : ARM_B;
+	char *inst;
+	switch ((opcode >> 12) & 0x5) {
+	case 0x1:
+		inst = "B.W";
+		instruction->type = ARM_B;
+		break;
+	case 0x4:
+		inst = "BLX";
+		instruction->type = ARM_BLX;
+		break;
+	case 0x5:
+		inst = "BL";
+		instruction->type = ARM_BL;
+		break;
+	default:
+		return ERROR_COMMAND_SYNTAX_ERROR;
+	}
 	instruction->info.b_bl_bx_blx.reg_operand = -1;
 	instruction->info.b_bl_bx_blx.target_address = address;
-	sprintf(cp, "%s\t%#8.8" PRIx32,
-			(opcode & (1 << 14)) ? "BL" : "B.W",
-			address);
+	sprintf(cp, "%s\t%#8.8" PRIx32, inst, address);
 
 	return ERROR_OK;
 }
@@ -3078,10 +3165,9 @@ static int t2ev_b_misc(uint32_t opcode, uint32_t address,
 
 	switch ((opcode >> 12) & 0x5) {
 		case 0x1:
+		case 0x4:
 		case 0x5:
 			return t2ev_b_bl(opcode, address, instruction, cp);
-		case 0x4:
-			goto undef;
 		case 0:
 			if (((opcode >> 23) & 0x07) != 0x07)
 				return t2ev_cond_b(opcode, address, instruction, cp);
diff --git a/src/target/arm_disassembler.h b/src/target/arm_disassembler.h
index 6f8f65d..e9f4d44 100644
--- a/src/target/arm_disassembler.h
+++ b/src/target/arm_disassembler.h
@@ -106,6 +106,8 @@ enum arm_instruction_type {
 	ARM_MCRR,
 	ARM_MRRC,
 	ARM_PLD,
+	ARM_DSB,
+	ARM_ISB,
 	ARM_QADD,
 	ARM_QDADD,
 	ARM_QSUB,
diff --git a/src/target/armv8.c b/src/target/armv8.c
index df5e251..1b8e450 100644
--- a/src/target/armv8.c
+++ b/src/target/armv8.c
@@ -45,8 +45,6 @@ static const struct {
 	const char *name;
 	unsigned psr;
 } armv8_mode_data[] = {
-	/* These special modes are currently only supported
-	 * by ARMv6M and ARMv7M profiles */
 	{
 		.name = "USR",
 		.psr = ARM_MODE_USR,
@@ -112,48 +110,6 @@ const char *armv8_mode_name(unsigned psr_mode)
 	return "UNRECOGNIZED";
 }
 
-int armv8_mode_to_number(enum arm_mode mode)
-{
-	switch (mode) {
-		case ARM_MODE_ANY:
-		/* map MODE_ANY to user mode */
-		case ARM_MODE_USR:
-			return 0;
-		case ARM_MODE_FIQ:
-			return 1;
-		case ARM_MODE_IRQ:
-			return 2;
-		case ARM_MODE_SVC:
-			return 3;
-		case ARM_MODE_ABT:
-			return 4;
-		case ARM_MODE_UND:
-			return 5;
-		case ARM_MODE_SYS:
-			return 6;
-		case ARM_MODE_MON:
-			return 7;
-		case ARMV8_64_EL0T:
-			return 8;
-		case ARMV8_64_EL1T:
-			return 9;
-		case ARMV8_64_EL1H:
-			return 10;
-		case ARMV8_64_EL2T:
-			return 11;
-		case ARMV8_64_EL2H:
-			return 12;
-		case ARMV8_64_EL3T:
-			return 13;
-		case ARMV8_64_EL3H:
-			return 14;
-
-		default:
-			LOG_ERROR("invalid mode value encountered %d", mode);
-			return -1;
-	}
-}
-
 static int armv8_read_reg(struct armv8_common *armv8, int regnum, uint64_t *regval)
 {
 	struct arm_dpm *dpm = &armv8->dpm;
@@ -533,9 +489,8 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 	/* Older ARMs won't have the J bit */
 	enum arm_state state = 0xFF;
 
-	if (((cpsr & 0x10) >> 4) == 0) {
-		state = ARM_STATE_AARCH64;
-	} else {
+	if ((cpsr & 0x10) != 0) {
+		/* Aarch32 state */
 		if (cpsr & (1 << 5)) {	/* T */
 			if (cpsr & (1 << 24)) { /* J */
 				LOG_WARNING("ThumbEE -- incomplete support");
@@ -549,12 +504,13 @@ void armv8_set_cpsr(struct arm *arm, uint32_t cpsr)
 			} else
 				state = ARM_STATE_ARM;
 		}
+	} else {
+		/* Aarch64 state */
+		state = ARM_STATE_AARCH64;
 	}
+
 	arm->core_state = state;
-	if (arm->core_state == ARM_STATE_AARCH64)
-		arm->core_mode = (mode << 4) | 0xf;
-	else
-		arm->core_mode = mode;
+	arm->core_mode = mode;
 
 	LOG_DEBUG("set CPSR %#8.8x: %s mode, %s state", (unsigned) cpsr,
 		armv8_mode_name(arm->core_mode),
diff --git a/src/target/armv8.h b/src/target/armv8.h
index 02663ca..0f3e66f 100644
--- a/src/target/armv8.h
+++ b/src/target/armv8.h
@@ -270,7 +270,7 @@ static inline unsigned int armv8_curel_from_core_mode(enum arm_mode core_mode)
 		return 3;
 	/* all Aarch64 modes */
 	default:
-		return (core_mode >> 6) & 3;
+		return (core_mode >> 2) & 3;
 	}
 }
 
diff --git a/src/target/armv8_dpm.c b/src/target/armv8_dpm.c
index f4e7a07..c79b1a0 100644
--- a/src/target/armv8_dpm.c
+++ b/src/target/armv8_dpm.c
@@ -561,12 +561,7 @@ int armv8_dpm_modeswitch(struct arm_dpm *dpm, enum arm_mode mode)
 
 	} else {
 		LOG_DEBUG("setting mode 0x%"PRIx32, mode);
-
-		/* else force to the specified mode */
-		if (is_arm_mode(mode))
-			cpsr = mode;
-		else
-			cpsr = mode >> 4;
+		cpsr = mode;
 	}
 
 	switch (cpsr & 0x1f) {
diff --git a/src/target/armv8_opcodes.c b/src/target/armv8_opcodes.c
index d3c0b3f..6887b29 100644
--- a/src/target/armv8_opcodes.c
+++ b/src/target/armv8_opcodes.c
@@ -42,6 +42,12 @@ static const uint32_t a64_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCCIVAC]	= ARMV8_SYS(SYSTEM_DCCIVAC, 0),
 		[ARMV8_OPC_ICIVAU]	= ARMV8_SYS(SYSTEM_ICIVAU, 0),
 		[ARMV8_OPC_HLT]		= ARMV8_HLT(11),
+		[ARMV8_OPC_LDRB_IP]	= ARMV8_LDRB_IP(1, 0),
+		[ARMV8_OPC_LDRH_IP]	= ARMV8_LDRH_IP(1, 0),
+		[ARMV8_OPC_LDRW_IP]	= ARMV8_LDRW_IP(1, 0),
+		[ARMV8_OPC_STRB_IP]	= ARMV8_STRB_IP(1, 0),
+		[ARMV8_OPC_STRH_IP]	= ARMV8_STRH_IP(1, 0),
+		[ARMV8_OPC_STRW_IP]	= ARMV8_STRW_IP(1, 0),
 };
 
 static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
@@ -63,6 +69,12 @@ static const uint32_t t32_opcodes[ARMV8_OPC_NUM] = {
 		[ARMV8_OPC_DCCIVAC]	= ARMV4_5_MCR(15, 0, 0, 7, 14, 1),
 		[ARMV8_OPC_ICIVAU]	= ARMV4_5_MCR(15, 0, 0, 7, 5, 1),
 		[ARMV8_OPC_HLT]		= ARMV8_HLT_A1(11),
+		[ARMV8_OPC_LDRB_IP]	= ARMV4_5_LDRB_IP(1, 0),
+		[ARMV8_OPC_LDRH_IP]	= ARMV4_5_LDRH_IP(1, 0),
+		[ARMV8_OPC_LDRW_IP]	= ARMV4_5_LDRW_IP(1, 0),
+		[ARMV8_OPC_STRB_IP]	= ARMV4_5_STRB_IP(1, 0),
+		[ARMV8_OPC_STRH_IP]	= ARMV4_5_STRH_IP(1, 0),
+		[ARMV8_OPC_STRW_IP]	= ARMV4_5_STRW_IP(1, 0),
 };
 
 void armv8_select_opcodes(struct armv8_common *armv8, bool state_is_aarch64)
diff --git a/src/target/armv8_opcodes.h b/src/target/armv8_opcodes.h
index 2d8ddd8..987198a 100644
--- a/src/target/armv8_opcodes.h
+++ b/src/target/armv8_opcodes.h
@@ -159,6 +159,14 @@
 #define ARMV8_MOVFSP_32(Rt) (0x11000000 | (0x1f << 5) | (Rt))
 #define ARMV8_MOVTSP_32(Rt) (0x11000000 | (Rt << 5) | (0x1F))
 
+#define ARMV8_LDRB_IP(Rd, Rn) (0x38401400 | (Rn << 5) | Rd)
+#define ARMV8_LDRH_IP(Rd, Rn) (0x78402400 | (Rn << 5) | Rd)
+#define ARMV8_LDRW_IP(Rd, Rn) (0xb8404400 | (Rn << 5) | Rd)
+
+#define ARMV8_STRB_IP(Rd, Rn) (0x38001400 | (Rn << 5) | Rd)
+#define ARMV8_STRH_IP(Rd, Rn) (0x78002400 | (Rn << 5) | Rd)
+#define ARMV8_STRW_IP(Rd, Rn) (0xb8004400 | (Rn << 5) | Rd)
+
 #define ARMV8_SYS(System, Rt) (0xD5080000 | ((System) << 5) | Rt)
 
 enum armv8_opcode {
@@ -180,6 +188,12 @@ enum armv8_opcode {
 	ARMV8_OPC_DCCIVAC,
 	ARMV8_OPC_ICIVAU,
 	ARMV8_OPC_HLT,
+	ARMV8_OPC_STRB_IP,
+	ARMV8_OPC_STRH_IP,
+	ARMV8_OPC_STRW_IP,
+	ARMV8_OPC_LDRB_IP,
+	ARMV8_OPC_LDRH_IP,
+	ARMV8_OPC_LDRW_IP,
 	ARMV8_OPC_NUM,
 };
 
diff --git a/src/target/cortex_m.c b/src/target/cortex_m.c
index 2f8c2a2..79af632 100644
--- a/src/target/cortex_m.c
+++ b/src/target/cortex_m.c
@@ -1707,6 +1707,97 @@ void cortex_m_deinit_target(struct target *target)
 	free(cortex_m);
 }
 
+int cortex_m_profiling(struct target *target, uint32_t *samples,
+			      uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds)
+{
+	struct timeval timeout, now;
+	struct armv7m_common *armv7m = target_to_armv7m(target);
+	uint32_t reg_value;
+	bool use_pcsr = false;
+	int retval = ERROR_OK;
+	struct reg *reg;
+
+	gettimeofday(&timeout, NULL);
+	timeval_add_time(&timeout, seconds, 0);
+
+	retval = target_read_u32(target, DWT_PCSR, &reg_value);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error while reading PCSR");
+		return retval;
+	}
+
+	if (reg_value != 0) {
+		use_pcsr = true;
+		LOG_INFO("Starting Cortex-M profiling. Sampling DWT_PCSR as fast as we can...");
+	} else {
+		LOG_INFO("Starting profiling. Halting and resuming the"
+			 " target as often as we can...");
+		reg = register_get_by_name(target->reg_cache, "pc", 1);
+	}
+
+	/* Make sure the target is running */
+	target_poll(target);
+	if (target->state == TARGET_HALTED)
+		retval = target_resume(target, 1, 0, 0, 0);
+
+	if (retval != ERROR_OK) {
+		LOG_ERROR("Error while resuming target");
+		return retval;
+	}
+
+	uint32_t sample_count = 0;
+
+	for (;;) {
+		if (use_pcsr) {
+			if (armv7m && armv7m->debug_ap) {
+				uint32_t read_count = max_num_samples - sample_count;
+				if (read_count > 1024)
+					read_count = 1024;
+
+				retval = mem_ap_read_buf_noincr(armv7m->debug_ap,
+							(void *)&samples[sample_count],
+							4, read_count, DWT_PCSR);
+				sample_count += read_count;
+			} else {
+				target_read_u32(target, DWT_PCSR, &samples[sample_count++]);
+			}
+		} else {
+			target_poll(target);
+			if (target->state == TARGET_HALTED) {
+				reg_value = buf_get_u32(reg->value, 0, 32);
+				/* current pc, addr = 0, do not handle breakpoints, not debugging */
+				retval = target_resume(target, 1, 0, 0, 0);
+				samples[sample_count++] = reg_value;
+				target_poll(target);
+				alive_sleep(10); /* sleep 10ms, i.e. <100 samples/second. */
+			} else if (target->state == TARGET_RUNNING) {
+				/* We want to quickly sample the PC. */
+				retval = target_halt(target);
+			} else {
+				LOG_INFO("Target not halted or running");
+				retval = ERROR_OK;
+				break;
+			}
+		}
+
+		if (retval != ERROR_OK) {
+			LOG_ERROR("Error while reading %s", use_pcsr ? "PCSR" : "target pc");
+			return retval;
+		}
+
+
+		gettimeofday(&now, NULL);
+		if (sample_count >= max_num_samples || timeval_compare(&now, &timeout) > 0) {
+			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
+			break;
+		}
+	}
+
+	*num_samples = sample_count;
+	return retval;
+}
+
+
 /* REVISIT cache valid/dirty bits are unmaintained.  We could set "valid"
  * on r/w if the core is not running, and clear on resume or reset ... or
  * at least, in a post_restore_context() method.
@@ -2451,4 +2542,6 @@ struct target_type cortexm_target = {
 	.init_target = cortex_m_init_target,
 	.examine = cortex_m_examine,
 	.deinit_target = cortex_m_deinit_target,
+
+	.profiling = cortex_m_profiling,
 };
diff --git a/src/target/cortex_m.h b/src/target/cortex_m.h
index 3d9714b..9500acc 100644
--- a/src/target/cortex_m.h
+++ b/src/target/cortex_m.h
@@ -48,6 +48,7 @@
 
 #define DWT_CTRL	0xE0001000
 #define DWT_CYCCNT	0xE0001004
+#define DWT_PCSR	0xE000101C
 #define DWT_COMP0	0xE0001020
 #define DWT_MASK0	0xE0001024
 #define DWT_FUNCTION0	0xE0001028
@@ -212,5 +213,7 @@ void cortex_m_enable_breakpoints(struct target *target);
 void cortex_m_enable_watchpoints(struct target *target);
 void cortex_m_dwt_setup(struct cortex_m_common *cm, struct target *target);
 void cortex_m_deinit_target(struct target *target);
+int cortex_m_profiling(struct target *target, uint32_t *samples,
+	uint32_t max_num_samples, uint32_t *num_samples, uint32_t seconds);
 
 #endif /* OPENOCD_TARGET_CORTEX_M_H */
diff --git a/src/target/embeddedice.c b/src/target/embeddedice.c
index 09d6fc8..7232ef1 100644
--- a/src/target/embeddedice.c
+++ b/src/target/embeddedice.c
@@ -28,6 +28,7 @@
 
 #include "embeddedice.h"
 #include "register.h"
+#include <helper/time_support.h>
 
 /**
  * @file
@@ -576,8 +577,8 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 	uint8_t field2_out[1];
 	int retval;
 	uint32_t hsact;
-	struct timeval lap;
 	struct timeval now;
+	struct timeval timeout_end;
 
 	if (hsbit == EICE_COMM_CTRL_WBIT)
 		hsact = 1;
@@ -610,7 +611,8 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 	fields[2].in_value = NULL;
 
 	jtag_add_dr_scan(jtag_info->tap, 3, fields, TAP_IDLE);
-	gettimeofday(&lap, NULL);
+	gettimeofday(&timeout_end, NULL);
+	timeval_add_time(&timeout_end, 0, timeout * 1000);
 	do {
 		jtag_add_dr_scan(jtag_info->tap, 3, fields, TAP_IDLE);
 		retval = jtag_execute_queue();
@@ -621,8 +623,7 @@ int embeddedice_handshake(struct arm_jtag *jtag_info, int hsbit, uint32_t timeou
 			return ERROR_OK;
 
 		gettimeofday(&now, NULL);
-	} while ((uint32_t)((now.tv_sec - lap.tv_sec) * 1000
-			+ (now.tv_usec - lap.tv_usec) / 1000) <= timeout);
+	} while (timeval_compare(&now, &timeout_end) <= 0);
 
 	LOG_ERROR("embeddedice handshake timeout");
 	return ERROR_TARGET_TIMEOUT;
diff --git a/src/target/hla_target.c b/src/target/hla_target.c
index 78dc8c5..a3e6835 100644
--- a/src/target/hla_target.c
+++ b/src/target/hla_target.c
@@ -814,4 +814,5 @@ struct target_type hla_target = {
 	.remove_breakpoint = cortex_m_remove_breakpoint,
 	.add_watchpoint = cortex_m_add_watchpoint,
 	.remove_watchpoint = cortex_m_remove_watchpoint,
+	.profiling = cortex_m_profiling,
 };
diff --git a/src/target/lakemont.c b/src/target/lakemont.c
index 2bd12fd..b81213e 100644
--- a/src/target/lakemont.c
+++ b/src/target/lakemont.c
@@ -444,6 +444,8 @@ static uint32_t get_tapstatus(struct target *t)
 static int enter_probemode(struct target *t)
 {
 	uint32_t tapstatus = 0;
+	int retries = 100;
+
 	tapstatus = get_tapstatus(t);
 	LOG_DEBUG("TS before PM enter = 0x%08" PRIx32, tapstatus);
 	if (tapstatus & TS_PM_BIT) {
@@ -456,15 +458,17 @@ static int enter_probemode(struct target *t)
 	scan.out[0] = 1;
 	if (drscan(t, scan.out, scan.in, 1) != ERROR_OK)
 		return ERROR_FAIL;
-	tapstatus = get_tapstatus(t);
-	LOG_DEBUG("TS after PM enter = 0x%08" PRIx32, tapstatus);
-	if ((tapstatus & TS_PM_BIT) && (!(tapstatus & TS_EN_PM_BIT)))
-		return ERROR_OK;
-	else {
-		LOG_ERROR("%s PM enter error, tapstatus = 0x%08" PRIx32
-				, __func__, tapstatus);
-		return ERROR_FAIL;
+
+	while (retries--) {
+		tapstatus = get_tapstatus(t);
+		LOG_DEBUG("TS after PM enter = 0x%08" PRIx32, tapstatus);
+		if ((tapstatus & TS_PM_BIT) && (!(tapstatus & TS_EN_PM_BIT)))
+			return ERROR_OK;
 	}
+
+	LOG_ERROR("%s PM enter error, tapstatus = 0x%08" PRIx32
+			, __func__, tapstatus);
+	return ERROR_FAIL;
 }
 
 static int exit_probemode(struct target *t)
@@ -966,6 +970,7 @@ int lakemont_poll(struct target *t)
 			return target_call_event_callbacks(t, TARGET_EVENT_HALTED);
 		}
 	}
+
 	return ERROR_OK;
 }
 
@@ -1111,15 +1116,137 @@ int lakemont_step(struct target *t, int current,
 	return retval;
 }
 
-/* TODO - implement resetbreak fully through CLTAP registers */
+static int lakemont_reset_break(struct target *t)
+{
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	struct jtag_tap *saved_tap = x86_32->curr_tap;
+	struct scan_field *fields = &scan.field;
+
+	int retval = ERROR_OK;
+
+	LOG_DEBUG("issuing port 0xcf9 reset");
+
+	/* prepare resetbreak setting the proper bits in CLTAPC_CPU_VPREQ */
+	x86_32->curr_tap = jtag_tap_by_position(1);
+	if (x86_32->curr_tap == NULL) {
+		x86_32->curr_tap = saved_tap;
+		LOG_ERROR("%s could not select quark_x10xx.cltap", __func__);
+		return ERROR_FAIL;
+	}
+
+	fields->in_value  = NULL;
+	fields->num_bits  = 8;
+
+	/* select CLTAPC_CPU_VPREQ instruction*/
+	scan.out[0] = 0x51;
+	fields->out_value = ((uint8_t *)scan.out);
+	jtag_add_ir_scan(x86_32->curr_tap, fields, TAP_IDLE);
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK) {
+		x86_32->curr_tap = saved_tap;
+		LOG_ERROR("%s irscan failed to execute queue", __func__);
+		return retval;
+	}
+
+	/* set enable_preq_on_reset & enable_preq_on_reset2 bits*/
+	scan.out[0] = 0x06;
+	fields->out_value  = ((uint8_t *)scan.out);
+	jtag_add_dr_scan(x86_32->curr_tap, 1, fields, TAP_IDLE);
+	retval = jtag_execute_queue();
+	if (retval != ERROR_OK) {
+		LOG_ERROR("%s drscan failed to execute queue", __func__);
+		x86_32->curr_tap = saved_tap;
+		return retval;
+	}
+
+	/* restore current tap */
+	x86_32->curr_tap = saved_tap;
+
+	return ERROR_OK;
+}
+
+/*
+ * If we ever get an adapter with support for PREQ# and PRDY#, we should
+ * update this function to add support for using those two signals.
+ *
+ * Meanwhile, we're assuming that we only support reset break.
+ */
 int lakemont_reset_assert(struct target *t)
 {
-	LOG_DEBUG("-");
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	/* write 0x6 to I/O port 0xcf9 to cause the reset */
+	uint8_t cf9_reset_val = 0x6;
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	if (t->state != TARGET_HALTED) {
+		LOG_DEBUG("target must be halted first");
+		retval = lakemont_halt(t);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("could not halt target");
+			return retval;
+		}
+		x86_32->forced_halt_for_reset = true;
+	}
+
+	if (t->reset_halt) {
+		retval = lakemont_reset_break(t);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	retval = x86_32_common_write_io(t, 0xcf9, BYTE, &cf9_reset_val);
+	if (retval != ERROR_OK) {
+		LOG_ERROR("could not write to port 0xcf9");
+		return retval;
+	}
+
+	if (!t->reset_halt && x86_32->forced_halt_for_reset) {
+		x86_32->forced_halt_for_reset = false;
+		retval = lakemont_resume(t, true, 0x00, false, true);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	/* remove breakpoints and watchpoints */
+	x86_32_common_reset_breakpoints_watchpoints(t);
+
 	return ERROR_OK;
 }
 
 int lakemont_reset_deassert(struct target *t)
 {
-	LOG_DEBUG("-");
+	int retval;
+
+	LOG_DEBUG(" ");
+
+	if (target_was_examined(t)) {
+		retval = lakemont_poll(t);
+		if (retval != ERROR_OK)
+			return retval;
+	}
+
+	if (t->reset_halt) {
+		/* entered PM after reset, update the state */
+		retval = lakemont_update_after_probemode_entry(t);
+		if (retval != ERROR_OK) {
+			LOG_ERROR("could not update state after probemode entry");
+			return retval;
+		}
+
+		if (t->state != TARGET_HALTED) {
+			LOG_WARNING("%s: ran after reset and before halt ...",
+				target_name(t));
+			if (target_was_examined(t)) {
+				retval = target_halt(t);
+				if (retval != ERROR_OK)
+					return retval;
+			} else {
+				t->state = TARGET_UNKNOWN;
+			}
+		}
+	}
+
 	return ERROR_OK;
 }
diff --git a/src/target/mips_ejtag.h b/src/target/mips_ejtag.h
index 71f5c1b..c64e858 100644
--- a/src/target/mips_ejtag.h
+++ b/src/target/mips_ejtag.h
@@ -36,6 +36,11 @@
 #define EJTAG_INST_TCBCONTROLA	0x10
 #define EJTAG_INST_TCBCONTROLB	0x11
 #define EJTAG_INST_TCBDATA		0x12
+#define EJTAG_INST_TCBCONTROLC	0x13
+#define EJTAG_INST_PCSAMPLE		0x14
+#define EJTAG_INST_TCBCONTROLD	0x15
+#define EJTAG_INST_TCBCONTROLE	0x16
+#define EJTAG_INST_FDC			0x17
 #define EJTAG_INST_BYPASS		0xFF
 
 /* microchip PIC32MX specific instructions */
diff --git a/src/target/openrisc/or1k.c b/src/target/openrisc/or1k.c
index 3895ddf..bcb648c 100644
--- a/src/target/openrisc/or1k.c
+++ b/src/target/openrisc/or1k.c
@@ -1248,8 +1248,7 @@ static int or1k_profiling(struct target *target, uint32_t *samples,
 		samples[sample_count++] = reg_value;
 
 		gettimeofday(&now, NULL);
-		if ((sample_count >= max_num_samples) ||
-			((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))) {
+		if ((sample_count >= max_num_samples) || timeval_compare(&now, &timeout) > 0) {
 			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
 			break;
 		}
diff --git a/src/target/quark_x10xx.c b/src/target/quark_x10xx.c
index 189f6cc..525d39a 100644
--- a/src/target/quark_x10xx.c
+++ b/src/target/quark_x10xx.c
@@ -51,48 +51,47 @@
 #include "lakemont.h"
 #include "x86_32_common.h"
 
-int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
+static int quark_x10xx_target_create(struct target *t, Jim_Interp *interp)
 {
-	struct x86_32_common *x86_32 = calloc(1, sizeof(struct x86_32_common));
-	if (x86_32 == NULL) {
-		LOG_ERROR("%s out of memory", __func__);
+	struct x86_32_common *x86_32 = calloc(1, sizeof(*x86_32));
+
+	if (!x86_32)
 		return ERROR_FAIL;
-	}
+
 	x86_32_common_init_arch_info(t, x86_32);
 	lakemont_init_arch_info(t, x86_32);
 	x86_32->core_type = LMT1;
-	return ERROR_OK;
-}
 
-int quark_x10xx_init_target(struct command_context *cmd_ctx, struct target *t)
-{
-	return lakemont_init_target(cmd_ctx, t);
+	return ERROR_OK;
 }
 
 struct target_type quark_x10xx_target = {
-	.name = "quark_x10xx",
+	.name			= "quark_x10xx",
+
 	/* Quark X1000 SoC */
-	.target_create = quark_x10xx_target_create,
-	.init_target = quark_x10xx_init_target,
+	.target_create		= quark_x10xx_target_create,
+
 	/* lakemont probemode specific code */
-	.poll = lakemont_poll,
-	.arch_state = lakemont_arch_state,
-	.halt = lakemont_halt,
-	.resume = lakemont_resume,
-	.step = lakemont_step,
-	.assert_reset = lakemont_reset_assert,
-	.deassert_reset = lakemont_reset_deassert,
+	.arch_state		= lakemont_arch_state,
+	.assert_reset		= lakemont_reset_assert,
+	.deassert_reset		= lakemont_reset_deassert,
+	.halt			= lakemont_halt,
+	.init_target		= lakemont_init_target,
+	.poll			= lakemont_poll,
+	.resume			= lakemont_resume,
+	.step			= lakemont_step,
+
 	/* common x86 code */
-	.commands = x86_32_command_handlers,
-	.get_gdb_reg_list = x86_32_get_gdb_reg_list,
-	.read_memory = x86_32_common_read_memory,
-	.write_memory = x86_32_common_write_memory,
-	.add_breakpoint = x86_32_common_add_breakpoint,
-	.remove_breakpoint = x86_32_common_remove_breakpoint,
-	.add_watchpoint = x86_32_common_add_watchpoint,
-	.remove_watchpoint = x86_32_common_remove_watchpoint,
-	.virt2phys = x86_32_common_virt2phys,
-	.read_phys_memory = x86_32_common_read_phys_mem,
-	.write_phys_memory = x86_32_common_write_phys_mem,
-	.mmu = x86_32_common_mmu,
+	.add_breakpoint		= x86_32_common_add_breakpoint,
+	.add_watchpoint		= x86_32_common_add_watchpoint,
+	.commands		= x86_32_command_handlers,
+	.get_gdb_reg_list	= x86_32_get_gdb_reg_list,
+	.mmu			= x86_32_common_mmu,
+	.read_memory		= x86_32_common_read_memory,
+	.read_phys_memory	= x86_32_common_read_phys_mem,
+	.remove_breakpoint	= x86_32_common_remove_breakpoint,
+	.remove_watchpoint	= x86_32_common_remove_watchpoint,
+	.virt2phys		= x86_32_common_virt2phys,
+	.write_memory		= x86_32_common_write_memory,
+	.write_phys_memory	= x86_32_common_write_phys_mem,
 };
diff --git a/src/target/riscv/riscv-013.c b/src/target/riscv/riscv-013.c
index c12406a..cd4254c 100644
--- a/src/target/riscv/riscv-013.c
+++ b/src/target/riscv/riscv-013.c
@@ -1166,8 +1166,10 @@ static int examine(struct target *target)
 	}
 
 	/* Reset the Debug Module. */
+	dmi_write(target, DMI_DMCONTROL, 0);
+	dmi_write(target, DMI_DMCONTROL, DMI_DMCONTROL_DMACTIVE);
+
 	uint32_t max_hartsel_mask = ((1L<<10)-1) << DMI_DMCONTROL_HARTSEL_OFFSET;
-	dmi_write(target, DMI_DMCONTROL, max_hartsel_mask);
 	dmi_write(target, DMI_DMCONTROL, max_hartsel_mask | DMI_DMCONTROL_DMACTIVE);
 	uint32_t dmcontrol = dmi_read(target, DMI_DMCONTROL);
 
diff --git a/src/target/target.c b/src/target/target.c
index 12457fa..ce7782e 100644
--- a/src/target/target.c
+++ b/src/target/target.c
@@ -1399,7 +1399,6 @@ int target_register_trace_callback(int (*callback)(struct target *target,
 int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int periodic, void *priv)
 {
 	struct target_timer_callback **callbacks_p = &target_timer_callbacks;
-	struct timeval now;
 
 	if (callback == NULL)
 		return ERROR_COMMAND_SYNTAX_ERROR;
@@ -1416,14 +1415,8 @@ int target_register_timer_callback(int (*callback)(void *priv), int time_ms, int
 	(*callbacks_p)->time_ms = time_ms;
 	(*callbacks_p)->removed = false;
 
-	gettimeofday(&now, NULL);
-	(*callbacks_p)->when.tv_usec = now.tv_usec + (time_ms % 1000) * 1000;
-	time_ms -= (time_ms % 1000);
-	(*callbacks_p)->when.tv_sec = now.tv_sec + (time_ms / 1000);
-	if ((*callbacks_p)->when.tv_usec > 1000000) {
-		(*callbacks_p)->when.tv_usec = (*callbacks_p)->when.tv_usec - 1000000;
-		(*callbacks_p)->when.tv_sec += 1;
-	}
+	gettimeofday(&(*callbacks_p)->when, NULL);
+	timeval_add_time(&(*callbacks_p)->when, 0, time_ms * 1000);
 
 	(*callbacks_p)->priv = priv;
 	(*callbacks_p)->next = NULL;
@@ -1558,14 +1551,8 @@ int target_call_trace_callbacks(struct target *target, size_t len, uint8_t *data
 static int target_timer_callback_periodic_restart(
 		struct target_timer_callback *cb, struct timeval *now)
 {
-	int time_ms = cb->time_ms;
-	cb->when.tv_usec = now->tv_usec + (time_ms % 1000) * 1000;
-	time_ms -= (time_ms % 1000);
-	cb->when.tv_sec = now->tv_sec + time_ms / 1000;
-	if (cb->when.tv_usec > 1000000) {
-		cb->when.tv_usec = cb->when.tv_usec - 1000000;
-		cb->when.tv_sec += 1;
-	}
+	cb->when = *now;
+	timeval_add_time(&cb->when, 0, cb->time_ms * 1000L);
 	return ERROR_OK;
 }
 
@@ -1609,9 +1596,7 @@ static int target_call_timer_callbacks_check_time(int checktime)
 
 		bool call_it = (*callback)->callback &&
 			((!checktime && (*callback)->periodic) ||
-			 now.tv_sec > (*callback)->when.tv_sec ||
-			 (now.tv_sec == (*callback)->when.tv_sec &&
-			  now.tv_usec >= (*callback)->when.tv_usec));
+			 timeval_compare(&now, &(*callback)->when) >= 0);
 
 		if (call_it)
 			target_call_timer_callback(*callback, &now);
@@ -2030,8 +2015,7 @@ static int target_profiling_default(struct target *target, uint32_t *samples,
 			break;
 
 		gettimeofday(&now, NULL);
-		if ((sample_count >= max_num_samples) ||
-			((now.tv_sec >= timeout.tv_sec) && (now.tv_usec >= timeout.tv_usec))) {
+		if ((sample_count >= max_num_samples) || timeval_compare(&now, &timeout) >= 0) {
 			LOG_INFO("Profiling completed. %" PRIu32 " samples.", sample_count);
 			break;
 		}
@@ -3131,6 +3115,10 @@ COMMAND_HANDLER(handle_md_command)
 		COMMAND_PARSE_NUMBER(uint, CMD_ARGV[1], count);
 
 	uint8_t *buffer = calloc(count, size);
+	if (buffer == NULL) {
+		LOG_ERROR("Failed to allocate md read buffer");
+		return ERROR_FAIL;
+	}
 
 	struct target *target = get_current_target(CMD_CTX);
 	int retval = fn(target, address, size, count, buffer);
@@ -3853,7 +3841,7 @@ typedef unsigned char UNIT[2];  /* unit of profiling */
 
 /* Dump a gmon.out histogram file. */
 static void write_gmon(uint32_t *samples, uint32_t sampleNum, const char *filename, bool with_range,
-			uint32_t start_address, uint32_t end_address, struct target *target)
+			uint32_t start_address, uint32_t end_address, struct target *target, uint32_t duration_ms)
 {
 	uint32_t i;
 	FILE *f = fopen(filename, "w");
@@ -3921,7 +3909,8 @@ static void write_gmon(uint32_t *samples, uint32_t sampleNum, const char *filena
 	writeLong(f, min, target);			/* low_pc */
 	writeLong(f, max, target);			/* high_pc */
 	writeLong(f, numBuckets, target);	/* # of buckets */
-	writeLong(f, 100, target);			/* KLUDGE! We lie, ca. 100Hz best case. */
+	float sample_rate = sampleNum / (duration_ms / 1000.0);
+	writeLong(f, sample_rate, target);
 	writeString(f, "seconds");
 	for (i = 0; i < (15-strlen("seconds")); i++)
 		writeData(f, &zero, 1);
@@ -3970,6 +3959,7 @@ COMMAND_HANDLER(handle_profile_command)
 		return ERROR_FAIL;
 	}
 
+	uint64_t timestart_ms = timeval_ms();
 	/**
 	 * Some cores let us sample the PC without the
 	 * annoying halt/resume step; for example, ARMv7 PCSR.
@@ -3981,6 +3971,7 @@ COMMAND_HANDLER(handle_profile_command)
 		free(samples);
 		return retval;
 	}
+	uint32_t duration_ms = timeval_ms() - timestart_ms;
 
 	assert(num_of_samples <= MAX_PROFILE_SAMPLE_NUM);
 
@@ -4013,7 +4004,7 @@ COMMAND_HANDLER(handle_profile_command)
 	}
 
 	write_gmon(samples, num_of_samples, CMD_ARGV[1],
-		   with_range, start_address, end_address, target);
+		   with_range, start_address, end_address, target, duration_ms);
 	command_print(CMD_CTX, "Wrote %s", CMD_ARGV[1]);
 
 	free(samples);
diff --git a/src/target/target_type.h b/src/target/target_type.h
index 0960e6d..0ab22bd 100644
--- a/src/target/target_type.h
+++ b/src/target/target_type.h
@@ -84,7 +84,7 @@ struct target_type {
 	 * "halt".
 	 *
 	 * reset run; halt
-     */
+	 */
 	int (*deassert_reset)(struct target *target);
 	int (*soft_reset_halt)(struct target *target);
 
diff --git a/src/target/x86_32_common.c b/src/target/x86_32_common.c
index 34f92ea..ef4f806 100644
--- a/src/target/x86_32_common.c
+++ b/src/target/x86_32_common.c
@@ -209,15 +209,16 @@ static int read_phys_mem(struct target *t, uint32_t phys_address,
 			LOG_ERROR("%s invalid read size", __func__);
 			break;
 		}
+		if (retval != ERROR_OK)
+			break;
 	}
 	/* restore CR0.PG bit if needed (regardless of retval) */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = true;
 	}
 	/* TODO: After reading memory from target, we must replace
 	 * software breakpoints with the original instructions again.
@@ -364,6 +365,9 @@ static int read_mem(struct target *t, uint32_t size,
 			break;
 	}
 
+	if (retval != ERROR_OK)
+		return retval;
+
 	/* read_hw_reg() will write to 4 bytes (uint32_t)
 	 * Watch out, the buffer passed into read_mem() might be 1 or 2 bytes.
 	 */
@@ -436,6 +440,10 @@ static int write_mem(struct target *t, uint32_t size,
 			LOG_ERROR("%s invalid write mem size", __func__);
 			return ERROR_FAIL;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	retval = x86_32->transaction_status(t);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("%s error on mem write", __func__);
@@ -606,7 +614,6 @@ int x86_32_common_read_memory(struct target *t, target_addr_t addr,
 			&& x86_32_common_read_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 			LOG_ERROR("%s failed to read memory from physical address " TARGET_ADDR_FMT,
 					  __func__, physaddr);
-			retval = ERROR_FAIL;
 		}
 		/* restore PG bit if it was cleared prior (regardless of retval) */
 		retval = x86_32->enable_paging(t);
@@ -662,7 +669,6 @@ int x86_32_common_write_memory(struct target *t, target_addr_t addr,
 			&& x86_32_common_write_phys_mem(t, physaddr, size, count, buf) != ERROR_OK) {
 			LOG_ERROR("%s failed to write memory to physical address " TARGET_ADDR_FMT,
 					__func__, physaddr);
-			retval = ERROR_FAIL;
 		}
 		/* restore PG bit if it was cleared prior (regardless of retval) */
 		retval = x86_32->enable_paging(t);
@@ -733,15 +739,19 @@ int x86_32_common_read_io(struct target *t, uint32_t addr,
 			LOG_ERROR("%s invalid read io size", __func__);
 			return ERROR_FAIL;
 	}
+
 	/* restore CR0.PG bit if needed */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = false;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	uint32_t regval = 0;
 	retval = x86_32->read_hw_reg(t, EAX, &regval, 0);
 	if (retval != ERROR_OK) {
@@ -818,15 +828,19 @@ int x86_32_common_write_io(struct target *t, uint32_t addr,
 			LOG_ERROR("%s invalid write io size", __func__);
 			return ERROR_FAIL;
 	}
+
 	/* restore CR0.PG bit if needed */
 	if (pg_disabled) {
-		retval = x86_32->enable_paging(t);
-		if (retval != ERROR_OK) {
+		int retval2 = x86_32->enable_paging(t);
+		if (retval2 != ERROR_OK) {
 			LOG_ERROR("%s could not enable paging", __func__);
-			return retval;
+			return retval2;
 		}
-		pg_disabled = false;
 	}
+
+	if (retval != ERROR_OK)
+		return retval;
+
 	retval = x86_32->transaction_status(t);
 	if (retval != ERROR_OK) {
 		LOG_ERROR("%s error on io write", __func__);
@@ -1141,7 +1155,6 @@ static int set_breakpoint(struct target *t, struct breakpoint *bp)
 			}
 		} else {
 			LOG_ERROR("%s core doesn't support SW breakpoints", __func__);
-			error = ERROR_FAIL;
 			return ERROR_FAIL;
 		}
 	}
@@ -1260,6 +1273,38 @@ static int unset_watchpoint(struct target *t, struct watchpoint *wp)
 	return ERROR_OK;
 }
 
+/* after reset breakpoints and watchpoints in memory are not valid anymore and
+ * debug registers are cleared.
+ * we can't afford to remove sw breakpoints using the default methods as the
+ * memory doesn't have the same layout yet and an access might crash the target,
+ * so we just clear the openocd breakpoints structures.
+ */
+void x86_32_common_reset_breakpoints_watchpoints(struct target *t)
+{
+	struct x86_32_common *x86_32 = target_to_x86_32(t);
+	struct x86_32_dbg_reg *debug_reg_list = x86_32->hw_break_list;
+	struct breakpoint *next_b;
+	struct watchpoint *next_w;
+
+	while (t->breakpoints) {
+		next_b = t->breakpoints->next;
+		free(t->breakpoints->orig_instr);
+		free(t->breakpoints);
+		t->breakpoints = next_b;
+	}
+
+	while (t->watchpoints) {
+		next_w = t->watchpoints->next;
+		free(t->watchpoints);
+		t->watchpoints = next_w;
+	}
+
+	for (int i = 0; i < x86_32->num_hw_bpoints; i++) {
+		debug_reg_list[i].used = 0;
+		debug_reg_list[i].bp_value = 0;
+	}
+}
+
 static int read_hw_reg_to_cache(struct target *t, int num)
 {
 	uint32_t reg_value;
diff --git a/src/target/x86_32_common.h b/src/target/x86_32_common.h
index 0aaa963..14e6e35 100644
--- a/src/target/x86_32_common.h
+++ b/src/target/x86_32_common.h
@@ -217,6 +217,7 @@ struct x86_32_common {
 	struct reg_cache *cache;
 	struct jtag_tap *curr_tap;
 	uint32_t stored_pc;
+	int forced_halt_for_reset;
 	int flush;
 
 	/* pm_regs are for probemode save/restore state */
@@ -326,5 +327,6 @@ int x86_32_common_add_breakpoint(struct target *t, struct breakpoint *bp);
 int x86_32_common_remove_breakpoint(struct target *t, struct breakpoint *bp);
 int x86_32_common_add_watchpoint(struct target *t, struct watchpoint *wp);
 int x86_32_common_remove_watchpoint(struct target *t, struct watchpoint *wp);
+void x86_32_common_reset_breakpoints_watchpoints(struct target *t);
 
 #endif /* OPENOCD_TARGET_X86_32_COMMON_H */
diff --git a/src/target/xscale.c b/src/target/xscale.c
index 8fe8a2c..87a3d0f 100644
--- a/src/target/xscale.c
+++ b/src/target/xscale.c
@@ -404,8 +404,7 @@ static int xscale_read_tx(struct target *target, int consume)
 		}
 
 		gettimeofday(&now, NULL);
-		if ((now.tv_sec > timeout.tv_sec) ||
-			((now.tv_sec == timeout.tv_sec) && (now.tv_usec > timeout.tv_usec))) {
+		if (timeval_compare(&now, &timeout) > 0) {
 			LOG_ERROR("time out reading TX register");
 			return ERROR_TARGET_TIMEOUT;
 		}
diff --git a/tcl/board/nordic_nrf52_ftx232.cfg b/tcl/board/nordic_nrf52_ftx232.cfg
new file mode 100644
index 0000000..938efed
--- /dev/null
+++ b/tcl/board/nordic_nrf52_ftx232.cfg
@@ -0,0 +1,11 @@
+#
+# nordic module NRF52 (nRF52832/52840) attached to an adafruit ft232h module
+# or any FT232H/FT2232H/FT4232H based board/module
+#
+
+source [find interface/ftdi/ft232h-module-swd.cfg]
+#source [find interface/ftdi/minimodule-swd.cfg]
+
+transport select swd
+
+source [find target/nrf52.cfg]
diff --git a/tcl/board/st_nucleo_l476rg.cfg b/tcl/board/st_nucleo_l4.cfg
index 4426c3b..1ab9da9 100644
--- a/tcl/board/st_nucleo_l476rg.cfg
+++ b/tcl/board/st_nucleo_l4.cfg
@@ -1,5 +1,5 @@
-# This is a ST NUCLEO L476RG board with a single STM32L476RGT6 chip.
-# http://www.st.com/web/catalog/tools/FM116/SC959/SS1532/LN1847/PF261636
+# Should work with all STM32L4 Nucleo Dev Boards.
+# http://www.st.com/en/evaluation-tools/stm32-mcu-nucleo.html
 
 source [find interface/stlink.cfg]
 
@@ -9,4 +9,3 @@ source [find target/stm32l4x.cfg]
 
 # use hardware reset
 reset_config srst_only srst_nogate
-
diff --git a/tcl/cpld/jtagspi.cfg b/tcl/cpld/jtagspi.cfg
index 60c3cb1..e720c39 100644
--- a/tcl/cpld/jtagspi.cfg
+++ b/tcl/cpld/jtagspi.cfg
@@ -6,12 +6,6 @@ if { [info exists JTAGSPI_IR] } {
 	set _JTAGSPI_IR $_USER1
 }
 
-if { [info exists DR_LENGTH] } {
-	set _DR_LENGTH $DR_LENGTH
-} else {
-	set _DR_LENGTH 1
-}
-
 if { [info exists TARGETNAME] } {
 	set _TARGETNAME $TARGETNAME
 } else {
@@ -25,7 +19,7 @@ if { [info exists FLASHNAME] } {
 }
 
 target create $_TARGETNAME testee -chain-position $_CHIPNAME.tap
-flash bank $_FLASHNAME jtagspi 0 0 0 0 $_TARGETNAME $_JTAGSPI_IR $_DR_LENGTH
+flash bank $_FLASHNAME jtagspi 0 0 0 0 $_TARGETNAME $_JTAGSPI_IR
 
 proc jtagspi_init {chain_id proxy_bit} {
 	# load proxy bitstream $proxy_bit and probe spi flash
diff --git a/tcl/cpld/xilinx-xcf-p.cfg b/tcl/cpld/xilinx-xcf-p.cfg
new file mode 100644
index 0000000..8e0a26c
--- /dev/null
+++ b/tcl/cpld/xilinx-xcf-p.cfg
@@ -0,0 +1,18 @@
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME xcf
+}
+
+# IDs acquired from Xilinx's DS123.pdf
+# XCF08P <v>5057093
+# XCF16P <v>5058093
+# XCF32P <v>5059093
+# The 4 top bits (28:31) are the device revision. Ignore it.
+jtag newtap $_CHIPNAME flash -irlen 16 -ignore-version \
+	-expected-id 0x05057093 \
+	-expected-id 0x05058093 \
+	-expected-id 0x05059093
+
+target create xcf.flash testee -chain-position $_CHIPNAME.flash
+flash bank XCF_P xcf 0 0 0 0 xcf.flash
diff --git a/tcl/cpld/xilinx-xcf-s.cfg b/tcl/cpld/xilinx-xcf-s.cfg
new file mode 100644
index 0000000..a3c79a3
--- /dev/null
+++ b/tcl/cpld/xilinx-xcf-s.cfg
@@ -0,0 +1,18 @@
+if { [info exists CHIPNAME] } {
+	set _CHIPNAME $CHIPNAME
+} else {
+	set _CHIPNAME xcf
+}
+
+# IDs acquired from Xilinx's DS123.pdf
+# XCF01S <v>5044093
+# XCF02S <v>5045093
+# XCF04S <v>5046093
+# The 4 top bits (28:31) are the device revision. Ignore it.
+jtag newtap $_CHIPNAME flash -irlen 8 -ignore-version \
+	-expected-id 0x05044093 \
+	-expected-id 0x05045093 \
+	-expected-id 0x05046093
+
+target create xcf.flash testee -chain-position $_CHIPNAME.flash
+flash bank XCF_S xcf 0 0 0 0 xcf.flash
diff --git a/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg b/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg
new file mode 100644
index 0000000..a83a008
--- /dev/null
+++ b/tcl/interface/ftdi/digilent_jtag_smt2_nc.cfg
@@ -0,0 +1,18 @@
+#
+# Digilent JTAG-SMT2-NC
+#
+# http://store.digilentinc.com/jtag-smt2-nc-surface-mount-programming-module/
+# https://reference.digilentinc.com/_media/jtag_smt2nc/jtag-smt2-nc_rm.pdf
+#
+# Based on reference sheet (above) and Xilinx KCU105 schematics
+# https://www.xilinx.com/products/boards-and-kits/kcu105.html#documentation
+#
+# Note that the digilent_jtag_smt2 layout does not work and hangs while
+# the ftdi_device_desc from digilent_hs2 is wrong.
+
+interface ftdi
+ftdi_device_desc "Digilent USB Device"
+ftdi_vid_pid 0x0403 0x6014
+ftdi_channel 0
+ftdi_layout_init 0x00e8 0x60eb
+reset_config none
diff --git a/tcl/interface/ftdi/ft232h-module-swd.cfg b/tcl/interface/ftdi/ft232h-module-swd.cfg
new file mode 100644
index 0000000..d2bd1da
--- /dev/null
+++ b/tcl/interface/ftdi/ft232h-module-swd.cfg
@@ -0,0 +1,52 @@
+#
+# ADAFRUIT FTDI FT232H as a SWD direct connect interface
+# Any FT232H based board may work
+#
+# http://www.ftdichip.com/Products/ICs/FT232H.htm
+#
+#
+
+interface ftdi
+
+ftdi_vid_pid 0x0403 0x6014
+
+# data MSB..LSB       direction (1:out) MSB..LSB
+# 0000'0000'0011'0000 0000'0000'0011'1011
+ftdi_layout_init 0x0030 0x003b
+# 0xfff8 0xfffb
+# Those signal are only required on some platforms or may required to be
+# enabled explicitely (e.g. nrf5x chips).
+ftdi_layout_signal nSRST -data 0x0010 -oe 0x0010
+ftdi_layout_signal nTRST -data 0x0020 -oe 0x0020
+
+# swd enable
+ftdi_layout_signal SWD_EN -data 0
+# tri-state (configure as input) TDO/TIO when reading
+ftdi_layout_signal SWDIO_OE -data 0
+
+transport select swd
+
+# re-configure TDO as tri-state
+#ftdi_layout_signal TDO -data 0x0002 -oe 0x0002
+#ftdi_layout_signal TDI -data 0x0004
+
+# Adafruit      FT232H    JTAG       SWD
+# Name  Pin     Name      Func       Func
+#  D0   J1-3    ADBUS0    TCK        SWDCLK
+#  D1   J1-4    ADBUS1    TDO/DI     SWDIO
+#  D2   J1-5    ADBUS2    TDI/DO     SWDIO
+#  D3   J1-6    ADBUS3    TMS        N/A
+#  D4   J1-7    ADBUS4    (GPIOL0)   /nSRST  optional module reset
+#  D5   J1-8    ADBUS5    (GPIOL1)   /nTRST  optional target reset
+#  D6   J1-9    ADBUS6    (GPIOL2)
+#  D7   J1-10   ADBUS7    (GPIOL3)
+#  C0   J2-1    ACBUS0    (GPIOH0)
+#  C1   J2-2    ACBUS1    (GPIOH1)
+#  C2   J2-3    ACBUS2    (GPIOH2)
+#  C3   J2-4    ACBUS3    (GPIOH3)
+#  C4   J2-5    ACBUS4    (GPIOH4)
+#  C5   J2-6    ACBUS5    (GPIOH5)
+#  C6   J2-7    ACBUS6    (GPIOH6)
+#  C7   J2-8    ACBUS7    (GPIOH7)
+#  C8   J2-9    ACBUS8
+#  C9   J2-10   ACBUS9
diff --git a/tcl/interface/ftdi/minimodule-swd.cfg b/tcl/interface/ftdi/minimodule-swd.cfg
new file mode 100644
index 0000000..5f0b212
--- /dev/null
+++ b/tcl/interface/ftdi/minimodule-swd.cfg
@@ -0,0 +1,54 @@
+#
+Supports SWD using the FT2232H or FT4232H minimodule.
+# Each can support 2 SWD interfaces.
+#
+# FT2232H or FT4232H minimodule channel 0 (Channel A)
+# Connector  FTDI              Target
+# Pin        Name
+# ---------  ------            ------
+# CN2-11     VIO               VDD_IO (Or connect to CN2-5 on the minimodule instead for a 3V3 interface)
+# CN2-2      GND               GND
+# CN2-7      ADBUS0 (TCK)      SWCLK
+# CN2-9      ADBUS2 (TDI/TDO)  SWDIO
+# CN2-10     ADBUS1 (TDO/TDI)  SWDIO
+# CN2-14     ADBUS4 (GPIOL0)   nRESET
+#
+# FT2232H minimodule channel 1 (Channel B)
+# FTDI       Target
+# ----       ------
+# CN2-11  -  VDD_IO
+# CN2-2   -  GND
+# CN3-26  -  SWCLK
+# CN3-25  -  SWDIO
+# CN3-24  -  SWDIO
+# CN3-21  -  nRESET
+#
+# FT4232H minimodule channel 1 (Channel B)
+# FTDI       Target
+# ----       ------
+# CN2-11  -  VDD_IO
+# CN2-2   -  GND
+# CN2-18  -  SWCLK
+# CN2-17  -  SWDIO
+# CN2-20  -  SWDIO
+# CN2-22  -  nRESET
+#
+
+interface ftdi
+
+#Select your module type and channel
+
+#ftdi_device_desc "FT2232H MiniModule"
+ftdi_vid_pid 0x0403 0x6010
+#ftdi_channel 1
+
+#ftdi_device_desc "FT4232H MiniModule"
+#ftdi_vid_pid 0x0403 0x6011
+#ftdi_channel 1
+
+ftdi_layout_init 0x0000 0x000b
+ftdi_layout_signal nSRST -data 0x0010 -oe 0x0010
+ftdi_layout_signal SWD_EN -data 0
+ftdi_layout_signal SWDIO_OE -data 0
+
+transport select swd
diff --git a/tcl/interface/raspberrypi-native.cfg b/tcl/interface/raspberrypi-native.cfg
index 6b73f35..c63dfdb 100644
--- a/tcl/interface/raspberrypi-native.cfg
+++ b/tcl/interface/raspberrypi-native.cfg
@@ -21,6 +21,10 @@ bcm2835gpio_speed_coeffs 113714 28
 # Header pin numbers: 23 22 19 21
 bcm2835gpio_jtag_nums 11 25 10 9
 
+# Each of the SWD lines need a gpio number set: swclk swdio
+# Header pin numbers: 23 22
+bcm2835gpio_swd_nums 11 25
+
 # If you define trst or srst, use appropriate reset_config
 # Header pin numbers: TRST - 26, SRST - 18
 
@@ -32,4 +36,3 @@ bcm2835gpio_jtag_nums 11 25 10 9
 
 # or if you have both connected,
 # reset_config trst_and_srst srst_push_pull
-
diff --git a/tcl/interface/raspberrypi2-native.cfg b/tcl/interface/raspberrypi2-native.cfg
index f846fa2..26a31c5 100644
--- a/tcl/interface/raspberrypi2-native.cfg
+++ b/tcl/interface/raspberrypi2-native.cfg
@@ -19,14 +19,11 @@ bcm2835gpio_speed_coeffs 146203 36
 
 # Each of the JTAG lines need a gpio number set: tck tms tdi tdo
 # Header pin numbers: 23 22 19 21
-# bcm2835gpio_jtag_nums 11 25 10 9
-
-# or if you have both connected,
-# reset_config trst_and_srst srst_push_pull
+bcm2835gpio_jtag_nums 11 25 10 9
 
 # Each of the SWD lines need a gpio number set: swclk swdio
-# Header pin numbers: 22 18
-bcm2835gpio_swd_nums 25 24
+# Header pin numbers: 23 22
+bcm2835gpio_swd_nums 11 25
 
 # If you define trst or srst, use appropriate reset_config
 # Header pin numbers: TRST - 26, SRST - 18
@@ -34,9 +31,8 @@ bcm2835gpio_swd_nums 25 24
 # bcm2835gpio_trst_num 7
 # reset_config trst_only
 
-bcm2835gpio_srst_num 18
-reset_config srst_only srst_push_pull
+# bcm2835gpio_srst_num 24
+# reset_config srst_only srst_push_pull
 
 # or if you have both connected,
 # reset_config trst_and_srst srst_push_pull
-
diff --git a/tcl/interface/sysfsgpio-raspberrypi.cfg b/tcl/interface/sysfsgpio-raspberrypi.cfg
index 3636422..9f5b87c 100644
--- a/tcl/interface/sysfsgpio-raspberrypi.cfg
+++ b/tcl/interface/sysfsgpio-raspberrypi.cfg
@@ -14,8 +14,18 @@ interface sysfsgpio
 # Header pin numbers: 23 22 19 21
 sysfsgpio_jtag_nums 11 25 10 9
 
-# At least one of srst or trst needs to be specified
+# Each of the SWD lines need a gpio number set: swclk swdio
+# Header pin numbers: 23 22
+sysfsgpio_swd_nums 11 25
+
+# If you define trst or srst, use appropriate reset_config
 # Header pin numbers: TRST - 26, SRST - 18
-sysfsgpio_trst_num 7
+
+# sysfsgpio_trst_num 7
+# reset_config trst_only
+
 # sysfsgpio_srst_num 24
+# reset_config srst_only srst_push_pull
 
+# or if you have both connected,
+# reset_config trst_and_srst srst_push_pull
diff --git a/tcl/target/imx7.cfg b/tcl/target/imx7.cfg
new file mode 100644
index 0000000..d16e95a
--- /dev/null
+++ b/tcl/target/imx7.cfg
@@ -0,0 +1,37 @@
+if { [info exists CHIPNAME] } {
+   set  _CHIPNAME $CHIPNAME
+} else {
+   set  _CHIPNAME imx7
+}
+
+# CoreSight Debug Access Port
+if { [info exists DAP_TAPID] } {
+        set _DAP_TAPID $DAP_TAPID
+} else {
+        set _DAP_TAPID 0x5ba00477
+}
+
+jtag newtap $_CHIPNAME dap -irlen 4 -ircapture 0x01 -irmask 0x0f \
+        -expected-id $_DAP_TAPID
+
+#
+# Cortex-A7 target
+#
+# GDB target: Cortex-A7, using DAP, configuring only one core
+# Base addresses of cores:
+# core 0  -  0x80070000
+# core 1  -  0x80072000
+set _TARGETNAME $_CHIPNAME.cpu_a7
+target create $_TARGETNAME.0 cortex_a -chain-position $_CHIPNAME.dap \
+        -coreid 0 -dbgbase 0x80070000
+
+target create $_TARGETNAME.1 cortex_a -chain-position $_CHIPNAME.dap \
+        -coreid 1 -dbgbase 0x80072000 -defer-examine
+#
+# Cortex-M4 target
+#
+set _TARGETNAME_2 $_CHIPNAME.cpu_m4
+target create $_TARGETNAME_2 cortex_m -chain-position $_CHIPNAME.dap -ap-num 4 \
+        -defer-examine
+
+targets $_TARGETNAME.0
diff --git a/tcl/target/ke0x.cfg b/tcl/target/ke0x.cfg
index 7927e0a..1f1b132 100644
--- a/tcl/target/ke0x.cfg
+++ b/tcl/target/ke0x.cfg
@@ -1,5 +1,5 @@
 #
-# Freescale Kinetis KE0x series devices
+# Freescale Kinetis KE0x and KEAx series devices
 #
 
 source [find target/swj-dp.tcl]
diff --git a/tcl/target/klx.cfg b/tcl/target/klx.cfg
index 7dd0404..1a2ee67 100644
--- a/tcl/target/klx.cfg
+++ b/tcl/target/klx.cfg
@@ -12,11 +12,11 @@ if { [info exists CHIPNAME] } {
 }
 
 # Work-area is a space in RAM used for flash programming
-# By default use 4kB
+# By default use 1KiB
 if { [info exists WORKAREASIZE] } {
    set _WORKAREASIZE $WORKAREASIZE
 } else {
-   set _WORKAREASIZE 0x1000
+   set _WORKAREASIZE 0x400
 }
 
 if { [info exists CPUTAPID] } {
diff --git a/tcl/target/stm8l152.cfg b/tcl/target/stm8l152.cfg
new file mode 100644
index 0000000..8545a5a
--- /dev/null
+++ b/tcl/target/stm8l152.cfg
@@ -0,0 +1,12 @@
+#config script for STM8L152
+
+set EEPROMSTART 0x1000
+set EEPROMEND 0x13ff
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0xaa
+   mwb 0x4800 0xaa
+   reset halt
+}
+
+source [find target/stm8l.cfg]
diff --git a/tcl/target/stm8s003.cfg b/tcl/target/stm8s003.cfg
new file mode 100644
index 0000000..34997be
--- /dev/null
+++ b/tcl/target/stm8s003.cfg
@@ -0,0 +1,11 @@
+#config script for STM8S003
+
+set FLASHEND 0x9FFF
+set BLOCKSIZE 0x40
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0x00
+   reset halt
+}
+
+source [find target/stm8s.cfg]
diff --git a/tcl/target/stm8s105.cfg b/tcl/target/stm8s105.cfg
new file mode 100644
index 0000000..820bcf7
--- /dev/null
+++ b/tcl/target/stm8s105.cfg
@@ -0,0 +1,8 @@
+#config script for STM8S105
+
+proc stm8_reset_rop {} {
+   mwb 0x4800 0x00
+   reset halt
+}
+
+source [find target/stm8s.cfg]
diff --git a/tools/release.sh b/tools/release.sh
index abd721a..ac8af64 100755
--- a/tools/release.sh
+++ b/tools/release.sh
@@ -102,7 +102,7 @@ do_stage() {
 	for EXT in tar.gz tar.bz2 zip; do
 		local FILE="${PACKAGE_RELEASE}.${EXT}"
 		# create archive signatures
-		for HASH in md5 sha1; do
+		for HASH in sha256; do
 			echo "sign: ${FILE}.${HASH}"
 			${HASH}sum "${FILE}" > "archives/${FILE}.${HASH}"
 		done
diff --git a/tools/scripts/checkpatch.pl b/tools/scripts/checkpatch.pl
index b977d36..0a119f1 100755
--- a/tools/scripts/checkpatch.pl
+++ b/tools/scripts/checkpatch.pl
@@ -699,7 +699,7 @@ sub ctx_statement_block {
 		# An else is really a conditional as long as its not else if
 		if ($level == 0 && $coff_set == 0 &&
 				(!defined($p) || $p =~ /(?:\s|\}|\+)/) &&
-				$remainder =~ /^(else)(?:\s|{)/ &&
+				$remainder =~ /^(else)(?:\s|\{)/ &&
 				$remainder !~ /^else\s+if\b/) {
 			$coff = $off + length($1) - 1;
 			$coff_set = 1;
@@ -782,7 +782,7 @@ sub statement_block_size {
 	my ($stmt) = @_;
 
 	$stmt =~ s/(^|\n)./$1/g;
-	$stmt =~ s/^\s*{//;
+	$stmt =~ s/^\s*\{//;
 	$stmt =~ s/}\s*$//;
 	$stmt =~ s/^\s*//;
 	$stmt =~ s/\s*$//;
@@ -1136,7 +1136,7 @@ sub annotate_values {
 			print "ASSIGN($1)\n" if ($dbg_values > 1);
 			$type = 'N';
 
-		} elsif ($cur =~/^(;|{|})/) {
+		} elsif ($cur =~/^(;|\{|})/) {
 			print "END($1)\n" if ($dbg_values > 1);
 			$type = 'E';
 			$av_pend_colon = 'O';
@@ -1783,7 +1783,7 @@ sub process {
 			}
 
 			my $s = $stat;
-			$s =~ s/{.*$//s;
+			$s =~ s/\{.*$//s;
 
 			# Ignore goto labels.
 			if ($s =~ /$Ident:\*$/s) {
@@ -1879,7 +1879,7 @@ sub process {
 			#print "realcnt<$realcnt> ctx_cnt<$ctx_cnt>\n";
 			#print "pre<$pre_ctx>\nline<$line>\nctx<$ctx>\nnext<$lines[$ctx_ln - 1]>\n";
 
-			if ($ctx !~ /{\s*/ && defined($lines[$ctx_ln -1]) && $lines[$ctx_ln - 1] =~ /^\+\s*{/) {
+			if ($ctx !~ /\{\s*/ && defined($lines[$ctx_ln -1]) && $lines[$ctx_ln - 1] =~ /^\+\s*\{/) {
 				ERROR("OPEN_BRACE",
 				      "that open brace { should be on the previous line\n" .
 					"$here\n$ctx\n$rawlines[$ctx_ln - 1]\n");
@@ -1920,7 +1920,7 @@ sub process {
 			my $continuation = 0;
 			my $check = 0;
 			$s =~ s/^.*\bdo\b//;
-			$s =~ s/^\s*{//;
+			$s =~ s/^\s*\{//;
 			if ($s =~ s/^\s*\\//) {
 				$continuation = 1;
 			}
@@ -2024,7 +2024,7 @@ sub process {
 		}
 
 # check for initialisation to aggregates open brace on the next line
-		if ($line =~ /^.\s*{/ &&
+		if ($line =~ /^.\s*\{/ &&
 		    $prevline =~ /(?:^|[^=])=\s*$/) {
 			ERROR("OPEN_BRACE",
 			      "that open brace { should be on the previous line\n" . $hereprev);
@@ -2231,7 +2231,7 @@ sub process {
 		}
 
 # open braces for enum, union and struct go on the same line.
-		if ($line =~ /^.\s*{/ &&
+		if ($line =~ /^.\s*\{/ &&
 		    $prevline =~ /^.\s*(?:typedef\s+)?(enum|union|struct)(?:\s+$Ident)?\s*$/) {
 			ERROR("OPEN_BRACE",
 			      "open brace '{' following $1 go on the same line\n" . $hereprev);
@@ -2251,7 +2251,7 @@ sub process {
 			my ($where, $prefix) = ($-[1], $1);
 			if ($prefix !~ /$Type\s+$/ &&
 			    ($where != 0 || $prefix !~ /^.\s+$/) &&
-			    $prefix !~ /{\s+$/) {
+			    $prefix !~ /\{\s+$/) {
 				ERROR("BRACKET_SPACE",
 				      "space prohibited before open square bracket '['\n" . $herecurr);
 			}
@@ -2493,7 +2493,7 @@ sub process {
 ## 		}
 
 #need space before brace following if, while, etc
-		if (($line =~ /\(.*\)\{/ && $line !~ /\($Type\){/) ||
+		if (($line =~ /\(.*\)\{/ && $line !~ /\($Type\)\{/) ||
 		    $line =~ /do\{/) {
 			ERROR("SPACING",
 			      "space required before the open brace '{'\n" . $herecurr);
@@ -2598,7 +2598,7 @@ sub process {
 
 # Check for illegal assignment in if conditional -- and check for trailing
 # statements after the conditional.
-		if ($line =~ /do\s*(?!{)/) {
+		if ($line =~ /do\s*(?!\{)/) {
 			my ($stat_next) = ctx_statement_block($line_nr_next,
 						$remain_next, $off_next);
 			$stat_next =~ s/\n./\n /g;
@@ -2630,7 +2630,7 @@ sub process {
 			substr($s, 0, length($c), '');
 			$s =~ s/\n.*//g;
 			$s =~ s/$;//g; 	# Remove any comments
-			if (length($c) && $s !~ /^\s*{?\s*\\*\s*$/ &&
+			if (length($c) && $s !~ /^\s*\{?\s*\\*\s*$/ &&
 			    $c !~ /}\s*while\s*/)
 			{
 				# Find out how long the conditional actually is.
@@ -2669,7 +2669,7 @@ sub process {
 		if ($line =~ /^.\s*(?:}\s*)?else\b(.*)/) {
 			my $s = $1;
 			$s =~ s/$;//g; 	# Remove any comments
-			if ($s !~ /^\s*(?:\sif|(?:{|)\s*\\?\s*$)/) {
+			if ($s !~ /^\s*(?:\sif|(?:\{|)\s*\\?\s*$)/) {
 				ERROR("TRAILING_STATEMENTS",
 				      "trailing statements should be on next line\n" . $herecurr);
 			}
@@ -2879,7 +2879,7 @@ sub process {
 
 					substr($block, 0, length($cond), '');
 
-					$seen++ if ($block =~ /^\s*{/);
+					$seen++ if ($block =~ /^\s*\{/);
 
 					#print "cond<$cond> block<$block> allowed<$allowed>\n";
 					if (statement_lines($cond) > 1) {