diff --git a/verilog/rtl/Peripherals/PWM/PWMDevice.v b/verilog/rtl/Peripherals/PWM/PWMDevice.v
index 4bbff18..fb7a8ca 100644
--- a/verilog/rtl/Peripherals/PWM/PWMDevice.v
+++ b/verilog/rtl/Peripherals/PWM/PWMDevice.v
@@ -14,7 +14,7 @@
 		output wire peripheralBus_busy,
 		input wire[15:0] peripheralBus_address,
 		input wire[3:0] peripheralBus_byteSelect,
-		output wire[31:0] peripheralBus_dataRead,
+		output reg[31:0] peripheralBus_dataRead,
 		input wire[31:0] peripheralBus_dataWrite,
 		output wire requestOutput,
 		
@@ -24,15 +24,19 @@
 		output wire pwm_irq
 	);
 
-	localparam WIDTH_BITS = $clog2(WIDTH);
 	localparam CLOCK_BITS = $clog2(CLOCK_WIDTH);
 
+	wire counterEnable;
+	wire[CLOCK_BITS-1:0] clockScale;
+	wire[OUTPUTS-1:0] compareEnable;
+	wire[OUTPUTS-1:0] outputEnable;
+	wire[OUTPUTS-1:0] riseInterruptEnable;
+	wire[OUTPUTS-1:0] fallInterruptEnable;
+
 	// Counter control
 	reg[CLOCK_WIDTH + WIDTH - 1:0] baseCounter = 'b0;
 	wire[CLOCK_WIDTH + WIDTH - 1:0] nextCounter = baseCounter + 1;
 	wire[WIDTH-1:0] counterValue = baseCounter >> clockScale;
-	wire[WIDTH:0] topBitMaskFull = { {WIDTH{1'b1}} << topBit, 1'b0 };
-	wire[WIDTH-1:0] topBitMask = topBitMaskFull[WIDTH-1:0];
 
 	// Device select
 	wire[11:0] localAddress;
@@ -44,31 +48,29 @@
 		.deviceEnable(deviceEnable));
 
 	// Register
-	// Configuration register 	 Default 0x3DC (for .WIDTH(16), .CLOCK_WIDTH(32))
-	// b00: counterEnable		 Default 0x0
-	// b01-b05: clockScale		 Default 0x0E
-	// b06-09: top 				 Default 0xF
+	// Configuration register 	 Default 0x000003 (for .CLOCK_WIDTH(32))
+	// b00-b04: clockScale		 Default 0x03
+	// b05: counterEnable		 Default 0x0
+	// b06: compareEnable0		 Default 0x0
+	// b07: compareEnable1		 Default 0x0
+	// b08: compareEnable2		 Default 0x0
+	// b09: compareEnable3		 Default 0x0
 	// b10: outputEnable0		 Default 0x0
 	// b11: outputEnable1		 Default 0x0
 	// b12: outputEnable2		 Default 0x0
 	// b13: outputEnable3		 Default 0x0
-	// b14: outputEnable0		 Default 0x0
-	// b15: outputEnable1		 Default 0x0
-	// b16: outputEnable2		 Default 0x0
-	// b17: outputEnable3		 Default 0x0
-	// b18: riseInterruptEnable0 Default 0x0
-	// b19: riseInterruptEnable1 Default 0x0
-	// b20: riseInterruptEnable2 Default 0x0
-	// b21: riseInterruptEnable3 Default 0x0
-	// b22: fallInterruptEnable0 Default 0x0
-	// b23: fallInterruptEnable1 Default 0x0
-	// b24: fallInterruptEnable2 Default 0x0
-	// b25: fallInterruptEnable3 Default 0x0
-	localparam CONFIG_WIDTH = 1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 4);
+	// b14: riseInterruptEnable0 Default 0x0
+	// b15: riseInterruptEnable1 Default 0x0
+	// b16: riseInterruptEnable2 Default 0x0
+	// b17: riseInterruptEnable3 Default 0x0
+	// b18: fallInterruptEnable0 Default 0x0
+	// b19: fallInterruptEnable1 Default 0x0
+	// b20: fallInterruptEnable2 Default 0x0
+	// b21: fallInterruptEnable3 Default 0x0
+	localparam CONFIG_WIDTH = 1 + CLOCK_BITS + (OUTPUTS * 4);
 	wire[CONFIG_WIDTH-1:0] configuration;
 	wire[31:0] configurationRegisterOutputData;
 	wire configurationRegisterOutputRequest;
-	ConfigurationRegister #(.WIDTH(CONFIG_WIDTH), .ADDRESS(12'h000), .DEFAULT({CONFIG_WIDTH{12'h3DC}})) configurationRegister(
 	ConfigurationRegister #(.WIDTH(CONFIG_WIDTH), .ADDRESS(12'h000), .DEFAULT({ {(CONFIG_WIDTH-5){1'b0}}, 5'h0E })) configurationRegister(
 		.clk(clk),
 		.rst(rst),
@@ -82,13 +84,41 @@
 		.requestOutput(configurationRegisterOutputRequest),
 		.currentValue(configuration));
 
-	wire counterEnable = configuration[0];
-	wire[CLOCK_BITS-1:0] clockScale = configuration[CLOCK_BITS:1];
-	wire[WIDTH_BITS-1:0] topBit = configuration[CLOCK_BITS + WIDTH_BITS:1 + CLOCK_BITS];
-	wire[OUTPUTS-1:0] compareEnable = configuration[(1 + CLOCK_BITS + WIDTH_BITS + OUTPUTS)-1:1 + CLOCK_BITS + WIDTH_BITS];
-	wire[OUTPUTS-1:0] outputEnable = configuration[(1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 2))-1:1 + CLOCK_BITS + WIDTH_BITS + OUTPUTS];
-	wire[OUTPUTS-1:0] riseInterruptEnable = configuration[(1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 3))-1:1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 2)];
-	wire[OUTPUTS-1:0] fallInterruptEnable = configuration[(1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 4))-1:1 + CLOCK_BITS + WIDTH_BITS + (OUTPUTS * 3)];
+	assign clockScale = configuration[CLOCK_BITS-1:0];
+	assign counterEnable = configuration[CLOCK_BITS];
+	assign compareEnable = configuration[(1 + CLOCK_BITS + OUTPUTS)-1:1 + CLOCK_BITS];
+	assign outputEnable = configuration[(1 + CLOCK_BITS + (OUTPUTS * 2))-1:1 + CLOCK_BITS + OUTPUTS];
+	assign riseInterruptEnable = configuration[(1 + CLOCK_BITS + (OUTPUTS * 3))-1:1 + CLOCK_BITS + (OUTPUTS * 2)];
+	assign fallInterruptEnable = configuration[(1 + CLOCK_BITS + (OUTPUTS * 4))-1:1 + CLOCK_BITS + (OUTPUTS * 3)];
+
+	// Counter top compare Default 0x1388 (for .WIDTH(16))
+	// With clockScale=0x03, this gives a 1kHz signal with a 200ns resolution
+	localparam DEFAULT_TOP_COMPARE_VALUE = 'h1388;
+	wire[31:0] topCompareRegisterOutputData;
+	wire topCompareRegisterOutputRequest;
+	wire topCompareRegisterBusBusy_nc;
+	wire[WIDTH-1:0] topCompareRegisterWriteData;
+	wire topCompareRegisterWriteDataEnable;
+	wire topCompareRegisterReadDataEnable_nc;
+	reg[WIDTH-1:0] topCompare;
+	DataRegister #(.WIDTH(WIDTH), .ADDRESS(12'h004)) topCompareRegister(
+		.clk(clk),
+		.rst(rst),
+		.enable(deviceEnable),
+		.peripheralBus_we(peripheralBus_we),
+		.peripheralBus_oe(peripheralBus_oe),
+		.peripheralBus_busy(topCompareRegisterBusBusy_nc),
+		.peripheralBus_address(localAddress),
+		.peripheralBus_byteSelect(peripheralBus_byteSelect),
+		.peripheralBus_dataWrite(peripheralBus_dataWrite),
+		.peripheralBus_dataRead(topCompareRegisterOutputData),
+		.requestOutput(topCompareRegisterOutputRequest),
+		.writeData(topCompareRegisterWriteData),
+		.writeData_en(topCompareRegisterWriteDataEnable),
+		.writeData_busy(1'b0),
+		.readData(topCompare),
+		.readData_en(topCompareRegisterReadDataEnable_nc),
+		.readData_busy(1'b0));
 
 	// Current data register (for .WIDTH(16), .OUTPUTS(4))
 	// b00-b15: counterValue
@@ -96,17 +126,17 @@
 	wire[OUTPUTS-1:0] outputs;
 	wire[31:0] dataRegisterOutputData;
 	wire dataRegisterOutputRequest;
-	wire dataRegisterBusBust_nc;
+	wire dataRegisterBusBusy_nc;
 	wire[WIDTH+OUTPUTS-1:0] dataRegisterWriteData_nc;
 	wire dataRegisterWriteDataEnable_nc;
 	wire dataRegisterReadDataEnable_nc;
-	DataRegister #(.WIDTH(WIDTH + OUTPUTS), .ADDRESS(12'h004)) dataRegister(
+	DataRegister #(.WIDTH(WIDTH + OUTPUTS), .ADDRESS(12'h008)) dataRegister(
 		.clk(clk),
 		.rst(rst),
 		.enable(deviceEnable),
 		.peripheralBus_we(peripheralBus_we),
 		.peripheralBus_oe(peripheralBus_oe),
-		.peripheralBus_busy(dataRegisterBusBust_nc),
+		.peripheralBus_busy(dataRegisterBusBusy_nc),
 		.peripheralBus_address(localAddress),
 		.peripheralBus_byteSelect(peripheralBus_byteSelect),
 		.peripheralBus_dataWrite(peripheralBus_dataWrite),
@@ -120,17 +150,45 @@
 		.readData_busy(1'b0));
 
 	always @(posedge clk) begin
-		if (rst || |(counterValue & topBitMask) || !counterEnable) baseCounter <= 'b0;
-		else if (counterEnable) baseCounter <= nextCounter;
+		if (rst) begin
+			baseCounter <= 'b0;
+			topCompare <= DEFAULT_TOP_COMPARE_VALUE;
+		end else begin
+			if (topCompareRegisterWriteDataEnable) begin
+				baseCounter <= 'b0;
+				topCompare <= topCompareRegisterWriteData;
+			end else begin
+				if (counterEnable) begin
+					if (nextCounter == topCompare) baseCounter <= 'b0;
+					else baseCounter <= nextCounter;
+				end else begin
+					baseCounter <= 'b0;
+				end
+			end
+		end
 	end
 
 	wire[OUTPUTS-1:0] compareRegisterOutputRequest;
 	wire[(32 * OUTPUTS) - 1:0] compareRegisterOutputData;
+	wire[31:0] compareValuesOutputData;
+	wire compareValuesOutputRequest;
 	Mux #(.WIDTH(32), .INPUTS(OUTPUTS), .DEFAULT(~32'b0)) mux(
 		.select(compareRegisterOutputRequest),
 		.in(compareRegisterOutputData),
-		.out(peripheralBus_dataRead),
-		.outputEnable(requestOutput));
+		.out(compareValuesOutputData),
+		.outputEnable(compareValuesOutputRequest));
+
+	assign requestOutput = configurationRegisterOutputRequest || topCompareRegisterOutputRequest || dataRegisterOutputRequest || compareValuesOutputRequest;
+
+	always @(*) begin
+		case (1'b1)
+			configurationRegisterOutputRequest: peripheralBus_dataRead <= configurationRegisterOutputData;
+			topCompareRegisterOutputRequest: peripheralBus_dataRead <= topCompareRegisterOutputData;
+			dataRegisterOutputRequest: peripheralBus_dataRead <= dataRegisterOutputData;
+			compareValuesOutputRequest: peripheralBus_dataRead <= compareValuesOutputData;
+			default: peripheralBus_dataRead <= 32'b0;
+		endcase
+	end
 
 	wire[OUTPUTS-1:0] compareRise;
 	wire[OUTPUTS-1:0] compareFall;
@@ -141,7 +199,7 @@
 		for (i = 0; i < OUTPUTS; i = i + 1) begin
 			// Compare value register
 			wire[WIDTH-1:0] compareValue;
-			ConfigurationRegister #(.WIDTH(WIDTH), .ADDRESS(12'h008 + (i * 12'h004)), .DEFAULT('b0)) compareRegister(
+			ConfigurationRegister #(.WIDTH(WIDTH), .ADDRESS(12'h010 + (i * 12'h004)), .DEFAULT('b0)) compareRegister(
 				.clk(clk),
 				.rst(rst),
 				.enable(deviceEnable),