diff --git a/minichlink/minichgdb.c b/minichlink/minichgdb.c
index 774a2f16..a11f5a12 100644
--- a/minichlink/minichgdb.c
+++ b/minichlink/minichgdb.c
@@ -33,7 +33,7 @@ void SendReplyFull( const char * replyMessage );
 // Several pieces from picorvd. https://github.com/aappleby/PicoRVD/
 int shadow_running_state = 1;
 int last_halt_reason = 5;
-uint32_t backup_regs[17];
+uint32_t backup_regs[33]; //0..15 + PC, or 0..32 + PC
 
 #define MAX_SOFTWARE_BREAKPOINTS 128
 int num_software_breakpoints = 0;
@@ -131,6 +131,9 @@ void RVNetPoll(void * dev )
 
 int RVReadCPURegister( void * dev, int regno, uint32_t * regret )
 {
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	int nrregs = iss->nr_registers_for_debug;
+
 	if( shadow_running_state )
 	{
 		MCF.HaltMode( dev, 5 );
@@ -138,8 +141,15 @@ int RVReadCPURegister( void * dev, int regno, uint32_t * regret )
 		shadow_running_state = 0;
 	}
 
-	if( regno == 32 ) regno = 16; // Hack - Make 32 also 16 for old GDBs.
-	if( regno > 16 ) return 0; // Invalid register.
+	if( nrregs == 16 )
+	{
+		if( regno == 32 ) regno = 16; // Hack - Make 32 also 16 for old GDBs.
+		if( regno > 16 ) return 0; // Invalid register.
+	}
+	else
+	{
+		if( regno > nrregs ) return 0;
+	}
 
 	*regret = backup_regs[regno];
 	return 0;
@@ -148,6 +158,9 @@ int RVReadCPURegister( void * dev, int regno, uint32_t * regret )
 
 int RVWriteCPURegister( void * dev, int regno, uint32_t value )
 {
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	int nrregs = iss->nr_registers_for_debug;
+
 	if( shadow_running_state )
 	{
 		MCF.HaltMode( dev, 5 );
@@ -155,8 +168,15 @@ int RVWriteCPURegister( void * dev, int regno, uint32_t value )
 		shadow_running_state = 0;
 	}
 
-	if( regno == 32 ) regno = 16; // Hack - Make 32 also 16 for old GDBs.
-	if( regno > 16 ) return 0; // Invalid register.
+	if( nrregs == 16 )
+	{
+		if( regno == 32 ) regno = 16; // Hack - Make 32 also 16 for old GDBs.
+		if( regno > 16 ) return 0; // Invalid register.
+	}
+	else
+	{
+		if( regno > nrregs ) return 0;
+	}
 
 	backup_regs[regno] = value;
 
@@ -177,6 +197,9 @@ int RVWriteCPURegister( void * dev, int regno, uint32_t value )
 
 void RVDebugExec( void * dev, int halt_reset_or_resume )
 {
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	int nrregs = iss->nr_registers_for_debug;
+
 	if( !MCF.HaltMode )
 	{
 		fprintf( stderr, "Error: Can't alter halt mode with this programmer.\n" );
@@ -189,7 +212,7 @@ void RVDebugExec( void * dev, int halt_reset_or_resume )
 		// First see if we already know about this breakpoint
 		int matchingbreakpoint = -1;
 		// For this we want to advance PC.
-		uint32_t exceptionptr = backup_regs[16];
+		uint32_t exceptionptr = backup_regs[nrregs];
 		uint32_t instruction = 0;
 
 		int i;
@@ -224,9 +247,9 @@ void RVDebugExec( void * dev, int halt_reset_or_resume )
 				MCF.ReadWord( dev, exceptionptr, &instruction );
 			}
 			if( instruction == 0x00100073 )
-				backup_regs[16]+=4;
+				backup_regs[nrregs]+=4;
 			else if( ( instruction & 0xffff ) == 0x9002 )
-				backup_regs[16]+=2;
+				backup_regs[nrregs]+=2;
 			else
 				; //No change, it is a normal instruction.
 
diff --git a/minichlink/minichlink.c b/minichlink/minichlink.c
index dc897e69..8992116d 100644
--- a/minichlink/minichlink.c
+++ b/minichlink/minichlink.c
@@ -23,7 +23,7 @@ void Sleep(uint32_t dwMilliseconds);
 static int64_t StringToMemoryAddress( const char * number ) __attribute__((used));
 static void StaticUpdatePROGBUFRegs( void * dev ) __attribute__((used));
 int DefaultReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob );
-
+void PostSetupConfigureInterface( void * dev );
 void TestFunction(void * v );
 struct MiniChlinkFunctions MCF;
 
@@ -149,6 +149,7 @@ int main( int argc, char ** argv )
 		printf( "Interface Setup\n" );
 	}
 
+	PostSetupConfigureInterface( dev );
 //	TestFunction( dev );
 
 	int iarg = 1;
@@ -1540,6 +1541,26 @@ int DefaultErase( void * dev, uint32_t address, uint32_t length, int type )
 	return -93;
 }
 
+void PostSetupConfigureInterface( void * dev )
+{
+	struct InternalState * iss = (struct InternalState*)(((struct ProgrammerStructBase*)dev)->internal);
+	switch( iss->target_chip_type )
+	{
+	case CHIP_CH32V10x:
+	case CHIP_CH57x:
+	case CHIP_CH56x:
+	case CHIP_CH32V20x:
+	case CHIP_CH32V30x:
+	case CHIP_CH58x:
+	default:
+		iss->nr_registers_for_debug = 32;
+		break;
+	case CHIP_CH32V003:
+		iss->nr_registers_for_debug = 16;
+		break;
+	}
+}
+
 int DefaultReadBinaryBlob( void * dev, uint32_t address_to_read_from, uint32_t read_size, uint8_t * blob )
 {
 	uint32_t rpos = address_to_read_from;
@@ -1625,7 +1646,7 @@ int DefaultReadAllCPURegisters( void * dev, uint32_t * regret )
 	MCF.WriteReg32( dev, DMABSTRACTAUTO, 0x00000001 ); // Disable Autoexec.
 	iss->statetag = STTAG( "RER2" );
 	int i;
-	for( i = 0; i < 16; i++ )
+	for( i = 0; i < iss->nr_registers_for_debug; i++ )
 	{
 		MCF.WriteReg32( dev, DMCOMMAND, 0x00220000 | 0x1000 | i ); // Read xN into DATA0.
 		if( MCF.ReadReg32( dev, DMDATA0, regret + i ) )
@@ -1646,7 +1667,7 @@ int DefaultWriteAllCPURegisters( void * dev, uint32_t * regret )
 	MCF.WriteReg32( dev, DMABSTRACTAUTO, 0x00000001 ); // Disable Autoexec.
 	iss->statetag = STTAG( "WER2" );
 	int i;
-	for( i = 0; i < 16; i++ )
+	for( i = 0; i < iss->nr_registers_for_debug; i++ )
 	{
 		MCF.WriteReg32( dev, DMCOMMAND, 0x00230000 | 0x1000 | i ); // Read xN into DATA0.
 		if( MCF.WriteReg32( dev, DMDATA0, regret[i] ) )
diff --git a/minichlink/minichlink.h b/minichlink/minichlink.h
index ba266941..170667f9 100644
--- a/minichlink/minichlink.h
+++ b/minichlink/minichlink.h
@@ -128,6 +128,7 @@ struct InternalState
 	int flash_size;
 	enum RiscVChip target_chip_type;
 	uint8_t flash_sector_status[MAX_FLASH_SECTORS];  // 0 means unerased/unknown. 1 means erased.
+	int nr_registers_for_debug; // Updated by PostSetupConfigureInterface
 };