diff --git a/CMakeLists.txt b/CMakeLists.txt
index 081dadc..b259849 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -15,10 +15,10 @@ set(FREQSEL 16MHz CACHE STRING "Select the operating frequency")
 set_property(CACHE FREQSEL PROPERTY STRINGS "20MHz" "16MHz")
 
 if(FREQSEL MATCHES "20MHz")
-set(F_CPU 20000000L)
+set(FREQSEL 20000000L)
 set(AVR_UPLOADTOOL_BASE_OPTIONS ${AVR_UPLOADTOOL_BASE_OPTIONS} -U osccfg:w:0x2:m)
 else()
-set(F_CPU 16000000L)
+set(FREQSEL 16000000L)
 set(AVR_UPLOADTOOL_BASE_OPTIONS ${AVR_UPLOADTOOL_BASE_OPTIONS} -U osccfg:w:0x1:m)
 endif()
 
@@ -115,9 +115,10 @@ target_link_options(mockingboard PUBLIC
     -B "${attiny_atpack_SOURCE_DIR}/gcc/dev/${AVR_MCU}"
 )
 target_compile_definitions(mockingboard PRIVATE
-    F_CPU=${F_CPU}
+    FREQSEL=${FREQSEL}
     CLK_PRESCALE=$<IF:$<BOOL:${CLK_PRESCALE}>,0x01,0x00>
     CLK_PRESCALE_DIV=${CLK_PRESCALE_DIV}
+    __CLK_PRESCALE_DIV=__${CLK_PRESCALE_DIV}
     TCB_CLKSEL=${TCB_CLKSEL}
 )
 target_compile_options(mockingboard PRIVATE
diff --git a/src/com232.c b/src/com232.c
index 6ff2756..8fcd158 100644
--- a/src/com232.c
+++ b/src/com232.c
@@ -20,12 +20,16 @@
     along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */
 
+#define USART_BAUD_RATE(BAUD_RATE)                                             \
+  (uint16_t)((float)(F_CPU * 64 / (16 * (float)BAUD_RATE)) + 0.5)
+
 #include <avr/interrupt.h>
 #include <avr/io.h>
 #include <avr/sfr_defs.h>
 #include <stdint.h>
 
 #include "com232.h"
+#include "timing.h"
 
 uint8_t RS232_RxCharBuffer[25], RS232_RxCharBegin, RS232_RxCharEnd;
 uint8_t readkey;
@@ -35,13 +39,16 @@ void RS232_Init(void) {
 
   PORTMUX.CTRLB = PORTMUX_USART0_ALTERNATE_gc; // Use PA1/PA2 for TxD/RxD
 
+  PORTA.DIRSET = PIN1_bm;
+  PORTA.DIRCLR = PIN2_bm;
+
   USART0.CTRLA = USART_RXCIE_bm;                 // Enable receive interrupts
   USART0.CTRLB = USART_RXEN_bm | USART_TXEN_bm | // Enable Rx/Tx and set receive
                  USART_RXMODE_NORMAL_gc;         //  mode normal
   USART0.CTRLC = USART_CMODE_ASYNCHRONOUS_gc | USART_PMODE_DISABLED_gc |
                  USART_CHSIZE_8BIT_gc |
                  USART_SBMODE_1BIT_gc; // Async UART with 8N1 config
-  USART0.BAUD = 256; // 250k baud rate (64*F_CPU/(16*250k)) for F_CPU = 16MHz
+  USART0.BAUD = USART_BAUD_RATE(250000);
 }
 
 ISR(USART0_RXC_vect) {
diff --git a/src/timing.h b/src/timing.h
index 8091796..7425a7f 100644
--- a/src/timing.h
+++ b/src/timing.h
@@ -1,14 +1,30 @@
 #ifndef _TIMING_HPP_
 #define _TIMING_HPP_
 
-#if CLK_PRESCALE == 0x01
-#error "Not implemented"
-#else
+#define __CLKCTRL_PDIV_2X_gc 2
+#define __CLKCTRL_PDIV_4X_gc 4
+#define __CLKCTRL_PDIV_8X_gc 8
+#define __CLKCTRL_PDIV_16X_gc 16
+#define __CLKCTRL_PDIV_32X_gc 32
+#define __CLKCTRL_PDIV_64X_gc 64
+#define __CLKCTRL_PDIV_6X_gc 6
+#define __CLKCTRL_PDIV_10X_gc 10
+#define __CLKCTRL_PDIV_12X_gc 12
+#define __CLKCTRL_PDIV_24X_gc 24
+#define __CLKCTRL_PDIV_48X_gc 48
 
-#if F_CPU == 20000000L
-#define CPU_CYCLE 50
-#elif F_CPU == 16000000L
-#define CPU_CYCLE 62.5
+#if CLK_PRESCALE == 0x01
+#define F_CPU (FREQSEL / __CLK_PRESCALE_DIV)
+#define CYCLE_MUL __CLK_PRESCALE_DIV
+#else
+#define F_CPU (FREQSEL)
+#define CYCLE_MUL 1
+#endif
+
+#if FREQSEL == 20000000L
+#define CPU_CYCLE (50 * CYCLE_MUL)
+#elif FREQSEL == 16000000L
+#define CPU_CYCLE (62.5 * CYCLE_MUL)
 #else
 #error "Not implemented"
 #endif
@@ -47,5 +63,3 @@
 #define AVCLAN_BIT_LENGTH (39.5e3 / TCB_TICK)
 
 #endif
-
-#endif
\ No newline at end of file