swd_host.c

00001 /**
00002  * @file    swd_host.c
00003  * @brief   Implementation of swd_host.h
00004  *
00005  * DAPLink Interface Firmware
00006  * Copyright (c) 2009-2019, ARM Limited, All Rights Reserved
00007  * Copyright 2019, Cypress Semiconductor Corporation 
00008  * or a subsidiary of Cypress Semiconductor Corporation.
00009  * SPDX-License-Identifier: Apache-2.0
00010  *
00011  * Licensed under the Apache License, Version 2.0 (the "License"); you may
00012  * not use this file except in compliance with the License.
00013  * You may obtain a copy of the License at
00014  *
00015  * http://www.apache.org/licenses/LICENSE-2.0
00016  *
00017  * Unless required by applicable law or agreed to in writing, software
00018  * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
00019  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00020  * See the License for the specific language governing permissions and
00021  * limitations under the License.
00022  */
00023 
00024 #ifndef TARGET_MCU_CORTEX_A
00025 #include "cmsis_os2.h"
00026 #include "target_config.h"
00027 #include "swd_host.h"
00028 #include "debug_cm.h"
00029 #include "DAP_config.h"
00030 #include "DAP.h"
00031 #include "target_family.h"
00032 #include "device.h"
00033 
00034 // Default NVIC and Core debug base addresses
00035 // TODO: Read these addresses from ROM.
00036 #define NVIC_Addr    (0xe000e000)
00037 #define DBG_Addr     (0xe000edf0)
00038 
00039 // AP CSW register, base value
00040 #define CSW_VALUE (CSW_RESERVED | CSW_MSTRDBG | CSW_HPROT | CSW_DBGSTAT | CSW_SADDRINC)
00041 
00042 #define DCRDR 0xE000EDF8
00043 #define DCRSR 0xE000EDF4
00044 #define DHCSR 0xE000EDF0
00045 #define REGWnR (1 << 16)
00046 
00047 #define MAX_SWD_RETRY 100//10
00048 #define MAX_TIMEOUT   1000000  // Timeout for syscalls on target
00049 
00050 // Use the CMSIS-Core definition if available.
00051 #if !defined(SCB_AIRCR_PRIGROUP_Pos)
00052 #define SCB_AIRCR_PRIGROUP_Pos              8U                                            /*!< SCB AIRCR: PRIGROUP Position */
00053 #define SCB_AIRCR_PRIGROUP_Msk             (7UL << SCB_AIRCR_PRIGROUP_Pos)                /*!< SCB AIRCR: PRIGROUP Mask */
00054 #endif
00055 
00056 typedef struct {
00057     uint32_t select;
00058     uint32_t csw;
00059 } DAP_STATE;
00060 
00061 typedef struct {
00062     uint32_t r[16];
00063     uint32_t xpsr;
00064 } DEBUG_STATE;
00065 
00066 static SWD_CONNECT_TYPE reset_connect = CONNECT_NORMAL;
00067 
00068 static DAP_STATE dap_state;
00069 static uint32_t  soft_reset = SYSRESETREQ;
00070 
00071 static uint32_t swd_get_apsel(uint32_t adr)
00072 {
00073     uint32_t apsel = target_get_apsel();
00074     if (!apsel)
00075         return adr & 0xff000000;
00076     else
00077         return apsel;
00078 }
00079 
00080 void swd_set_reset_connect(SWD_CONNECT_TYPE type)
00081 {
00082     reset_connect = type;
00083 }
00084 
00085 void int2array(uint8_t *res, uint32_t data, uint8_t len)
00086 {
00087     uint8_t i = 0;
00088 
00089     for (i = 0; i < len; i++) {
00090         res[i] = (data >> 8 * i) & 0xff;
00091     }
00092 }
00093 
00094 uint8_t swd_transfer_retry(uint32_t req, uint32_t *data)
00095 {
00096     uint8_t i, ack;
00097 
00098     for (i = 0; i < MAX_SWD_RETRY; i++) {
00099         ack = SWD_Transfer(req, data);
00100 
00101         // if ack != WAIT
00102         if (ack != DAP_TRANSFER_WAIT) {
00103             return ack;
00104         }
00105     }
00106 
00107     return ack;
00108 }
00109 
00110 void swd_set_soft_reset(uint32_t soft_reset_type)
00111 {
00112     soft_reset = soft_reset_type;
00113 }
00114 
00115 uint8_t swd_init(void)
00116 {
00117     //TODO - DAP_Setup puts GPIO pins in a hi-z state which can
00118     //       cause problems on re-init.  This needs to be investigated
00119     //       and fixed.
00120     DAP_Setup();
00121     PORT_SWD_SETUP();
00122     return 1;
00123 }
00124 
00125 uint8_t swd_off(void)
00126 {
00127     PORT_OFF();
00128     return 1;
00129 }
00130 
00131 uint8_t swd_clear_errors(void)
00132 {
00133     if (!swd_write_dp(DP_ABORT, STKCMPCLR | STKERRCLR | WDERRCLR | ORUNERRCLR)) {
00134         return 0;
00135     }
00136     return 1;
00137 }
00138 
00139 // Read debug port register.
00140 uint8_t swd_read_dp(uint8_t adr, uint32_t *val)
00141 {
00142     uint32_t tmp_in;
00143     uint8_t tmp_out[4];
00144     uint8_t ack;
00145     uint32_t tmp;
00146     tmp_in = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(adr);
00147     ack = swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
00148     *val = 0;
00149     tmp = tmp_out[3];
00150     *val |= (tmp << 24);
00151     tmp = tmp_out[2];
00152     *val |= (tmp << 16);
00153     tmp = tmp_out[1];
00154     *val |= (tmp << 8);
00155     tmp = tmp_out[0];
00156     *val |= (tmp << 0);
00157     return (ack == 0x01);
00158 }
00159 
00160 // Write debug port register
00161 uint8_t swd_write_dp(uint8_t adr, uint32_t val)
00162 {
00163     uint32_t req;
00164     uint8_t data[4];
00165     uint8_t ack;
00166 
00167     //check if the right bank is already selected
00168     if ((adr == DP_SELECT) && (dap_state.select == val)) {
00169         return 1;
00170     }
00171 
00172     req = SWD_REG_DP | SWD_REG_W | SWD_REG_ADR(adr);
00173     int2array(data, val, 4);
00174     ack = swd_transfer_retry(req, (uint32_t *)data);
00175     if ((ack == DAP_TRANSFER_OK) && (adr == DP_SELECT)) {
00176         dap_state.select = val;
00177     }
00178     return (ack == 0x01);
00179 }
00180 
00181 // Read access port register.
00182 uint8_t swd_read_ap(uint32_t adr, uint32_t *val)
00183 {
00184     uint8_t tmp_in, ack;
00185     uint8_t tmp_out[4];
00186     uint32_t tmp;
00187     uint32_t apsel = swd_get_apsel(adr);
00188     uint32_t bank_sel = adr & APBANKSEL;
00189 
00190     if (!swd_write_dp(DP_SELECT, apsel | bank_sel)) {
00191         return 0;
00192     }
00193 
00194     tmp_in = SWD_REG_AP | SWD_REG_R | SWD_REG_ADR(adr);
00195     // first dummy read
00196     swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
00197     ack = swd_transfer_retry(tmp_in, (uint32_t *)tmp_out);
00198     *val = 0;
00199     tmp = tmp_out[3];
00200     *val |= (tmp << 24);
00201     tmp = tmp_out[2];
00202     *val |= (tmp << 16);
00203     tmp = tmp_out[1];
00204     *val |= (tmp << 8);
00205     tmp = tmp_out[0];
00206     *val |= (tmp << 0);
00207     return (ack == 0x01);
00208 }
00209 
00210 // Write access port register
00211 uint8_t swd_write_ap(uint32_t adr, uint32_t val)
00212 {
00213     uint8_t data[4];
00214     uint8_t req, ack;
00215     uint32_t apsel = swd_get_apsel(adr);
00216     uint32_t bank_sel = adr & APBANKSEL;
00217 
00218     if (!swd_write_dp(DP_SELECT, apsel | bank_sel)) {
00219         return 0;
00220     }
00221 
00222     switch (adr) {
00223         case AP_CSW:
00224             if (dap_state.csw == val) {
00225                 return 1;
00226             }
00227 
00228             dap_state.csw = val;
00229             break;
00230 
00231         default:
00232             break;
00233     }
00234 
00235     req = SWD_REG_AP | SWD_REG_W | SWD_REG_ADR(adr);
00236     int2array(data, val, 4);
00237 
00238     if (swd_transfer_retry(req, (uint32_t *)data) != 0x01) {
00239         return 0;
00240     }
00241 
00242     req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
00243     ack = swd_transfer_retry(req, NULL);
00244     return (ack == 0x01);
00245 }
00246 
00247 
00248 // Write 32-bit word aligned values to target memory using address auto-increment.
00249 // size is in bytes.
00250 static uint8_t swd_write_block(uint32_t address, uint8_t *data, uint32_t size)
00251 {
00252     uint8_t tmp_in[4], req;
00253     uint32_t size_in_words;
00254     uint32_t i, ack;
00255 
00256     if (size == 0) {
00257         return 0;
00258     }
00259 
00260     size_in_words = size / 4;
00261 
00262     // CSW register
00263     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
00264         return 0;
00265     }
00266 
00267     // TAR write
00268     req = SWD_REG_AP | SWD_REG_W | (1 << 2);
00269     int2array(tmp_in, address, 4);
00270 
00271     if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
00272         return 0;
00273     }
00274 
00275     // DRW write
00276     req = SWD_REG_AP | SWD_REG_W | (3 << 2);
00277 
00278     for (i = 0; i < size_in_words; i++) {
00279         if (swd_transfer_retry(req, (uint32_t *)data) != 0x01) {
00280             return 0;
00281         }
00282 
00283         data += 4;
00284     }
00285 
00286     // dummy read
00287     req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
00288     ack = swd_transfer_retry(req, NULL);
00289     return (ack == 0x01);
00290 }
00291 
00292 // Read 32-bit word aligned values from target memory using address auto-increment.
00293 // size is in bytes.
00294 static uint8_t swd_read_block(uint32_t address, uint8_t *data, uint32_t size)
00295 {
00296     uint8_t tmp_in[4], req, ack;
00297     uint32_t size_in_words;
00298     uint32_t i;
00299 
00300     if (size == 0) {
00301         return 0;
00302     }
00303 
00304     size_in_words = size / 4;
00305 
00306     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
00307         return 0;
00308     }
00309 
00310     // TAR write
00311     req = SWD_REG_AP | SWD_REG_W | AP_TAR;
00312     int2array(tmp_in, address, 4);
00313 
00314     if (swd_transfer_retry(req, (uint32_t *)tmp_in) != DAP_TRANSFER_OK) {
00315         return 0;
00316     }
00317 
00318     // read data
00319     req = SWD_REG_AP | SWD_REG_R | AP_DRW;
00320 
00321     // initiate first read, data comes back in next read
00322     if (swd_transfer_retry(req, NULL) != 0x01) {
00323         return 0;
00324     }
00325 
00326     for (i = 0; i < (size_in_words - 1); i++) {
00327         if (swd_transfer_retry(req, (uint32_t *)data) != DAP_TRANSFER_OK) {
00328             return 0;
00329         }
00330 
00331         data += 4;
00332     }
00333 
00334     // read last word
00335     req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
00336     ack = swd_transfer_retry(req, (uint32_t *)data);
00337     return (ack == 0x01);
00338 }
00339 
00340 // Read target memory.
00341 static uint8_t swd_read_data(uint32_t addr, uint32_t *val)
00342 {
00343     uint8_t tmp_in[4];
00344     uint8_t tmp_out[4];
00345     uint8_t req, ack;
00346     uint32_t tmp;
00347     // put addr in TAR register
00348     int2array(tmp_in, addr, 4);
00349     req = SWD_REG_AP | SWD_REG_W | (1 << 2);
00350 
00351     if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
00352         return 0;
00353     }
00354 
00355     // read data
00356     req = SWD_REG_AP | SWD_REG_R | (3 << 2);
00357 
00358     if (swd_transfer_retry(req, (uint32_t *)tmp_out) != 0x01) {
00359         return 0;
00360     }
00361 
00362     // dummy read
00363     req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
00364     ack = swd_transfer_retry(req, (uint32_t *)tmp_out);
00365     *val = 0;
00366     tmp = tmp_out[3];
00367     *val |= (tmp << 24);
00368     tmp = tmp_out[2];
00369     *val |= (tmp << 16);
00370     tmp = tmp_out[1];
00371     *val |= (tmp << 8);
00372     tmp = tmp_out[0];
00373     *val |= (tmp << 0);
00374     return (ack == 0x01);
00375 }
00376 
00377 // Write target memory.
00378 static uint8_t swd_write_data(uint32_t address, uint32_t data)
00379 {
00380     uint8_t tmp_in[4];
00381     uint8_t req, ack;
00382     // put addr in TAR register
00383     int2array(tmp_in, address, 4);
00384     req = SWD_REG_AP | SWD_REG_W | (1 << 2);
00385 
00386     if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
00387         return 0;
00388     }
00389 
00390     // write data
00391     int2array(tmp_in, data, 4);
00392     req = SWD_REG_AP | SWD_REG_W | (3 << 2);
00393 
00394     if (swd_transfer_retry(req, (uint32_t *)tmp_in) != 0x01) {
00395         return 0;
00396     }
00397 
00398     // dummy read
00399     req = SWD_REG_DP | SWD_REG_R | SWD_REG_ADR(DP_RDBUFF);
00400     ack = swd_transfer_retry(req, NULL);
00401     return (ack == 0x01) ? 1 : 0;
00402 }
00403 
00404 // Read 32-bit word from target memory.
00405 uint8_t swd_read_word(uint32_t addr, uint32_t *val)
00406 {
00407     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
00408         return 0;
00409     }
00410 
00411     if (!swd_read_data(addr, val)) {
00412         return 0;
00413     }
00414 
00415     return 1;
00416 }
00417 
00418 // Write 32-bit word to target memory.
00419 uint8_t swd_write_word(uint32_t addr, uint32_t val)
00420 {
00421     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE32)) {
00422         return 0;
00423     }
00424 
00425     if (!swd_write_data(addr, val)) {
00426         return 0;
00427     }
00428 
00429     return 1;
00430 }
00431 
00432 // Read 8-bit byte from target memory.
00433 uint8_t swd_read_byte(uint32_t addr, uint8_t *val)
00434 {
00435     uint32_t tmp;
00436 
00437     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
00438         return 0;
00439     }
00440 
00441     if (!swd_read_data(addr, &tmp)) {
00442         return 0;
00443     }
00444 
00445     *val = (uint8_t)(tmp >> ((addr & 0x03) << 3));
00446     return 1;
00447 }
00448 
00449 // Write 8-bit byte to target memory.
00450 uint8_t swd_write_byte(uint32_t addr, uint8_t val)
00451 {
00452     uint32_t tmp;
00453 
00454     if (!swd_write_ap(AP_CSW, CSW_VALUE | CSW_SIZE8)) {
00455         return 0;
00456     }
00457 
00458     tmp = val << ((addr & 0x03) << 3);
00459 
00460     if (!swd_write_data(addr, tmp)) {
00461         return 0;
00462     }
00463 
00464     return 1;
00465 }
00466 
00467 // Read unaligned data from target memory.
00468 // size is in bytes.
00469 uint8_t swd_read_memory(uint32_t address, uint8_t *data, uint32_t size)
00470 {
00471     uint32_t n;
00472 
00473     // Read bytes until word aligned
00474     while ((size > 0) && (address & 0x3)) {
00475         if (!swd_read_byte(address, data)) {
00476             return 0;
00477         }
00478 
00479         address++;
00480         data++;
00481         size--;
00482     }
00483 
00484     // Read word aligned blocks
00485     while (size > 3) {
00486         // Limit to auto increment page size
00487         n = TARGET_AUTO_INCREMENT_PAGE_SIZE - (address & (TARGET_AUTO_INCREMENT_PAGE_SIZE - 1));
00488 
00489         if (size < n) {
00490             n = size & 0xFFFFFFFC; // Only count complete words remaining
00491         }
00492 
00493         if (!swd_read_block(address, data, n)) {
00494             return 0;
00495         }
00496 
00497         address += n;
00498         data += n;
00499         size -= n;
00500     }
00501 
00502     // Read remaining bytes
00503     while (size > 0) {
00504         if (!swd_read_byte(address, data)) {
00505             return 0;
00506         }
00507 
00508         address++;
00509         data++;
00510         size--;
00511     }
00512 
00513     return 1;
00514 }
00515 
00516 // Write unaligned data to target memory.
00517 // size is in bytes.
00518 uint8_t swd_write_memory(uint32_t address, uint8_t *data, uint32_t size)
00519 {
00520     uint32_t n = 0;
00521 
00522     // Write bytes until word aligned
00523     while ((size > 0) && (address & 0x3)) {
00524         if (!swd_write_byte(address, *data)) {
00525             return 0;
00526         }
00527 
00528         address++;
00529         data++;
00530         size--;
00531     }
00532 
00533     // Write word aligned blocks
00534     while (size > 3) {
00535         // Limit to auto increment page size
00536         n = TARGET_AUTO_INCREMENT_PAGE_SIZE - (address & (TARGET_AUTO_INCREMENT_PAGE_SIZE - 1));
00537 
00538         if (size < n) {
00539             n = size & 0xFFFFFFFC; // Only count complete words remaining
00540         }
00541 
00542         if (!swd_write_block(address, data, n)) {
00543             return 0;
00544         }
00545 
00546         address += n;
00547         data += n;
00548         size -= n;
00549     }
00550 
00551     // Write remaining bytes
00552     while (size > 0) {
00553         if (!swd_write_byte(address, *data)) {
00554             return 0;
00555         }
00556 
00557         address++;
00558         data++;
00559         size--;
00560     }
00561 
00562     return 1;
00563 }
00564 
00565 // Execute system call.
00566 static uint8_t swd_write_debug_state(DEBUG_STATE *state)
00567 {
00568     uint32_t i, status;
00569 
00570     if (!swd_write_dp(DP_SELECT, 0)) {
00571         return 0;
00572     }
00573 
00574     // R0, R1, R2, R3
00575     for (i = 0; i < 4; i++) {
00576         if (!swd_write_core_register(i, state->r[i])) {
00577             return 0;
00578         }
00579     }
00580 
00581     // R9
00582     if (!swd_write_core_register(9, state->r[9])) {
00583         return 0;
00584     }
00585 
00586     // R13, R14, R15
00587     for (i = 13; i < 16; i++) {
00588         if (!swd_write_core_register(i, state->r[i])) {
00589             return 0;
00590         }
00591     }
00592 
00593     // xPSR
00594     if (!swd_write_core_register(16, state->xpsr)) {
00595         return 0;
00596     }
00597 
00598     if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_MASKINTS | C_HALT)) {
00599         return 0;
00600     }
00601 
00602     if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_MASKINTS)) {
00603         return 0;
00604     }
00605 
00606     // check status
00607     if (!swd_read_dp(DP_CTRL_STAT, &status)) {
00608         return 0;
00609     }
00610 
00611     if (status & (STICKYERR | WDATAERR)) {
00612         return 0;
00613     }
00614 
00615     return 1;
00616 }
00617 
00618 uint8_t swd_read_core_register(uint32_t n, uint32_t *val)
00619 {
00620     int i = 0, timeout = 100;
00621 
00622     if (!swd_write_word(DCRSR, n)) {
00623         return 0;
00624     }
00625 
00626     // wait for S_REGRDY
00627     for (i = 0; i < timeout; i++) {
00628         if (!swd_read_word(DHCSR, val)) {
00629             return 0;
00630         }
00631 
00632         if (*val & S_REGRDY) {
00633             break;
00634         }
00635     }
00636 
00637     if (i == timeout) {
00638         return 0;
00639     }
00640 
00641     if (!swd_read_word(DCRDR, val)) {
00642         return 0;
00643     }
00644 
00645     return 1;
00646 }
00647 
00648 uint8_t swd_write_core_register(uint32_t n, uint32_t val)
00649 {
00650     int i = 0, timeout = 100;
00651 
00652     if (!swd_write_word(DCRDR, val)) {
00653         return 0;
00654     }
00655 
00656     if (!swd_write_word(DCRSR, n | REGWnR)) {
00657         return 0;
00658     }
00659 
00660     // wait for S_REGRDY
00661     for (i = 0; i < timeout; i++) {
00662         if (!swd_read_word(DHCSR, &val)) {
00663             return 0;
00664         }
00665 
00666         if (val & S_REGRDY) {
00667             return 1;
00668         }
00669     }
00670 
00671     return 0;
00672 }
00673 
00674 static uint8_t swd_wait_until_halted(void)
00675 {
00676     // Wait for target to stop
00677     uint32_t val, i, timeout = MAX_TIMEOUT;
00678 
00679     for (i = 0; i < timeout; i++) {
00680         if (!swd_read_word(DBG_HCSR, &val)) {
00681             return 0;
00682         }
00683 
00684         if (val & S_HALT) {
00685             return 1;
00686         }
00687     }
00688 
00689     return 0;
00690 }
00691 
00692 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)
00693 {
00694     DEBUG_STATE state = {{0}, 0};
00695     // Call flash algorithm function on target and wait for result.
00696     state.r[0]     = arg1;                   // R0: Argument 1
00697     state.r[1]     = arg2;                   // R1: Argument 2
00698     state.r[2]     = arg3;                   // R2: Argument 3
00699     state.r[3]     = arg4;                   // R3: Argument 4
00700     state.r[9]     = sysCallParam->static_base;    // SB: Static Base
00701     state.r[13]    = sysCallParam->stack_pointer;  // SP: Stack Pointer
00702     state.r[14]    = sysCallParam->breakpoint;     // LR: Exit Point
00703     state.r[15]    = entry;                        // PC: Entry Point
00704     state.xpsr     = 0x01000000;          // xPSR: T = 1, ISR = 0
00705 
00706     if (!swd_write_debug_state(&state)) {
00707         return 0;
00708     }
00709 
00710     if (!swd_wait_until_halted()) {
00711         return 0;
00712     }
00713 
00714     if (!swd_read_core_register(0, &state.r[0])) {
00715         return 0;
00716     }
00717 
00718     //remove the C_MASKINTS
00719     if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT)) {
00720         return 0;
00721     }
00722 
00723     if ( return_type == FLASHALGO_RETURN_POINTER ) {
00724         // Flash verify functions return pointer to byte following the buffer if successful.
00725         if (state.r[0] != (arg1 + arg2)) {
00726             return 0;
00727         }
00728     }
00729     else {
00730         // Flash functions return 0 if successful.
00731         if (state.r[0] != 0) {
00732             return 0;
00733         }
00734     }
00735 
00736     return 1;
00737 }
00738 
00739 // SWD Reset
00740 static uint8_t swd_reset(void)
00741 {
00742     uint8_t tmp_in[8];
00743     uint8_t i = 0;
00744 
00745     for (i = 0; i < 8; i++) {
00746         tmp_in[i] = 0xff;
00747     }
00748 
00749     SWJ_Sequence(51, tmp_in);
00750     return 1;
00751 }
00752 
00753 // SWD Switch
00754 static uint8_t swd_switch(uint16_t val)
00755 {
00756     uint8_t tmp_in[2];
00757     tmp_in[0] = val & 0xff;
00758     tmp_in[1] = (val >> 8) & 0xff;
00759     SWJ_Sequence(16, tmp_in);
00760     return 1;
00761 }
00762 
00763 // SWD Read ID
00764 static uint8_t swd_read_idcode(uint32_t *id)
00765 {
00766     uint8_t tmp_in[1];
00767     uint8_t tmp_out[4];
00768     tmp_in[0] = 0x00;
00769     SWJ_Sequence(8, tmp_in);
00770 
00771     if (swd_read_dp(0, (uint32_t *)tmp_out) != 0x01) {
00772         return 0;
00773     }
00774 
00775     *id = (tmp_out[3] << 24) | (tmp_out[2] << 16) | (tmp_out[1] << 8) | tmp_out[0];
00776     return 1;
00777 }
00778 
00779 
00780 uint8_t JTAG2SWD()
00781 {
00782     uint32_t tmp = 0;
00783 
00784     if (!swd_reset()) {
00785         return 0;
00786     }
00787 
00788     if (!swd_switch(0xE79E)) {
00789         return 0;
00790     }
00791 
00792     if (!swd_reset()) {
00793         return 0;
00794     }
00795 
00796     if (!swd_read_idcode(&tmp)) {
00797         return 0;
00798     }
00799 
00800     return 1;
00801 }
00802 
00803 uint8_t swd_init_debug(void)
00804 {
00805     uint32_t tmp = 0;
00806     int i = 0;
00807     int timeout = 100;
00808     // init dap state with fake values
00809     dap_state.select = 0xffffffff;
00810     dap_state.csw = 0xffffffff;
00811 
00812     int8_t retries = 4;
00813     int8_t do_abort = 0;
00814     do {
00815         if (do_abort) {
00816             //do an abort on stale target, then reset the device
00817             swd_write_dp(DP_ABORT, DAPABORT);
00818             swd_set_target_reset(1);
00819             osDelay(2);
00820             swd_set_target_reset(0);
00821             osDelay(2);
00822             do_abort = 0;
00823         }
00824         swd_init();
00825         // call a target dependant function
00826         // this function can do several stuff before really
00827         // initing the debug
00828         if (g_target_family && g_target_family->target_before_init_debug ) {
00829             g_target_family->target_before_init_debug ();
00830         }
00831 
00832         if (!JTAG2SWD()) {
00833             do_abort = 1;
00834             continue;
00835         }
00836 
00837         if (!swd_clear_errors()) {
00838             do_abort = 1;
00839             continue;
00840         }
00841 
00842         if (!swd_write_dp(DP_SELECT, 0)) {
00843             do_abort = 1;
00844             continue;
00845 
00846         }
00847 
00848         // Power up
00849         if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
00850             do_abort = 1;
00851             continue;
00852         }
00853 
00854         for (i = 0; i < timeout; i++) {
00855             if (!swd_read_dp(DP_CTRL_STAT, &tmp)) {
00856                 do_abort = 1;
00857                 break;
00858             }
00859             if ((tmp & (CDBGPWRUPACK | CSYSPWRUPACK)) == (CDBGPWRUPACK | CSYSPWRUPACK)) {
00860                 // Break from loop if powerup is complete
00861                 break;
00862             }
00863         }
00864         if ((i == timeout) || (do_abort == 1)) {
00865             // Unable to powerup DP
00866             do_abort = 1;
00867             continue;
00868         }
00869 
00870         if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ | TRNNORMAL | MASKLANE)) {
00871             do_abort = 1;
00872             continue;
00873         }
00874 
00875         // call a target dependant function:
00876         // some target can enter in a lock state
00877         // this function can unlock these targets
00878         if (g_target_family && g_target_family->target_unlock_sequence ) {
00879             g_target_family->target_unlock_sequence ();
00880         }
00881 
00882         if (!swd_write_dp(DP_SELECT, 0)) {
00883             do_abort = 1;
00884             continue;
00885         }
00886 
00887         return 1;
00888 
00889     } while (--retries > 0);
00890 
00891     return 0;
00892 }
00893 
00894 uint8_t swd_set_target_state_hw(target_state_t state)
00895 {
00896     uint32_t val;
00897     int8_t ap_retries = 2;
00898     /* Calling swd_init prior to entering RUN state causes operations to fail. */
00899     if (state != RUN) {
00900         swd_init();
00901     }
00902 
00903     switch (state) {
00904         case RESET_HOLD:
00905             swd_set_target_reset(1);
00906             break;
00907 
00908         case RESET_RUN:
00909             swd_set_target_reset(1);
00910             osDelay(2);
00911             swd_set_target_reset(0);
00912             osDelay(2);
00913             swd_off();
00914             break;
00915 
00916         case RESET_PROGRAM:
00917             if (!swd_init_debug()) {
00918                 return 0;
00919             }
00920 
00921             if (reset_connect == CONNECT_UNDER_RESET) {
00922                 // Assert reset
00923                 swd_set_target_reset(1);
00924                 osDelay(2);
00925             }
00926 
00927             // Enable debug
00928             while(swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN) == 0) {
00929                 if( --ap_retries <=0 )
00930                     return 0;
00931                 // Target is in invalid state?
00932                 swd_set_target_reset(1);
00933                 osDelay(2);
00934                 swd_set_target_reset(0);
00935                 osDelay(2);
00936             }
00937 
00938             // Enable halt on reset
00939             if (!swd_write_word(DBG_EMCR, VC_CORERESET)) {
00940                 return 0;
00941             }
00942 
00943             if (reset_connect == CONNECT_NORMAL) {
00944                 // Assert reset
00945                 swd_set_target_reset(1);
00946                 osDelay(2);
00947             }
00948 
00949             // Deassert reset
00950             swd_set_target_reset(0);
00951             osDelay(2);
00952 
00953             do {
00954                 if (!swd_read_word(DBG_HCSR, &val)) {
00955                     return 0;
00956                 }
00957             } while ((val & S_HALT) == 0);
00958 
00959             // Disable halt on reset
00960             if (!swd_write_word(DBG_EMCR, 0)) {
00961                 return 0;
00962             }
00963 
00964             break;
00965 
00966         case NO_DEBUG:
00967             if (!swd_write_word(DBG_HCSR, DBGKEY)) {
00968                 return 0;
00969             }
00970 
00971             break;
00972 
00973         case DEBUG:
00974             if (!JTAG2SWD()) {
00975                 return 0;
00976             }
00977 
00978             if (!swd_clear_errors()) {
00979                 return 0;
00980             }
00981 
00982             // Ensure CTRL/STAT register selected in DPBANKSEL
00983             if (!swd_write_dp(DP_SELECT, 0)) {
00984                 return 0;
00985             }
00986 
00987             // Power up
00988             if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
00989                 return 0;
00990             }
00991 
00992             // Enable debug
00993             if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
00994                 return 0;
00995             }
00996 
00997             break;
00998 
00999         case HALT:
01000             if (!swd_init_debug()) {
01001                 return 0;
01002             }
01003 
01004             // Enable debug and halt the core (DHCSR <- 0xA05F0003)
01005             if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT)) {
01006                 return 0;
01007             }
01008 
01009             // Wait until core is halted
01010             do {
01011                 if (!swd_read_word(DBG_HCSR, &val)) {
01012                     return 0;
01013                 }
01014             } while ((val & S_HALT) == 0);
01015             break;
01016 
01017         case RUN:
01018             if (!swd_write_word(DBG_HCSR, DBGKEY)) {
01019                 return 0;
01020             }
01021             swd_off();
01022             break;
01023 
01024         case POST_FLASH_RESET:
01025             // This state should be handled in target_reset.c, nothing needs to be done here.
01026             break;
01027 
01028         default:
01029             return 0;
01030     }
01031 
01032     return 1;
01033 }
01034 
01035 uint8_t swd_set_target_state_sw(target_state_t state)
01036 {
01037     uint32_t val;
01038     int8_t ap_retries = 2;
01039     /* Calling swd_init prior to enterring RUN state causes operations to fail. */
01040     if (state != RUN) {
01041         swd_init();
01042     }
01043 
01044     switch (state) {
01045         case RESET_HOLD:
01046             swd_set_target_reset(1);
01047             break;
01048 
01049         case RESET_RUN:
01050             swd_set_target_reset(1);
01051             osDelay(2);
01052             swd_set_target_reset(0);
01053             osDelay(2);
01054             swd_off();
01055             break;
01056 
01057         case RESET_PROGRAM:
01058             if (!swd_init_debug()) {
01059                 return 0;
01060             }
01061 
01062             // Enable debug and halt the core (DHCSR <- 0xA05F0003)
01063             while (swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT) == 0) {
01064                 if ( --ap_retries <=0 ) {
01065                     return 0;
01066                 }
01067                 // Target is in invalid state?
01068                 swd_set_target_reset(1);
01069                 osDelay(2);
01070                 swd_set_target_reset(0);
01071                 osDelay(2);
01072             }
01073 
01074             // Wait until core is halted
01075             do {
01076                 if (!swd_read_word(DBG_HCSR, &val)) {
01077                     return 0;
01078                 }
01079             } while ((val & S_HALT) == 0);
01080 
01081             // Enable halt on reset
01082             if (!swd_write_word(DBG_EMCR, VC_CORERESET)) {
01083                 return 0;
01084             }
01085 
01086             // Perform a soft reset
01087             if (!swd_read_word(NVIC_AIRCR, &val)) {
01088                 return 0;
01089             }
01090 
01091             if (!swd_write_word(NVIC_AIRCR, VECTKEY | (val & SCB_AIRCR_PRIGROUP_Msk) | soft_reset)) {
01092                 return 0;
01093             }
01094 
01095             osDelay(2);
01096 
01097             do {
01098                 if (!swd_read_word(DBG_HCSR, &val)) {
01099                     return 0;
01100                 }
01101             } while ((val & S_HALT) == 0);
01102 
01103             // Disable halt on reset
01104             if (!swd_write_word(DBG_EMCR, 0)) {
01105                 return 0;
01106             }
01107 
01108             break;
01109 
01110         case NO_DEBUG:
01111             if (!swd_write_word(DBG_HCSR, DBGKEY)) {
01112                 return 0;
01113             }
01114 
01115             break;
01116 
01117         case DEBUG:
01118             if (!JTAG2SWD()) {
01119                 return 0;
01120             }
01121 
01122             if (!swd_clear_errors()) {
01123                 return 0;
01124             }
01125 
01126             // Ensure CTRL/STAT register selected in DPBANKSEL
01127             if (!swd_write_dp(DP_SELECT, 0)) {
01128                 return 0;
01129             }
01130 
01131             // Power up
01132             if (!swd_write_dp(DP_CTRL_STAT, CSYSPWRUPREQ | CDBGPWRUPREQ)) {
01133                 return 0;
01134             }
01135 
01136             // Enable debug
01137             if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN)) {
01138                 return 0;
01139             }
01140 
01141             break;
01142 
01143         case HALT:
01144             if (!swd_init_debug()) {
01145                 return 0;
01146             }
01147 
01148             // Enable debug and halt the core (DHCSR <- 0xA05F0003)
01149             if (!swd_write_word(DBG_HCSR, DBGKEY | C_DEBUGEN | C_HALT)) {
01150                 return 0;
01151             }
01152 
01153             // Wait until core is halted
01154             do {
01155                 if (!swd_read_word(DBG_HCSR, &val)) {
01156                     return 0;
01157                 }
01158             } while ((val & S_HALT) == 0);
01159             break;
01160 
01161         case RUN:
01162             if (!swd_write_word(DBG_HCSR, DBGKEY)) {
01163                 return 0;
01164             }
01165             swd_off();
01166             break;
01167 
01168         case POST_FLASH_RESET:
01169             // This state should be handled in target_reset.c, nothing needs to be done here.
01170             break;
01171 
01172         default:
01173             return 0;
01174     }
01175 
01176     return 1;
01177 }
01178 #endif