Arrow / Mbed OS DAPLink Reset

Repostiory containing DAPLink source code with Reset Pin workaround for HANI_IOT board.

Upstream: https://github.com/ARMmbed/DAPLink

source/daplink/interface/swd_host_ca.c@0:01f31e923fe2, 2020-04-07 (annotated)

Committer:
Pawel Zarembski
Date:
Tue Apr 07 12:55:42 2020 +0200
Revision:
0:01f31e923fe2
hani: DAPLink with reset workaround

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Pawel Zarembski 0:01f31e923fe2 1 /**
Pawel Zarembski 0:01f31e923fe2 2 * @file swd_host.c
Pawel Zarembski 0:01f31e923fe2 3 * @brief Implementation of swd_host.h
Pawel Zarembski 0:01f31e923fe2 4 *
Pawel Zarembski 0:01f31e923fe2 5 * DAPLink Interface Firmware
Pawel Zarembski 0:01f31e923fe2 6 * Copyright (c) 2009-2019, ARM Limited, All Rights Reserved
Pawel Zarembski 0:01f31e923fe2 7 * Copyright 2019, Cypress Semiconductor Corporation
Pawel Zarembski 0:01f31e923fe2 8 * or a subsidiary of Cypress Semiconductor Corporation.
Pawel Zarembski 0:01f31e923fe2 9 * SPDX-License-Identifier: Apache-2.0
Pawel Zarembski 0:01f31e923fe2 10 *
Pawel Zarembski 0:01f31e923fe2 11 * Licensed under the Apache License, Version 2.0 (the "License"); you may
Pawel Zarembski 0:01f31e923fe2 12 * not use this file except in compliance with the License.
Pawel Zarembski 0:01f31e923fe2 13 * You may obtain a copy of the License at
Pawel Zarembski 0:01f31e923fe2 14 *
Pawel Zarembski 0:01f31e923fe2 15 * http://www.apache.org/licenses/LICENSE-2.0
Pawel Zarembski 0:01f31e923fe2 16 *
Pawel Zarembski 0:01f31e923fe2 17 * Unless required by applicable law or agreed to in writing, software
Pawel Zarembski 0:01f31e923fe2 18 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
Pawel Zarembski 0:01f31e923fe2 19 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
Pawel Zarembski 0:01f31e923fe2 20 * See the License for the specific language governing permissions and
Pawel Zarembski 0:01f31e923fe2 21 * limitations under the License.
Pawel Zarembski 0:01f31e923fe2 22 */
Pawel Zarembski 0:01f31e923fe2 23
Pawel Zarembski 0:01f31e923fe2 24 #ifdef TARGET_MCU_CORTEX_A
Pawel Zarembski 0:01f31e923fe2 25
Pawel Zarembski 0:01f31e923fe2 26 #include "cmsis_os2.h"
Pawel Zarembski 0:01f31e923fe2 27 #include "target_config.h"
Pawel Zarembski 0:01f31e923fe2 28 #include "swd_host.h"
Pawel Zarembski 0:01f31e923fe2 29 #include "debug_ca.h"
Pawel Zarembski 0:01f31e923fe2 30 #include "DAP_config.h"
Pawel Zarembski 0:01f31e923fe2 31 #include "DAP.h"
Pawel Zarembski 0:01f31e923fe2 32 #include "target_family.h"
Pawel Zarembski 0:01f31e923fe2 33
Pawel Zarembski 0:01f31e923fe2 34 // Default NVIC and Core debug base addresses
Pawel Zarembski 0:01f31e923fe2 35 // TODO: Read these addresses from ROM.
Pawel Zarembski 0:01f31e923fe2 36 #define NVIC_Addr (0xe000e000)
Pawel Zarembski 0:01f31e923fe2 37 #define DBG_Addr (0xe000edf0)
Pawel Zarembski 0:01f31e923fe2 38
Pawel Zarembski 0:01f31e923fe2 39 // AP CSW register, base value
Pawel Zarembski 0:01f31e923fe2 40 #define CSW_VALUE (0x80000000 | CSW_RESERVED | CSW_MSTRDBG | CSW_HPROT | CSW_DBGSTAT | CSW_PADDRINC)
Pawel Zarembski 0:01f31e923fe2 41
Pawel Zarembski 0:01f31e923fe2 42 // SWD register access
Pawel Zarembski 0:01f31e923fe2 43 #define SWD_REG_AP (1)
Pawel Zarembski 0:01f31e923fe2 44 #define SWD_REG_DP (0)
Pawel Zarembski 0:01f31e923fe2 45 #define SWD_REG_R (1<<1)
Pawel Zarembski 0:01f31e923fe2 46 #define SWD_REG_W (0<<1)
Pawel Zarembski 0:01f31e923fe2 47 #define SWD_REG_ADR(a) (a & 0x0c)
Pawel Zarembski 0:01f31e923fe2 48
Pawel Zarembski 0:01f31e923fe2 49 #define CMD_MRC (0xEE100E15) /* 1110 1110 0001 0000 RRRR 1110 0001 0101 */
Pawel Zarembski 0:01f31e923fe2 50 #define CMD_MCR (0xEE000E15) /* 1110 1110 0000 0000 RRRR 1110 0001 0101 */
Pawel Zarembski 0:01f31e923fe2 51 #define CMD_MSR (0xE12CF000) /* 1110 0001 0010 1100 1111 0000 0000 RRRR */
Pawel Zarembski 0:01f31e923fe2 52 #define CMD_MRS (0xE14F0000) /* 1110 0001 0100 1111 RRRR 0000 0000 0000 */
Pawel Zarembski 0:01f31e923fe2 53 #define CMD_MOV (0xE1A00000) /* 1110 0001 1010 0000 DDDD 0000 0000 RRRR */ /* D = distination */
Pawel Zarembski 0:01f31e923fe2 54
Pawel Zarembski 0:01f31e923fe2 55 #define DBGDSCR_HALTED (0x00000001)
Pawel Zarembski 0:01f31e923fe2 56
Pawel Zarembski 0:01f31e923fe2 57 #define SELECT_MEM (0x00000000) /* setting of SELECT access memmory */
Pawel Zarembski 0:01f31e923fe2 58 #define SELECT_DBG (0x01000000) /* setting of SELECT access Debug Register */
Pawel Zarembski 0:01f31e923fe2 59
Pawel Zarembski 0:01f31e923fe2 60 #define MAX_SWD_RETRY 10
Pawel Zarembski 0:01f31e923fe2 61 #define MAX_TIMEOUT 100000 // Timeout for syscalls on target
Pawel Zarembski 0:01f31e923fe2 62
Pawel Zarembski 0:01f31e923fe2 63
Pawel Zarembski 0:01f31e923fe2 64 typedef struct {
Pawel Zarembski 0:01f31e923fe2 65 uint32_t select;
Pawel Zarembski 0:01f31e923fe2 66 uint32_t csw;
Pawel Zarembski 0:01f31e923fe2 67 } DAP_STATE;
Pawel Zarembski 0:01f31e923fe2 68
Pawel Zarembski 0:01f31e923fe2 69 typedef struct {
Pawel Zarembski 0:01f31e923fe2 70 uint32_t r[16];
Pawel Zarembski 0:01f31e923fe2 71 uint32_t xpsr;
Pawel Zarembski 0:01f31e923fe2 72 } DEBUG_STATE;
Pawel Zarembski 0:01f31e923fe2 73
Pawel Zarembski 0:01f31e923fe2 74 static DAP_STATE dap_state;
Pawel Zarembski 0:01f31e923fe2 75 static uint32_t soft_reset = SYSRESETREQ;
Pawel Zarembski 0:01f31e923fe2 76 static uint32_t select_state = SELECT_MEM;
Pawel Zarembski 0:01f31e923fe2 77 static volatile uint32_t swd_init_debug_flag = 0;
Pawel Zarembski 0:01f31e923fe2 78
Pawel Zarembski 0:01f31e923fe2 79 /* Add static functions */
Pawel Zarembski 0:01f31e923fe2 80 static uint8_t swd_restart_req(void);
Pawel Zarembski 0:01f31e923fe2 81 static uint8_t swd_enable_debug(void);
Pawel Zarembski 0:01f31e923fe2 82
Pawel Zarembski 0:01f31e923fe2 83 void swd_set_reset_connect(SWD_CONNECT_TYPE type)
Pawel Zarembski 0:01f31e923fe2 84 {
Pawel Zarembski 0:01f31e923fe2 85 }
Pawel Zarembski 0:01f31e923fe2 86
Pawel Zarembski 0:01f31e923fe2 87 void int2array(uint8_t *res, uint32_t data, uint8_t len)
Pawel Zarembski 0:01f31e923fe2 88 {
Pawel Zarembski 0:01f31e923fe2 89 uint8_t i = 0;
Pawel Zarembski 0:01f31e923fe2 90
Pawel Zarembski 0:01f31e923fe2 91 for (i = 0; i < len; i++) {
Pawel Zarembski 0:01f31e923fe2 92 res[i] = (data >> 8 * i) & 0xff;
Pawel Zarembski 0:01f31e923fe2 93 }
Pawel Zarembski 0:01f31e923fe2 94 }
Pawel Zarembski 0:01f31e923fe2 95
Pawel Zarembski 0:01f31e923fe2 96 uint8_t swd_transfer_retry(uint32_t req, uint32_t *data)
Pawel Zarembski 0:01f31e923fe2 97 {
Pawel Zarembski 0:01f31e923fe2 98 uint8_t i, ack;
Pawel Zarembski 0:01f31e923fe2 99
Pawel Zarembski 0:01f31e923fe2 100 for (i = 0; i < MAX_SWD_RETRY; i++) {
Pawel Zarembski 0:01f31e923fe2 101 ack = SWD_Transfer(req, data);
Pawel Zarembski 0:01f31e923fe2 102
Pawel Zarembski 0:01f31e923fe2 103 // if ack != WAIT
Pawel Zarembski 0:01f31e923fe2 104 if (ack != DAP_TRANSFER_WAIT) {
Pawel Zarembski 0:01f31e923fe2 105 return ack;
Pawel Zarembski 0:01f31e923fe2 106 }
Pawel Zarembski 0:01f31e923fe2 107 }
Pawel Zarembski 0:01f31e923fe2 108
Pawel Zarembski 0:01f31e923fe2 109 return ack;
Pawel Zarembski 0:01f31e923fe2 110 }
Pawel Zarembski 0:01f31e923fe2 111
Pawel Zarembski 0:01f31e923fe2 112 void swd_set_soft_reset(uint32_t soft_reset_type)
Pawel Zarembski 0:01f31e923fe2 113 {
Pawel Zarembski 0:01f31e923fe2 114 soft_reset = soft_reset_type;
Pawel Zarembski 0:01f31e923fe2 115 }
Pawel Zarembski 0:01f31e923fe2 116
Pawel Zarembski 0:01f31e923fe2 117 uint8_t swd_init(void)
Pawel Zarembski 0:01f31e923fe2 118 {
Pawel Zarembski 0:01f31e923fe2 119 //TODO - DAP_Setup puts GPIO pins in a hi-z state which can
Pawel Zarembski 0:01f31e923fe2 120 // cause problems on re-init. This needs to be investigated
Pawel Zarembski 0:01f31e923fe2 121 // and fixed.
Pawel Zarembski 0:01f31e923fe2 122 DAP_Setup();
Pawel Zarembski 0:01f31e923fe2 123 PORT_SWD_SETUP();
Pawel Zarembski 0:01f31e923fe2 124 return 1;
Pawel Zarembski 0:01f31e923fe2 125 }
Pawel Zarembski 0:01f31e923fe2 126
Pawel Zarembski 0:01f31e923fe2 127 uint8_t swd_off(void)
Pawel Zarembski 0:01f31e923fe2 128 {
Pawel Zarembski 0:01f31e923fe2 129 PORT_OFF();
Pawel Zarembski 0:01f31e923fe2 130 return 1;
Pawel Zarembski 0:01f31e923fe2 131 }
Pawel Zarembski 0:01f31e923fe2 132
Pawel Zarembski 0:01f31e923fe2 133 // Read debug port register.
Pawel Zarembski 0:01f31e923fe2 134 uint8_t swd_read_dp(uint8_t adr, uint32_t *val)
Pawel Zarembski 0:01f31e923fe2 135 {
Pawel Zarembski 0:01f31e923fe2 136 uint32_t tmp_in;
Pawel Zarembski 0:01f31e923fe2 137 uint8_t tmp_out[4];
Pawel Zarembski 0:01f31e923fe2 138 uint8_t ack;
Pawel Zarembski 0:01f31e923fe2 139 uint32_t tmp;
Pawel Zarembski 0:01f31e923fe2 140 tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(adr);
Pawel Zarembski 0:01f31e923fe2 141 ack = swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
Pawel Zarembski 0:01f31e923fe2 142 *val = 0;
Pawel Zarembski 0:01f31e923fe2 143 tmp = tmp_out[3];
Pawel Zarembski 0:01f31e923fe2 144 *val |= (tmp << 24);
Pawel Zarembski 0:01f31e923fe2 145 tmp = tmp_out[2];
Pawel Zarembski 0:01f31e923fe2 146 *val |= (tmp << 16);
Pawel Zarembski 0:01f31e923fe2 147 tmp = tmp_out[1];
Pawel Zarembski 0:01f31e923fe2 148 *val |= (tmp << 8);
Pawel Zarembski 0:01f31e923fe2 149 tmp = tmp_out[0];
Pawel Zarembski 0:01f31e923fe2 150 *val |= (tmp << 0);
Pawel Zarembski 0:01f31e923fe2 151 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 152 }
Pawel Zarembski 0:01f31e923fe2 153
Pawel Zarembski 0:01f31e923fe2 154 // Write debug port register
Pawel Zarembski 0:01f31e923fe2 155 uint8_t swd_write_dp(uint8_t adr, uint32_t val)
Pawel Zarembski 0:01f31e923fe2 156 {
Pawel Zarembski 0:01f31e923fe2 157 uint32_t req;
Pawel Zarembski 0:01f31e923fe2 158 uint8_t data[4];
Pawel Zarembski 0:01f31e923fe2 159 uint8_t ack;
Pawel Zarembski 0:01f31e923fe2 160
Pawel Zarembski 0:01f31e923fe2 161 switch (adr) {
Pawel Zarembski 0:01f31e923fe2 162 case DP_SELECT:
Pawel Zarembski 0:01f31e923fe2 163 if (dap_state.select == val) {
Pawel Zarembski 0:01f31e923fe2 164 return 1;
Pawel Zarembski 0:01f31e923fe2 165 }
Pawel Zarembski 0:01f31e923fe2 166
Pawel Zarembski 0:01f31e923fe2 167 dap_state.select = val;
Pawel Zarembski 0:01f31e923fe2 168 break;
Pawel Zarembski 0:01f31e923fe2 169
Pawel Zarembski 0:01f31e923fe2 170 default:
Pawel Zarembski 0:01f31e923fe2 171 break;
Pawel Zarembski 0:01f31e923fe2 172 }
Pawel Zarembski 0:01f31e923fe2 173
Pawel Zarembski 0:01f31e923fe2 174 req = SWD_REG_DP | SWD_REG_W | SWD_REG_ADR(adr);
Pawel Zarembski 0:01f31e923fe2 175 int2array(data, val, 4);
Pawel Zarembski 0:01f31e923fe2 176 ack = swd_transfer_retry(req, (uint32_t *)data);
Pawel Zarembski 0:01f31e923fe2 177 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 178 }
Pawel Zarembski 0:01f31e923fe2 179
Pawel Zarembski 0:01f31e923fe2 180 // Read access port register.
Pawel Zarembski 0:01f31e923fe2 181 uint8_t swd_read_ap(uint32_t adr, uint32_t *val)
Pawel Zarembski 0:01f31e923fe2 182 {
Pawel Zarembski 0:01f31e923fe2 183 uint8_t tmp_in, ack;
Pawel Zarembski 0:01f31e923fe2 184 uint8_t tmp_out[4];
Pawel Zarembski 0:01f31e923fe2 185 uint32_t tmp;
Pawel Zarembski 0:01f31e923fe2 186 uint32_t apsel = adr & 0xff000000;
Pawel Zarembski 0:01f31e923fe2 187 uint32_t bank_sel = adr & APBANKSEL;
Pawel Zarembski 0:01f31e923fe2 188
Pawel Zarembski 0:01f31e923fe2 189 if (!swd_write_dp(DP_SELECT, apsel | bank_sel)) {
Pawel Zarembski 0:01f31e923fe2 190 return 0;
Pawel Zarembski 0:01f31e923fe2 191 }
Pawel Zarembski 0:01f31e923fe2 192
Pawel Zarembski 0:01f31e923fe2 193 tmp_in = SWD_REG_AP | SWD_REG_R | SWD_REG_ADR(adr);
Pawel Zarembski 0:01f31e923fe2 194 // first dummy read
Pawel Zarembski 0:01f31e923fe2 195 swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
Pawel Zarembski 0:01f31e923fe2 196 ack = swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
Pawel Zarembski 0:01f31e923fe2 197 *val = 0;
Pawel Zarembski 0:01f31e923fe2 198 tmp = tmp_out[3];
Pawel Zarembski 0:01f31e923fe2 199 *val |= (tmp << 24);
Pawel Zarembski 0:01f31e923fe2 200 tmp = tmp_out[2];
Pawel Zarembski 0:01f31e923fe2 201 *val |= (tmp << 16);
Pawel Zarembski 0:01f31e923fe2 202 tmp = tmp_out[1];
Pawel Zarembski 0:01f31e923fe2 203 *val |= (tmp << 8);
Pawel Zarembski 0:01f31e923fe2 204 tmp = tmp_out[0];
Pawel Zarembski 0:01f31e923fe2 205 *val |= (tmp << 0);
Pawel Zarembski 0:01f31e923fe2 206 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 207 }
Pawel Zarembski 0:01f31e923fe2 208
Pawel Zarembski 0:01f31e923fe2 209 // Write access port register
Pawel Zarembski 0:01f31e923fe2 210 uint8_t swd_write_ap(uint32_t adr, uint32_t val)
Pawel Zarembski 0:01f31e923fe2 211 {
Pawel Zarembski 0:01f31e923fe2 212 uint8_t data[4];
Pawel Zarembski 0:01f31e923fe2 213 uint8_t req, ack;
Pawel Zarembski 0:01f31e923fe2 214 uint32_t apsel = adr & 0xff000000;
Pawel Zarembski 0:01f31e923fe2 215 uint32_t bank_sel = adr & APBANKSEL;
Pawel Zarembski 0:01f31e923fe2 216
Pawel Zarembski 0:01f31e923fe2 217 if (!swd_write_dp(DP_SELECT, apsel | bank_sel)) {
Pawel Zarembski 0:01f31e923fe2 218 return 0;
Pawel Zarembski 0:01f31e923fe2 219 }
Pawel Zarembski 0:01f31e923fe2 220
Pawel Zarembski 0:01f31e923fe2 221 switch (adr) {
Pawel Zarembski 0:01f31e923fe2 222 case AP_CSW:
Pawel Zarembski 0:01f31e923fe2 223 if (dap_state.csw == val) {
Pawel Zarembski 0:01f31e923fe2 224 return 1;
Pawel Zarembski 0:01f31e923fe2 225 }
Pawel Zarembski 0:01f31e923fe2 226
Pawel Zarembski 0:01f31e923fe2 227 dap_state.csw = val;
Pawel Zarembski 0:01f31e923fe2 228 break;
Pawel Zarembski 0:01f31e923fe2 229
Pawel Zarembski 0:01f31e923fe2 230 default:
Pawel Zarembski 0:01f31e923fe2 231 break;
Pawel Zarembski 0:01f31e923fe2 232 }
Pawel Zarembski 0:01f31e923fe2 233
Pawel Zarembski 0:01f31e923fe2 234 req = SWD_REG_AP | SWD_REG_W | SWD_REG_ADR(adr);
Pawel Zarembski 0:01f31e923fe2 235 int2array(data, val, 4);
Pawel Zarembski 0:01f31e923fe2 236
Pawel Zarembski 0:01f31e923fe2 237 if (swd_transfer_retry(req, (uint32_t *)data) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 238 return 0;
Pawel Zarembski 0:01f31e923fe2 239 }
Pawel Zarembski 0:01f31e923fe2 240
Pawel Zarembski 0:01f31e923fe2 241 req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
Pawel Zarembski 0:01f31e923fe2 242 ack = swd_transfer_retry(req, NULL);
Pawel Zarembski 0:01f31e923fe2 243 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 244 }
Pawel Zarembski 0:01f31e923fe2 245
Pawel Zarembski 0:01f31e923fe2 246 uint8_t swd_ca_select_state(uint32_t addr) {
Pawel Zarembski 0:01f31e923fe2 247 uint8_t tmp_in[4];
Pawel Zarembski 0:01f31e923fe2 248 uint32_t work_select_state;
Pawel Zarembski 0:01f31e923fe2 249
Pawel Zarembski 0:01f31e923fe2 250 if ((DEBUG_REGSITER_BASE <= addr) && (addr <= DBGCID3)) {
Pawel Zarembski 0:01f31e923fe2 251 work_select_state = SELECT_DBG;
Pawel Zarembski 0:01f31e923fe2 252 } else {
Pawel Zarembski 0:01f31e923fe2 253 work_select_state = SELECT_MEM;
Pawel Zarembski 0:01f31e923fe2 254 }
Pawel Zarembski 0:01f31e923fe2 255 if (select_state != work_select_state) {
Pawel Zarembski 0:01f31e923fe2 256 // SELECT
Pawel Zarembski 0:01f31e923fe2 257 select_state = work_select_state;
Pawel Zarembski 0:01f31e923fe2 258 int2array(tmp_in, select_state, 4);
Pawel Zarembski 0:01f31e923fe2 259 if (swd_transfer_retry(0x08, (uint32_t *)tmp_in) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 260 return 0;
Pawel Zarembski 0:01f31e923fe2 261 }
Pawel Zarembski 0:01f31e923fe2 262 }
Pawel Zarembski 0:01f31e923fe2 263 return 1;
Pawel Zarembski 0:01f31e923fe2 264 }
Pawel Zarembski 0:01f31e923fe2 265
Pawel Zarembski 0:01f31e923fe2 266
Pawel Zarembski 0:01f31e923fe2 267 // Write 32-bit word aligned values to target memory using address auto-increment.
Pawel Zarembski 0:01f31e923fe2 268 // size is in bytes.
Pawel Zarembski 0:01f31e923fe2 269 static uint8_t swd_write_block(uint32_t address, uint8_t *data, uint32_t size)
Pawel Zarembski 0:01f31e923fe2 270 {
Pawel Zarembski 0:01f31e923fe2 271 uint8_t tmp_in[4], req;
Pawel Zarembski 0:01f31e923fe2 272 uint32_t size_in_words;
Pawel Zarembski 0:01f31e923fe2 273 uint32_t i, ack = 0x01;
Pawel Zarembski 0:01f31e923fe2 274 uint32_t *work_write_data;
Pawel Zarembski 0:01f31e923fe2 275
Pawel Zarembski 0:01f31e923fe2 276 if (size == 0) {
Pawel Zarembski 0:01f31e923fe2 277 return 0;
Pawel Zarembski 0:01f31e923fe2 278 }
Pawel Zarembski 0:01f31e923fe2 279
Pawel Zarembski 0:01f31e923fe2 280 size_in_words = size / 4;
Pawel Zarembski 0:01f31e923fe2 281
Pawel Zarembski 0:01f31e923fe2 282 // CSW register
Pawel Zarembski 0:01f31e923fe2 283 if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
Pawel Zarembski 0:01f31e923fe2 284 return 0;
Pawel Zarembski 0:01f31e923fe2 285 }
Pawel Zarembski 0:01f31e923fe2 286
Pawel Zarembski 0:01f31e923fe2 287 if (swd_ca_select_state(address) == 0) {
Pawel Zarembski 0:01f31e923fe2 288 return 0;
Pawel Zarembski 0:01f31e923fe2 289 }
Pawel Zarembski 0:01f31e923fe2 290
Pawel Zarembski 0:01f31e923fe2 291 // TAR write
Pawel Zarembski 0:01f31e923fe2 292 req = SWD_REG_AP | SWD_REG_W | (1 << 2);
Pawel Zarembski 0:01f31e923fe2 293 int2array(tmp_in, address, 4);
Pawel Zarembski 0:01f31e923fe2 294
Pawel Zarembski 0:01f31e923fe2 295 if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 296 return 0;
Pawel Zarembski 0:01f31e923fe2 297 }
Pawel Zarembski 0:01f31e923fe2 298
Pawel Zarembski 0:01f31e923fe2 299 // DRW write
Pawel Zarembski 0:01f31e923fe2 300 req = SWD_REG_AP | SWD_REG_W | (3 << 2);
Pawel Zarembski 0:01f31e923fe2 301 work_write_data = (uint32_t *)data;
Pawel Zarembski 0:01f31e923fe2 302 for (i = 0; i < size_in_words; i++) {
Pawel Zarembski 0:01f31e923fe2 303 int2array(tmp_in, *work_write_data, 4);
Pawel Zarembski 0:01f31e923fe2 304 ack = swd_transfer_retry(req, (uint32_t *)tmp_in);
Pawel Zarembski 0:01f31e923fe2 305 if (ack != 0x01) {
Pawel Zarembski 0:01f31e923fe2 306 return 0;
Pawel Zarembski 0:01f31e923fe2 307 }
Pawel Zarembski 0:01f31e923fe2 308 work_write_data++;
Pawel Zarembski 0:01f31e923fe2 309 }
Pawel Zarembski 0:01f31e923fe2 310 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 311 }
Pawel Zarembski 0:01f31e923fe2 312
Pawel Zarembski 0:01f31e923fe2 313 // Read target memory.
Pawel Zarembski 0:01f31e923fe2 314 static uint8_t swd_read_data(uint32_t addr, uint32_t *val)
Pawel Zarembski 0:01f31e923fe2 315 {
Pawel Zarembski 0:01f31e923fe2 316 uint8_t tmp_in[4];
Pawel Zarembski 0:01f31e923fe2 317 uint8_t tmp_out[4];
Pawel Zarembski 0:01f31e923fe2 318 uint8_t req, ack;
Pawel Zarembski 0:01f31e923fe2 319 uint32_t tmp;
Pawel Zarembski 0:01f31e923fe2 320
Pawel Zarembski 0:01f31e923fe2 321 if (swd_ca_select_state(addr) == 0) {
Pawel Zarembski 0:01f31e923fe2 322 return 0;
Pawel Zarembski 0:01f31e923fe2 323 }
Pawel Zarembski 0:01f31e923fe2 324
Pawel Zarembski 0:01f31e923fe2 325 // put addr in TAR register
Pawel Zarembski 0:01f31e923fe2 326 int2array(tmp_in, addr, 4);
Pawel Zarembski 0:01f31e923fe2 327 req = SWD_REG_AP | SWD_REG_W | (1 << 2);
Pawel Zarembski 0:01f31e923fe2 328
Pawel Zarembski 0:01f31e923fe2 329 if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 330 return 0;
Pawel Zarembski 0:01f31e923fe2 331 }
Pawel Zarembski 0:01f31e923fe2 332
Pawel Zarembski 0:01f31e923fe2 333 // read data
Pawel Zarembski 0:01f31e923fe2 334 req = SWD_REG_AP | SWD_REG_R | (3 << 2);
Pawel Zarembski 0:01f31e923fe2 335
Pawel Zarembski 0:01f31e923fe2 336 if (swd_transfer_retry(req, (uint32_t *)tmp_out) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 337 return 0;
Pawel Zarembski 0:01f31e923fe2 338 }
Pawel Zarembski 0:01f31e923fe2 339
Pawel Zarembski 0:01f31e923fe2 340 // dummy read
Pawel Zarembski 0:01f31e923fe2 341 req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
Pawel Zarembski 0:01f31e923fe2 342 ack = swd_transfer_retry(req, (uint32_t *)tmp_out);
Pawel Zarembski 0:01f31e923fe2 343 *val = 0;
Pawel Zarembski 0:01f31e923fe2 344 tmp = tmp_out[3];
Pawel Zarembski 0:01f31e923fe2 345 *val |= (tmp << 24);
Pawel Zarembski 0:01f31e923fe2 346 tmp = tmp_out[2];
Pawel Zarembski 0:01f31e923fe2 347 *val |= (tmp << 16);
Pawel Zarembski 0:01f31e923fe2 348 tmp = tmp_out[1];
Pawel Zarembski 0:01f31e923fe2 349 *val |= (tmp << 8);
Pawel Zarembski 0:01f31e923fe2 350 tmp = tmp_out[0];
Pawel Zarembski 0:01f31e923fe2 351 *val |= (tmp << 0);
Pawel Zarembski 0:01f31e923fe2 352 return (ack == 0x01);
Pawel Zarembski 0:01f31e923fe2 353 }
Pawel Zarembski 0:01f31e923fe2 354
Pawel Zarembski 0:01f31e923fe2 355 // Write target memory.
Pawel Zarembski 0:01f31e923fe2 356 static uint8_t swd_write_data(uint32_t address, uint32_t data)
Pawel Zarembski 0:01f31e923fe2 357 {
Pawel Zarembski 0:01f31e923fe2 358 uint8_t tmp_in[4];
Pawel Zarembski 0:01f31e923fe2 359 uint8_t req, ack;
Pawel Zarembski 0:01f31e923fe2 360
Pawel Zarembski 0:01f31e923fe2 361 if (swd_ca_select_state(address) == 0) {
Pawel Zarembski 0:01f31e923fe2 362 return 0;
Pawel Zarembski 0:01f31e923fe2 363 }
Pawel Zarembski 0:01f31e923fe2 364
Pawel Zarembski 0:01f31e923fe2 365 // put addr in TAR register
Pawel Zarembski 0:01f31e923fe2 366 int2array(tmp_in, address, 4);
Pawel Zarembski 0:01f31e923fe2 367 req = SWD_REG_AP | SWD_REG_W | (1 << 2);
Pawel Zarembski 0:01f31e923fe2 368
Pawel Zarembski 0:01f31e923fe2 369 if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 370 return 0;
Pawel Zarembski 0:01f31e923fe2 371 }
Pawel Zarembski 0:01f31e923fe2 372
Pawel Zarembski 0:01f31e923fe2 373 // write data
Pawel Zarembski 0:01f31e923fe2 374 int2array(tmp_in, data, 4);
Pawel Zarembski 0:01f31e923fe2 375 req = SWD_REG_AP | SWD_REG_W | (3 << 2);
Pawel Zarembski 0:01f31e923fe2 376 ack = swd_transfer_retry(req, (uint32_t *)tmp_in);
Pawel Zarembski 0:01f31e923fe2 377
Pawel Zarembski 0:01f31e923fe2 378 return (ack == 0x01) ? 1 : 0;
Pawel Zarembski 0:01f31e923fe2 379 }
Pawel Zarembski 0:01f31e923fe2 380
Pawel Zarembski 0:01f31e923fe2 381 // Read 32-bit word from target memory.
Pawel Zarembski 0:01f31e923fe2 382 uint8_t swd_read_word(uint32_t addr, uint32_t *val)
Pawel Zarembski 0:01f31e923fe2 383 {
Pawel Zarembski 0:01f31e923fe2 384 if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
Pawel Zarembski 0:01f31e923fe2 385 return 0;
Pawel Zarembski 0:01f31e923fe2 386 }
Pawel Zarembski 0:01f31e923fe2 387
Pawel Zarembski 0:01f31e923fe2 388 if (!swd_read_data(addr, val)) {
Pawel Zarembski 0:01f31e923fe2 389 return 0;
Pawel Zarembski 0:01f31e923fe2 390 }
Pawel Zarembski 0:01f31e923fe2 391
Pawel Zarembski 0:01f31e923fe2 392 return 1;
Pawel Zarembski 0:01f31e923fe2 393 }
Pawel Zarembski 0:01f31e923fe2 394
Pawel Zarembski 0:01f31e923fe2 395 // Write 32-bit word to target memory.
Pawel Zarembski 0:01f31e923fe2 396 uint8_t swd_write_word(uint32_t addr, uint32_t val)
Pawel Zarembski 0:01f31e923fe2 397 {
Pawel Zarembski 0:01f31e923fe2 398 if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
Pawel Zarembski 0:01f31e923fe2 399 return 0;
Pawel Zarembski 0:01f31e923fe2 400 }
Pawel Zarembski 0:01f31e923fe2 401
Pawel Zarembski 0:01f31e923fe2 402 if (!swd_write_data(addr, val)) {
Pawel Zarembski 0:01f31e923fe2 403 return 0;
Pawel Zarembski 0:01f31e923fe2 404 }
Pawel Zarembski 0:01f31e923fe2 405
Pawel Zarembski 0:01f31e923fe2 406 return 1;
Pawel Zarembski 0:01f31e923fe2 407 }
Pawel Zarembski 0:01f31e923fe2 408
Pawel Zarembski 0:01f31e923fe2 409 // Read unaligned data from target memory.
Pawel Zarembski 0:01f31e923fe2 410 // size is in bytes.
Pawel Zarembski 0:01f31e923fe2 411 uint8_t swd_read_memory(uint32_t address, uint8_t *data, uint32_t size)
Pawel Zarembski 0:01f31e923fe2 412 {
Pawel Zarembski 0:01f31e923fe2 413 uint32_t read_size;
Pawel Zarembski 0:01f31e923fe2 414 uint32_t* read_data;
Pawel Zarembski 0:01f31e923fe2 415
Pawel Zarembski 0:01f31e923fe2 416 read_size = (size / 4);
Pawel Zarembski 0:01f31e923fe2 417 read_data = (uint32_t*)data;
Pawel Zarembski 0:01f31e923fe2 418 /* Write bytes until end */
Pawel Zarembski 0:01f31e923fe2 419 while ((read_size > 0)) {
Pawel Zarembski 0:01f31e923fe2 420 if (!swd_read_data(address, read_data)) {
Pawel Zarembski 0:01f31e923fe2 421 return 0;
Pawel Zarembski 0:01f31e923fe2 422 }
Pawel Zarembski 0:01f31e923fe2 423 address+=4;
Pawel Zarembski 0:01f31e923fe2 424 read_data++;
Pawel Zarembski 0:01f31e923fe2 425 read_size--;
Pawel Zarembski 0:01f31e923fe2 426 }
Pawel Zarembski 0:01f31e923fe2 427
Pawel Zarembski 0:01f31e923fe2 428 return 1;
Pawel Zarembski 0:01f31e923fe2 429 }
Pawel Zarembski 0:01f31e923fe2 430
Pawel Zarembski 0:01f31e923fe2 431 // Write unaligned data to target memory.
Pawel Zarembski 0:01f31e923fe2 432 // size is in bytes.
Pawel Zarembski 0:01f31e923fe2 433 uint8_t swd_write_memory(uint32_t address, uint8_t *data, uint32_t size)
Pawel Zarembski 0:01f31e923fe2 434 {
Pawel Zarembski 0:01f31e923fe2 435 uint32_t n;
Pawel Zarembski 0:01f31e923fe2 436
Pawel Zarembski 0:01f31e923fe2 437 while (size > 3) {
Pawel Zarembski 0:01f31e923fe2 438 // Limit to auto increment page size
Pawel Zarembski 0:01f31e923fe2 439 n = TARGET_AUTO_INCREMENT_PAGE_SIZE - (address & (TARGET_AUTO_INCREMENT_PAGE_SIZE - 1));
Pawel Zarembski 0:01f31e923fe2 440 if (size < n) {
Pawel Zarembski 0:01f31e923fe2 441 n = size & 0xFFFFFFFC; // Only count complete words remaining
Pawel Zarembski 0:01f31e923fe2 442 }
Pawel Zarembski 0:01f31e923fe2 443
Pawel Zarembski 0:01f31e923fe2 444 if (!swd_write_block(address, data, n)) {
Pawel Zarembski 0:01f31e923fe2 445 return 0;
Pawel Zarembski 0:01f31e923fe2 446 }
Pawel Zarembski 0:01f31e923fe2 447
Pawel Zarembski 0:01f31e923fe2 448 address += n;
Pawel Zarembski 0:01f31e923fe2 449 data += n;
Pawel Zarembski 0:01f31e923fe2 450 size -= n;
Pawel Zarembski 0:01f31e923fe2 451 }
Pawel Zarembski 0:01f31e923fe2 452 /* Auto increment is end */
Pawel Zarembski 0:01f31e923fe2 453 /* Return the CSW reg value to SIZE8 */
Pawel Zarembski 0:01f31e923fe2 454 if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
Pawel Zarembski 0:01f31e923fe2 455 return 0;
Pawel Zarembski 0:01f31e923fe2 456 }
Pawel Zarembski 0:01f31e923fe2 457
Pawel Zarembski 0:01f31e923fe2 458 return 1;
Pawel Zarembski 0:01f31e923fe2 459 }
Pawel Zarembski 0:01f31e923fe2 460
Pawel Zarembski 0:01f31e923fe2 461 // Execute system call.
Pawel Zarembski 0:01f31e923fe2 462 static uint8_t swd_write_debug_state(DEBUG_STATE *state)
Pawel Zarembski 0:01f31e923fe2 463 {
Pawel Zarembski 0:01f31e923fe2 464 uint32_t i, status;
Pawel Zarembski 0:01f31e923fe2 465
Pawel Zarembski 0:01f31e923fe2 466 if (!swd_write_dp(DP_SELECT, 0)) {
Pawel Zarembski 0:01f31e923fe2 467 return 0;
Pawel Zarembski 0:01f31e923fe2 468 }
Pawel Zarembski 0:01f31e923fe2 469
Pawel Zarembski 0:01f31e923fe2 470 // R0, R1, R2, R3
Pawel Zarembski 0:01f31e923fe2 471 for (i = 0; i < 4; i++) {
Pawel Zarembski 0:01f31e923fe2 472 if (!swd_write_core_register(i, state->r[i])) {
Pawel Zarembski 0:01f31e923fe2 473 return 0;
Pawel Zarembski 0:01f31e923fe2 474 }
Pawel Zarembski 0:01f31e923fe2 475 }
Pawel Zarembski 0:01f31e923fe2 476
Pawel Zarembski 0:01f31e923fe2 477 // R9
Pawel Zarembski 0:01f31e923fe2 478 if (!swd_write_core_register(9, state->r[9])) {
Pawel Zarembski 0:01f31e923fe2 479 return 0;
Pawel Zarembski 0:01f31e923fe2 480 }
Pawel Zarembski 0:01f31e923fe2 481
Pawel Zarembski 0:01f31e923fe2 482 // R13, R14
Pawel Zarembski 0:01f31e923fe2 483 for (i = 13; i < 15; i++) {
Pawel Zarembski 0:01f31e923fe2 484 if (!swd_write_core_register(i, state->r[i])) {
Pawel Zarembski 0:01f31e923fe2 485 return 0;
Pawel Zarembski 0:01f31e923fe2 486 }
Pawel Zarembski 0:01f31e923fe2 487 }
Pawel Zarembski 0:01f31e923fe2 488
Pawel Zarembski 0:01f31e923fe2 489 // xPSR
Pawel Zarembski 0:01f31e923fe2 490 /* xPSR write */
Pawel Zarembski 0:01f31e923fe2 491 /* write PSR (write r6) */
Pawel Zarembski 0:01f31e923fe2 492 if (!swd_write_core_register(6, state->xpsr)) {
Pawel Zarembski 0:01f31e923fe2 493 return 0;
Pawel Zarembski 0:01f31e923fe2 494 }
Pawel Zarembski 0:01f31e923fe2 495 /* MSR (PSR <- r6) */
Pawel Zarembski 0:01f31e923fe2 496 if (!swd_write_word(DBGITR, CMD_MSR | (6))) {
Pawel Zarembski 0:01f31e923fe2 497 return 0;
Pawel Zarembski 0:01f31e923fe2 498 }
Pawel Zarembski 0:01f31e923fe2 499
Pawel Zarembski 0:01f31e923fe2 500 /* R15(PC) */
Pawel Zarembski 0:01f31e923fe2 501 /* MRC R7 */
Pawel Zarembski 0:01f31e923fe2 502 if (!swd_write_core_register(7, state->r[15])) {
Pawel Zarembski 0:01f31e923fe2 503 return 0;
Pawel Zarembski 0:01f31e923fe2 504 }
Pawel Zarembski 0:01f31e923fe2 505 /* MOV R15, R7 */
Pawel Zarembski 0:01f31e923fe2 506 if (!swd_write_word(DBGITR, CMD_MOV | (15 << 12) | (7))) {
Pawel Zarembski 0:01f31e923fe2 507 return 0;
Pawel Zarembski 0:01f31e923fe2 508 }
Pawel Zarembski 0:01f31e923fe2 509 if (!swd_restart_req()) {
Pawel Zarembski 0:01f31e923fe2 510 return 0;
Pawel Zarembski 0:01f31e923fe2 511 }
Pawel Zarembski 0:01f31e923fe2 512
Pawel Zarembski 0:01f31e923fe2 513 // check status
Pawel Zarembski 0:01f31e923fe2 514 if (!swd_read_dp(DP_CTRL_STAT, &status)) {
Pawel Zarembski 0:01f31e923fe2 515 return 0;
Pawel Zarembski 0:01f31e923fe2 516 }
Pawel Zarembski 0:01f31e923fe2 517
Pawel Zarembski 0:01f31e923fe2 518 if (status & (STICKYERR | WDATAERR)) {
Pawel Zarembski 0:01f31e923fe2 519 return 0;
Pawel Zarembski 0:01f31e923fe2 520 }
Pawel Zarembski 0:01f31e923fe2 521
Pawel Zarembski 0:01f31e923fe2 522 return 1;
Pawel Zarembski 0:01f31e923fe2 523 }
Pawel Zarembski 0:01f31e923fe2 524
Pawel Zarembski 0:01f31e923fe2 525 static uint8_t swd_restart_req(void) {
Pawel Zarembski 0:01f31e923fe2 526 uint32_t val, i, timeout = MAX_TIMEOUT;
Pawel Zarembski 0:01f31e923fe2 527 /* Clear ITRen */
Pawel Zarembski 0:01f31e923fe2 528 if (!swd_read_word(DBGDSCR, &val)) {
Pawel Zarembski 0:01f31e923fe2 529 return 0;
Pawel Zarembski 0:01f31e923fe2 530 }
Pawel Zarembski 0:01f31e923fe2 531 val = val & ~0x00002000;
Pawel Zarembski 0:01f31e923fe2 532 if (!swd_write_word(DBGDSCR, val)) {
Pawel Zarembski 0:01f31e923fe2 533 return 0;
Pawel Zarembski 0:01f31e923fe2 534 }
Pawel Zarembski 0:01f31e923fe2 535 for (i = 0; i < timeout; i++) {
Pawel Zarembski 0:01f31e923fe2 536 /* read DBGDSCR */
Pawel Zarembski 0:01f31e923fe2 537 if (!swd_read_word(DBGDSCR, &val)) {
Pawel Zarembski 0:01f31e923fe2 538 return 0;
Pawel Zarembski 0:01f31e923fe2 539 }
Pawel Zarembski 0:01f31e923fe2 540 /* wait Clear UND_I, ADABORT_I, SDABORT_I[bit:8-6] and InstrCompl_I[bit24] set to 1 */
Pawel Zarembski 0:01f31e923fe2 541 if ((val & 0x010001C0) == 0x01000000) {
Pawel Zarembski 0:01f31e923fe2 542 break;
Pawel Zarembski 0:01f31e923fe2 543 } else if (i == (timeout -1)) {
Pawel Zarembski 0:01f31e923fe2 544 return 0;
Pawel Zarembski 0:01f31e923fe2 545 }
Pawel Zarembski 0:01f31e923fe2 546 }
Pawel Zarembski 0:01f31e923fe2 547 /* DBGDRCR Restart req */
Pawel Zarembski 0:01f31e923fe2 548 if (!swd_write_word(DBGDRCR, 0x00000002 )) {
Pawel Zarembski 0:01f31e923fe2 549 return 0;
Pawel Zarembski 0:01f31e923fe2 550 }
Pawel Zarembski 0:01f31e923fe2 551 for (i = 0; i < timeout; i++) {
Pawel Zarembski 0:01f31e923fe2 552 /* read DBGDSCR */
Pawel Zarembski 0:01f31e923fe2 553 if (!swd_read_word(DBGDSCR, &val)) {
Pawel Zarembski 0:01f31e923fe2 554 return 0;
Pawel Zarembski 0:01f31e923fe2 555 }
Pawel Zarembski 0:01f31e923fe2 556 if ((val & 0x00000002) == 0x00000002) {
Pawel Zarembski 0:01f31e923fe2 557 /* restarted */
Pawel Zarembski 0:01f31e923fe2 558 return 1;
Pawel Zarembski 0:01f31e923fe2 559 }
Pawel Zarembski 0:01f31e923fe2 560 }
Pawel Zarembski 0:01f31e923fe2 561 return 0;
Pawel Zarembski 0:01f31e923fe2 562 }
Pawel Zarembski 0:01f31e923fe2 563
Pawel Zarembski 0:01f31e923fe2 564 static uint8_t swd_enable_debug(void) {
Pawel Zarembski 0:01f31e923fe2 565 uint32_t val;
Pawel Zarembski 0:01f31e923fe2 566 if (!swd_read_word(DBGDSCR, &val)) {
Pawel Zarembski 0:01f31e923fe2 567 return 0;
Pawel Zarembski 0:01f31e923fe2 568 }
Pawel Zarembski 0:01f31e923fe2 569 /* DBGDSCR ITRen = 1(ARM instruction enable) */
Pawel Zarembski 0:01f31e923fe2 570 /* and ExtDCCmode = 01(stall mode) */
Pawel Zarembski 0:01f31e923fe2 571 val = val | 0x00106000;
Pawel Zarembski 0:01f31e923fe2 572 if (!swd_write_word(DBGDSCR, val)) {
Pawel Zarembski 0:01f31e923fe2 573 return 0;
Pawel Zarembski 0:01f31e923fe2 574 }
Pawel Zarembski 0:01f31e923fe2 575 return 1;
Pawel Zarembski 0:01f31e923fe2 576 }
Pawel Zarembski 0:01f31e923fe2 577
Pawel Zarembski 0:01f31e923fe2 578 uint8_t swd_read_core_register(uint32_t n, uint32_t *val)
Pawel Zarembski 0:01f31e923fe2 579 {
Pawel Zarembski 0:01f31e923fe2 580 if (!swd_write_word(DBGITR, CMD_MCR | (n << 12))) {
Pawel Zarembski 0:01f31e923fe2 581 return 0;
Pawel Zarembski 0:01f31e923fe2 582 }
Pawel Zarembski 0:01f31e923fe2 583
Pawel Zarembski 0:01f31e923fe2 584 if (!swd_read_word(DBGDTRTX, val)){
Pawel Zarembski 0:01f31e923fe2 585 return 0;
Pawel Zarembski 0:01f31e923fe2 586 }
Pawel Zarembski 0:01f31e923fe2 587
Pawel Zarembski 0:01f31e923fe2 588 return 1;
Pawel Zarembski 0:01f31e923fe2 589 }
Pawel Zarembski 0:01f31e923fe2 590
Pawel Zarembski 0:01f31e923fe2 591 uint8_t swd_write_core_register(uint32_t n, uint32_t val)
Pawel Zarembski 0:01f31e923fe2 592 {
Pawel Zarembski 0:01f31e923fe2 593 if (!swd_write_word(DBGDTRRX, val)){
Pawel Zarembski 0:01f31e923fe2 594 return 0;
Pawel Zarembski 0:01f31e923fe2 595 }
Pawel Zarembski 0:01f31e923fe2 596
Pawel Zarembski 0:01f31e923fe2 597 /* Write MRC */
Pawel Zarembski 0:01f31e923fe2 598 if (!swd_write_word(DBGITR, (CMD_MRC | (n << 12)))) {
Pawel Zarembski 0:01f31e923fe2 599 return 0;
Pawel Zarembski 0:01f31e923fe2 600 }
Pawel Zarembski 0:01f31e923fe2 601
Pawel Zarembski 0:01f31e923fe2 602 return 1;
Pawel Zarembski 0:01f31e923fe2 603 }
Pawel Zarembski 0:01f31e923fe2 604
Pawel Zarembski 0:01f31e923fe2 605 static uint8_t swd_wait_until_halted(void)
Pawel Zarembski 0:01f31e923fe2 606 {
Pawel Zarembski 0:01f31e923fe2 607 uint32_t val, i, timeout = MAX_TIMEOUT;
Pawel Zarembski 0:01f31e923fe2 608 for (i = 0; i < timeout; i++) {
Pawel Zarembski 0:01f31e923fe2 609 /* read DBGDSCR */
Pawel Zarembski 0:01f31e923fe2 610 if (!swd_read_word(DBGDSCR, &val)) {
Pawel Zarembski 0:01f31e923fe2 611 return 0;
Pawel Zarembski 0:01f31e923fe2 612 }
Pawel Zarembski 0:01f31e923fe2 613
Pawel Zarembski 0:01f31e923fe2 614 if ((val & DBGDSCR_HALTED) == DBGDSCR_HALTED) {
Pawel Zarembski 0:01f31e923fe2 615 return 1;
Pawel Zarembski 0:01f31e923fe2 616 }
Pawel Zarembski 0:01f31e923fe2 617 osDelay(1);
Pawel Zarembski 0:01f31e923fe2 618 }
Pawel Zarembski 0:01f31e923fe2 619
Pawel Zarembski 0:01f31e923fe2 620 return 0;
Pawel Zarembski 0:01f31e923fe2 621 }
Pawel Zarembski 0:01f31e923fe2 622
Pawel Zarembski 0:01f31e923fe2 623 uint8_t swd_flash_syscall_exec(const program_syscall_t *sysCallParam, uint32_t entry, uint32_t arg1, uint32_t arg2, uint32_t arg3, uint32_t arg4, flash_algo_return_t return_type)
Pawel Zarembski 0:01f31e923fe2 624 {
Pawel Zarembski 0:01f31e923fe2 625 DEBUG_STATE state = {{0}, 0};
Pawel Zarembski 0:01f31e923fe2 626 // Call flash algorithm function on target and wait for result.
Pawel Zarembski 0:01f31e923fe2 627 state.r[0] = arg1; // R0: Argument 1
Pawel Zarembski 0:01f31e923fe2 628 state.r[1] = arg2; // R1: Argument 2
Pawel Zarembski 0:01f31e923fe2 629 state.r[2] = arg3; // R2: Argument 3
Pawel Zarembski 0:01f31e923fe2 630 state.r[3] = arg4; // R3: Argument 4
Pawel Zarembski 0:01f31e923fe2 631 state.r[9] = sysCallParam->static_base; // SB: Static Base
Pawel Zarembski 0:01f31e923fe2 632 state.r[13] = sysCallParam->stack_pointer; // SP: Stack Pointer
Pawel Zarembski 0:01f31e923fe2 633 state.r[14] = sysCallParam->breakpoint; // LR: Exit Point
Pawel Zarembski 0:01f31e923fe2 634 state.r[15] = entry; // PC: Entry Point
Pawel Zarembski 0:01f31e923fe2 635 state.xpsr = 0x00000000; // xPSR: T = 1, ISR = 0
Pawel Zarembski 0:01f31e923fe2 636
Pawel Zarembski 0:01f31e923fe2 637 if (!swd_write_debug_state(&state)) {
Pawel Zarembski 0:01f31e923fe2 638 return 0;
Pawel Zarembski 0:01f31e923fe2 639 }
Pawel Zarembski 0:01f31e923fe2 640
Pawel Zarembski 0:01f31e923fe2 641 if (!swd_wait_until_halted()) {
Pawel Zarembski 0:01f31e923fe2 642 return 0;
Pawel Zarembski 0:01f31e923fe2 643 }
Pawel Zarembski 0:01f31e923fe2 644
Pawel Zarembski 0:01f31e923fe2 645 if (!swd_enable_debug()) {
Pawel Zarembski 0:01f31e923fe2 646 return 0;
Pawel Zarembski 0:01f31e923fe2 647 }
Pawel Zarembski 0:01f31e923fe2 648
Pawel Zarembski 0:01f31e923fe2 649 if (!swd_read_core_register(0, &state.r[0])) {
Pawel Zarembski 0:01f31e923fe2 650 return 0;
Pawel Zarembski 0:01f31e923fe2 651 }
Pawel Zarembski 0:01f31e923fe2 652
Pawel Zarembski 0:01f31e923fe2 653 if ( return_type == FLASHALGO_RETURN_POINTER ) {
Pawel Zarembski 0:01f31e923fe2 654 // Flash verify functions return pointer to byte following the buffer if successful.
Pawel Zarembski 0:01f31e923fe2 655 if (state.r[0] != (arg1 + arg2)) {
Pawel Zarembski 0:01f31e923fe2 656 return 0;
Pawel Zarembski 0:01f31e923fe2 657 }
Pawel Zarembski 0:01f31e923fe2 658 }
Pawel Zarembski 0:01f31e923fe2 659 else {
Pawel Zarembski 0:01f31e923fe2 660 // Flash functions return 0 if successful.
Pawel Zarembski 0:01f31e923fe2 661 if (state.r[0] != 0) {
Pawel Zarembski 0:01f31e923fe2 662 return 0;
Pawel Zarembski 0:01f31e923fe2 663 }
Pawel Zarembski 0:01f31e923fe2 664 }
Pawel Zarembski 0:01f31e923fe2 665
Pawel Zarembski 0:01f31e923fe2 666 return 1;
Pawel Zarembski 0:01f31e923fe2 667 }
Pawel Zarembski 0:01f31e923fe2 668
Pawel Zarembski 0:01f31e923fe2 669 // SWD Reset
Pawel Zarembski 0:01f31e923fe2 670 static uint8_t swd_reset(void)
Pawel Zarembski 0:01f31e923fe2 671 {
Pawel Zarembski 0:01f31e923fe2 672 uint8_t tmp_in[8];
Pawel Zarembski 0:01f31e923fe2 673 uint8_t i = 0;
Pawel Zarembski 0:01f31e923fe2 674
Pawel Zarembski 0:01f31e923fe2 675 for (i = 0; i < 8; i++) {
Pawel Zarembski 0:01f31e923fe2 676 tmp_in[i] = 0xff;
Pawel Zarembski 0:01f31e923fe2 677 }
Pawel Zarembski 0:01f31e923fe2 678
Pawel Zarembski 0:01f31e923fe2 679 SWJ_Sequence(51, tmp_in);
Pawel Zarembski 0:01f31e923fe2 680 return 1;
Pawel Zarembski 0:01f31e923fe2 681 }
Pawel Zarembski 0:01f31e923fe2 682
Pawel Zarembski 0:01f31e923fe2 683 // SWD Switch
Pawel Zarembski 0:01f31e923fe2 684 static uint8_t swd_switch(uint16_t val)
Pawel Zarembski 0:01f31e923fe2 685 {
Pawel Zarembski 0:01f31e923fe2 686 uint8_t tmp_in[2];
Pawel Zarembski 0:01f31e923fe2 687 tmp_in[0] = val & 0xff;
Pawel Zarembski 0:01f31e923fe2 688 tmp_in[1] = (val >> 8) & 0xff;
Pawel Zarembski 0:01f31e923fe2 689 SWJ_Sequence(16, tmp_in);
Pawel Zarembski 0:01f31e923fe2 690 return 1;
Pawel Zarembski 0:01f31e923fe2 691 }
Pawel Zarembski 0:01f31e923fe2 692
Pawel Zarembski 0:01f31e923fe2 693 // SWD Read ID
Pawel Zarembski 0:01f31e923fe2 694 static uint8_t swd_read_idcode(uint32_t *id)
Pawel Zarembski 0:01f31e923fe2 695 {
Pawel Zarembski 0:01f31e923fe2 696 uint8_t tmp_in[1];
Pawel Zarembski 0:01f31e923fe2 697 uint8_t tmp_out[4];
Pawel Zarembski 0:01f31e923fe2 698 tmp_in[0] = 0x00;
Pawel Zarembski 0:01f31e923fe2 699 SWJ_Sequence(8, tmp_in);
Pawel Zarembski 0:01f31e923fe2 700
Pawel Zarembski 0:01f31e923fe2 701 if (swd_read_dp(0, (uint32_t *)tmp_out) != 0x01) {
Pawel Zarembski 0:01f31e923fe2 702 return 0;
Pawel Zarembski 0:01f31e923fe2 703 }
Pawel Zarembski 0:01f31e923fe2 704
Pawel Zarembski 0:01f31e923fe2 705 *id = (tmp_out[3] << 24) | (tmp_out[2] << 16) | (tmp_out[1] << 8) | tmp_out[0];
Pawel Zarembski 0:01f31e923fe2 706 return 1;
Pawel Zarembski 0:01f31e923fe2 707 }
Pawel Zarembski 0:01f31e923fe2 708
Pawel Zarembski 0:01f31e923fe2 709
Pawel Zarembski 0:01f31e923fe2 710 uint8_t JTAG2SWD()
Pawel Zarembski 0:01f31e923fe2 711 {
Pawel Zarembski 0:01f31e923fe2 712 uint32_t tmp = 0;
Pawel Zarembski 0:01f31e923fe2 713
Pawel Zarembski 0:01f31e923fe2 714 if (!swd_reset()) {
Pawel Zarembski 0:01f31e923fe2 715 return 0;
Pawel Zarembski 0:01f31e923fe2 716 }
Pawel Zarembski 0:01f31e923fe2 717
Pawel Zarembski 0:01f31e923fe2 718 if (!swd_switch(0xE79E)) {
Pawel Zarembski 0:01f31e923fe2 719 return 0;
Pawel Zarembski 0:01f31e923fe2 720 }
Pawel Zarembski 0:01f31e923fe2 721
Pawel Zarembski 0:01f31e923fe2 722 if (!swd_reset()) {
Pawel Zarembski 0:01f31e923fe2 723 return 0;
Pawel Zarembski 0:01f31e923fe2 724 }
Pawel Zarembski 0:01f31e923fe2 725
Pawel Zarembski 0:01f31e923fe2 726 if (!swd_read_idcode(&tmp)) {
Pawel Zarembski 0:01f31e923fe2 727 return 0;
Pawel Zarembski 0:01f31e923fe2 728 }
Pawel Zarembski 0:01f31e923fe2 729
Pawel Zarembski 0:01f31e923fe2 730 return 1;
Pawel Zarembski 0:01f31e923fe2 731 }
Pawel Zarembski 0:01f31e923fe2 732
Pawel Zarembski 0:01f31e923fe2 733 uint8_t swd_init_debug(void)
Pawel Zarembski 0:01f31e923fe2 734 {
Pawel Zarembski 0:01f31e923fe2 735 uint32_t tmp = 0;
Pawel Zarembski 0:01f31e923fe2 736
Pawel Zarembski 0:01f31e923fe2 737 if (swd_init_debug_flag != 0) {
Pawel Zarembski 0:01f31e923fe2 738 return 1;
Pawel Zarembski 0:01f31e923fe2 739 }
Pawel Zarembski 0:01f31e923fe2 740 swd_init_debug_flag = 1;
Pawel Zarembski 0:01f31e923fe2 741
Pawel Zarembski 0:01f31e923fe2 742 // init dap state with fake values
Pawel Zarembski 0:01f31e923fe2 743 dap_state.select = 0xffffffff;
Pawel Zarembski 0:01f31e923fe2 744 dap_state.csw = 0xffffffff;
Pawel Zarembski 0:01f31e923fe2 745 swd_init();
Pawel Zarembski 0:01f31e923fe2 746 // call a target dependant function
Pawel Zarembski 0:01f31e923fe2 747 // this function can do several stuff before really
Pawel Zarembski 0:01f31e923fe2 748 // initing the debug
Pawel Zarembski 0:01f31e923fe2 749 if (g_target_family && g_target_family->target_before_init_debug) {
Pawel Zarembski 0:01f31e923fe2 750 g_target_family->target_before_init_debug();
Pawel Zarembski 0:01f31e923fe2 751 }
Pawel Zarembski 0:01f31e923fe2 752
Pawel Zarembski 0:01f31e923fe2 753 if (!JTAG2SWD()) {
Pawel Zarembski 0:01f31e923fe2 754 return 0;
Pawel Zarembski 0:01f31e923fe2 755 }
Pawel Zarembski 0:01f31e923fe2 756
Pawel Zarembski 0:01f31e923fe2 757 if (!swd_write_dp(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
Pawel Zarembski 0:01f31e923fe2 758 return 0;
Pawel Zarembski 0:01f31e923fe2 759 }
Pawel Zarembski 0:01f31e923fe2 760
Pawel Zarembski 0:01f31e923fe2 761 // Ensure CTRL/STAT register selected in DPBANKSEL
Pawel Zarembski 0:01f31e923fe2 762 if (!swd_write_dp(DP_SELECT, 0)) {
Pawel Zarembski 0:01f31e923fe2 763 return 0;
Pawel Zarembski 0:01f31e923fe2 764 }
Pawel Zarembski 0:01f31e923fe2 765
Pawel Zarembski 0:01f31e923fe2 766 // Power up
Pawel Zarembski 0:01f31e923fe2 767 if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
Pawel Zarembski 0:01f31e923fe2 768 return 0;
Pawel Zarembski 0:01f31e923fe2 769 }
Pawel Zarembski 0:01f31e923fe2 770
Pawel Zarembski 0:01f31e923fe2 771 do {
Pawel Zarembski 0:01f31e923fe2 772 if (!swd_read_dp(DP_CTRL_STAT, &tmp)) {
Pawel Zarembski 0:01f31e923fe2 773 return 0;
Pawel Zarembski 0:01f31e923fe2 774 }
Pawel Zarembski 0:01f31e923fe2 775 } while ((tmp & (CDBGPWRUPACK | CSYSPWRUPACK)) != (CDBGPWRUPACK | CSYSPWRUPACK));
Pawel Zarembski 0:01f31e923fe2 776
Pawel Zarembski 0:01f31e923fe2 777 if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | TRNNORMAL | MASKLANE)) {
Pawel Zarembski 0:01f31e923fe2 778 return 0;
Pawel Zarembski 0:01f31e923fe2 779 }
Pawel Zarembski 0:01f31e923fe2 780
Pawel Zarembski 0:01f31e923fe2 781 // call a target dependant function:
Pawel Zarembski 0:01f31e923fe2 782 // some target can enter in a lock state
Pawel Zarembski 0:01f31e923fe2 783 // this function can unlock these targets
Pawel Zarembski 0:01f31e923fe2 784 if (g_target_family && g_target_family->target_unlock_sequence) {
Pawel Zarembski 0:01f31e923fe2 785 g_target_family->target_unlock_sequence();
Pawel Zarembski 0:01f31e923fe2 786 }
Pawel Zarembski 0:01f31e923fe2 787
Pawel Zarembski 0:01f31e923fe2 788 if (!swd_write_dp(DP_SELECT, 0)) {
Pawel Zarembski 0:01f31e923fe2 789 return 0;
Pawel Zarembski 0:01f31e923fe2 790 }
Pawel Zarembski 0:01f31e923fe2 791
Pawel Zarembski 0:01f31e923fe2 792 return 1;
Pawel Zarembski 0:01f31e923fe2 793 }
Pawel Zarembski 0:01f31e923fe2 794
Pawel Zarembski 0:01f31e923fe2 795 uint8_t swd_uninit_debug(void)
Pawel Zarembski 0:01f31e923fe2 796 {
Pawel Zarembski 0:01f31e923fe2 797 return 1;
Pawel Zarembski 0:01f31e923fe2 798 }
Pawel Zarembski 0:01f31e923fe2 799
Pawel Zarembski 0:01f31e923fe2 800 uint8_t swd_set_target_state_hw(target_state_t state)
Pawel Zarembski 0:01f31e923fe2 801 {
Pawel Zarembski 0:01f31e923fe2 802 uint32_t val;
Pawel Zarembski 0:01f31e923fe2 803 swd_init();
Pawel Zarembski 0:01f31e923fe2 804
Pawel Zarembski 0:01f31e923fe2 805 switch (state) {
Pawel Zarembski 0:01f31e923fe2 806 case RESET_HOLD:
Pawel Zarembski 0:01f31e923fe2 807 swd_set_target_reset(1);
Pawel Zarembski 0:01f31e923fe2 808 break;
Pawel Zarembski 0:01f31e923fe2 809
Pawel Zarembski 0:01f31e923fe2 810 case RESET_RUN:
Pawel Zarembski 0:01f31e923fe2 811 swd_set_target_reset(1);
Pawel Zarembski 0:01f31e923fe2 812 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 813 swd_set_target_reset(0);
Pawel Zarembski 0:01f31e923fe2 814 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 815 swd_off();
Pawel Zarembski 0:01f31e923fe2 816 break;
Pawel Zarembski 0:01f31e923fe2 817
Pawel Zarembski 0:01f31e923fe2 818 case RESET_PROGRAM:
Pawel Zarembski 0:01f31e923fe2 819 swd_set_target_reset(1);
Pawel Zarembski 0:01f31e923fe2 820 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 821 swd_set_target_reset(0);
Pawel Zarembski 0:01f31e923fe2 822 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 823
Pawel Zarembski 0:01f31e923fe2 824 if (!swd_init_debug()) {
Pawel Zarembski 0:01f31e923fe2 825 return 0;
Pawel Zarembski 0:01f31e923fe2 826 }
Pawel Zarembski 0:01f31e923fe2 827
Pawel Zarembski 0:01f31e923fe2 828 if (!swd_enable_debug()) {
Pawel Zarembski 0:01f31e923fe2 829 return 0;
Pawel Zarembski 0:01f31e923fe2 830 }
Pawel Zarembski 0:01f31e923fe2 831 /* DBGDRCR halt req*/
Pawel Zarembski 0:01f31e923fe2 832 val = 0x00000001;
Pawel Zarembski 0:01f31e923fe2 833 if (!swd_write_word(DBGDRCR, val )) {
Pawel Zarembski 0:01f31e923fe2 834 return 0;
Pawel Zarembski 0:01f31e923fe2 835 }
Pawel Zarembski 0:01f31e923fe2 836 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 837 if (!swd_wait_until_halted()) {
Pawel Zarembski 0:01f31e923fe2 838 return 0;
Pawel Zarembski 0:01f31e923fe2 839 }
Pawel Zarembski 0:01f31e923fe2 840
Pawel Zarembski 0:01f31e923fe2 841 break;
Pawel Zarembski 0:01f31e923fe2 842
Pawel Zarembski 0:01f31e923fe2 843 case NO_DEBUG:
Pawel Zarembski 0:01f31e923fe2 844 if (!swd_write_word(DBG_HCSR, DBGKEY)) {
Pawel Zarembski 0:01f31e923fe2 845 return 0;
Pawel Zarembski 0:01f31e923fe2 846 }
Pawel Zarembski 0:01f31e923fe2 847
Pawel Zarembski 0:01f31e923fe2 848 break;
Pawel Zarembski 0:01f31e923fe2 849
Pawel Zarembski 0:01f31e923fe2 850 case DEBUG:
Pawel Zarembski 0:01f31e923fe2 851 if (!JTAG2SWD()) {
Pawel Zarembski 0:01f31e923fe2 852 return 0;
Pawel Zarembski 0:01f31e923fe2 853 }
Pawel Zarembski 0:01f31e923fe2 854
Pawel Zarembski 0:01f31e923fe2 855 if (!swd_write_dp(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
Pawel Zarembski 0:01f31e923fe2 856 return 0;
Pawel Zarembski 0:01f31e923fe2 857 }
Pawel Zarembski 0:01f31e923fe2 858
Pawel Zarembski 0:01f31e923fe2 859 // Ensure CTRL/STAT register selected in DPBANKSEL
Pawel Zarembski 0:01f31e923fe2 860 if (!swd_write_dp(DP_SELECT, 0)) {
Pawel Zarembski 0:01f31e923fe2 861 return 0;
Pawel Zarembski 0:01f31e923fe2 862 }
Pawel Zarembski 0:01f31e923fe2 863
Pawel Zarembski 0:01f31e923fe2 864 // Power up
Pawel Zarembski 0:01f31e923fe2 865 if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
Pawel Zarembski 0:01f31e923fe2 866 return 0;
Pawel Zarembski 0:01f31e923fe2 867 }
Pawel Zarembski 0:01f31e923fe2 868
Pawel Zarembski 0:01f31e923fe2 869 // Enable debug
Pawel Zarembski 0:01f31e923fe2 870 if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
Pawel Zarembski 0:01f31e923fe2 871 return 0;
Pawel Zarembski 0:01f31e923fe2 872 }
Pawel Zarembski 0:01f31e923fe2 873
Pawel Zarembski 0:01f31e923fe2 874 break;
Pawel Zarembski 0:01f31e923fe2 875
Pawel Zarembski 0:01f31e923fe2 876 default:
Pawel Zarembski 0:01f31e923fe2 877 return 0;
Pawel Zarembski 0:01f31e923fe2 878 }
Pawel Zarembski 0:01f31e923fe2 879
Pawel Zarembski 0:01f31e923fe2 880 return 1;
Pawel Zarembski 0:01f31e923fe2 881 }
Pawel Zarembski 0:01f31e923fe2 882
Pawel Zarembski 0:01f31e923fe2 883 uint8_t swd_set_target_state_sw(target_state_t state)
Pawel Zarembski 0:01f31e923fe2 884 {
Pawel Zarembski 0:01f31e923fe2 885 uint32_t val;
Pawel Zarembski 0:01f31e923fe2 886 swd_init();
Pawel Zarembski 0:01f31e923fe2 887 switch (state) {
Pawel Zarembski 0:01f31e923fe2 888 case RESET_HOLD:
Pawel Zarembski 0:01f31e923fe2 889 swd_set_target_reset(1);
Pawel Zarembski 0:01f31e923fe2 890 break;
Pawel Zarembski 0:01f31e923fe2 891
Pawel Zarembski 0:01f31e923fe2 892 case RESET_RUN:
Pawel Zarembski 0:01f31e923fe2 893 swd_set_target_reset(1);
Pawel Zarembski 0:01f31e923fe2 894 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 895 swd_set_target_reset(0);
Pawel Zarembski 0:01f31e923fe2 896 osDelay(2);
Pawel Zarembski 0:01f31e923fe2 897 swd_off();
Pawel Zarembski 0:01f31e923fe2 898 break;
Pawel Zarembski 0:01f31e923fe2 899
Pawel Zarembski 0:01f31e923fe2 900 case RESET_PROGRAM:
Pawel Zarembski 0:01f31e923fe2 901 if (!swd_init_debug()) {
Pawel Zarembski 0:01f31e923fe2 902 return 0;
Pawel Zarembski 0:01f31e923fe2 903 }
Pawel Zarembski 0:01f31e923fe2 904
Pawel Zarembski 0:01f31e923fe2 905 // Enable debug and halt the core (DHCSR <- 0xA05F0003)
Pawel Zarembski 0:01f31e923fe2 906 if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT)) {
Pawel Zarembski 0:01f31e923fe2 907 return 0;
Pawel Zarembski 0:01f31e923fe2 908 }
Pawel Zarembski 0:01f31e923fe2 909
Pawel Zarembski 0:01f31e923fe2 910 // Wait until core is halted
Pawel Zarembski 0:01f31e923fe2 911 do {
Pawel Zarembski 0:01f31e923fe2 912 if (!swd_read_word(DBG_HCSR, &val)) {
Pawel Zarembski 0:01f31e923fe2 913 return 0;
Pawel Zarembski 0:01f31e923fe2 914 }
Pawel Zarembski 0:01f31e923fe2 915 } while ((val & S_HALT) == 0);
Pawel Zarembski 0:01f31e923fe2 916
Pawel Zarembski 0:01f31e923fe2 917 // Enable halt on reset
Pawel Zarembski 0:01f31e923fe2 918 if (!swd_write_word(DBG_EMCR, VC_CORERESET)) {
Pawel Zarembski 0:01f31e923fe2 919 return 0;
Pawel Zarembski 0:01f31e923fe2 920 }
Pawel Zarembski 0:01f31e923fe2 921
Pawel Zarembski 0:01f31e923fe2 922 // Perform a soft reset
Pawel Zarembski 0:01f31e923fe2 923 if (!swd_write_word(NVIC_AIRCR, VECTKEY | soft_reset)) {
Pawel Zarembski 0:01f31e923fe2 924 return 0;
Pawel Zarembski 0:01f31e923fe2 925 }
Pawel Zarembski 0:01f31e923fe2 926
Pawel Zarembski 0:01f31e923fe2 927 break;
Pawel Zarembski 0:01f31e923fe2 928
Pawel Zarembski 0:01f31e923fe2 929 case NO_DEBUG:
Pawel Zarembski 0:01f31e923fe2 930 if (!swd_write_word(DBG_HCSR, DBGKEY)) {
Pawel Zarembski 0:01f31e923fe2 931 return 0;
Pawel Zarembski 0:01f31e923fe2 932 }
Pawel Zarembski 0:01f31e923fe2 933
Pawel Zarembski 0:01f31e923fe2 934 break;
Pawel Zarembski 0:01f31e923fe2 935
Pawel Zarembski 0:01f31e923fe2 936 case DEBUG:
Pawel Zarembski 0:01f31e923fe2 937 if (!JTAG2SWD()) {
Pawel Zarembski 0:01f31e923fe2 938 return 0;
Pawel Zarembski 0:01f31e923fe2 939 }
Pawel Zarembski 0:01f31e923fe2 940
Pawel Zarembski 0:01f31e923fe2 941 if (!swd_write_dp(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
Pawel Zarembski 0:01f31e923fe2 942 return 0;
Pawel Zarembski 0:01f31e923fe2 943 }
Pawel Zarembski 0:01f31e923fe2 944
Pawel Zarembski 0:01f31e923fe2 945 // Ensure CTRL/STAT register selected in DPBANKSEL
Pawel Zarembski 0:01f31e923fe2 946 if (!swd_write_dp(DP_SELECT, 0)) {
Pawel Zarembski 0:01f31e923fe2 947 return 0;
Pawel Zarembski 0:01f31e923fe2 948 }
Pawel Zarembski 0:01f31e923fe2 949
Pawel Zarembski 0:01f31e923fe2 950 // Power up
Pawel Zarembski 0:01f31e923fe2 951 if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
Pawel Zarembski 0:01f31e923fe2 952 return 0;
Pawel Zarembski 0:01f31e923fe2 953 }
Pawel Zarembski 0:01f31e923fe2 954
Pawel Zarembski 0:01f31e923fe2 955 // Enable debug
Pawel Zarembski 0:01f31e923fe2 956 if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
Pawel Zarembski 0:01f31e923fe2 957 return 0;
Pawel Zarembski 0:01f31e923fe2 958 }
Pawel Zarembski 0:01f31e923fe2 959
Pawel Zarembski 0:01f31e923fe2 960 break;
Pawel Zarembski 0:01f31e923fe2 961
Pawel Zarembski 0:01f31e923fe2 962 default:
Pawel Zarembski 0:01f31e923fe2 963 return 0;
Pawel Zarembski 0:01f31e923fe2 964 }
Pawel Zarembski 0:01f31e923fe2 965
Pawel Zarembski 0:01f31e923fe2 966 return 1;
Pawel Zarembski 0:01f31e923fe2 967 }
Pawel Zarembski 0:01f31e923fe2 968
Pawel Zarembski 0:01f31e923fe2 969 #endif