diff --git a/README.md b/README.md
index 8b48408..f4c9b23 100644
--- a/README.md
+++ b/README.md
@@ -28,3 +28,6 @@ libdeps_dir = depends
 In diesem Fall sollte man davon absehen, die automatisch vom PIO 
 heruntergeladenen Bibliotheken ins Git des eigenen Projektes einzuchecken.
 Es empfiehlt sich in dem Fall `depends` ins `.gitignore` einzufügen.
+
+## Interrrupts
+Die UART-Schnittstelle und TWI bzw. I^2C arbeiten mit Interrupts. Bei der Initialisierung der jeweiligen (oder auch beiden) Schnittstelle mit UART_Init() bzw. TWI_Init() werden die Interrupts global aktiviert. Die Schnittstellen sind sofort funktionsfähig und können direkt verwendet werden. 
\ No newline at end of file
diff --git a/src/dataflash.c b/src/dataflash.c
index a2510b3..86317d2 100644
--- a/src/dataflash.c
+++ b/src/dataflash.c
@@ -4,8 +4,10 @@
 #include "spi.h"
 #include "dataflash.h"
 
-void
-dataflash_opcode_and_address (unsigned char opcode, unsigned int page, unsigned int offset)
+// Prevent Double Initialization
+unsigned short dataflash_flag = 0;
+
+void dataflash_opcode_and_address(unsigned char opcode, unsigned int page, unsigned int offset)
 {
   SPI_MasterTransfer(opcode);
   SPI_MasterTransfer((unsigned char)(page >> 7) & 0x1f);
@@ -13,158 +15,156 @@ dataflash_opcode_and_address (unsigned char opcode, unsigned int page, unsigned
   SPI_MasterTransfer((unsigned char)(offset));
 }
 
-void
-dataflash_wait (void)
+void dataflash_wait(void)
 {
   DATAFLASH_Chip_Select;
   SPI_MasterTransfer(DATAFLASH_STATUS_REGISTER_READ);
-  
-  while (!(SPI_MasterTransferRead(0x00) & 0x80));
-  
+
+  while (!(SPI_MasterTransferRead(0x00) & 0x80))
+    ;
+
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_init (void)
+void dataflash_init(void)
 {
-  // AT45DB081 doesn't actually need an intialization,
-  // only the ChipSelect should be configured as an output.
-  DDRB |= (1 << PB4);
-  DATAFLASH_Chip_Unselect;
-  SPI_MasterInit();
+  // Check if already initialized
+  if (!(dataflash_flag & 1))
+  {
+    // Backup Status Register and disable Interrupts
+    uint8_t sreg = SREG;
+    cli();
+    // AT45DB081 doesn't actually need an intialization,
+    // only the ChipSelect should be configured as an output.
+    DDRB |= (1 << PB4);
+    DATAFLASH_Chip_Unselect;
+    SPI_MasterInit();
+
+    // Restore Status Register
+    SREG = sreg;
+
+    // Set dataflash Init Flag
+    dataflash_flag = 1;
+  }
 }
 
-void
-dataflash_buffer_to_page (unsigned int page, unsigned char buffer)
+void dataflash_buffer_to_page(unsigned int page, unsigned char buffer)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
 
   switch (buffer)
-    {
-    default:
-      dataflash_opcode_and_address
-        (DATAFLASH_BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_BUILT_IN_ERASE,
-         page, 0x00);
-      break;
-    
-    case 2:
-      dataflash_opcode_and_address
-        (DATAFLASH_BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_BUILT_IN_ERASE,
-         page, 0x00);
-      break;
-    }
+  {
+  default:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_1_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_BUILT_IN_ERASE,
+                                 page, 0x00);
+    break;
+
+  case 2:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_2_TO_MAIN_MEMORY_PAGE_PROGRAM_WITH_BUILT_IN_ERASE,
+                                 page, 0x00);
+    break;
+  }
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_page_to_buffer (unsigned int page, unsigned char buffer)
+void dataflash_page_to_buffer(unsigned int page, unsigned char buffer)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
 
   switch (buffer)
-    {
-    default:
-      dataflash_opcode_and_address 
-        (DATAFLASH_MAIN_MEMORY_PAGE_TO_BUFFER_1_TRANSFER,
-         page, 0x00);
-      break;
-    
-    case 2:
-      dataflash_opcode_and_address 
-        (DATAFLASH_MAIN_MEMORY_PAGE_TO_BUFFER_2_TRANSFER,
-         page, 0x00);
-      break;
-    }
+  {
+  default:
+    dataflash_opcode_and_address(DATAFLASH_MAIN_MEMORY_PAGE_TO_BUFFER_1_TRANSFER,
+                                 page, 0x00);
+    break;
+
+  case 2:
+    dataflash_opcode_and_address(DATAFLASH_MAIN_MEMORY_PAGE_TO_BUFFER_2_TRANSFER,
+                                 page, 0x00);
+    break;
+  }
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_buffer_read (unsigned char buffer, unsigned int offset,
-                       unsigned int length, unsigned char *array)
+void dataflash_buffer_read(unsigned char buffer, unsigned int offset,
+                           unsigned int length, unsigned char *array)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
 
   switch (buffer)
-    {
-    default:
-      dataflash_opcode_and_address 
-        (DATAFLASH_BUFFER_1_READ_LF,
-         0x00, offset);
-      break;
-    
-    case 2:
-      dataflash_opcode_and_address 
-        (DATAFLASH_BUFFER_2_READ_LF,
-         0x00, offset);
-      break;
-    }
-  
+  {
+  default:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_1_READ_LF,
+                                 0x00, offset);
+    break;
+
+  case 2:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_2_READ_LF,
+                                 0x00, offset);
+    break;
+  }
+
   offset = 0x00;
-  
+
   while (length--)
-    {
-      array[offset++] = SPI_MasterTransferRead(0x00);
-    }
+  {
+    array[offset++] = SPI_MasterTransferRead(0x00);
+  }
 
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_buffer_write (unsigned char buffer, unsigned int offset,
-                        unsigned int length, unsigned char *array)
+void dataflash_buffer_write(unsigned char buffer, unsigned int offset,
+                            unsigned int length, unsigned char *array)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
 
   switch (buffer)
-    {
-    default:
-      dataflash_opcode_and_address 
-        (DATAFLASH_BUFFER_1_WRITE,
-         0x00, offset);
-      break;
-    
-    case 2:
-      dataflash_opcode_and_address 
-        (DATAFLASH_BUFFER_2_WRITE,
-         0x00, offset);
-      break;
-    }
-  
+  {
+  default:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_1_WRITE,
+                                 0x00, offset);
+    break;
+
+  case 2:
+    dataflash_opcode_and_address(DATAFLASH_BUFFER_2_WRITE,
+                                 0x00, offset);
+    break;
+  }
+
   offset = 0x00;
 
   while (length--)
-    {
-      SPI_MasterTransfer(array[offset++]);
-    }
+  {
+    SPI_MasterTransfer(array[offset++]);
+  }
 
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_read (unsigned int page, unsigned int offset,
-                unsigned int length, unsigned char *array)
+void dataflash_read(unsigned int page, unsigned int offset,
+                    unsigned int length, unsigned char *array)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
 
   dataflash_opcode_and_address(DATAFLASH_CONTINUOUS_ARRAY_READ_LF, page, offset);
-  
+
   offset = 0x00;
-  
+
   while (length--)
-    {
-      array[offset++] = SPI_MasterTransferRead(0x00);
-    }
+  {
+    array[offset++] = SPI_MasterTransferRead(0x00);
+  }
 
   DATAFLASH_Chip_Unselect;
 }
 
-void
-dataflash_chip_erase (void)
+void dataflash_chip_erase(void)
 {
   dataflash_wait();
   DATAFLASH_Chip_Select;
diff --git a/src/fifo.c b/src/fifo.c
index 1095447..0888b71 100644
--- a/src/fifo.c
+++ b/src/fifo.c
@@ -1,47 +1,53 @@
 #include <stdint.h>
 #include "fifo.h"
 
-static uint8_t _fifo_get (fifo_t* f);
+static uint8_t _fifo_get(fifo_t *f);
 
-void fifo_init (fifo_t* f, uint8_t* buffer, const uint8_t size)
+void fifo_init(fifo_t *f, uint8_t *buffer, const uint8_t size)
 {
+  // Backup Status Register and disable Interrupts
+  uint8_t sreg = SREG;
+  cli();
   f->count = 0;
   f->pread = f->pwrite = buffer;
   f->read2end = f->write2end = f->size = size;
+  // Restore Status Register
+  SREG = sreg;
 }
 
-uint8_t fifo_get_wait (fifo_t* f)
+uint8_t fifo_get_wait(fifo_t *f)
 {
-  while (!f->count);
-  
-  return _fifo_get(f);	
+  while (!f->count)
+    ;
+
+  return _fifo_get(f);
 }
 
-int16_t fifo_get_nowait (fifo_t* f)
+int16_t fifo_get_nowait(fifo_t *f)
 {
   if (!f->count)
     return -1;
-    
-  return (int)_fifo_get(f);	
+
+  return (int)_fifo_get(f);
 }
 
-uint8_t fifo_put (fifo_t* f, const uint8_t data)
+uint8_t fifo_put(fifo_t *f, const uint8_t data)
 {
   if (f->count >= f->size)
     return 0;
-    
-  uint8_t* pwrite = f->pwrite;
-  
+
+  uint8_t *pwrite = f->pwrite;
+
   *(pwrite++) = data;
-  
+
   uint8_t write2end = f->write2end;
-  
+
   if (--write2end == 0)
-    {
-      write2end = f->size;
-      pwrite -= write2end;
-    }
-  
+  {
+    write2end = f->size;
+    pwrite -= write2end;
+  }
+
   f->write2end = write2end;
   f->pwrite = pwrite;
 
@@ -49,30 +55,30 @@ uint8_t fifo_put (fifo_t* f, const uint8_t data)
   cli();
   f->count++;
   SREG = sreg;
-  
+
   return 1;
 }
 
-static uint8_t 
-_fifo_get (fifo_t* f)
+static uint8_t
+_fifo_get(fifo_t *f)
 {
-  uint8_t* pread = f->pread;
+  uint8_t *pread = f->pread;
   uint8_t data = *(pread++);
   uint8_t read2end = f->read2end;
-  
+
   if (--read2end == 0)
-    {
-      read2end = f->size;
-      pread -= read2end;
-    }
-  
+  {
+    read2end = f->size;
+    pread -= read2end;
+  }
+
   f->pread = pread;
   f->read2end = read2end;
-  
+
   uint8_t sreg = SREG;
   cli();
   f->count--;
   SREG = sreg;
-  
+
   return data;
 }
diff --git a/src/lcd.c b/src/lcd.c
index ed14ac5..4d399d9 100644
--- a/src/lcd.c
+++ b/src/lcd.c
@@ -15,138 +15,140 @@ uint8_t lcd_frameupdate = 0;
 uint8_t lcd_textx = 0;
 uint8_t lcd_texty = 0;
 
-void
-LCD_Send (uint8_t data)
+// Prevent Double Initialization
+unsigned short LCD_flag = 0;
+
+void LCD_Send(uint8_t data)
 {
   SPI_MasterTransfer(data);
 }
 
-void
-LCD_Init (void)
+void LCD_Init(void)
 {
-  SPI_MasterInit();
+  // Check if already initialized
+  if (!(LCD_flag & 1))
+  {
+    SPI_MasterInit();
 
-  /* Set Register Select and Chip Select as Output */
-  DDRC |= (1<<PC7)|(1<<PC6);
-  
-  /* Backup Status Register and disable Interrupts */
-  uint8_t sreg = SREG;
-  cli();
-  
-  /* Starting Init Command Sequence */
-  LCD_Command_Mode;
-  LCD_Chip_Select;
-
-  LCD_Send(LCD_RESET);
-  LCD_Send(LCD_BIAS_1_7);
-  LCD_Send(LCD_ELECTRONIC_VOLUME_MODE_SET);
-  LCD_Send(0x08);
-  LCD_Send(LCD_ADC_SELECT_NORMAL);
-  LCD_Send(LCD_COMMON_OUTPUT_MODE_REVERSE);
-  LCD_Send(LCD_V5_VOLTAGE_REGULATOR | 0x05);
-  LCD_Send(LCD_POWER_CONTROLLER_SET | 0x07);
-  LCD_Send(LCD_DISPLAY_ON);
-  
-  LCD_Chip_Unselect;
-  LCD_Data_Mode;
-  
-  LCD_Clear();
-  
-  /* Restore Status Register */
-  SREG = sreg;
-  
-  // Initialize TWI for Backlight Control
-  TWI_Init();
-  Backlight_Off();
-  
-  // Initialize Dataflash
-  dataflash_init();
+    // Set Register Select and Chip Select as Output
+    DDRC |= (1 << PC7) | (1 << PC6);
+
+    // Backup Status Register and disable Interrupts
+    uint8_t sreg = SREG;
+    cli();
+
+    // Starting Init Command Sequence
+    LCD_Command_Mode;
+    LCD_Chip_Select;
+
+    LCD_Send(LCD_RESET);
+    LCD_Send(LCD_BIAS_1_7);
+    LCD_Send(LCD_ELECTRONIC_VOLUME_MODE_SET);
+    LCD_Send(0x08);
+    LCD_Send(LCD_ADC_SELECT_NORMAL);
+    LCD_Send(LCD_COMMON_OUTPUT_MODE_REVERSE);
+    LCD_Send(LCD_V5_VOLTAGE_REGULATOR | 0x05);
+    LCD_Send(LCD_POWER_CONTROLLER_SET | 0x07);
+    LCD_Send(LCD_DISPLAY_ON);
+
+    LCD_Chip_Unselect;
+    LCD_Data_Mode;
+
+    LCD_Clear();
+
+    // Restore Status Register
+    SREG = sreg;
+
+    // Initialize TWI for Backlight Control
+    TWI_Init();
+    Backlight_Off();
+
+    // Initialize Dataflash
+    dataflash_init();
+    // Set UART Init Flag
+    LCD_flag = 1;
+  }
 }
 
-void
-LCD_Clear (void)
+void LCD_Clear(void)
 {
   uint8_t x = 0, y = 0;
-  
+
   for (y = 0; y < 8; y++)
     for (x = 0; x < 128; x++)
       lcd_framebuffer[y][x] = 0;
-  
+
   // update every line (8bits height)
   lcd_frameupdate = 0xff;
   LCD_Update();
 }
 
-void
-LCD_Update (void)
+void LCD_Update(void)
 {
   int8_t page = 7;
-  
-  /* Backup Status Register and disable Interrupts */
+
+  // Backup Status Register and disable Interrupts
   uint8_t sreg = SREG;
   cli();
-  
+
   do
+  {
+    if (lcd_frameupdate & (1 << page))
     {
-      if (lcd_frameupdate & (1<<page))
-        {
-          LCD_Chip_Select;
-          LCD_Command_Mode;
-          
-          LCD_Send(LCD_PAGE_ADDRESS_SET | page);
-          LCD_Send(LCD_COLUMN_ADDRESS_SET_H);
-          LCD_Send(LCD_COLUMN_ADDRESS_SET_L);
-          
-          LCD_Data_Mode;
-          
-          for (uint8_t x = 0; x < 128; x++)
-            LCD_Send(lcd_framebuffer[page][x]);
-          
-          LCD_Chip_Unselect;
-        }
+      LCD_Chip_Select;
+      LCD_Command_Mode;
+
+      LCD_Send(LCD_PAGE_ADDRESS_SET | page);
+      LCD_Send(LCD_COLUMN_ADDRESS_SET_H);
+      LCD_Send(LCD_COLUMN_ADDRESS_SET_L);
+
+      LCD_Data_Mode;
+
+      for (uint8_t x = 0; x < 128; x++)
+        LCD_Send(lcd_framebuffer[page][x]);
+
+      LCD_Chip_Unselect;
     }
-  while (page--);
-  
+  } while (page--);
+
   lcd_frameupdate = 0;
-  
-  /* Restore Status Register */
+
+  // Restore Status Register
   SREG = sreg;
 }
 
-void
-LCD_DrawPixel (uint8_t x, uint8_t y, uint8_t mode)
+void LCD_DrawPixel(uint8_t x, uint8_t y, uint8_t mode)
 {
   // Check if x and y are within display coordinates
   if ((x < 128) && (y < 64))
+  {
+    // Precalculate Page and Pixel values
+    uint8_t page = (y / 8);
+    uint8_t pixel = (1 << (y % 8));
+
+    switch (mode)
     {
-      // Precalculate Page and Pixel values
-      uint8_t page = (y / 8);
-      uint8_t pixel = (1 << (y % 8));
-      
-      switch (mode)
-        {
-        case 0:
-          // Clear Pixel
-          lcd_framebuffer[page][x] &= ~pixel;
-          break;
-        
-        case 2:
-          // Toggle Pixel
-          lcd_framebuffer[page][x] ^= pixel;
-          break;
-        
-        default:
-          // Set Pixel
-          lcd_framebuffer[page][x] |= pixel;
-          break;
-        }
-        
-      lcd_frameupdate |= (1 << page);
+    case 0:
+      // Clear Pixel
+      lcd_framebuffer[page][x] &= ~pixel;
+      break;
+
+    case 2:
+      // Toggle Pixel
+      lcd_framebuffer[page][x] ^= pixel;
+      break;
+
+    default:
+      // Set Pixel
+      lcd_framebuffer[page][x] |= pixel;
+      break;
     }
+
+    lcd_frameupdate |= (1 << page);
+  }
 }
 
-void
-LCD_DrawLine (uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, uint8_t mode)
+void LCD_DrawLine(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, uint8_t mode)
 {
   // look here: http://de.wikipedia.org/wiki/Bresenham-Algorithmus
   int8_t dx = abs(x1 - x0);
@@ -155,32 +157,31 @@ LCD_DrawLine (uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, uint8_t mode)
   int8_t sy = y0 < y1 ? 1 : -1;
   int8_t err = (dx > dy ? dx : -dy) / 2;
   int8_t e2;
-  
+
   for (;;)
+  {
+    LCD_DrawPixel(x0, y0, mode);
+
+    if (x0 == x1 && y0 == y1)
+      break;
+
+    e2 = err;
+
+    if (e2 > -dx)
+    {
+      err -= dy;
+      x0 += sx;
+    }
+
+    if (e2 < dy)
     {
-      LCD_DrawPixel(x0, y0, mode);
-      
-      if (x0 == x1 && y0 == y1)
-        break;
-      
-      e2 = err;
-      
-      if (e2 > -dx)
-        {
-          err -= dy;
-          x0 += sx;
-        }
-      
-      if (e2 < dy)
-        {
-          err += dx;
-          y0 += sy;
-        }
+      err += dx;
+      y0 += sy;
     }
+  }
 }
 
-void
-LCD_DrawCircle (uint8_t x0, uint8_t y0, uint8_t radius, uint8_t mode)
+void LCD_DrawCircle(uint8_t x0, uint8_t y0, uint8_t radius, uint8_t mode)
 {
   // look here: http://de.wikipedia.org/wiki/Bresenham-Algorithmus
   int8_t f = 1 - radius;
@@ -188,114 +189,107 @@ LCD_DrawCircle (uint8_t x0, uint8_t y0, uint8_t radius, uint8_t mode)
   int8_t ddF_y = -2 * radius;
   int8_t x = 0;
   int8_t y = radius;
-  
+
   LCD_DrawPixel(x0, y0 + radius, mode);
   LCD_DrawPixel(x0, y0 - radius, mode);
   LCD_DrawPixel(x0 + radius, y0, mode);
   LCD_DrawPixel(x0 - radius, y0, mode);
-  
+
   while (x < y)
+  {
+    if (f >= 0)
     {
-      if (f >= 0)
-        {
-          y--;
-          ddF_y += 2;
-          f += ddF_y;
-        }
-      
-      x++;
-      ddF_x += 2;
-      f += ddF_x + 1;
-      
-      LCD_DrawPixel(x0 + x, y0 + y, mode);
-      LCD_DrawPixel(x0 - x, y0 + y, mode);
-      LCD_DrawPixel(x0 + x, y0 - y, mode);
-      LCD_DrawPixel(x0 - x, y0 - y, mode);
-      LCD_DrawPixel(x0 + y, y0 + x, mode);
-      LCD_DrawPixel(x0 - y, y0 + x, mode);
-      LCD_DrawPixel(x0 + y, y0 - x, mode);
-      LCD_DrawPixel(x0 - y, y0 - x, mode);
+      y--;
+      ddF_y += 2;
+      f += ddF_y;
     }
+
+    x++;
+    ddF_x += 2;
+    f += ddF_x + 1;
+
+    LCD_DrawPixel(x0 + x, y0 + y, mode);
+    LCD_DrawPixel(x0 - x, y0 + y, mode);
+    LCD_DrawPixel(x0 + x, y0 - y, mode);
+    LCD_DrawPixel(x0 - x, y0 - y, mode);
+    LCD_DrawPixel(x0 + y, y0 + x, mode);
+    LCD_DrawPixel(x0 - y, y0 + x, mode);
+    LCD_DrawPixel(x0 + y, y0 - x, mode);
+    LCD_DrawPixel(x0 - y, y0 - x, mode);
+  }
 }
 
-void
-LCD_PutChar (const char c)
+void LCD_PutChar(const char c)
 {
   // basic support for cr und new line
   switch (c)
-    {
-    case '\r':
-      // Carriage Return
-      lcd_textx = 0;
-      break;
-      
-    case '\n':
-      // New Line
-      if (lcd_texty < 7)
-        lcd_texty++;
-      break;
-      
-    default:
-      for (uint8_t x = 0; x < 6; x++)
-        lcd_framebuffer[lcd_texty][(lcd_textx * 6) + x] = pgm_read_byte(&font[(uint8_t)(c)][x]);
-      
-      lcd_frameupdate |= (1 << lcd_texty);
-  
-      if (lcd_textx < 20)
-        lcd_textx++;
-      break;
-    }
+  {
+  case '\r':
+    // Carriage Return
+    lcd_textx = 0;
+    break;
+
+  case '\n':
+    // New Line
+    if (lcd_texty < 7)
+      lcd_texty++;
+    break;
+
+  default:
+    for (uint8_t x = 0; x < 6; x++)
+      lcd_framebuffer[lcd_texty][(lcd_textx * 6) + x] = pgm_read_byte(&font[(uint8_t)(c)][x]);
+
+    lcd_frameupdate |= (1 << lcd_texty);
+
+    if (lcd_textx < 20)
+      lcd_textx++;
+    break;
+  }
 }
 
-void
-LCD_PutString (const char *s)
+void LCD_PutString(const char *s)
 {
   // no empty strings allowed!
   if (*s)
+  {
+    do
     {
-      do
-        {
-          LCD_PutChar(*s);
-        }
-      while (*(++s));
-    }
+      LCD_PutChar(*s);
+    } while (*(++s));
+  }
 }
 
-void
-LCD_PutString_P (PGM_P s)
+void LCD_PutString_P(PGM_P s)
 {
-  while(1)
-    {
-      unsigned char c = pgm_read_byte (s);
-      s++;
+  while (1)
+  {
+    unsigned char c = pgm_read_byte(s);
+    s++;
 
-      if (c == '\0')
-        break;
-      
-      LCD_PutChar(c);
-    }
+    if (c == '\0')
+      break;
+
+    LCD_PutChar(c);
+  }
 }
 
-void
-LCD_GotoXY (uint8_t x, uint8_t y)
+void LCD_GotoXY(uint8_t x, uint8_t y)
 {
   lcd_textx = x;
   lcd_texty = y;
 }
 
-void
-LCD_WipeLine (unsigned char line)
+void LCD_WipeLine(unsigned char line)
 {
   unsigned char x;
-  
+
   for (x = 0; x < 128; x++)
     lcd_framebuffer[line][x] = 0x00;
-  
+
   lcd_frameupdate |= (1 << line);
 }
 
-void
-Backlight_Off (void)
+void Backlight_Off(void)
 {
   TWI_Start();
   TWI_Address_RW(0xc4);
@@ -308,8 +302,7 @@ Backlight_Off (void)
   TWI_Stop();
 }
 
-void
-Backlight_LED (uint8_t led_selector)
+void Backlight_LED(uint8_t led_selector)
 {
   TWI_Start();
   TWI_Address_RW(0xc4);
@@ -318,12 +311,11 @@ Backlight_LED (uint8_t led_selector)
   TWI_Stop();
 }
 
-void
-Backlight_PWM (uint8_t pwm, uint8_t prescaler, uint8_t value)
+void Backlight_PWM(uint8_t pwm, uint8_t prescaler, uint8_t value)
 {
   TWI_Start();
   TWI_Address_RW(0xc4);
-  
+
   if (pwm)
     TWI_Write(0x13);
   else
@@ -334,29 +326,27 @@ Backlight_PWM (uint8_t pwm, uint8_t prescaler, uint8_t value)
   TWI_Stop();
 }
 
-void
-LCD_SavePage (unsigned int page)
+void LCD_SavePage(unsigned int page)
 {
   // transfer framebuffer to dataflash using buffer 2
   unsigned char line = 0;
-  
+
   for (line = 0; line < 8; line++)
-    {
-      dataflash_buffer_write(2, 0, 128, lcd_framebuffer[line]);
-      dataflash_buffer_to_page(page + line, 2);
-    }
+  {
+    dataflash_buffer_write(2, 0, 128, lcd_framebuffer[line]);
+    dataflash_buffer_to_page(page + line, 2);
+  }
 }
 
-void
-LCD_LoadPage (unsigned int page)
+void LCD_LoadPage(unsigned int page)
 {
   // transfer dataflash page to framebuffer
   unsigned char line = 0;
 
   for (line = 0; line < 8; line++)
-    {
-      dataflash_read(page + line, 0, 128, lcd_framebuffer[line]);
-    }
+  {
+    dataflash_read(page + line, 0, 128, lcd_framebuffer[line]);
+  }
 
   // mark all lines to be updated
   lcd_frameupdate = 0xff;
diff --git a/src/spi.c b/src/spi.c
index 37f442a..9a718f3 100644
--- a/src/spi.c
+++ b/src/spi.c
@@ -6,45 +6,52 @@
 // Prevent Double Initialization
 unsigned short SPI_flag = 0;
 
-void
-SPI_MasterInit (void)
+void SPI_MasterInit(void)
 {
-  /* Check if already initialized */
+  // Check if already initialized
   if (!(SPI_flag & 1))
-    {
-      /* Set MOSI and SCK output */
-      DDRB |= (1<<PB5)|(1<<PB7);
-      
-      /* Enable SPI, Master */
-      SPCR = (1<<SPE)|(1<<MSTR);
-      
-      /* Set Double SPI Speed Bit, SPI clock will be fck/2 */
-      SPSR = (1<<SPI2X);
-      
-      /* Set SPI Init Flag */
-      SPI_flag = 1;
-    }
+  {
+    // Backup Status Register and disable Interrupts
+    uint8_t sreg = SREG;
+    cli();
+
+    // Set MOSI, SCK and SS output
+    DDRB |= (1 << PB5) | (1 << PB7) | (1 << PB4);
+
+    // Enable SPI, Master
+    SPCR = (1 << SPE) | (1 << MSTR);
+
+    // Set Double SPI Speed Bit, SPI clock will be fck/2
+    SPSR = (1 << SPI2X);
+
+    // Restore Status Register
+    SREG = sreg;
+
+    // Set SPI Init Flag
+    SPI_flag = 1;
+  }
 }
 
-void
-SPI_MasterTransfer (unsigned char c)
+void SPI_MasterTransfer(unsigned char c)
 {
-  /* Start transmission */
+  // Start transmission
   SPDR = c;
-  
-  /* Wait for transmission complete */
-  while (!(SPSR & (1<<SPIF)));
+
+  // Wait for transmission complete
+  while (!(SPSR & (1 << SPIF)))
+    ;
 }
 
 unsigned char
-SPI_MasterTransferRead (unsigned char c)
+SPI_MasterTransferRead(unsigned char c)
 {
-  /* Start transmission */
+  // Start transmission
   SPDR = c;
-  
-  /* Wait for transmission complete */
-  while (!(SPSR & (1<<SPIF)));
-  
-  /* Return incoming character */
+
+  // Wait for transmission complete
+  while (!(SPSR & (1 << SPIF)))
+    ;
+
+  // Return incoming character
   return SPDR;
 }
diff --git a/src/twi.c b/src/twi.c
index de1a830..9b6a730 100644
--- a/src/twi.c
+++ b/src/twi.c
@@ -6,80 +6,94 @@
 
 volatile uint8_t twi_timeout = 0x00;
 
-void
-TWI_Init (void)
+// Prevent Double Initialization
+unsigned short TWI_flag = 0;
+
+void TWI_Init(void)
 {
-  // Port Setup
-  DDRC &= ~((1 << PC0)|(1 << PC1));
-  PORTC |= (1 << PC0)|(1 << PC1);
-  
-  // Setup TWI Speed to 400kHZ (TWPS = 1)
-  TWBR = 3;
-  
-  // Using TIMER2 to detect timeout
-  TCCR2B = (7 << CS20);
-  TIMSK2 = (1 << OCIE2A);
-  
-  OCR2A = 125;
-  
-  // Interrupts REQUIRED!
-  sei();
+  // Check if already initialized
+  if (!(TWI_flag & 1))
+  {
+    //disable global interrupts
+    cli();
+
+    // Port Setup
+    DDRC &= ~((1 << PC0) | (1 << PC1));
+    PORTC |= (1 << PC0) | (1 << PC1);
+
+    // Setup TWI Speed to 400kHZ (TWPS = 1)
+    TWBR = 3;
+
+    // Using TIMER2 to detect timeout
+    TCCR2B = (7 << CS20);
+    TIMSK2 = (1 << OCIE2A);
+
+    OCR2A = 125;
+
+    // Set TWI Init Flag
+    TWI_flag = 1;
+
+    // Interrupts REQUIRED!
+    sei();
+  }
 }
 
 int16_t
-TWI_Start (void)
+TWI_Start(void)
 {
   twi_timeout = 10;
-  
-  TWCR = (1 << TWINT)|(1 << TWSTA)|(1 << TWEN);
-  while ((twi_timeout) && (!(TWCR & (1 << TWINT))));
-  
+
+  TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
+  while ((twi_timeout) && (!(TWCR & (1 << TWINT))))
+    ;
+
   if (twi_timeout)
-    return (int16_t) (TWSR & 0xf8);
+    return (int16_t)(TWSR & 0xf8);
   else
     return -1;
 }
 
 int16_t
-TWI_Address_RW (uint8_t address)
+TWI_Address_RW(uint8_t address)
 {
   twi_timeout = 10;
-  
+
   TWDR = address;
-  TWCR = (1 << TWINT)|(1 << TWEN);
-  while ((twi_timeout) && (!(TWCR & (1 << TWINT))));
-  
+  TWCR = (1 << TWINT) | (1 << TWEN);
+  while ((twi_timeout) && (!(TWCR & (1 << TWINT))))
+    ;
+
   if (twi_timeout)
-    return (int16_t) (TWSR & 0xf8);
+    return (int16_t)(TWSR & 0xf8);
   else
     return -1;
 }
 
 int16_t
-TWI_Write (uint8_t data)
+TWI_Write(uint8_t data)
 {
   twi_timeout = 10;
-  
+
   TWDR = data;
-  TWCR = (1 << TWINT)|(1 << TWEN);
-  while ((twi_timeout) && (!(TWCR & (1 << TWINT))));
-  
+  TWCR = (1 << TWINT) | (1 << TWEN);
+  while ((twi_timeout) && (!(TWCR & (1 << TWINT))))
+    ;
+
   if (twi_timeout)
-    return (int16_t) (TWSR & 0xf8);
+    return (int16_t)(TWSR & 0xf8);
   else
     return -1;
 }
 
-void
-TWI_Stop (void)
+void TWI_Stop(void)
 {
   twi_timeout = 10;
-  
-  TWCR = (1 << TWINT)|(1 << TWEN)|(1 << TWSTO);
+
+  TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
 }
 
 // ISR will be called every 8ms to decrease twi_timeout if > 0
-ISR (TIMER2_COMPA_vect)
+ISR(TIMER2_COMPA_vect)
 {
   OCR2A += 125;
 
diff --git a/src/uart.c b/src/uart.c
index 00b903f..e5ebdb4 100644
--- a/src/uart.c
+++ b/src/uart.c
@@ -16,11 +16,16 @@ uint8_t uart_outbuf[UART_BUFSIZE_OUT];
 fifo_t uart_infifo;
 fifo_t uart_outfifo;
 
+// Prevent Double Initialization
+unsigned short UART_flag = 0;
+
 void
 UART_Init (void)
 {
-  // Save Status Register and disable Interrupts
-  uint8_t sreg = SREG;
+  // Check if already initialized
+  if (!(UART_flag & 1))
+  {
+  //disable Interrupts
   cli();
   
   // Set Baudrate according to datasheet (16MHz -> 9600 Baud -> 103)
@@ -32,12 +37,16 @@ UART_Init (void)
   // Reset Complete-Flags
   UCSR0A = (1 << RXC0)|(1 << TXC0);
   
-  // Reset Status Register
-  SREG = sreg;
+  // Interrupts REQUIRED!
+  sei();
   
   // Initialize FIFO Buffers
   fifo_init(&uart_infifo, uart_inbuf, UART_BUFSIZE_IN);
   fifo_init(&uart_outfifo, uart_outbuf, UART_BUFSIZE_OUT);
+
+  // Set UART Init Flag
+  UART_flag = 1;
+  }
 }
 
 int8_t