diff --git a/fw_test/main.c b/fw_test/main.c
index ddf33f6..ad5ea7e 100644
--- a/fw_test/main.c
+++ b/fw_test/main.c
@@ -12,55 +12,57 @@
 
 
 
-void init_leds(void){
+static void init_leds(void){
 	//enable LED channels as output
 	DDRC |= (1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3);
 	// both LEDs off
 	PORTC &= ~((1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3));
  	return;
-};
+}
 
-void inline led_on(int leds){
+static void inline led_on(int leds){
  	PORTC |= leds;
-};
+}
 
-void inline led_off(int leds){
+static void inline led_off(int leds){
 	PORTC &= ~leds;
-};
+	return;
+}
 
+static void inline buzzr_off(){
+	PORTC &= ~(1 << PORTC5);
+	PORTB &= ~(1 << PORTB2);
+}
 
-
-void init_buzzr(){
+static void init_buzzr(void){
 	//its on B2 and C5, for reasons
 	DDRC |= (1 << PORTC5);
 	DDRB |= (1 << PORTB2);
 	//switch it off
 	buzzr_off();
 	return;
-};
+}
 
-void buzzr_up(){
+static void buzzr_up(void){
 	PORTC &= ~(1 << PORTC5);
 	PORTB |= (1 << PORTB2);
 	return;
-};
+}
 
-void buzzr_down(){
+static void buzzr_down(){
 	PORTC |= (1 << PORTC5);
 	PORTB &= ~(1 << PORTB2);
-};
+}
 
-void inline buzzr_off(){
-	PORTC &= ~(1 << PORTC5);
-	PORTB &= ~(1 << PORTB2);
-};
-void buzzr_inv(){
+
+
+static void buzzr_inv(){
 	PORTC ^= (1 << PORTC5);
 	PORTB ^= (1 << PORTB2);
-};
+}
 
 
-void init_motor(void)
+static void init_motor(void)
 {
 	/* vibration motor on B1, initially off: */
 	DDRB  |= (1 << PORTB1);
@@ -68,18 +70,19 @@ void init_motor(void)
 	return;
 }
 
-void set_motor(int val){
+static void set_motor(int val){
 	PORTB = ~(val);
 	PORTB = PORTB;
 	return;
-};
+}
 
 
-void init_switch(void){
+static void init_switch(void){
+	DDRD &= ~( (1 << PORTD1) | (1<<PORTD0))
 	return;
-};
+}
 
-void blinkrattlebeep(void){
+static void blinkrattlebeep(void){
 	led_off(LED_L|LED_R);
 	_delay_ms(100);	
 	led_on(LED_L);
@@ -97,9 +100,9 @@ void blinkrattlebeep(void){
 		buzzr_inv();
 	};
 	buzzr_off();
-};
+}
 
-void mode_motortest(void)
+static void mode_motortest(void)
 {
 	set_motor(MOTOR_ON);
 	_delay_ms(400);
@@ -109,7 +112,7 @@ void mode_motortest(void)
 }
 
 
-void mode_blinktest(void)
+static void mode_blinktest(void)
 {
 	led_off(LED_L|LED_R);
 	_delay_ms(100);	
@@ -119,9 +122,19 @@ void mode_blinktest(void)
 	led_on(LED_R);
 	_delay_ms(100);
 	led_off(LED_R);	
-	return
+	return;
 }
 
+static void mode_beeptest(void)
+{
+	buzzr_up();
+	for (int i=0; i<100; i++){
+		_delay_ms(2);
+		buzzr_inv();
+	};
+	buzzr_off();
+	return;
+}
 
 void __attribute__((noreturn)) 
 main(void)
@@ -131,21 +144,42 @@ main(void)
 	init_motor();
 	init_buzzr();
 	init_leds();
+	init_switch();
 	uint8_t mode = 0;
 
-
-
-
 	for(;;) /* ever */  {
 		//do something
 
 		//TODO: check switches and change mode
 
 		switch(mode){
-			case 1 : led_on(LED_L|LED_R); _delay_ms(500); led_off(LED_L|LED_R); break;
-			case 2 : break;
-			default : blinkrattlebeep();
-		};
+			case 1: 
+				mode_blinktest();
+				break;
+			case 2:
+				mode_motortest();
+				break;
+			case 3: 
+				mode_beeptest();
+				break();
+			case 4:
+			default:
+				mode=0;
+			case 0:
+				blinkrattlebeep();
+
+		}
+		
+		switch( PIND & 3){
+			case 1:
+				mode++
+				break;
+			case 2:
+				mode --;
+				break;
+			case 3:
+			default:
+		}
 
 	}
 	/* never  return 0; */