diff --git a/src/gos/chibios.c b/src/gos/chibios.c
index 67f70561..2254f52f 100644
--- a/src/gos/chibios.c
+++ b/src/gos/chibios.c
@@ -8,6 +8,7 @@
 /**
  * @file    src/gos/chibios.c
  * @brief   GOS ChibiOS Operating System support.
+ * @details Supports both, ChibiOS/RT 2.x and 3.x
  */
 #include "gfx.h"
 
@@ -15,20 +16,41 @@
 
 #include <string.h>
 
-#if !CH_USE_MUTEXES
-	#error "GOS: CH_USE_MUTEXES must be defined in chconf.h"
-#endif
-#if !CH_USE_SEMAPHORES
-	#error "GOS: CH_USE_SEMAPHORES must be defined in chconf.h"
+#if CH_KERNEL_MAJOR == 2
+
+	#if !CH_USE_MUTEXES
+		#error "GOS: CH_USE_MUTEXES must be defined in chconf.h"
+	#endif
+	#if !CH_USE_SEMAPHORES
+		#error "GOS: CH_USE_SEMAPHORES must be defined in chconf.h"
+	#endif
+	
+#elif CH_KERNEL_MAJOR == 3
+
+	#if !CH_CFG_USE_MUTEXES
+		#error "GOS: CH_USE_MUTEXES must be defined in chconf.h"
+	#endif
+	#if !CH_CFG_USE_SEMAPHORES
+		#error "GOS: CH_USE_SEMAPHORES must be defined in chconf.h"
+	#endif
+	
 #endif
 
 void _gosInit(void)
 {
 	/* Don't initialise if the user already has */
+	
+	#if CH_KERNEL_MAJOR == 2
 	if (!chThdSelf()) {
 		halInit();
 		chSysInit();
 	}
+	#elif CH_KERNEL_MAJOR == 3
+	if (!chThdGetSelfX()) {
+		halInit();
+		chSysInit();
+	}
+	#endif
 }
 
 void _gosDeinit(void)
@@ -36,7 +58,8 @@ void _gosDeinit(void)
 	/* ToDo */
 }
 
-void *gfxRealloc(void *ptr, size_t oldsz, size_t newsz) {
+void *gfxRealloc(void *ptr, size_t oldsz, size_t newsz)
+{
 	void *np;
 
 	if (newsz <= oldsz)
@@ -52,7 +75,8 @@ void *gfxRealloc(void *ptr, size_t oldsz, size_t newsz) {
 	return np;
 }
 
-void gfxSleepMilliseconds(delaytime_t ms) {
+void gfxSleepMilliseconds(delaytime_t ms)
+{
 	switch(ms) {
 		case TIME_IMMEDIATE:	chThdYield();				return;
 		case TIME_INFINITE:		chThdSleep(TIME_INFINITE);	return;
@@ -60,7 +84,8 @@ void gfxSleepMilliseconds(delaytime_t ms) {
 	}
 }
 
-void gfxSleepMicroseconds(delaytime_t ms) {
+void gfxSleepMicroseconds(delaytime_t ms)
+{
 	switch(ms) {
 		case TIME_IMMEDIATE:								return;
 		case TIME_INFINITE:		chThdSleep(TIME_INFINITE);	return;
@@ -68,34 +93,52 @@ void gfxSleepMicroseconds(delaytime_t ms) {
 	}
 }
 
-void gfxSemInit(gfxSem *psem, semcount_t val, semcount_t limit) {
+void gfxSemInit(gfxSem *psem, semcount_t val, semcount_t limit)
+{
 	if (val > limit)
 		val = limit;
 
 	psem->limit = limit;
-	chSemInit(&psem->sem, val);
+	
+	#if CH_KERNEL_MAJOR == 2
+		chSemInit(&psem->sem, val);
+	#elif CH_KERNEL_MAJOR == 3
+		chSemObjectInit(&psem->sem, val);
+	#endif
 }
 
-void gfxSemDestroy(gfxSem *psem) {
+void gfxSemDestroy(gfxSem *psem)
+{
 	chSemReset(&psem->sem, 1);
 }
 
-bool_t gfxSemWait(gfxSem *psem, delaytime_t ms) {
-	switch(ms) {
-	case TIME_IMMEDIATE:	return chSemWaitTimeout(&psem->sem, TIME_IMMEDIATE) != RDY_TIMEOUT;
-	case TIME_INFINITE:		chSemWait(&psem->sem);	return TRUE;
-	default:				return chSemWaitTimeout(&psem->sem, MS2ST(ms)) != RDY_TIMEOUT;
-	}
+bool_t gfxSemWait(gfxSem *psem, delaytime_t ms)
+{
+	#if CH_KERNEL_MAJOR == 2
+		switch(ms) {
+		case TIME_IMMEDIATE:	return chSemWaitTimeout(&psem->sem, TIME_IMMEDIATE) != RDY_TIMEOUT;
+		case TIME_INFINITE:		chSemWait(&psem->sem);	return TRUE;
+		default:				return chSemWaitTimeout(&psem->sem, MS2ST(ms)) != RDY_TIMEOUT;
+		}
+	#elif CH_KERNEL_MAJOR == 3
+		switch(ms) {
+		case TIME_IMMEDIATE:	return chSemWaitTimeout(&psem->sem, TIME_IMMEDIATE) != MSG_TIMEOUT;
+		case TIME_INFINITE:		chSemWait(&psem->sem);	return TRUE;
+		default:				return chSemWaitTimeout(&psem->sem, MS2ST(ms)) != MSG_TIMEOUT;
+		}
+	#endif
 }
 
-bool_t gfxSemWaitI(gfxSem *psem) {
+bool_t gfxSemWaitI(gfxSem *psem)
+{
 	if (chSemGetCounterI(&psem->sem) <= 0)
 		return FALSE;
 	chSemFastWaitI(&psem->sem);
 	return TRUE;
 }
 
-void gfxSemSignal(gfxSem *psem) {
+void gfxSemSignal(gfxSem *psem)
+{
 	chSysLock();
 
 	if (gfxSemCounterI(psem) < psem->limit)
@@ -105,12 +148,14 @@ void gfxSemSignal(gfxSem *psem) {
 	chSysUnlock();
 }
 
-void gfxSemSignalI(gfxSem *psem) {
+void gfxSemSignalI(gfxSem *psem)
+{
 	if (gfxSemCounterI(psem) < psem->limit)
 		chSemSignalI(&psem->sem);
 }
 
-gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_t prio, DECLARE_THREAD_FUNCTION((*fn),p), void *param) {
+gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_t prio, DECLARE_THREAD_FUNCTION((*fn),p), void *param)
+{
 	if (!stackarea) {
 		if (!stacksz) stacksz = 256;
 		return chThdCreateFromHeap(0, stacksz, prio, fn, param);
@@ -124,4 +169,3 @@ gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_
 
 #endif /* GFX_USE_OS_CHIBIOS */
 /** @} */
-
diff --git a/src/gos/chibios.h b/src/gos/chibios.h
index 3da247b1..9f7f5e37 100644
--- a/src/gos/chibios.h
+++ b/src/gos/chibios.h
@@ -8,6 +8,7 @@
 /**
  * @file    src/gos/chibios.h
  * @brief   GOS - Operating System Support header file for ChibiOS.
+ * @details Supports both, ChibiOS/RT 2.x and 3.x
  */
 
 #ifndef _GOS_CHIBIOS_H
@@ -47,13 +48,27 @@ typedef tprio_t		threadpriority_t;
 #define DECLARE_THREAD_STACK(name, sz)			WORKING_AREA(name, sz)
 #define DECLARE_THREAD_FUNCTION(fnName, param)	threadreturn_t fnName(void *param)
 
-typedef struct {
-	Semaphore	sem;
-	semcount_t	limit;
+#if CH_KERNEL_MAJOR == 2
+	typedef struct {
+		Semaphore	sem;
+		semcount_t	limit;
 	} gfxSem;
 
-typedef Mutex		gfxMutex;
-typedef Thread *	gfxThreadHandle;
+	typedef Mutex		gfxMutex;
+	typedef Thread*		gfxThreadHandle;
+#elif CH_KERNEL_MAJOR == 3
+	#undef DECLARE_THREAD_STACK
+	#define DECLARE_THREAD_STACK(a, b)  THD_WORKING_AREA(a, b)
+	
+	typedef struct {
+		semaphore_t	sem;
+		semcount_t	limit;
+	} gfxSem;
+
+	typedef mutex_t		gfxMutex;
+	typedef thread_t*	gfxThreadHandle;
+#endif
+
 
 /*===========================================================================*/
 /* Function declarations.                                                    */
@@ -63,19 +78,27 @@ typedef Thread *	gfxThreadHandle;
 extern "C" {
 #endif
 
+// First the kernel version specific ones
+#if CH_KERNEL_MAJOR == 2
+	#define gfxSystemTicks()			chTimeNow()
+	#define gfxMutexInit(pmutex)		chMtxInit(pmutex)
+	#define gfxMutexExit(pmutex)		chMtxUnlock()
+#elif CH_KERNEL_MAJOR == 3
+	#define gfxSystemTicks()			chVTGetSystemTimeX()
+	#define gfxMutexInit(pmutex)		chMtxObjectInit(pmutex)
+	#define gfxMutexExit(pmutex)		chMtxUnlock(pmutex)
+#endif
+
 #define gfxHalt(msg)				{ chDbgPanic(msg); chSysHalt(); }
 #define gfxExit()					chSysHalt()
 #define gfxAlloc(sz)				chHeapAlloc(0, sz)
 #define gfxFree(ptr)				chHeapFree(ptr)
 #define gfxYield()					chThdYield()
-#define gfxSystemTicks()			chTimeNow()
 #define gfxMillisecondsToTicks(ms)	MS2ST(ms)
 #define gfxSystemLock()				chSysLock()
 #define gfxSystemUnlock()			chSysUnlock()
-#define gfxMutexInit(pmutex)		chMtxInit(pmutex)
 #define gfxMutexDestroy(pmutex)		(void)pmutex
 #define gfxMutexEnter(pmutex)		chMtxLock(pmutex)
-#define gfxMutexExit(pmutex)		chMtxUnlock()
 void *gfxRealloc(void *ptr, size_t oldsz, size_t newsz);
 void gfxSleepMilliseconds(delaytime_t ms);
 void gfxSleepMicroseconds(delaytime_t ms);
@@ -98,4 +121,3 @@ gfxThreadHandle gfxThreadCreate(void *stackarea, size_t stacksz, threadpriority_
 
 #endif /* GFX_USE_OS_CHIBIOS */
 #endif /* _GOS_CHIBIOS_H */
-