diff --git a/minichlink/microgdbstub.h b/minichlink/microgdbstub.h
index d619fefabfa4103c2f89004d418e42ffb841d8ee..6f6500a7ead7cb85e83224f7d3efa5030b3bc5b0 100644
--- a/minichlink/microgdbstub.h
+++ b/minichlink/microgdbstub.h
@@ -25,6 +25,7 @@ void RVNetPoll(void * dev );
 int RVSendGDBHaltReason( void * dev );
 void RVNetConnect( void * dev );
 int RVReadCPURegister( void * dev, int regno, uint32_t * regret );
+int RVWriteCPURegister( void * dev, int regno, uint32_t value );
 void RVDebugExec( void * dev, int halt_reset_or_resume );
 int RVReadMem( void * dev, uint32_t memaddy, uint8_t * payload, int len );
 int RVHandleBreakpoint( void * dev, int set, uint32_t address );
@@ -161,7 +162,6 @@ void HandleGDBPacket( void * dev, char * data, int len )
 {
 	int i;
 
-	//printf( ":::%s:::\n", data );
 	// Got a packet?
 	if( data[0] != '$' ) return;
 
@@ -209,9 +209,14 @@ void HandleGDBPacket( void * dev, char * data, int len )
 		break;
 	case 'c':
 	case 'C':
+		RVDebugExec( dev, (cmd == 's' )?9:(cmd == 'C')?4:2 );
+		SendReplyFull( "OK" );
+		break;
 	case 's':
-		RVDebugExec( dev, (cmd == 'C')?4:2 );
+		RVDebugExec( dev, 4 );
 		SendReplyFull( "OK" );
+		//RVHandleGDBBreakRequest( dev );
+		RVSendGDBHaltReason( dev );
 		break;
 	case 'D':
 		// Handle disconnect.
@@ -220,8 +225,18 @@ void HandleGDBPacket( void * dev, char * data, int len )
 	case 'k':
 		RVHandleKillRequest( dev ); // no reply.
 		break;
-	case 'Z':
-	case 'z':
+	case 'P': // Set register
+	{
+		uint32_t reg, value;
+		if( ReadHex( &data, -1, &reg ) < 0 ) goto err;
+		if( *(data++) != ',' ) goto err;
+		if( ReadHex( &data, -1, &value ) < 0 ) goto err;
+		printf( "REG: %02x = %08x\n", reg, value );
+		RVWriteCPURegister( dev, reg, value );
+		break;
+	}
+	case 'Z': // set
+	case 'z': // unset
 	{
 		uint32_t type = 0;
 		uint32_t addr = 0;
diff --git a/minichlink/minichgdb.c b/minichlink/minichgdb.c
index 7cce3e21221640c9bad4ae0f04f2239a8a64e23c..2943e9694084ff073917f6f063d27e02a3a5e5c4 100644
--- a/minichlink/minichgdb.c
+++ b/minichlink/minichgdb.c
@@ -145,6 +145,36 @@ int RVReadCPURegister( void * dev, int regno, uint32_t * regret )
 	return 0;
 }
 
+
+int RVWriteCPURegister( void * dev, int regno, uint32_t value )
+{
+	if( shadow_running_state )
+	{
+		MCF.HaltMode( dev, 5 );
+		RVCommandPrologue( dev );
+		shadow_running_state = 0;
+	}
+
+	if( regno == 32 ) regno = 16; // Hack - Make 32 also 16 for old GDBs.
+	if( regno > 16 ) return 0; // Invalid register.
+
+	backup_regs[regno] = value;
+
+	if( !MCF.WriteAllCPURegisters )
+	{
+		fprintf( stderr, "ERROR: MCF.WriteAllCPURegisters is not implemented on this platform\n" );
+		return -99;
+	}
+
+	int r;
+	if( ( r = MCF.WriteAllCPURegisters( dev, backup_regs ) ) )
+	{
+		fprintf( stderr, "Error: WriteAllCPURegisters failed (%d)\n", r );
+		return r;
+	}
+	return 0;
+}
+
 void RVDebugExec( void * dev, int halt_reset_or_resume )
 {
 	if( !MCF.HaltMode )
@@ -199,7 +229,13 @@ void RVDebugExec( void * dev, int halt_reset_or_resume )
 				backup_regs[16]+=2;
 			else
 				; //No change, it is a normal instruction.
+
+			if( halt_reset_or_resume == 4 )
+			{
+				MCF.SetEnableBreakpoints( dev, 1, 1 );
+			}
 		}
+
 		halt_reset_or_resume = 2;
 	}