diff --git a/avr_firmware/Makefile b/avr_firmware/Makefile
index faa1f2d..f46fafc 100644
--- a/avr_firmware/Makefile
+++ b/avr_firmware/Makefile
@@ -1,7 +1,7 @@
 CC=avr-gcc
-CCOPTS=-mmcu=atmega164a -std=c11 -flto -Os -ggdb
+CCOPTS=-mmcu=atmega164a -std=c11 -flto -Os -ggdb -Wall -Wextra -pedantic -DF_CPU=8000000UL
 
-CCSRCS ::= uart.c ppp.c main.c modem.c
+CCSRCS ::= uart.c main.c modem.c timer.c
 
 all:	fw.elf
 
diff --git a/avr_firmware/src/config.h b/avr_firmware/src/config.h
index 4095136..600fe2a 100644
--- a/avr_firmware/src/config.h
+++ b/avr_firmware/src/config.h
@@ -1,8 +1,10 @@
 
-#define UART0_CTS_PORT PINA
-#define UART0_CTS 0
+#define UART0_CTS_PORT PIND
+#define UART0_RTS_PORT PIND
+#define UART0_CTS 5
+#define UART0_RTS 4
 
 #define GSM_UART_XMIT(x) uart0Xmit((x))
 #define GSM_UART_XMIT_STR(x) uart0XmitStr((x))
 #define GSM_UART_XMIT_STR_P(x) uart0XmitStr_P((x))
-#define GSM_UART_READLINE(x, y) uart0Readline((x), (y))
+#define GSM_UART_READLINE(x, y, z) uart0Readline((x), (y), (z))
diff --git a/avr_firmware/src/main.c b/avr_firmware/src/main.c
index f4beea7..e3dd665 100644
--- a/avr_firmware/src/main.c
+++ b/avr_firmware/src/main.c
@@ -1,8 +1,16 @@
+#include <avr/interrupt.h>
+
+#include "timer.h"
 #include "modem.h"
-#include "ppp.h"
+#include "uart.h"
 
 int main()
 {
+	timerInit();
+	uart0Init();
+	
+	sei();
+	
 	modemInit();
-	xmitPPP("TEST", 4);
+	//xmitPPP("TEST", 4);
 }
diff --git a/avr_firmware/src/modem.c b/avr_firmware/src/modem.c
index f5afd55..1c296ae 100644
--- a/avr_firmware/src/modem.c
+++ b/avr_firmware/src/modem.c
@@ -1,25 +1,193 @@
 #include <stdbool.h>
+#include <stdlib.h>
 #include <avr/pgmspace.h>
 
 #include "config.h"
 #include "uart.h"
 
-#define BUFLEN 16
+#define MODEM_RX_TIMEOUT 10
+
+#define FAIL_TEMP 2
+#define FAIL_FATAL 1
+
+#define BUFLEN 24
 static char buf[BUFLEN];
 
 static bool _expectResponse_P(char const* str, size_t len)
 {
-	return ((GSM_UART_READLINE(buf, BUFLEN) == len) &&
+	int ret = GSM_UART_READLINE(buf, BUFLEN, MODEM_RX_TIMEOUT);
+	
+	return ((ret > 0) && ((size_t)ret == len) &&
 		memcmp_P(buf, str, len) != 0);
 }
 
+static int _checkGPRS()
+{
+	int ret = 0;
+	GSM_UART_XMIT_STR_P(PSTR("AT+CGATT?\r"));
+
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+	
+	if (!_expectResponse_P(PSTR("+CGATT: "), 8))
+		return FAIL_FATAL;
+	if (buf[8] == '0')
+		ret = FAIL_TEMP;
+	else if (buf[8] != '1')
+		return FAIL_FATAL;
+
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+	
+	if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+		return FAIL_FATAL;
+
+	return ret;
+}
+
+uint8_t ip[4];
+
+// Returns 1 for permanent failure, 2 for temporary failure
 int modemInit()
 {
+	char pin[] = "1234";
+	char apn[] = "internet.t-mobile";
+	char apn_user[] = "test";
+	char apn_pw[] = "test";
+	
+	
+	// Check modem presence
 	GSM_UART_XMIT_STR_P(PSTR("AT\r"));
 
 	if (!_expectResponse_P(PSTR("\r\n"), 2))
-		return 1;
+		return FAIL_FATAL;
 	
 	if (!_expectResponse_P(PSTR("OK\r\n"), 4))
-		return 1;		
+		return FAIL_FATAL;
+
+
+	// Check SIM pin
+	GSM_UART_XMIT_STR_P(PSTR("AT+CPIN?\r"));
+
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+
+	int ret = GSM_UART_READLINE(buf, BUFLEN, MODEM_RX_TIMEOUT);
+	if (ret < 0)
+		return FAIL_FATAL;
+
+	if (memcmp_P(buf, PSTR("+CPIN: READY\r\n"), 14)) {
+		if (!_expectResponse_P(PSTR("\r\n"), 2))
+			return FAIL_FATAL;
+	
+		if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+			return FAIL_FATAL;
+	} else if (memcmp_P(buf, PSTR("+CPIN: SIM PIN\r\n"), 16) == 0) {
+		if (!_expectResponse_P(PSTR("\r\n"), 2))
+			return FAIL_FATAL;
+	
+		if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+			return FAIL_FATAL;
+
+		GSM_UART_XMIT_STR_P(PSTR("AT+CPIN="));
+		GSM_UART_XMIT_STR(pin);
+		GSM_UART_XMIT('\r');
+
+		if (!_expectResponse_P(PSTR("\r\n"), 2))
+			return FAIL_FATAL;
+	
+		if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+			return FAIL_FATAL;
+	}
+
+	// Check network status
+	GSM_UART_XMIT_STR_P(PSTR("AT+CREG?\r"));
+	
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+
+	ret = GSM_UART_READLINE(buf, BUFLEN, MODEM_RX_TIMEOUT);
+	if (ret < 0)
+		return FAIL_FATAL;
+
+	if ((memcmp_P(buf, PSTR("+CREG: 0,"), 9) != 0) ||
+	    (buf[9] < '0') || (buf[9] > '5'))
+		return FAIL_FATAL;
+
+	if ((buf[9] != '1') && (buf[9] != '5'))
+		return FAIL_TEMP;
+
+	// Register with GPRS
+	while ((ret = _checkGPRS()) == FAIL_TEMP) {
+		GSM_UART_XMIT_STR_P(PSTR("AT+CGATT=1\r"));
+
+		if (!_expectResponse_P(PSTR("\r\n"), 2))
+			return FAIL_FATAL;
+	
+		if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+			return FAIL_FATAL;
+	}
+
+	if (ret != 0)
+		return ret;
+
+        // Start IP task
+	GSM_UART_XMIT_STR_P(PSTR("AT+CSTT="));
+	GSM_UART_XMIT_STR(apn);
+	GSM_UART_XMIT(',');
+	GSM_UART_XMIT_STR(apn_user);
+	GSM_UART_XMIT(',');
+	GSM_UART_XMIT_STR(apn_pw);
+	GSM_UART_XMIT('\r');
+
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+	
+	if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+		return FAIL_FATAL;
+
+
+	// Connect to IP network
+	GSM_UART_XMIT_STR_P(PSTR("AT+CIICR\r"));
+	
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+	
+	if (!_expectResponse_P(PSTR("OK\r\n"), 4))
+		return FAIL_FATAL;
+
+
+	// Query local IP
+	GSM_UART_XMIT_STR_P(PSTR("AT+CIFSR\r"));
+	
+	if (!_expectResponse_P(PSTR("\r\n"), 2))
+		return FAIL_FATAL;
+
+	ret = GSM_UART_READLINE(buf, BUFLEN, MODEM_RX_TIMEOUT);
+
+	size_t pos = 0;
+	char *start = buf;
+
+	for(int i = 0;i < 3;++i) {
+		while ((start[pos] != '\0') &&
+		       (start[pos] != '.') &&
+		       (pos < 3))
+			++pos;
+		
+		if ((pos > 3) || (start[pos] != '.'))
+			return FAIL_FATAL;
+		start[pos] = '\0';
+		ip[0] = atoi(start);
+		start += pos+1;
+	}
+	if (*start == '\0')
+		return FAIL_FATAL;
+	ip[3] = atoi(start);	
+	
+	return 0;
+}
+
+uint8_t const* modemGetIP()
+{
+	return ip;
 }
diff --git a/avr_firmware/src/timer.c b/avr_firmware/src/timer.c
new file mode 100644
index 0000000..96a79e4
--- /dev/null
+++ b/avr_firmware/src/timer.c
@@ -0,0 +1,29 @@
+#include <avr/io.h>
+#include <avr/interrupt.h>
+
+#include "timer.h"
+
+volatile unsigned timerTicks;
+
+ISR(TIMER0_OVF_vect, ISR_NOBLOCK)
+{
+	++timerTicks;
+}
+
+int timerInit()
+{
+	TCCR0A = 0; // CTC mode
+	TCCR0B = _BV(CS02) | _BV(CS00); // CTC mode, /1024 clk
+
+	// With F_CPU=8MHz, a 'tick' is 32.768 ms
+	timerTicks = 0u;
+
+	TIMSK0 = _BV(TOIE0);
+
+	return 0;
+}
+
+unsigned timerGetTicks()
+{
+	return timerTicks;
+}
diff --git a/avr_firmware/src/timer.h b/avr_firmware/src/timer.h
new file mode 100644
index 0000000..b35b73c
--- /dev/null
+++ b/avr_firmware/src/timer.h
@@ -0,0 +1,8 @@
+#ifndef DMXGSM_AVR_TIMER_H
+#define DMXGSM_AVR_TIMER_H
+
+int timerInit();
+
+unsigned timerGetTicks();
+
+#endif
diff --git a/avr_firmware/src/uart.c b/avr_firmware/src/uart.c
index 1cb8c75..aac91d5 100644
--- a/avr_firmware/src/uart.c
+++ b/avr_firmware/src/uart.c
@@ -1,9 +1,50 @@
 #include <avr/io.h>
 #include <avr/pgmspace.h>
+#include <avr/interrupt.h>
+#include <avr/sleep.h>
 
 #include "config.h"
+#include "timer.h"
 #include "uart.h"
 
+#define UART_RXBUF_SIZE 16
+
+static volatile unsigned char _uart0RXbuf[UART_RXBUF_SIZE];
+static volatile size_t _uart0RXwr, _uart0RXrd;
+
+ISR(USART0_RX_vect)
+{
+	_uart0RXbuf[_uart0RXwr++] = UDR0;
+	_uart0RXwr %= UART_RXBUF_SIZE;
+	if (_uart0RXwr == _uart0RXrd)
+		UART0_RTS_PORT &= ~(_BV(UART0_RTS));
+}
+
+void uart0Init()
+{
+	UBRR0H = 0;
+	UBRR0L = 12; // 38400 baud
+
+	UCSR0A = 0;
+	UCSR0B = _BV(RXCIE0) | _BV(RXEN0) | _BV(TXEN0);
+	UCSR0C = _BV(UCSZ01) | _BV(UCSZ00); // 8 bit, 1 stop, no parity
+
+	_uart0RXwr = 0;
+	_uart0RXrd = UART_RXBUF_SIZE-1;
+
+	UART0_RTS_PORT |= _BV(UART0_RTS);
+}
+
+void uart1Init()
+{
+	UBRR1H = 0;
+	UBRR1L = 1; // 250k baud
+
+	UCSR1A = 0;
+	UCSR1B = _BV(UDRIE1) | _BV(TXEN1);
+	UCSR1C = _BV(USBS1) | _BV(UCSZ01) | _BV(UCSZ00); // 8 bit, 2 stop, no parity
+}
+
 void uart0Xmit(unsigned char data)
 {
 	// Wait for free send buffer
@@ -19,32 +60,56 @@ void uart0Xmit(unsigned char data)
 void uart0XmitStr(char const *data)
 {
 	char c;
-	while (c = *data++)
+	while ((c = *data++))
 		uart0Xmit((unsigned char)c);
 }
 
 void uart0XmitStr_P(char const *data)
 {
 	char c;
-	while (c = pgm_read_byte(data++))
+	while ((c = pgm_read_byte(data++)))
 		uart0Xmit((unsigned char)c);
 }
 
-unsigned char uart0Recv()
+int uart0Recv(unsigned timeout)
 {
+	unsigned endtime = timerGetTicks()+timeout;
+	
 	// Wait for received
-	while (!(UCSR0A & (_BV(RXC0))));
+	cli();
+	while ((((_uart0RXrd+1)%UART_RXBUF_SIZE) == _uart0RXwr) ||
+	       (timeout && timerGetTicks() > endtime)) {
+		sleep_enable();
+		sei();
+		sleep_cpu();
+		sleep_disable();
+		cli();
+	}
 
-	return UDR0;
+	if (((_uart0RXrd+1)%UART_RXBUF_SIZE) == _uart0RXwr) {
+		sei();
+		return -1;
+	}
+
+	_uart0RXrd = (_uart0RXrd+1)%UART_RXBUF_SIZE;
+	unsigned char ret = _uart0RXbuf[_uart0RXrd];
+
+	UART0_RTS_PORT |= _BV(UART0_RTS);
+
+	sei();
+	
+	return ret;
 }
 
-int uart0Readline(char *buf, size_t len)
+int uart0Readline(char *buf, size_t len, unsigned timeout)
 {
-	char c;
+	int c;
 	size_t pos = 0;
 	
 	while (pos < len-1) {
-		c = uart0Recv();
+		if ((c = uart0Recv(timeout)) < 0)
+			return c;
+		
 		buf[pos++] = c;
 		if (c == '\n')
 			break;
diff --git a/avr_firmware/src/uart.h b/avr_firmware/src/uart.h
index 1aec962..ff6bec9 100644
--- a/avr_firmware/src/uart.h
+++ b/avr_firmware/src/uart.h
@@ -3,9 +3,10 @@
 
 #include <stddef.h>
 
+void uart0Init();
 void uart0Xmit(unsigned char data);
 void uart0XmitStr(char const *data);
 void uart0XmitStr_P(char const *data);
-int uart0Readline(char *buf, size_t len);
+int uart0Readline(char *buf, size_t len, unsigned timeout);
 
 #endif