target/adi_v5_swd: fix SWD multidrop

[fw/openocd] / src / target / adi_v5_swd.c

diff --git a/src/target/adi_v5_swd.c b/src/target/adi_v5_swd.c
index d10667a3375df96bd88126d6ad4b55a1d0e8344f..bd85eb217ab54cb18d86e2d0df5e4e42ef1b9d4d 100644 (file)
--- a/src/target/adi_v5_swd.c
+++ b/src/target/adi_v5_swd.c
@@ -1,19 +1,8 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+
 /***************************************************************************
  *
  *   Copyright (C) 2010 by David Brownell
- *
- *   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/>.
  ***************************************************************************/
 
 /**
@@ -188,6 +177,19 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
        assert(dap_is_multidrop(dap));
 
        swd_send_sequence(dap, LINE_RESET);
+       /* From ARM IHI 0074C ADIv6.0, chapter B4.3.3 "Connection and line reset
+        * sequence":
+        * - line reset sets DP_SELECT_DPBANK to zero;
+        * - read of DP_DPIDR takes the connection out of reset;
+        * - write of DP_TARGETSEL keeps the connection in reset;
+        * - other accesses return protocol error (SWDIO not driven by target).
+        *
+        * Read DP_DPIDR to get out of reset. Initialize dap->select to zero to
+        * skip the write to DP_SELECT, avoiding the protocol error. Set again
+        * dap->select to DP_SELECT_INVALID because the rest of the register is
+        * unknown after line reset.
+        */
+       dap->select = 0;
 
        retval = swd_queue_dp_write_inner(dap, DP_TARGETSEL, dap->multidrop_targetsel);
        if (retval != ERROR_OK)
@@ -207,6 +209,8 @@ static int swd_multidrop_select_inner(struct adiv5_dap *dap, uint32_t *dpidr_ptr
                        return retval;
        }
 
+       dap->select = DP_SELECT_INVALID;
+
        retval = swd_queue_dp_read_inner(dap, DP_DLPIDR, &dlpidr);
        if (retval != ERROR_OK)
                return retval;
@@ -353,6 +357,7 @@ static int swd_connect_single(struct adiv5_dap *dap)
 
                dap->switch_through_dormant = !dap->switch_through_dormant;
        } while (timeval_ms() < timeout);
+
        dap->select = DP_SELECT_INVALID;
 
        if (retval != ERROR_OK) {