diff --git a/Makefile b/Makefile
index 6a10604cd..019a1cfb9 100644
--- a/Makefile
+++ b/Makefile
@@ -1,6 +1,7 @@
 #  Part of Grbl
 #
 #  Copyright (c) 2009-2011 Simen Svale Skogsrud
+#  Copyright (c) 2011 Flemming Frandsen
 #
 #  Grbl is free software: you can redistribute it and/or modify
 #  it under the terms of the GNU General Public License as published by
@@ -27,8 +28,8 @@
 #                is connected.
 # FUSES ........ Parameters for avrdude to flash the fuses appropriately.
 
-DEVICE     = atmega328p
-CLOCK      = 16000000
+DEVICE     = atmega324p
+CLOCK      = 20000000
 PROGRAMMER = -c avrisp2 -P usb
 OBJECTS    = main.o motion_control.o gcode.o spindle_control.o wiring_serial.o serial_protocol.o stepper.o \
              eeprom.o settings.o planner.o
@@ -40,7 +41,7 @@ FUSES      = -U hfuse:w:0xd2:m -U lfuse:w:0xff:m
 # Tune the lines below only if you know what you are doing:
 
 AVRDUDE = avrdude $(PROGRAMMER) -p $(DEVICE) -B 10 -F 
-COMPILE = avr-gcc -Wall -Os -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections
+COMPILE = avr-gcc -Wall -Os -pedantic-errors -Werror -std=c99 -Wl,-u,vfprintf -lprintf_min -DF_CPU=$(CLOCK) -mmcu=$(DEVICE) -I. -ffunction-sections
 
 # symbolic targets:
 all:	grbl.hex
diff --git a/gcode.c b/gcode.c
index 00f868199..1b97a7a78 100644
--- a/gcode.c
+++ b/gcode.c
@@ -3,6 +3,7 @@
   Part of Grbl
 
   Copyright (c) 2009-2011 Simen Svale Skogsrud
+  Copyright (c) 2011 Flemming Frandsen
 
   Grbl is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
@@ -98,7 +99,7 @@ void gc_init()
     gc.absolute_mode = TRUE;
 }
 
-inline float to_millimeters(double value)
+float to_millimeters(double value)
 {
     return (gc.inches_mode ? (value * MM_PER_INCH) : value);
 }
@@ -185,7 +186,7 @@ uint8_t gc_execute_line(char *line)
 	    case 1:
 		gc.motion_mode = MOTION_MODE_LINEAR;
 		break;
-#ifdef __AVR_ATmega328P__
+#ifndef CFG_TINY
 	    case 2:
 		gc.motion_mode = MOTION_MODE_CW_ARC;
 		break;
@@ -355,7 +356,7 @@ uint8_t gc_execute_line(char *line)
 		    (gc.inverse_feed_rate_mode) ? inverse_feed_rate : gc.
 		    feed_rate, gc.inverse_feed_rate_mode);
 	    break;
-#ifdef __AVR_ATmega328P__
+#ifndef CFG_TINY
 	case MOTION_MODE_CW_ARC:
 	case MOTION_MODE_CCW_ARC:
 	    if (radius_mode) {
diff --git a/motion_control.c b/motion_control.c
index 63a8c30bf..42a0089ef 100644
--- a/motion_control.c
+++ b/motion_control.c
@@ -33,7 +33,9 @@
 void mc_dwell(uint32_t milliseconds)
 {
     st_synchronize();
-    _delay_ms(milliseconds);
+    while (milliseconds--) {
+      _delay_ms(1);
+    }
 }
 
 // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
@@ -42,7 +44,7 @@ void mc_dwell(uint32_t milliseconds)
 // axis in axis_l which will be the axis for linear travel if you are tracing a helical motion.
 // position is a pointer to a vector representing the current position in millimeters.
 
-#ifdef __AVR_ATmega328P__
+#ifndef CFG_TINY
 // The arc is approximated by generating a huge number of tiny, linear segments. The length of each 
 // segment is configured in settings.mm_per_arc_segment.  
 void mc_arc(double theta, double angular_travel, double radius,
diff --git a/motion_control.h b/motion_control.h
index 3ad7fea8c..3ebae52d7 100644
--- a/motion_control.h
+++ b/motion_control.h
@@ -31,7 +31,7 @@
 // NOTE: Although this function structurally belongs in this module, there is nothing to do but
 // to forward the request to the planner. For efficiency the function is implemented with a macro.
 
-#ifdef __AVR_ATmega328P__
+#ifndef CFG_TINY
 // Execute an arc. theta == start angle, angular_travel == number of radians to go along the arc,
 // positive angular_travel means clockwise, negative means counterclockwise. Radius == the radius of the
 // circle in millimeters. axis_1 and axis_2 selects the circle plane in tool space. Stick the remaining
diff --git a/planner.c b/planner.c
index ed96e95ba..cefb49c01 100644
--- a/planner.c
+++ b/planner.c
@@ -64,10 +64,10 @@
 #include "wiring_serial.h"
 
 // The number of linear motions that can be in the plan at any give time
-#ifdef __AVR_ATmega328P__
-#define BLOCK_BUFFER_SIZE 20
-#else
+#ifdef CFG_TINY
 #define BLOCK_BUFFER_SIZE 5
+#else
+#define BLOCK_BUFFER_SIZE 20
 #endif
 
 static block_t block_buffer[BLOCK_BUFFER_SIZE];	// A ring buffer for motion instructions
@@ -365,14 +365,14 @@ int plan_is_acceleration_manager_enabled()
     return (acceleration_manager_enabled);
 }
 
-inline void plan_discard_current_block()
+void plan_discard_current_block()
 {
     if (block_buffer_head != block_buffer_tail) {
 	block_buffer_tail = (block_buffer_tail + 1) % BLOCK_BUFFER_SIZE;
     }
 }
 
-inline block_t *plan_get_current_block()
+block_t *plan_get_current_block()
 {
     if (block_buffer_head == block_buffer_tail) {
 	return (NULL);
diff --git a/planner.h b/planner.h
index 8ddcd7375..60de348c3 100644
--- a/planner.h
+++ b/planner.h
@@ -63,10 +63,10 @@ void plan_buffer_line(double x, double y, double z, double feed_rate,
 
 // Called when the current block is no longer needed. Discards the block and makes the memory
 // availible for new blocks.
-inline void plan_discard_current_block();
+void plan_discard_current_block();
 
 // Gets the current block. Returns NULL if buffer empty
-inline block_t *plan_get_current_block();
+block_t *plan_get_current_block();
 
 // Enables or disables acceleration-management for upcoming blocks
 void plan_set_acceleration_manager_enabled(int enabled);
diff --git a/stepper.c b/stepper.c
index 44d935f17..eb90d23a1 100644
--- a/stepper.c
+++ b/stepper.c
@@ -88,7 +88,7 @@ void st_wake_up()
 
 // Initializes the trapezoid generator from the current block. Called whenever a new 
 // block begins.
-inline void trapezoid_generator_reset()
+void trapezoid_generator_reset()
 {
     trapezoid_adjusted_rate = current_block->initial_rate;
     trapezoid_tick_cycle_counter = 0;	// Always start a new trapezoid with a full acceleration tick
@@ -98,7 +98,7 @@ inline void trapezoid_generator_reset()
 // This is called ACCELERATION_TICKS_PER_SECOND times per second by the step_event
 // interrupt. It can be assumed that the trapezoid-generator-parameters and the
 // current_block stays untouched by outside handlers for the duration of this function call.
-inline void trapezoid_generator_tick()
+void trapezoid_generator_tick()
 {
     if (current_block) {
 	if (step_events_completed < current_block->accelerate_until) {
diff --git a/wiring_serial.c b/wiring_serial.c
index 088bfd1b3..df380aeb1 100644
--- a/wiring_serial.c
+++ b/wiring_serial.c
@@ -3,6 +3,7 @@
   Part of Arduino - http://www.arduino.cc/
 
   Copyright (c) 2005-2006 David A. Mellis
+  Copyright (c) 2011 Flemming Frandsen
 
   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
@@ -32,10 +33,10 @@
 // using a ring buffer (I think), in which rx_buffer_head is the index of the
 // location to which to write the next incoming character and rx_buffer_tail
 // is the index of the location from which to read.
-#ifdef __AVR_ATmega328P__
-#define RX_BUFFER_SIZE 256
-#else
+#ifdef CFG_TINY
 #define RX_BUFFER_SIZE 64
+#else
+#define RX_BUFFER_SIZE 256
 #endif
 
 unsigned char rx_buffer[RX_BUFFER_SIZE];
@@ -96,7 +97,7 @@ void serialFlush()
     rx_buffer_head = rx_buffer_tail;
 }
 
-SIGNAL(USART_RX_vect)
+SIGNAL(USART0_RX_vect)
 {
     unsigned char c = UDR0;
     int i = (rx_buffer_head + 1) % RX_BUFFER_SIZE;