diff --git a/firmware/midifs.c b/firmware/midifs.c
index 44da299..a0a3145 100644
--- a/firmware/midifs.c
+++ b/firmware/midifs.c
@@ -1,8 +1,14 @@
-// midifs.c
-// Typ: C-Sourcerfile
-// Project: MIDI Footswitch
-// Date: 17.06.2014
-// Author: ska
+/**
+ * MIDI Footswitch
+ *
+ * @file   midifs.c
+ * @author Stefan Kalscheuer
+ * @date   2014-06-17
+ * @brief  Main program
+ *
+ * Platform:  ATtiny2313
+ *            Internal RC-oscillator 8 MHz
+ */
 
 #define F_CPU 8000000UL
 
@@ -12,185 +18,213 @@
 #include "midifs.h"
 #include "usart.h"
 
-volatile unsigned char status_channel, status, channel, data1, data2;
-volatile unsigned int iBtn1_counter = 0, iBtn2_counter = 0, iBtn3_counter = 0, iBtn4_counter = 0, iSw1_counter = 0, iSw2_counter = 0;
-volatile unsigned char cBtn1 = 0, cBtn2 = 0, cBtn3 = 0, cBtn4 = 0, cSw1 = 0, cSw2 = 0;
-volatile unsigned char cChannel = 0, cBank = 0; cTuner = 0;
-unsigned int cnt = 0;
+// Counter for debouncing inputs.
+volatile uint16_t counterBtn1 = 0;
+volatile uint16_t counterBtn2 = 0;
+volatile uint16_t counterBtn3 = 0;
+volatile uint16_t counterBtn4 = 0;
+volatile uint16_t counterSw1 = 0;
+volatile uint16_t counterSw2 = 0;
 
-int main (void)
-{
-	DDRB = 0x00;				// PB1..7 as inputs
-	PORTB = 0xFF;				// interlan pull-ups for PB0..7y
-	DDRD = 0x30;				// PD4..5 as outputs
-	PORTD = 0x0C;				// internal pull-ups for PD2..3
+// Input states.
+volatile uint8_t stateBtn1 = 0;
+volatile uint8_t stateBtn2 = 0;
+volatile uint8_t stateBtn3 = 0;
+volatile uint8_t stateBtn4 = 0;
+volatile uint8_t stateSw1 = 0;
+volatile uint8_t stateSw2 = 0;
 
-	OUT_LED = 0x30;				// LEDs off
+// Channel, bank and tuner states.
+volatile uint8_t currentChannel = 0;
+volatile uint8_t currentBank = 0;
+volatile uint8_t currentTuner = 0;
 
-	//	Timer1 init
-	TCCR1A |= (1 << WGM01);			//	set timer 0 to CTC-Mode
-	TCCR1B &= ~(1 << CS10);			//	prescaler 8  (1MHz)
-	TCCR1B |= (1 << CS11);
-	TCCR1B &= ~(1 << CS12);
-	OCR1A = 499;				//	period of 1 ms
+// Global interrupt counter.
+uint16_t cnt = 0;
 
-	cli();					// disable interrupts
-	TIMSK |= (1 << TOIE1);			// activate timer 1
-	sei();					//  enable interrupts
-
-	//USART_Init(38400);			// initialize USART with 38400 baud (debug)
-	USART_Init(31250);			// initialize USART with 31250 baud
-
-	while (1) {
-		if (iBtn1_counter > BTN_THRESHOLD && cBtn1 == 0)		// Button 1
-		{
-			Bank(cBank);
-			USART_Transmit(0xC0);	// Channel 1
-			USART_Transmit(0x00);
-
-			cChannel = 0;
-			cTuner = 0;
-
-			cBtn1 = 1;
-		}
-		else if (iBtn1_counter < BTN_THRESHOLD && cBtn1 == 1)
-			cBtn1 = 0;
-
-		if(iBtn2_counter > BTN_THRESHOLD && cBtn2 == 0)			// Button 2
-		{
-			Bank(cBank);
-			USART_Transmit(0xC0);	// Channel 2
-			USART_Transmit(0x01);
-			cChannel = 1;
-			cTuner = 0;
-
-			cBtn2 = 1;
-		}
-		else if (iBtn2_counter < BTN_THRESHOLD && cBtn2 == 1)
-			cBtn2 = 0;
-
-		if(iBtn3_counter > BTN_THRESHOLD && cBtn3 == 0)			// Button 3
-		{
-			Bank(cBank);
-			USART_Transmit(0xC0);	// Channel 3
-			USART_Transmit(0x02);
-			cChannel = 2;
-			cTuner = 0;
-
-			cBtn3 = 1;
-		}
-		else if (iBtn3_counter < BTN_THRESHOLD && cBtn3 == 1)
-			cBtn3 = 0;
-
-		if(iBtn4_counter > BTN_THRESHOLD && cBtn4 == 0)			// Button 4
-		{
-			USART_Transmit(0xB0);	// Tuner on
-			USART_Transmit(0x0F);
-			USART_Transmit(0x7F);
-
-			//OUT_LED |= (1 << LED_RT);	// LEDs off
-			//OUT_LED |= (1 << LED_GN);
-
-			cTuner = 1;
-
-			cBtn4 = 1;
-		}
-		else if (iBtn4_counter < BTN_THRESHOLD && cBtn4 == 1)
-			cBtn4 = 0;
-
-		if (iSw1_counter > BTN_THRESHOLD && cSw1 == 0)			// Switch Pos. 1
-		{
-			Bank(0);
-			cBank = 0;
-			cTuner = 0;
-
-			cSw1 = 1;
-		}
-		else if (iSw1_counter < BTN_THRESHOLD && cSw1 == 1)
-		{
-			cSw1 = 0;
-		}
-
-		if(iSw2_counter > BTN_THRESHOLD && cSw2 == 0)			// Switch Pos. 2
-		{
-			Bank(1);
-			cBank = 1;
-			cTuner = 0;
-
-			cSw2 = 1;
-		}
-		else if (iSw2_counter < BTN_THRESHOLD && cSw2 == 1)
-		{
-			cSw2 = 0;
-		}
-
-	}
+/**
+ * Switch to given bank.
+ *
+ * @param bank Bank number (supported 1 and 2).
+ * @return void
+ */
+void setBank(uint8_t bank) {
+    if (bank == 0) {
+        USART_Transmit(0xB0);       // Bank 1
+        USART_Transmit(0x00);
+        USART_Transmit(0x00);
+        USART_Transmit(0xC0);
+        USART_Transmit(0x00);
+        USART_Transmit(currentChannel);
+        OUT_LED &= ~(1 << LED_RT);  // LED green
+        OUT_LED |= (1 << LED_GN);
+    } else {
+        USART_Transmit(0xB0);       // Bank 2
+        USART_Transmit(0x00);
+        USART_Transmit(0x01);
+        USART_Transmit(0xC0);
+        USART_Transmit(0x00);
+        USART_Transmit(currentChannel);
+        OUT_LED &= ~(1 << LED_GN);  // LED green
+        OUT_LED |= (1 << LED_RT);
+    }
 }
 
-void Bank(unsigned char num) {
-	if(num == 0) {
-		USART_Transmit(0xB0);		// Bank 1
-		USART_Transmit(0x00);
-		USART_Transmit(0x00);
-		USART_Transmit(0xC0);
-		USART_Transmit(0x00);
-		USART_Transmit(cChannel);
-		OUT_LED &= ~(1 << LED_RT);	// LED green
-		OUT_LED |= (1 << LED_GN);
-	}
-	else {
-		USART_Transmit(0xB0);		// Bank 2
-		USART_Transmit(0x00);
-		USART_Transmit(0x01);
-		USART_Transmit(0xC0);
-		USART_Transmit(0x00);
-		USART_Transmit(cChannel);
-		OUT_LED &= ~(1 << LED_GN);	// LED green
-		OUT_LED |= (1 << LED_RT);
-	}
+/**
+ * Main routine.
+ *
+ * @return This method should never terminate.
+ */
+int main(void) {
+    DDRB = 0x00;                // PB1..7 as inputs
+    PORTB = 0xFF;               // interlan pull-ups for PB0..7y
+    DDRD = 0x30;                // PD4..5 as outputs
+    PORTD = 0x0C;               // internal pull-ups for PD2..3
+
+    OUT_LED = 0x30;             // LEDs off
+
+    //	Timer1 init
+    TCCR1A |= (1 << WGM01);     // Set timer 0 to CTC-Mode
+    TCCR1B &= ~(1 << CS10);     // Prescaler 8  (1MHz)
+    TCCR1B |= (1 << CS11);
+    TCCR1B &= ~(1 << CS12);
+    OCR1A = 499;                // Pperiod of 1 ms
+
+    cli();                      // Disable interrupts
+    TIMSK |= (1 << TOIE1);      // Activate timer 1
+    sei();                      // Enable interrupts
+
+    //USART_Init(38400);		// Initialize USART with 38400 baud (debug)
+    USART_Init(31250);          // Initialize USART with 31250 baud
+
+    while (1) {
+        if (counterBtn1 > BTN_THRESHOLD && stateBtn1 == 0) {  // Button 1
+            setBank(currentBank);
+            USART_Transmit(0xC0);   // Channel 1
+            USART_Transmit(0x00);
+
+            currentChannel = 0;
+            currentTuner = 0;
+
+            stateBtn1 = 1;
+        } else if (counterBtn1 < BTN_THRESHOLD && stateBtn1 == 1) {
+            stateBtn1 = 0;
+        }
+
+        if (counterBtn2 > BTN_THRESHOLD && stateBtn2 == 0) {  // Button 2
+            setBank(currentBank);
+            USART_Transmit(0xC0);   // Channel 2
+            USART_Transmit(0x01);
+            currentChannel = 1;
+            currentTuner = 0;
+
+            stateBtn2 = 1;
+        } else if (counterBtn2 < BTN_THRESHOLD && stateBtn2 == 1) {
+            stateBtn2 = 0;
+        }
+
+        if (counterBtn3 > BTN_THRESHOLD && stateBtn3 == 0) {  // Button 3
+            setBank(currentBank);
+            USART_Transmit(0xC0);   // Channel 3
+            USART_Transmit(0x02);
+            currentChannel = 2;
+            currentTuner = 0;
+
+            stateBtn3 = 1;
+        } else if (counterBtn3 < BTN_THRESHOLD && stateBtn3 == 1) {
+            stateBtn3 = 0;
+        }
+
+        if (counterBtn4 > BTN_THRESHOLD && stateBtn4 == 0) {  // Button 4
+            USART_Transmit(0xB0);   // Tuner on
+            USART_Transmit(0x0F);
+            USART_Transmit(0x7F);
+
+            //OUT_LED |= (1 << LED_RT);	// LEDs off
+            //OUT_LED |= (1 << LED_GN);
+
+            currentTuner = 1;
+
+            stateBtn4 = 1;
+        } else if (counterBtn4 < BTN_THRESHOLD && stateBtn4 == 1) {
+            stateBtn4 = 0;
+        }
+
+        if (counterSw1 > BTN_THRESHOLD && stateSw1 == 0) {    // Switch Pos. 1
+            setBank(0);
+            currentBank = 0;
+            currentTuner = 0;
+
+            stateSw1 = 1;
+        } else if (counterSw1 < BTN_THRESHOLD && stateSw1 == 1) {
+            stateSw1 = 0;
+        }
+
+        if (counterSw2 > BTN_THRESHOLD && stateSw2 == 0) {    // Switch Pos. 2
+            setBank(1);
+            currentBank = 1;
+            currentTuner = 0;
+
+            stateSw2 = 1;
+        } else if (counterSw2 < BTN_THRESHOLD && stateSw2 == 1) {
+            stateSw2 = 0;
+        }
+
+    }
 }
 
-/* interrupt function:	TIMER1_OVF1_vect
+/**
+ * interrupt function:	TIMER1_OVF1_vect
  *
  * Timer interrupt. Increments counters for inputs
  */
-ISR(TIMER1_OVF_vect)
-{
-	if(++cnt == 250) cnt = 0;
-	if(cTuner == 1 && cnt == 1) {
-		if(cBank == 1) OUT_LED ^= (1<<LED_GN);
-		else OUT_LED ^= (1<<LED_RT);
-	}
+ISR(TIMER1_OVF_vect) {
+    if (++cnt == 250) {
+        cnt = 0;
+    }
 
-	if (bit_is_clear(IN_BTN, BTN1)) {					// Button 1
-		if (iBtn1_counter < BTN_MAX) iBtn1_counter++;
-	} else {
-		if (iBtn1_counter > 0) iBtn1_counter--;
-	}
-	if (bit_is_clear(IN_BTN, BTN2)) {					// Button 2
-		if (iBtn2_counter < BTN_MAX) iBtn2_counter++;
-	} else {
-		if (iBtn2_counter > 0) iBtn2_counter--;
-	}
-	if (bit_is_clear(IN_BTN, BTN3)) {					// Button 3
-		if (iBtn3_counter < BTN_MAX) iBtn3_counter++;
-	} else {
-		if (iBtn3_counter > 0) iBtn3_counter--;
-	}
-	if (bit_is_clear(IN_BTN, BTN4)) {					// Button 4
-		if (iBtn4_counter < BTN_MAX) iBtn4_counter++;
-	} else {
-		if (iBtn4_counter > 0) iBtn4_counter--;
-	}
-	if (bit_is_clear(IN_SW, SW1)) {						// Switch 1
-		if (iSw1_counter < BTN_MAX) iSw1_counter++;
-	} else {
-		if (iSw1_counter > 0) iSw1_counter--;
-	}
-	if (bit_is_clear(IN_SW, SW2)) {						// Switch 2
-		if (iSw2_counter < BTN_MAX) iSw2_counter++;
-	} else {
-		if (iSw2_counter > 0) iSw2_counter--;
-	}
+    if (currentTuner == 1 && cnt == 1) {
+        if (currentBank == 1) {
+            OUT_LED ^= (1 << LED_GN);
+        } else {
+            OUT_LED ^= (1 << LED_RT);
+        }
+    }
+
+    if (bit_is_clear(IN_BTN, BTN1) && counterBtn1 < BTN_MAX) {  // Button 1
+        counterBtn1++;
+    } else if (counterBtn1 > 0) {
+        counterBtn1--;
+    }
+
+    if (bit_is_clear(IN_BTN, BTN2) && counterBtn2 < BTN_MAX) {  // Button 2
+        counterBtn2++;
+    } else  if (counterBtn2 > 0) {
+        counterBtn2--;
+    }
+
+
+    if (bit_is_clear(IN_BTN, BTN3) && counterBtn3 < BTN_MAX) {  // Button 3
+        counterBtn3++;
+    } else if (counterBtn3 > 0) {
+        counterBtn3--;
+    }
+
+    if (bit_is_clear(IN_BTN, BTN4) && counterBtn4 < BTN_MAX) {  // Button 4
+       counterBtn4++;
+    } else if (counterBtn4 > 0) {
+        counterBtn4--;
+    }
+
+    if (bit_is_clear(IN_SW, SW1) && counterSw1 < BTN_MAX) {     // Switch 1
+        counterSw1++;
+    } else if (counterSw1 > 0) {
+        counterSw1--;
+    }
+
+    if (bit_is_clear(IN_SW, SW2) && counterSw2 < BTN_MAX) {     // Switch 2
+        counterSw2++;
+    } else if (counterSw2 > 0) {
+        counterSw2--;
+    }
 }
-
diff --git a/firmware/midifs.h b/firmware/midifs.h
index 4315956..a22481f 100644
--- a/firmware/midifs.h
+++ b/firmware/midifs.h
@@ -1,39 +1,24 @@
-// midi-to-parport.h
-// Typ: C-Headerfile
-// Project: MIDI-to-Parallelport interface
-// Date: 27.08.2012
-// Author: ska
+/**
+ * MIDI Footswitch
+ *
+ * @file   main.h
+ * @author Stefan Kalscheuer
+ * @date   2014-06-17
+ */
 
-#define IN_SW	PIND
-#define SW1		PD2
-#define SW2		PD3
+#define IN_SW   PIND
+#define SW1     PD2
+#define SW2     PD3
 
-#define	IN_BTN	PINB
-#define BTN1	PB0
-#define	BTN2	PB1
-#define	BTN3	PB2
-#define BTN4	PB3
+#define IN_BTN  PINB
+#define BTN1    PB0
+#define BTN2    PB1
+#define BTN3    PB2
+#define BTN4    PB3
 
-#define OUT_LED	PORTD
-#define LED_RT	PD5
-#define LED_GN	PD4
+#define OUT_LED PORTD
+#define LED_RT  PD5
+#define LED_GN  PD4
 
-#define BTN_THRESHOLD	50
-#define BTN_MAX			100
-
-
-void delayms(unsigned int millis) {
-//  uint16_t loop;
-  while ( millis ) {
-    _delay_ms(1);
-    millis--;
-  }
-}
-
-void delayus(unsigned long millis) {
-//  uint16_t loop;
-  while ( millis ) {
-    _delay_us(1);
-    millis--;
-  }
-}
+#define BTN_THRESHOLD   50
+#define BTN_MAX         100
diff --git a/firmware/usart.h b/firmware/usart.h
index 931593b..613fbe2 100644
--- a/firmware/usart.h
+++ b/firmware/usart.h
@@ -1,36 +1,63 @@
-void USART_Init( unsigned long baud )
-{
-	/* Calculate UBRR */
-	unsigned int ubrr_val = (unsigned int)((F_CPU/(baud*16L))-1);
-	/* Set baud rate */
-	UBRRH = (unsigned char)(ubrr_val>>8);
-	UBRRL = (unsigned char)ubrr_val;
-	/* Enable transmitter */
-	UCSRB |= (1<<TXEN);
-	/* Set frame format: 8data, 1stop bit */
-	UCSRC = (3<<UCSZ0);
+/**
+ * MIDI Footswitch
+ *
+ * @file   main.h
+ * @author Stefan Kalscheuer
+ * @date   2014-06-17
+ * @brief  USART helper funcitons
+ */
+
+/**
+ * Initialize USART interface with given BAUD rate.
+ *
+ * @param baud Baud rate.
+ * @return void
+ */
+void USART_Init(uint32_t baud) {
+    /* Calculate UBRR */
+    uint16_t ubrr_val = (uint16_t) ((F_CPU / (baud * 16L)) - 1);
+    /* Set baud rate */
+    UBRRH = (uint8_t) (ubrr_val >> 8);
+    UBRRL = (uint8_t) ubrr_val;
+    /* Enable transmitter */
+    UCSRB |= (1 << TXEN);
+    /* Set frame format: 8data, 1stop bit */
+    UCSRC = (3 << UCSZ0);
 }
 
-unsigned char USART_Receive( void )
-{
-	/* Wait for data to be received */
-	while ( !(UCSRA & (1<<RXC)) )
-		;
-	/* Get and return received data from buffer */
-	return UDR;
+/**
+ * Receive USART data from buffer.
+ *
+ * @return Data byte from buffer.
+ */
+unsigned char USART_Receive(void) {
+    /* Wait for data to be received */
+    while (!(UCSRA & (1 << RXC)));
+    /* Get and return received data from buffer */
+    return UDR;
 }
 
-void USART_Transmit( unsigned char data )
-{
-	/* Wait for empty transmit buffer */
-	while ( !( UCSRA & (1<<UDRE)) )
-		;
-	/* Put data into buffer, sends the data */
-	UDR = data;
+/**
+ * Transmit USART data byte.
+ *
+ * @param data Data byte to transmit.
+ * @return void
+ */
+void USART_Transmit(uint8_t data) {
+    /* Wait for empty transmit buffer */
+    while (!(UCSRA & (1 << UDRE)));
+    /* Put data into buffer, sends the data */
+    UDR = data;
 }
 
-void USART_Flush( void )
-{
-	unsigned char dummy;
-	while ( UCSRA & (1<<RXC) ) dummy = UDR;
+/**
+ * Flush USART buffer.
+ *
+ * @return void
+ */
+void USART_Flush(void) {
+    uint8_t dummy;
+    while (UCSRA & (1 << RXC)) {
+        dummy = UDR;
+    }
 }