diff --git a/src/ftdi_device.cpp b/src/ftdi_device.cpp
index 90df503e4516b906c93b7f7251b442c81ed23c30..76a70d8eee7d98ef7cd0632cc49f66ca97d6f59f 100644
--- a/src/ftdi_device.cpp
+++ b/src/ftdi_device.cpp
@@ -31,11 +31,12 @@ static void init_core_1();
 static void set_other_cores_idle();
 
 
-FT_STATUS open_device(DWORD device_index, int cpu_type)
+FT_STATUS ftdi_open_device(DWORD device_index, int cpu_type)
 {
 	device.device_index = device_index;
 	device.cpu_type = cpu_type;
 	device.first_run = true;
+	device.active_cpu = 0;
 	
 	// Open FTDI device handle
 	FT_STATUS ftStatus = FT_Open(device_index, &device.ft_handle);
@@ -99,7 +100,7 @@ FT_STATUS open_device(DWORD device_index, int cpu_type)
 	return ftStatus;
 }
 
-void close_device()
+void ftdi_close_device()
 {
 	// Reset device before closing handle, good practice
 	FT_SetBitMode(device.ft_handle, 0x0, 0x00);
@@ -110,11 +111,25 @@ void close_device()
 	printf("Goodbye\n");
 }
 
-int get_connected_cpu_type()
+int ftdi_get_connected_cpu_type()
 {
 	return device.cpu_type;
 }
 
+void ftdi_set_active_cpu(uint32_t cpu)
+{
+	if ((device.cpu_type == 0 && cpu > 2) || (device.cpu_type == 1 && cpu > 4))
+		return;
+
+	device.active_cpu = cpu;
+}
+
+
+uint32_t ftdi_get_active_cpu()
+{
+	return device.active_cpu;
+}
+
 DWORD get_devices_count()
 {
 	//FT_STATUS ftStatus;
@@ -509,6 +524,33 @@ static void set_other_cores_idle()
 	
 }
 
+void ftdi_set_cpu_idle(uint32_t cpu)
+{
+	uint32_t tmp = dsu_get_reg_tbr(0) & ~0xfff;
+
+	dsu_set_noforce_debug_mode(cpu);
+	dsu_set_cpu_break_on_iu_watchpoint(cpu);
+
+	dsu_set_force_debug_on_watchpoint(cpu);
+
+	dsu_set_reg_tbr(cpu, tmp);
+	dsu_set_reg_pc(cpu, tmp);
+	dsu_set_reg_npc(cpu, tmp + 0x4);
+
+	dsu_clear_iu_reg_file(cpu);
+	// Default invalid mask
+	dsu_set_reg_wim(cpu, 0x2);
+	
+	// Set CWP to 7
+	dsu_set_reg_psr(cpu, 0xf34010e1);
+
+	dsu_clear_cpu_break_on_iu_watchpoint(cpu);
+	// Resume cpu 1
+	dsu_clear_force_debug_on_watchpoint(cpu);
+
+	dsu_clear_cpu_error_mode(cpu);
+}
+
 static FT_STATUS reset_JTAG_state_machine()
 {
 	BYTE out_buf[] = {
diff --git a/src/ftdi_device.hpp b/src/ftdi_device.hpp
index 29ff43782aff2e92a750865949992820f5e66bba..ab6cb282031c49ebbae06fd039d1987ad0145c78 100644
--- a/src/ftdi_device.hpp
+++ b/src/ftdi_device.hpp
@@ -18,6 +18,7 @@
 #include "lib/ftdi/ftd2xx.h"
 
 #include <stdbool.h>
+#include <stdint.h>
 
 const unsigned int CODE_ADDR_COMM = 0x2; // address/command register opcode, 35-bit length
 const DWORD CODE_DATA = 0x3;			 // data register opcode, 33-bit length
@@ -40,12 +41,17 @@ typedef struct {
 	DWORD device_index;
 	int cpu_type;
 	bool first_run;
+	uint32_t active_cpu;
 } ftdi_device;
 
-FT_STATUS open_device(DWORD device_index, int cpu_type);
-void close_device();
+FT_STATUS ftdi_open_device(DWORD device_index, int cpu_type);
+void ftdi_close_device();
 
-int get_connected_cpu_type();
+int ftdi_get_connected_cpu_type();
+
+void ftdi_set_active_cpu(uint32_t cpu);
+uint32_t ftdi_get_active_cpu();
+void ftdi_set_cpu_idle(uint32_t cpu);
 
 DWORD get_devices_count();
 void get_device_list();
diff --git a/src/leon3_dsu.c b/src/leon3_dsu.c
index 531dc6b22945fc683505e6bd0dd3c0c295403c4e..bbff9075c959dee361cc159bd04868997c7b98f0 100644
--- a/src/leon3_dsu.c
+++ b/src/leon3_dsu.c
@@ -16,7 +16,7 @@
  *
  * @defgroup leon3_dsu LEON3 DSU interface
  *
- * @brief Access to the DSU of the GR712RC LEON3FT (and possibly others)
+ * @brief Access to the DSU of the GR712RC LEON3FT, LEON4 (and possibly others)
  *
  *
  * ## Overview
@@ -403,7 +403,7 @@ void dsu_clear_cpu_error_mode(uint32_t cpu)
 
 
 /**
- * @brief read out the DSU trap register; ONLY WORKS FOR ONE PROCESSOR ATM!
+ * @brief read out the DSU trap register
  *
  * @param cpu the cpu number
  *
@@ -414,7 +414,7 @@ void dsu_clear_cpu_error_mode(uint32_t cpu)
 
 uint32_t dsu_get_reg_trap(uint32_t cpu)
 {
-	return ioread32((uint32_t) DSU_CTRL + DSU_REG_TRAP);
+	return ioread32((uint32_t) DSU_BASE(cpu) + DSU_REG_TRAP);
 }
 
 
@@ -477,45 +477,101 @@ void dsu_get_global_reg(uint32_t cpu, uint32_t *buffer)
 	ioread32(DSU_REG_GLOBAL(cpu), buffer, 8);
 }
 
+/**
+ * @brief all local register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param winndow the window from where the registers are returned
+ * @param the buffer to write the results to, needs to have enough space allocated
+ */
 void dsu_get_local_reg_window(uint32_t cpu, uint32_t window, uint32_t *buffer)
 {
 	ioread32(DSU_REG_LOCAL(cpu, window), buffer, 8);
 }
 
 
+/**
+ * @brief all input register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param winndow the window from where the registers are returned
+ * @param the buffer to write the results to, needs to have enough space allocated
+ */
 void dsu_get_input_reg_window(uint32_t cpu, uint32_t window, uint32_t *buffer)
 {
 	ioread32(DSU_REG_IN(cpu, window), buffer, 8);
 }
 
-
+/**
+ * @brief all output register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param winndow the window from where the registers are returned
+ * @param the buffer to write the results to, needs to have enough space allocated
+ */
 void dsu_get_output_reg_window(uint32_t cpu, uint32_t window, uint32_t *buffer)
 {
 	ioread32(DSU_REG_OUT(cpu, window), buffer, 8);
 }
 
 
+/**
+ * @brief get a single local register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param cwp the window from which the register value is retrieved
+ * @param reg_num the register number to be retrieved
+ */
 uint32_t dsu_get_local_reg_single(uint32_t cpu, uint32_t cwp, uint32_t reg_num)
 {
 	return ioread32(dsu_get_local_reg_addr(cpu, reg_num, cwp));
 }
 
+
+/**
+ * @brief get a single input register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param cwp the window from which the register value is retrieved
+ * @param reg_num the register number to be retrieved
+ */
 uint32_t dsu_get_input_reg_single(uint32_t cpu, uint32_t cwp, uint32_t reg_num)
 {
 	return ioread32(dsu_get_input_reg_addr(cpu, reg_num, cwp));
 }
 
+/**
+ * @brief get a output local register of a specific window
+ *
+ * @param cpu the cpu number
+ * @param cwp the window from which the register value is retrieved
+ * @param reg_num the register number to be retrieved
+ */
 uint32_t dsu_get_output_reg_single(uint32_t cpu, uint32_t cwp, uint32_t reg_num)
 {
 	return ioread32(dsu_get_output_reg_addr(cpu, reg_num, cwp));
 }
 
+/**
+ * @brief get a single global register value
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to be retrieved
+ */
 uint32_t dsu_get_global_reg_single(uint32_t cpu, uint32_t reg_num)
 {
 	return ioread32(dsu_get_global_reg_addr(cpu, reg_num));
 }
 
 
+/**
+ * @brief get a floating point register value
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to be retrieved
+ *
+ * @return union of the register as uint32_t and float
+ */
 union float_value dsu_get_float_reg(uint32_t cpu, uint32_t reg_num)
 {
 	union float_value val = {0};
@@ -527,7 +583,14 @@ union float_value dsu_get_float_reg(uint32_t cpu, uint32_t reg_num)
 	return val;
 }
 
-
+/**
+ * @brief get a double point register value
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to be retrieved
+ *
+ * @return union of the register as uint64_t and double
+ */
 union double_value dsu_get_double_reg(uint32_t cpu, uint32_t reg_num)
 {
 	uint32_t address = DSU_BASE(cpu) + DSU_FPU_REG + 32 * 4 + reg_num * 8;
@@ -617,10 +680,9 @@ void dsu_set_cpu_wake_up(uint32_t cpu)
 
 	//iowrite32((uint32_t)0x80000210, 1 << cpu); // LEON3
 	// iowrite32be(1 << cpu, (uint32_t)0xFF904010); // LEON4
-	iowrite32((uint32_t)ADDRESSES[get_connected_cpu_type()][WAKE_STATE], 1 << cpu);
+	iowrite32((uint32_t)ADDRESSES[ftdi_get_connected_cpu_type()][WAKE_STATE], 1 << cpu);
 }
 
-
 /**
  * @brief  get status of a cpu core
  *
@@ -632,7 +694,7 @@ void dsu_set_cpu_wake_up(uint32_t cpu)
 uint32_t dsu_get_cpu_state(uint32_t cpu)
 {
 	//return (ioread32((uint32_t)0x80000210) >> cpu) & 1;
-	return (ioread32((uint32_t)ADDRESSES[get_connected_cpu_type()][WAKE_STATE]) >> cpu) & 1;
+	return (ioread32((uint32_t)ADDRESSES[ftdi_get_connected_cpu_type()][WAKE_STATE]) >> cpu) & 1;
 }
 
 
@@ -1128,6 +1190,14 @@ void dsu_get_instr_trace_buffer(uint32_t cpu, struct instr_trace_buffer_line *bu
 }
 
 
+/**
+ * @brief set a local register value
+ *
+ * @param cpu the cpu number
+ * @param cwp the window of the register to set
+ * @param reg_num the register number to be set
+ * @param value the value to set the register to
+ */
 void dsu_set_local_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t value)
 {
 	uint32_t address = dsu_get_local_reg_addr(cpu, reg_num, cwp);
@@ -1136,6 +1206,14 @@ void dsu_set_local_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t va
 }
 
 
+/**
+ * @brief set a input register value
+ *
+ * @param cpu the cpu number
+ * @param cwp the window of the register to set
+ * @param reg_num the register number to be set
+ * @param value the value to set the register to
+ */
 void dsu_set_input_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t value)
 {
 	uint32_t address = dsu_get_input_reg_addr(cpu, reg_num, cwp);
@@ -1144,6 +1222,14 @@ void dsu_set_input_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t va
 }
 
 
+/**
+ * @brief set a output register value
+ *
+ * @param cpu the cpu number
+ * @param cwp the window of the register to set
+ * @param reg_num the register number to be set
+ * @param value the value to set the register to
+ */
 void dsu_set_output_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t value)
 {
 	uint32_t address = dsu_get_output_reg_addr(cpu, reg_num, cwp);
@@ -1151,7 +1237,13 @@ void dsu_set_output_reg(uint32_t cpu, uint32_t cwp, uint32_t reg_num, uint32_t v
 	iowrite32(address, value);
 }
 
-
+/**
+ * @brief set a global register value
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to be set
+ * @param value the value to set the register to
+ */
 void dsu_set_global_reg(uint32_t cpu, uint32_t reg_num, uint32_t value)
 {
 	uint32_t address = dsu_get_global_reg_addr(cpu, reg_num);
@@ -1160,6 +1252,13 @@ void dsu_set_global_reg(uint32_t cpu, uint32_t reg_num, uint32_t value)
 }
 
 
+/**
+ * @brief set a float register
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to set
+ * @param value the value to set the register to as union of float and uint32_t
+ */
 void dsu_set_float_reg(uint32_t cpu, uint32_t reg_num, union float_value value)
 {
 	uint32_t address = DSU_BASE(cpu) + DSU_FPU_REG + reg_num * 4;
@@ -1169,7 +1268,13 @@ void dsu_set_float_reg(uint32_t cpu, uint32_t reg_num, union float_value value)
 	iowrite32(address, value.u);	
 }
 
-
+/**
+ * @brief set a double register
+ *
+ * @param cpu the cpu number
+ * @param reg_num the register number to set
+ * @param value the value to set the register to as union of double and uint64_t
+ */
 void dsu_set_double_reg(uint32_t cpu, uint32_t reg_num, union double_value value)
 {
 	uint32_t address = DSU_BASE(cpu) + DSU_FPU_REG + 32 * 4 + reg_num * 8;
diff --git a/src/leon3_dsu.h b/src/leon3_dsu.h
index cded64baa8bdb70729c001a804c0fbd76094cfaa..75cdf27b8d6901f3403e6456558e0c6a9d053569 100644
--- a/src/leon3_dsu.h
+++ b/src/leon3_dsu.h
@@ -94,7 +94,7 @@
 
 
 // TODO: this needs to be refactored
-#define DSU_CTRL ADDRESSES[get_connected_cpu_type()][DSU]
+#define DSU_CTRL ADDRESSES[ftdi_get_connected_cpu_type()][DSU]
 #define DSU_OFFSET_CPU(x)	((x & 0x07) << 24)
 #define DSU_BASE(x)		(DSU_CTRL + DSU_OFFSET_CPU(x))
 
diff --git a/src/uviemon.cpp b/src/uviemon.cpp
index 7ebc905a2a12f47b90742205cc935d2168f78adc..353630d13f2cb10aebaf476975e86a8a265b4621 100644
--- a/src/uviemon.cpp
+++ b/src/uviemon.cpp
@@ -165,7 +165,7 @@ int main(int argc, char *argv[])
 		return 1;
 	}
 
-	if (!FT_SUCCESS(open_device(device_index, cpu_type))) {
+	if (!FT_SUCCESS(ftdi_open_device(device_index, cpu_type))) {
 		fprintf(stderr, "Unable to use device %d. Aborting...\n", device_index);
 		return 1;
 	}
@@ -221,5 +221,7 @@ int main(int argc, char *argv[])
 	
 	console();
 
+	ftdi_close_device();
+
 	return 0;
 }
diff --git a/src/uviemon_cli.cpp b/src/uviemon_cli.cpp
index b43c860a629a3bd1a64ffb4dcb51ca13621533ed..18f3d71b5b53c1514316a9eb1cd959309139c4f8 100644
--- a/src/uviemon_cli.cpp
+++ b/src/uviemon_cli.cpp
@@ -50,9 +50,7 @@ using namespace std;
 
 //static int command_count;
 
-#define COMMAND_COUNT 16
-
-static command commands[COMMAND_COUNT] =  {
+static const command commands[] =  {
 	{ "help", &help },
 	{ "scan", &scan },
 	{ "reset", &reset },
@@ -69,6 +67,7 @@ static command commands[COMMAND_COUNT] =  {
 
 	{ "inst", &inst },
 	{ "reg", &reg },
+	{ "cpu", &cpu },
 
 	{ "wash", &washc },
 
@@ -239,7 +238,7 @@ int parse_input(char *input)
 	 * if the number of commands ever becomes so large that a simple
 	 * naive search won't be enough. This part has to be rewritten. 
 	 */
-	for(int i = 0; i < COMMAND_COUNT; i++) {
+	for(uint32_t i = 0; i < (sizeof(commands) / sizeof(commands[0])); i++) {
 		if (strcmp(command, commands[i].command_name) == 0) {
 			commands[i].function(command, param_count - 1, params);
 			command_found = true;
@@ -314,7 +313,7 @@ void scan(const char *command, int param_count, char params[MAX_PARAMETERS][MAX_
 
 void run(const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH])
 {
-	BYTE tt = runCPU(0); // Execute on CPU Core 1
+	BYTE tt = runCPU(ftdi_get_active_cpu()); // Execute on CPU Core 1
 
 	if (tt < 0x80) // Hardware traps
 	{
@@ -400,7 +399,7 @@ void washc(const char *command, int param_count, char params[MAX_PARAMETERS][MAX
 {
 	WORD size = 16;
 	// was 0x40000000
-	DWORD address = ADDRESSES[get_connected_cpu_type()][SDRAM_START_ADDRESS], c = 0;
+	DWORD address = ADDRESSES[ftdi_get_connected_cpu_type()][SDRAM_START_ADDRESS], c = 0;
 
 	if (param_count > 0) {
 		if ( (size = parse_parameter(params[0])) == 0 ) {
@@ -492,7 +491,7 @@ void wmemx(const char *command, int param_count, char params[MAX_PARAMETERS][MAX
 void inst(const char* command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH])
 {
 	uint32_t instr_count = 11;
-	uint32_t cpu = 0;
+	uint32_t cpu = ftdi_get_active_cpu();
 
 	if (param_count > 1) {
 		printf("Inst only needs 1 parameter: the number of lines");
@@ -551,9 +550,10 @@ void inst(const char* command, int param_count, char params[MAX_PARAMETERS][MAX_
 				 * second is program counter with last two bits set to zero
 				 */
 				parse_opcode(operation, buffer[j].field[3], buffer[j].field[2] & 0xFFFFFFFC); 
-				printf("    %9u  %08x  ",
+				printf("    %9u  %08x  %30s",
 					   buffer[j].field[0] & 0x3FFFFFFF,
-					   buffer[j].field[2] & 0xFFFFFFFC);
+					   buffer[j].field[2] & 0xFFFFFFFC,
+					   operation);
 				printf(" [");
 
 
@@ -582,7 +582,7 @@ void inst(const char* command, int param_count, char params[MAX_PARAMETERS][MAX_
 
 void reg (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH])
 {
-	uint32_t cpu = 0;
+	uint32_t cpu = ftdi_get_active_cpu();
 	uint32_t input, reg_value;
 	int base;
 	char *input_str;
@@ -694,6 +694,50 @@ void reg (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_
 
 }
 
+
+void cpu (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH])
+{
+    /* TODO: refactor to put this information in ftdi device */
+	uint32_t cpu_count = ftdi_get_connected_cpu_type() == LEON3 ? 2 : 4;
+	uint32_t cpu;
+	
+	if (param_count == 0) {
+
+		for(uint32_t i = 0; i < cpu_count; i++) {
+			printf("   cpu %d: %-8s %s\n", i,
+				   dsu_get_cpu_state(i) ? "disabled" : "enabled",
+				   ftdi_get_active_cpu() == i ? "active" : "");
+		}
+		
+	} else if (param_count == 2) {
+		errno = 0;
+
+		cpu = strtol(params[1], NULL, 10);
+
+		if (errno != 0) {
+			printf("Could not parse cpu number: %s", params[1]);
+			return;
+		}
+		
+		
+		if (strcmp(params[0], "enable") == 0) {
+			dsu_set_cpu_wake_up(cpu);
+			printf("Cpu %d enabled\n", cpu);
+		} else if (strcmp(params[0], "disable") == 0)  {
+			ftdi_set_cpu_idle(cpu);
+			printf("Cpu %d disabled\n", cpu);
+		} else if (strcmp(params[0], "active") == 0) {
+			if (dsu_get_cpu_state(cpu)) {
+				dsu_set_cpu_wake_up(cpu);
+				printf("Cpu %d enabled\n", cpu);
+			}
+			
+			ftdi_set_active_cpu(cpu);
+			printf("Set cpu %d active\n", cpu);
+		}
+	}
+}
+
 static void print_register_error_msg(const char * const reg)
 {
 	printf("No such register %s\n", reg);
@@ -738,7 +782,8 @@ static void parse_opcode(char *buffer, uint32_t opcode, uint32_t address)
 	info.buffer = (bfd_byte *)&opcode;
 	info.buffer_length = 1;
 	
-	disassembler_p(0, &info);*/
+	disassembler_p(0, &info);
+	 */
 	
 	struct opcode op;
 	op.opcode_raw = opcode;
@@ -1022,7 +1067,7 @@ void load(string path)
 	}
 
 	// Begin writing to this address was 0x40000000
-	const DWORD addr = ADDRESSES[get_connected_cpu_type()][SDRAM_START_ADDRESS];
+	const DWORD addr = ADDRESSES[ftdi_get_connected_cpu_type()][SDRAM_START_ADDRESS];
 	iowrite32(addr, writeBuffer, writeSize, true);
 
 	cout << "Loading file complete!" << endl;
@@ -1066,7 +1111,7 @@ void verify(std::string path)
 	DWORD readBuffer[readSize];
 
 	// Begin writing from this address was 0x40000000; 
-	const DWORD addr = ADDRESSES[get_connected_cpu_type()][SDRAM_START_ADDRESS];
+	const DWORD addr = ADDRESSES[ftdi_get_connected_cpu_type()][SDRAM_START_ADDRESS];
 	ioread32(addr, readBuffer, readSize, true);
 
 	cout << "Verifying file... " << flush;
diff --git a/src/uviemon_cli.hpp b/src/uviemon_cli.hpp
index 40ca86e9082929734f6b75d68a48a44a1beea5c5..5c6cfb34cbd6893a081a832f82ef1b1209edfe1d 100644
--- a/src/uviemon_cli.hpp
+++ b/src/uviemon_cli.hpp
@@ -117,7 +117,7 @@ void bdump (const char *command, int param_count, char params[MAX_PARAMETERS][MA
 void washc (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH]);
 void inst  (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH]);
 void reg   (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH]);
-
+void cpu   (const char *command, int param_count, char params[MAX_PARAMETERS][MAX_PARAM_LENGTH]);
 
 void wmem(DWORD addr, DWORD data);
 void wmemh(DWORD addr, WORD data);
diff --git a/src/uviemon_reg.c b/src/uviemon_reg.c
index fd625e5d5276ddfd41da54f61770f4bc842d38bb..63808567b43fab82f7bd5ad2db84e0aeb5bba815 100644
--- a/src/uviemon_reg.c
+++ b/src/uviemon_reg.c
@@ -57,7 +57,7 @@ static union double_value get_reg_double(struct register_desc desc);
 static void set_reg_double(struct register_desc desc, union double_value value);
 
 
-static struct register_func_standard function_handler[] = {
+static const struct register_func_standard function_handler[] = {
 	{ "inv", none,     NULL,            NULL },
 	{ "psr", standard_reg, &get_reg_psr,    &set_reg_psr },
 	{ "tbr", standard_reg, &get_reg_tbr,    &set_reg_tbr },