123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510 |
- /*
- * This Source Code Form is subject to the terms of the MIT License.
- * If a copy of the MIT License was not distributed with this file,
- * you can obtain one at https://opensource.org/licenses/MIT
- */
- #include "main.h"
- #include "system.h"
- #include "swd.h"
- #include "target.h"
- #define MWAIT __asm__ __volatile__( \
- ".syntax unified \n" \
- " movs r0, #0x30 \n" \
- "1: subs r0, #1 \n" \
- " bne 1b \n" \
- ".syntax divided" : : : \
- "cc", "r0")
- #define N_READ_TURN (3u)
- static uint8_t swdParity( uint8_t const * data, uint8_t const len );
- static void swdDatasend( uint8_t const * data, uint8_t const len );
- static void swdDataIdle( void );
- static void swdDataPP( void );
- static void swdTurnaround( void );
- static void swdReset( void );
- static void swdDataRead( uint8_t * const data, uint8_t const len );
- static void swdBuildHeader( swdAccessDirection_t const adir, swdPortSelect_t const portSel, uint8_t const A32, uint8_t * const header);
- static swdStatus_t swdReadPacket( swdPortSelect_t const portSel, uint8_t const A32, uint32_t * const data );
- static swdStatus_t swdWritePacket( swdPortSelect_t const portSel, uint8_t const A32, uint32_t const data );
- static swdStatus_t swdReadAP0( uint32_t * const data );
- #ifdef UNUSED_EXPERIMENTAL
- static swdStatus_t swdReadDPCtrl( uint32_t * const data );
- static swdStatus_t swdReadAPCtrl( uint32_t * const data );
- static swdStatus_t swdReadWIREMODE( uint32_t * const data );
- static swdStatus_t swdReadDHCSR( uint32_t * const data );
- static swdStatus_t swdWriteAHBAddr( uint32_t const addr, uint32_t const data );
- static swdStatus_t swdCoreHalt( void );
- static swdStatus_t swdGetRegister( uint8_t const regId, uint32_t * const data );
- #endif
- static uint8_t swdParity( uint8_t const * data, uint8_t const len )
- {
- uint8_t par = 0u;
- uint8_t cdata = 0u;
- uint8_t i = 0u;
- for (i=0u; i<len; ++i)
- {
- if ((i & 0x07u) == 0u)
- {
- cdata = *data;
- ++data;
- }
- par ^= (cdata & 0x01u);
- cdata >>= 1u;
- }
- return par;
- }
- static void swdDatasend( uint8_t const * data, uint8_t const len )
- {
- uint8_t cdata = 0u;
- uint8_t i = 0u;
- for (i=0u; i<len; ++i)
- {
- if ((i & 0x07u) == 0x00u)
- {
- cdata = *data;
- ++data;
- }
- if ((cdata & 0x01u) == 0x01u)
- {
- GPIOA_BSRR |= (1 << PIN_SWDIO);
- }
- else
- {
- GPIOA_BSRR |= (1 << (PIN_SWDIO + 16));
- }
- MWAIT;
- GPIOA_BSRR |= (1 << PIN_SWCLK);
- MWAIT;
- GPIOA_BSRR |= (1 << (PIN_SWCLK + 16));
- cdata >>= 1u;
- MWAIT;
- }
- return ;
- }
- static void swdDataIdle( void )
- {
- GPIOA_BSRR |= (1 << PIN_SWDIO);
- MWAIT;
- GPIOA_MODE &= ~(0x03u << (PIN_SWDIO * 2));
- MWAIT;
- return ;
- }
- static void swdDataPP( void )
- {
- MWAIT;
- GPIOA_BSRR |= (1 << (PIN_SWDIO + 16));
- GPIOA_MODE |= (1 << (PIN_SWDIO * 2));
- MWAIT;
- return ;
- }
- static void swdTurnaround( void )
- {
- GPIOA_BSRR |= (1 << (PIN_SWCLK));
- MWAIT;
- GPIOA_BSRR |= (1 << (PIN_SWCLK + 16));
- MWAIT;
- return ;
- }
- static void swdDataRead( uint8_t * const data, uint8_t const len )
- {
- uint8_t i = 0u;
- uint8_t cdata = 0u;
- MWAIT;
- swdDataIdle();
- MWAIT;
- for (i=0u; i<len; ++i)
- {
- cdata >>= 1u;
- cdata |= (GPIOA_IDR & (0x01u << (PIN_SWDIO))) ? 0x80u : 0x00u;
- data[(((len + 7u) >> 3u) - (i >> 3u)) - 1u] = cdata;
- swdTurnaround();
- /* clear buffer after reading 8 bytes */
- if ((i & 0x07u) == 0x07u)
- {
- cdata = 0u;
- }
- }
- return ;
- }
- static void swdReset( void )
- {
- uint8_t i = 0u;
- MWAIT;
- GPIOA_ODR |= (1 << PIN_SWDIO) | (1 << PIN_SWCLK);
- MWAIT;
- /* Switch from JTAG to SWD mode. Not required for SWD-only devices (STM32F0x). */
- #ifdef DO_JTAG_RESET
- /* 50 clk+x */
- for (i=0u; i < (50u + 10u); ++i)
- {
- swdTurnaround();
- }
- uint8_t send1[] = {0u, 1u, 1u, 1u, 1u, 0u, 0u, 1u, 1u, 1u, 1u, 0u, 0u, 1u, 1u, 1u};
- /* send 0111 1001 1110 0111 */
- for (i = 0u; i < 16u; ++i)
- {
- if (send1[i])
- GPIOD_BSRR |= (1 << PIN_SWDIO);
- else
- GPIOD_BSRR |= (1 << (PIN_SWDIO + 16));
- MWAIT;
- swdTurnaround();
- }
- #endif
- /* 50 clk+x */
- for (i = 0u; i < (50u + 10u); ++i)
- {
- swdTurnaround();
- }
- GPIOA_BSRR |= (1 << (PIN_SWDIO + 16));
- for (i = 0u; i < 3u; ++i)
- {
- swdTurnaround();
- }
- return ;
- }
- static void swdBuildHeader( swdAccessDirection_t const adir, swdPortSelect_t const portSel, uint8_t const A32, uint8_t * const header)
- {
- if (portSel == swdPortSelectAP)
- {
- *header |= 0x02u; /* Access AP */
- }
- if (adir == swdAccessDirectionRead)
- {
- *header |= 0x04u; /* read access */
- }
- switch (A32)
- {
- case 0x01u:
- *header |= 0x08u;
- break;
- case 0x02u:
- *header |= 0x10u;
- break;
- case 0x03u:
- *header |= 0x18u;
- break;
- default:
- case 0x00u:
- break;
- }
- *header |= swdParity(header, 7u) << 5u;
- *header |= 0x01u; /* startbit */
- *header |= 0x80u;
- }
- static swdStatus_t swdReadPacket( swdPortSelect_t const portSel, uint8_t const A32, uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- uint8_t header = 0x00u;
- uint8_t rp[1] = {0x00u};
- uint8_t resp[5] = {0u};
- uint8_t i = 0u;
- swdBuildHeader( swdAccessDirectionRead, portSel, A32, &header );
- swdDatasend( &header, 8u );
- swdDataIdle();
- swdTurnaround();
- swdDataRead( rp, 3u );
- swdDataRead( resp, 33u );
- swdDataPP();
- for (i=0u; i < N_READ_TURN; ++i)
- {
- swdTurnaround();
- }
- *data = resp[4] | (resp[3] << 8u) | (resp[2] << 16u) | (resp[1] << 24u);
- ret = rp[0];
- return ret;
- }
- static swdStatus_t swdWritePacket( swdPortSelect_t const portSel, uint8_t const A32, uint32_t const data )
- {
- swdStatus_t ret = swdStatusNone;
- uint8_t header = 0x00u;
- uint8_t rp[1] = {0x00u};
- uint8_t data1[5] = {0u};
- uint8_t i = 0u;
- swdBuildHeader( swdAccessDirectionWrite, portSel, A32, &header );
- swdDatasend( &header, 8u );
- MWAIT;
- swdDataIdle();
- MWAIT;
- swdTurnaround();
- swdDataRead( rp, 3u );
- swdDataIdle();
- swdTurnaround();
- swdDataPP();
- data1[0] = data & 0xFFu;
- data1[1] = (data >> 8u) & 0xFFu;
- data1[2] = (data >> 16u) & 0xFFu;
- data1[3] = (data >> 24u) & 0xFFu;
- data1[4] = swdParity(data1, 8u * 4u);
- swdDatasend( data1, 33u );
- swdDataPP();
- for (i=0u; i < 20u; ++i)
- {
- swdTurnaround();
- }
- ret = rp[0];
- return ret;
- }
- swdStatus_t swdReadIdcode( uint32_t * const idCode )
- {
- uint32_t ret = 0u;
- ret = swdReadPacket(swdPortSelectDP, 0x00u, idCode);
- return ret;
- }
- swdStatus_t swdSelectAPnBank(uint8_t const ap, uint8_t const bank)
- {
- swdStatus_t ret = swdStatusNone;
- uint32_t data = 0x00000000u;
- data |= (uint32_t) (ap & 0xFFu) << 24u;
- data |= (uint32_t) (bank & 0x0Fu) << 0u;
- /* write to select register */
- ret |= swdWritePacket(swdPortSelectDP, 0x02u, data);
- return ret;
- }
- static swdStatus_t swdReadAP0( uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- swdReadPacket(swdPortSelectAP, 0x00u, data);
- return ret;
- }
- swdStatus_t swdSetAP32BitMode( uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- swdSelectAPnBank( 0x00u, 0x00u );
- uint32_t d = 0u;
- ret |= swdReadAP0( &d );
- ret |= swdReadPacket(swdPortSelectDP, 0x03u, &d);
- d &= ~(0x07u);
- d |= 0x02u;
- ret |= swdWritePacket(swdPortSelectAP, 0x00u, d);
- ret |= swdReadAP0( &d );
- ret |= swdReadPacket(swdPortSelectDP, 0x03u, &d);
- if (data != NULL)
- {
- *data = d;
- }
- return ret;
- }
- swdStatus_t swdSelectAHBAP( void )
- {
- swdStatus_t ret = swdSelectAPnBank(0x00u, 0x00u);
- return ret;
- }
- swdStatus_t swdReadAHBAddr( uint32_t const addr, uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- uint32_t d = 0u;
- ret |= swdWritePacket(swdPortSelectAP, 0x01u, addr);
- ret |= swdReadPacket(swdPortSelectAP, 0x03u, &d);
- ret |= swdReadPacket(swdPortSelectDP, 0x03u, &d);
- *data = d;
- return ret;
- }
- swdStatus_t swdEnableDebugIF( void )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdWritePacket(swdPortSelectDP, 0x01u, 0x50000000u);
- return ret;
- }
- swdStatus_t swdInit( uint32_t * const idcode )
- {
- swdStatus_t ret = swdStatusNone;
- swdReset();
- ret |= swdReadIdcode( idcode );
- return ret;
- }
- #ifdef UNUSED_EXPERIMENTAL
- static swdStatus_t swdReadDPCtrl( uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdSelectAPnBank(0x00u, 0x00u);
- ret |= swdReadPacket(swdPortSelectAP, 0x01u, data);
- return ret;
- }
- static swdStatus_t swdReadAPCtrl( uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdReadPacket(swdPortSelectDP, 0x01u, data);
- return ret;
- }
- static swdStatus_t swdReadWIREMODE( uint32_t * const data )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdWritePacket(swdPortSelectDP, 0x02u, 0x00000001u);
- ret |= swdReadPacket(swdPortSelectDP, 0x01u, data);
- return data;
- }
- static swdStatus_t swdReadDHCSR( uint32_t * const data )
- {
- swdStatus_t ret = swdReadAHBAddr(0xE000EDF0u, data);
- return ret;
- }
- static swdStatus_t swdWriteAHBAddr( uint32_t const addr, uint32_t const data )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdWritePacket(swdPortSelectAP, 0x01u, addr);
- ret |= swdWritePacket(swdPortSelectAP, 0x03u, data);
- return ret;
- }
- static swdStatus_t swdCoreHalt( void )
- {
- swdStatus_t ret = swdStatusNone;
- ret |= swdWriteAHBAddr(0xE000EDF0u, 0xA05F0003u);
- return ret;
- }
- static swdStatus_t swdGetRegister( uint8_t const regId, uint32_t * const data )
- {
- swdWriteAHBAddr(0xE000EDF4u, regId & 0x1Fu);
- swdStatus_t ret = swdReadAHBAddr(0xE000EDF8u, data);
- return ret;
- }
- #endif
|