diff --git a/minichlink/minichgdb.c b/minichlink/minichgdb.c
index 13bc6ed24ac9a1d9a6426a77926b306a6369d563..9526355b165bcc10966acc5a4ec6fcd6cc9d75ee 100644
--- a/minichlink/minichgdb.c
+++ b/minichlink/minichgdb.c
@@ -56,6 +56,12 @@ int shadow_running_state = 1;
int last_halt_reason = 5;
uint32_t backup_regs[17];
+#define MAX_SOFTWARE_BREAKPOINTS 128
+int num_software_breakpoints = 0;
+uint8_t software_breakpoint_type[MAX_SOFTWARE_BREAKPOINTS]; // 0 = not in use, 1 = 32-bit, 2 = 16-bit.
+uint32_t software_breakpoint_addy[MAX_SOFTWARE_BREAKPOINTS];
+uint32_t previous_word_at_breakpoint_address[MAX_SOFTWARE_BREAKPOINTS];
+
int IsGDBServerInShadowHaltState( void * dev ) { return !shadow_running_state; }
void RVCommandPrologue( void * dev )
@@ -140,7 +146,7 @@ int RVReadCPURegister( void * dev, int regno, uint32_t * regret )
if( shadow_running_state )
{
MCF.HaltMode( dev, 0 );
- RVCommandPrologue( dev ); printf( "GGGGG1\n" );
+ RVCommandPrologue( dev );
shadow_running_state = 0;
}
@@ -214,18 +220,88 @@ int RVReadMem( void * dev, uint32_t memaddy, uint8_t * payload, int len )
return ret;
}
+static int InternalDisableBreakpoint( void * dev, int i )
+{
+ int r;
+ if( software_breakpoint_type[i] == 1 )
+ {
+ //32-bit instruction
+ r = MCF.WriteBinaryBlob( dev, software_breakpoint_addy[i], 4, (uint8_t*)&previous_word_at_breakpoint_address[i] );
+ }
+ else
+ {
+ //16-bit instruction
+ r = MCF.WriteBinaryBlob( dev, software_breakpoint_addy[i], 2, (uint8_t*)&previous_word_at_breakpoint_address[i] );
+ }
+ previous_word_at_breakpoint_address[i] = 0;
+ software_breakpoint_type[i] = 0;
+ software_breakpoint_addy[i] = 0;
+ return r;
+}
+
int RVHandleBreakpoint( void * dev, int set, uint32_t address )
{
- if( set )
+ int i;
+ int first_free = -1;
+ for( i = 0; i < MAX_SOFTWARE_BREAKPOINTS; i++ )
{
- MCF.SetEnableBreakpoints( dev, 1, 1 );
+ if( software_breakpoint_type[i] && software_breakpoint_addy[i] == address )
+ break;
+ if( first_free < 0 && software_breakpoint_type[i] == 0 )
+ first_free = i;
+ }
+
+ if( i != MAX_SOFTWARE_BREAKPOINTS )
+ {
+ // There is already a break slot here.
+ if( !set )
+ {
+ InternalDisableBreakpoint( dev, i );
+ }
+ else
+ {
+ // Already set.
+ }
}
else
{
- MCF.SetEnableBreakpoints( dev, 1, 0 );
+ if( first_free == -1 )
+ {
+ fprintf( stderr, "Error: Too many breakpoints\n" );
+ return -1;
+ }
+ if( set )
+ {
+ i = first_free;
+ uint32_t readval_at_addy;
+ int r = MCF.ReadBinaryBlob( dev, address, 4, (uint8_t*)&readval_at_addy );
+ if( r ) return -5;
+ if( ( readval_at_addy & 3 ) == 3 ) // Check opcode LSB's.
+ {
+ // 32-bit instruction.
+ software_breakpoint_type[i] = 1;
+ software_breakpoint_addy[i] = address;
+ previous_word_at_breakpoint_address[i] = readval_at_addy;
+ uint32_t ebreak = 0x00100073; // ebreak
+ MCF.WriteBinaryBlob( dev, address, 4, (uint8_t*)&ebreak );
+ }
+ else
+ {
+ // 16-bit instructions
+ software_breakpoint_type[i] = 2;
+ software_breakpoint_addy[i] = address;
+ previous_word_at_breakpoint_address[i] = readval_at_addy & 0xffff;
+ uint32_t ebreak = 0x9002; // c.ebreak
+ MCF.WriteBinaryBlob( dev, address, 2, (uint8_t*)&ebreak );
+ }
+ }
+ else
+ {
+ // Already unset.
+ }
}
- return -1;
+ return 0;
}
int RVWriteRAM(void * dev, uint32_t memaddy, uint32_t length, uint8_t * payload )
@@ -245,6 +321,16 @@ void RVHandleDisconnect( void * dev )
{
MCF.HaltMode( dev, 0 );
MCF.SetEnableBreakpoints( dev, 0, 0 );
+
+ int i;
+ for( i = 0; i < MAX_SOFTWARE_BREAKPOINTS; i++ )
+ {
+ if( software_breakpoint_type[i] )
+ {
+ InternalDisableBreakpoint( dev, i );
+ }
+ }
+
if( shadow_running_state == 0 )
{
RVCommandEpilogue( dev );
@@ -257,7 +343,6 @@ void RVHandleGDBBreakRequest( void * dev )
{
if( shadow_running_state )
{
- printf( "RVHandleGDBBreakRequest\n" );
MCF.HaltMode( dev, 0 );
}
}
@@ -365,6 +450,7 @@ void SendGDBReply( const void * data, int len, int docs )
if( listenMode == 2 )
{
+ //printf( ">>>>%s<<<<(%d)\n", data );
send( serverSocket, data, len, MSG_NOSIGNAL );
}
}
@@ -424,7 +510,9 @@ void HandleGDBPacket( void * dev, char * data, int len )
if( *(data++) != ',' ) goto err;
if( ReadHex( &data, -1, &time ) < 0 ) goto err;
if( RVHandleBreakpoint( dev, cmd == 'Z', addr ) == 0 )
+ {
SendReplyFull( "OK" );
+ }
else
goto err;
break;
diff --git a/minichlink/minichlink.c b/minichlink/minichlink.c
index e51d7da49b2c78fe624f9ab6dc44277193e02c2e..0e3a8191011943dff347c1a0b26357999aa91815 100644
--- a/minichlink/minichlink.c
+++ b/minichlink/minichlink.c
@@ -886,6 +886,10 @@ int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob
if( (address_to_write & 0xff000000) == 0x08000000 || (address_to_write & 0xff000000) == 0x00000000 || (address_to_write & 0x1FFFF800) == 0x1FFFF000 )
is_flash = 1;
+ // We can't write into flash when mapped to 0x00000000
+ if( is_flash )
+ address_to_write |= 0x08000000;
+
if( is_flash && MCF.BlockWrite64 && ( address_to_write & 0x3f ) == 0 && ( blob_size & 0x3f ) == 0 )
{
int i;
@@ -913,6 +917,7 @@ int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob
int eblock = ( address_to_write + blob_size + 0x3f) >> 6;
int b;
int rsofar = 0;
+
for( b = sblock; b < eblock; b++ )
{
int offset_in_block = address_to_write - (b * 64);
@@ -965,14 +970,12 @@ int DefaultWriteBinaryBlob( void * dev, uint32_t address_to_write, uint32_t blob
if( is_flash )
{
MCF.ReadBinaryBlob( dev, base, 64, tempblock );
-
MCF.Erase( dev, base, 64, 0 );
MCF.WriteWord( dev, 0x40022010, CR_PAGE_PG ); // THIS IS REQUIRED, (intptr_t)&FLASH->CTLR = 0x40022010
MCF.WriteWord( dev, 0x40022010, CR_BUF_RST | CR_PAGE_PG ); // (intptr_t)&FLASH->CTLR = 0x40022010
// Permute tempblock
int tocopy = end_o_plus_one_in_block - offset_in_block;
- printf( "MEMCPY: %p %p %d (%d %d)\n", tempblock + offset_in_block, blob + rsofar, tocopy, end_o_plus_one_in_block, offset_in_block );
memcpy( tempblock + offset_in_block, blob + rsofar, tocopy );
rsofar += tocopy;
@@ -1178,7 +1181,7 @@ int DefaultReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t r
{
int r;
int remain = rend - rpos;
-printf( "%08x %08x %d\n", rpos, rend, remain );
+
if( ( rpos & 3 ) == 0 && remain >= 4 )
{
uint32_t rw;