danielinux/rdp1-bypass
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity
rdp1-bypass/swd.c
Daniele Lacamera c6350f2f65 Initial commit
2018-07-26 08:08:29 +02:00

510 lines
9.1 KiB
C
Raw Permalink Blame History

/*
* 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
Reference in a new issue View git blame Copy permalink