diff --git a/demos/ARM7-LPC214x-G++/Makefile b/demos/ARM7-LPC214x-G++/Makefile
index f2bdcd2ac..e05b79362 100644
--- a/demos/ARM7-LPC214x-G++/Makefile
+++ b/demos/ARM7-LPC214x-G++/Makefile
@@ -68,23 +68,24 @@ UDEFS =
 UADEFS =
 
 # List ARM-mode C source files here
-CSRC = ../../ports/ARM7-LPC214x/chcore.c \
-       ../../ports/ARM7-LPC214x/vic.c  \
-       ../../ports/ARM7-LPC214x/lpc214x_serial.c  \
-       ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
-       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
-       ../../src/chserial.c \
-       ../../src/lib/evtimer.c ../../test/test.c \
-       board.c
+ACSRC = ../../ports/ARM7-LPC214x/chcore.c \
+        ../../ports/ARM7-LPC214x/vic.c  \
+        ../../ports/ARM7-LPC214x/lpc214x_serial.c  \
+        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
+        ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+        ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+        ../../src/chserial.c \
+        ../../src/lib/evtimer.c ../../test/test.c \
+        board.c
 
 # List ARM-mode C++ source files here
-CPPSRC = ../../src/lib/ch.cpp main.cpp
+ACPPSRC = ../../src/lib/ch.cpp main.cpp
 
 # List THUMB-mode C sources here
-# NOTE: If any module is compiled in thumb mode then -mthumb-interwork is
-#       enabled for all modules and that lowers performance.
-TSRC =
+TCSRC =
+
+# List THUMB-mode C++ source files here
+TCPPSRC =
 
 # List ASM source files here
 ASMSRC = ../../ports/ARM7-LPC214x/crt0.s ../../ports/ARM7/chsys.s
@@ -130,11 +131,15 @@ INCDIR   = $(patsubst %,-I%,$(DINCDIR) $(UINCDIR))
 LIBDIR   = $(patsubst %,-L%,$(DLIBDIR) $(ULIBDIR))
 DEFS     = $(DDEFS) $(UDEFS)
 ADEFS    = $(DADEFS) $(UADEFS)
-AOBJS    = $(CSRC:.c=.o)
-TOBJS    = $(TSRC:.c=.o)
-CPPOBJS  = $(CPPSRC:.cpp=.o)
+ASRC	 = $(ACSRC)$(ACPPSRC)
+TSRC	 = $(TCSRC)$(TCPPSRC)
+SRC	 = $(ASRC)$(TSRC)
+ACOBJS   = $(ACSRC:.c=.o)
+ACPPOBJS = $(ACPPSRC:.cpp=.o)
+TCOBJS   = $(TCSRC:.c=.o)
+TCPPOBJS = $(TCPPSRC:.cpp=.o)
 ASMOBJS  = $(ASMSRC:.s=.o)
-OBJS	 = $(ASMOBJS) $(CPPOBJS) $(AOBJS) $(TOBJS)
+OBJS	 = $(ASMOBJS) $(ACOBJS) $(TCOBJS) $(ACPPOBJS) $(TCPPOBJS)
 LIBS     = $(DLIBS) $(ULIBS)
 MCFLAGS  = -mcpu=$(MCU)
 
@@ -149,7 +154,7 @@ ifneq ($(TSRC),)
   CFLAGS += -D THUMB_PRESENT
   CPPFLAGS += -D THUMB_PRESENT
   ASFLAGS += -D THUMB_PRESENT
-  ifneq ($(CSRC),)
+  ifneq ($(ASRC),)
     # Mixed ARM and THUMB case.
     CFLAGS += -mthumb-interwork
     CPPFLAGS += -mthumb-interwork
@@ -173,15 +178,19 @@ CPPFLAGS += -MD -MP -MF .dep/$(@F).d
 
 all: $(OBJS) $(PROJECT).elf $(PROJECT).hex $(PROJECT).bin $(PROJECT).dmp
 
-$(CPPOBJS) : %.o : %.cpp
+$(ACPPOBJS) : %.o : %.cpp
 	@echo
 	$(CPPC) -c $(CPPFLAGS) $(AOPT) -I . $(INCDIR) $< -o $@
 
-$(AOBJS) : %.o : %.c
+$(TCPPOBJS) : %.o : %.cpp
+	@echo
+	$(CPPC) -c $(CPPFLAGS) $(TOPT) -I . $(INCDIR) $< -o $@
+
+$(ACOBJS) : %.o : %.c
 	@echo
 	$(CC) -c $(CFLAGS) $(AOPT) -I . $(INCDIR) $< -o $@
 
-$(TOBJS) : %.o : %.c
+$(TCOBJS) : %.o : %.c
 	@echo
 	$(CC) -c $(CFLAGS) $(TOPT) -I . $(INCDIR) $< -o $@
 
@@ -191,7 +200,7 @@ $(ASMOBJS) : %.o : %.s
 
 %elf: $(OBJS)
 	@echo
-	$(LD) $(ASMOBJS) $(AOBJS) $(TOBJS) $(CPPOBJS) $(LDFLAGS) $(LIBS) -o $@
+	$(LD) $(ASMOBJS) $(ACOBJS) $(TCOBJS) $(ACPPOBJS) $(TCPPOBJS) $(LDFLAGS) $(LIBS) -o $@
 
 %hex: %elf
 	$(HEX) $< $@
@@ -209,12 +218,14 @@ clean:
 	-rm -f $(PROJECT).map
 	-rm -f $(PROJECT).hex
 	-rm -f $(PROJECT).bin
-	-rm -f $(CSRC:.c=.c.bak)
-	-rm -f $(CSRC:.c=.lst)
-	-rm -f $(CPPSRC:.cpp=.c.bak)
-	-rm -f $(CPPSRC:.cpp=.lst)
-	-rm -f $(TSRC:.c=.c.bak)
-	-rm -f $(TSRC:.c=.lst)
+	-rm -f $(ACSRC:.c=.c.bak)
+	-rm -f $(ACSRC:.c=.lst)
+	-rm -f $(TCSRC:.c=.c.bak)
+	-rm -f $(TCSRC:.c=.lst)
+	-rm -f $(ACPPSRC:.cpp=.c.bak)
+	-rm -f $(ACPPSRC:.cpp=.lst)
+	-rm -f $(TCPPSRC:.cpp=.c.bak)
+	-rm -f $(TCPPSRC:.cpp=.lst)
 	-rm -f $(ASMSRC:.s=.s.bak)
 	-rm -f $(ASMSRC:.s=.lst)
 	-rm -fR .dep
diff --git a/demos/ARM7-LPC214x-G++/Makefile.thumb b/demos/ARM7-LPC214x-G++/Makefile.thumb
index 0df66f2d3..16ab4d33f 100644
--- a/demos/ARM7-LPC214x-G++/Makefile.thumb
+++ b/demos/ARM7-LPC214x-G++/Makefile.thumb
@@ -18,6 +18,12 @@
 
 TRGT = arm-elf-
 CC   = $(TRGT)gcc
+CPPC = $(TRGT)g++
+# Enable loading with g++ only if you need C++ runtime support.
+# NOTE: You can use C++ even without C++ support if you are careful. C++
+#       runtime support makes code size explode.
+LD   = $(TRGT)gcc
+#LD   = $(TRGT)g++
 CP   = $(TRGT)objcopy
 AS   = $(TRGT)gcc -x assembler-with-cpp
 OD   = $(TRGT)objdump
@@ -62,24 +68,30 @@ UDEFS =
 UADEFS =
 
 # List ARM-mode C source files here
-ASRC =
+ACSRC =
+
+# List ARM-mode C++ source files here
+ACPPSRC =
 
 # List THUMB-mode C sources here
-# NOTE: If any module is compiled in thumb mode then -mthumb-interwork is
-#       enabled for all modules and that lowers performance.
-TSRC = ../../ports/ARM7-LPC214x/chcore.c \
-       ../../ports/ARM7-LPC214x/vic.c  \
-       ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
-       ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
-       ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
-       ../../src/chserial.c \
-       board.c main.c
+TCSRC = ../../ports/ARM7-LPC214x/chcore.c \
+        ../../ports/ARM7-LPC214x/vic.c  \
+        ../../ports/ARM7-LPC214x/lpc214x_serial.c  \
+        ../../src/chinit.c ../../src/chdebug.c ../../src/chlists.c ../../src/chdelta.c \
+        ../../src/chschd.c ../../src/chthreads.c ../../src/chsem.c ../../src/chmtx.c \
+        ../../src/chevents.c ../../src/chmsg.c ../../src/chsleep.c ../../src/chqueues.c \
+        ../../src/chserial.c \
+        ../../src/lib/evtimer.c ../../test/test.c \
+        board.c
+
+# List THUMB-mode C++ source files here
+TCPPSRC = ../../src/lib/ch.cpp main.cpp
 
 # List ASM source files here
 ASMSRC = ../../ports/ARM7-LPC214x/crt0.s ../../ports/ARM7/chsys.s
 
 # List all user directories here
-UINCDIR = ../../src/include ../../src/lib \
+UINCDIR = ../../src/include ../../src/lib ../../test \
           ../../ports/ARM7 ../../ports/ARM7-LPC214x
 
 # List the user directory to look for the libraries here
@@ -99,51 +111,66 @@ TOPT = -mthumb -D THUMB
 #       chconf.h.
 # NOTE: -falign-functions=16 may improve the performance, not always, but
 #       increases the code size.
-OPT = -Os -ggdb -fomit-frame-pointer
+OPT = -O2 -ggdb -fomit-frame-pointer
 #OPT += -ffixed-r7
 OPT += -falign-functions=16
 
+# C++ specific options here
+# NOTE: -fno-rtti saves a LOT of code space, remove it only if you really need
+#       RTTI.
+CPPOPT = -fno-rtti
+
 # Define warning options here
-WARN = -Wall -Wstrict-prototypes
+WARN = -Wall
 
 #
 # End of user defines
 ##############################################################################################
 
-INCDIR  = $(patsubst %,-I%,$(DINCDIR) $(UINCDIR))
-LIBDIR  = $(patsubst %,-L%,$(DLIBDIR) $(ULIBDIR))
-DEFS    = $(DDEFS) $(UDEFS)
-ADEFS   = $(DADEFS) $(UADEFS)
-AOBJS   = $(ASRC:.c=.o)
-TOBJS   = $(TSRC:.c=.o)
-OBJS	= $(ASMOBJS) $(AOBJS) $(TOBJS)
-ASMOBJS = $(ASMSRC:.s=.o)
-LIBS    = $(DLIBS) $(ULIBS)
-MCFLAGS = -mcpu=$(MCU)
+INCDIR   = $(patsubst %,-I%,$(DINCDIR) $(UINCDIR))
+LIBDIR   = $(patsubst %,-L%,$(DLIBDIR) $(ULIBDIR))
+DEFS     = $(DDEFS) $(UDEFS)
+ADEFS    = $(DADEFS) $(UADEFS)
+ASRC	 = $(ACSRC)$(ACPPSRC)
+TSRC	 = $(TCSRC)$(TCPPSRC)
+SRC	 = $(ASRC)$(TSRC)
+ACOBJS   = $(ACSRC:.c=.o)
+ACPPOBJS = $(ACPPSRC:.cpp=.o)
+TCOBJS   = $(TCSRC:.c=.o)
+TCPPOBJS = $(TCPPSRC:.cpp=.o)
+ASMOBJS  = $(ASMSRC:.s=.o)
+OBJS	 = $(ASMOBJS) $(ACOBJS) $(TCOBJS) $(ACPPOBJS) $(TCPPOBJS)
+LIBS     = $(DLIBS) $(ULIBS)
+MCFLAGS  = -mcpu=$(MCU)
 
-ASFLAGS = $(MCFLAGS) -Wa,-amhls=$(<:.s=.lst) $(ADEFS)
-CPFLAGS = $(MCFLAGS) $(OPT) $(WARN) -Wa,-alms=$(<:.c=.lst) $(DEFS)
-LDFLAGS = $(MCFLAGS) -nostartfiles -T$(LDSCRIPT) -Wl,-Map=$(PROJECT).map,--cref,--no-warn-mismatch $(LIBDIR)
-ODFLAGS	= -x --syms
+ASFLAGS  = $(MCFLAGS) -Wa,-amhls=$(<:.s=.lst) $(ADEFS)
+CFLAGS   = $(MCFLAGS) $(OPT) $(WARN) -Wa,-alms=$(<:.c=.lst) $(DEFS)
+CPPFLAGS = $(MCFLAGS) $(OPT) $(CPPOPT) $(WARN) -Wa,-alms=$(<:.cpp=.lst) $(DEFS)
+LDFLAGS  = $(MCFLAGS) -nostartfiles -T$(LDSCRIPT) -Wl,-Map=$(PROJECT).map,--cref,--no-warn-mismatch $(LIBDIR)
+ODFLAGS  = -x --syms
 
 # Thumb interwork enabled only if needed because it kills performance.
 ifneq ($(TSRC),)
-  CPFLAGS += -D THUMB_PRESENT
+  CFLAGS += -D THUMB_PRESENT
+  CPPFLAGS += -D THUMB_PRESENT
   ASFLAGS += -D THUMB_PRESENT
   ifneq ($(ASRC),)
     # Mixed ARM and THUMB case.
-    CPFLAGS += -mthumb-interwork
+    CFLAGS += -mthumb-interwork
+    CPPFLAGS += -mthumb-interwork
     LDFLAGS += -mthumb-interwork
   else
     # Pure THUMB case, THUMB C code cannot be called by ARM asm code directly.
-    CPFLAGS += -D THUMB_NO_INTERWORKING
+    CFLAGS += -D THUMB_NO_INTERWORKING
+    CPPFLAGS += -D THUMB_NO_INTERWORKING
     LDFLAGS += -mthumb
     ASFLAGS += -D THUMB_NO_INTERWORKING
   endif
 endif
 
 # Generate dependency information
-CPFLAGS += -MD -MP -MF .dep/$(@F).d
+CFLAGS += -MD -MP -MF .dep/$(@F).d
+CPPFLAGS += -MD -MP -MF .dep/$(@F).d
 
 #
 # Makefile rules
@@ -151,13 +178,21 @@ CPFLAGS += -MD -MP -MF .dep/$(@F).d
 
 all: $(OBJS) $(PROJECT).elf $(PROJECT).hex $(PROJECT).bin $(PROJECT).dmp
 
-$(AOBJS) : %.o : %.c
+$(ACPPOBJS) : %.o : %.cpp
 	@echo
-	$(CC) -c $(CPFLAGS) $(AOPT) -I . $(INCDIR) $< -o $@
+	$(CPPC) -c $(CPPFLAGS) $(AOPT) -I . $(INCDIR) $< -o $@
 
-$(TOBJS) : %.o : %.c
+$(TCPPOBJS) : %.o : %.cpp
 	@echo
-	$(CC) -c $(CPFLAGS) $(TOPT) -I . $(INCDIR) $< -o $@
+	$(CPPC) -c $(CPPFLAGS) $(TOPT) -I . $(INCDIR) $< -o $@
+
+$(ACOBJS) : %.o : %.c
+	@echo
+	$(CC) -c $(CFLAGS) $(AOPT) -I . $(INCDIR) $< -o $@
+
+$(TCOBJS) : %.o : %.c
+	@echo
+	$(CC) -c $(CFLAGS) $(TOPT) -I . $(INCDIR) $< -o $@
 
 $(ASMOBJS) : %.o : %.s
 	@echo
@@ -165,7 +200,7 @@ $(ASMOBJS) : %.o : %.s
 
 %elf: $(OBJS)
 	@echo
-	$(CC) $(ASMOBJS) $(AOBJS) $(TOBJS) $(LDFLAGS) $(LIBS) -o $@
+	$(LD) $(ASMOBJS) $(ACOBJS) $(TCOBJS) $(ACPPOBJS) $(TCPPOBJS) $(LDFLAGS) $(LIBS) -o $@
 
 %hex: %elf
 	$(HEX) $< $@
@@ -183,10 +218,14 @@ clean:
 	-rm -f $(PROJECT).map
 	-rm -f $(PROJECT).hex
 	-rm -f $(PROJECT).bin
-	-rm -f $(ASRC:.c=.c.bak)
-	-rm -f $(ASRC:.c=.lst)
-	-rm -f $(TSRC:.c=.c.bak)
-	-rm -f $(TSRC:.c=.lst)
+	-rm -f $(ACSRC:.c=.c.bak)
+	-rm -f $(ACSRC:.c=.lst)
+	-rm -f $(TCSRC:.c=.c.bak)
+	-rm -f $(TCSRC:.c=.lst)
+	-rm -f $(ACPPSRC:.cpp=.c.bak)
+	-rm -f $(ACPPSRC:.cpp=.lst)
+	-rm -f $(TCPPSRC:.cpp=.c.bak)
+	-rm -f $(TCPPSRC:.cpp=.lst)
 	-rm -f $(ASMSRC:.s=.s.bak)
 	-rm -f $(ASMSRC:.s=.lst)
 	-rm -fR .dep
diff --git a/demos/ARM7-LPC214x-G++/main.cpp b/demos/ARM7-LPC214x-G++/main.cpp
index 1d8acfb5e..39b31e89a 100644
--- a/demos/ARM7-LPC214x-G++/main.cpp
+++ b/demos/ARM7-LPC214x-G++/main.cpp
@@ -28,7 +28,8 @@
 using namespace chibios_rt;
 
 /*
- * LED blinking sequences.
+ * LED blink sequences.
+ * NOTE: Sequences must always be terminated by a GOTO instruction.
  */
 #define SLEEP           0
 #define GOTO            1
@@ -39,9 +40,9 @@ using namespace chibios_rt;
 typedef struct {
   uint8_t       action;
   uint32_t      value;
-} bitop_t;
+} seqop_t;
 
-bitop_t LED1_sequence[] =
+static const seqop_t LED1_sequence[] =
 {
   {BITCLEAR, 0x00000400},
   {SLEEP,    200},
@@ -50,7 +51,7 @@ bitop_t LED1_sequence[] =
   {GOTO,     0}
 };
 
-bitop_t LED2_sequence[] =
+static const seqop_t LED2_sequence[] =
 {
   {SLEEP,    1000},
   {BITCLEAR, 0x00000800},
@@ -60,7 +61,7 @@ bitop_t LED2_sequence[] =
   {GOTO,     1}
 };
 
-bitop_t LED3_sequence[] =
+static const seqop_t LED3_sequence[] =
 {
   {BITCLEAR, 0x80000000},
   {SLEEP,    200},
@@ -70,14 +71,16 @@ bitop_t LED3_sequence[] =
 };
 
 /**
- * Blinker thread class. It can drive LEDs or other output pins.
+ * Sequencer thread class. It can drive LEDs or other output pins.
  */
-class BlinkerThread : BaseThread {
+class SequencerThread : BaseThread {
 private:
-  WorkingArea(wa, 64);
-  bitop_t *base, *curr, *top;
+
+  WorkingArea(wa, 64);                          // Thread working area.
+  const seqop_t *base, *curr;                   // Thread local variables.
 
 protected:
+
   virtual msg_t Main(void) {
 
     while (TRUE) {
@@ -102,7 +105,7 @@ protected:
   }
 
 public:
-  BlinkerThread(bitop_t *sequence) : BaseThread(NORMALPRIO, 0, wa, sizeof wa) {
+  SequencerThread(const seqop_t *sequence) : BaseThread(NORMALPRIO, 0, wa, sizeof wa) {
 
     base = curr = sequence;
   }
@@ -127,19 +130,19 @@ int main(int argc, char **argv) {
   static EvTimer evt;
   struct EventListener el0;
 
-  System::Init();
+  System::Init();                       // ChibiOS/RT goes live here.
 
-  evtInit(&evt, 500);                   /* Initializes an event timer object.   */
-  evtStart(&evt);                       /* Starts the event timer.              */
-  chEvtRegister(&evt.et_es, &el0, 0);   /* Registers on the timer event source. */
+  evtInit(&evt, 500);                   // Initializes an event timer object.
+  evtStart(&evt);                       // Starts the event timer.
+  chEvtRegister(&evt.et_es, &el0, 0);   // Registers on the timer event source.
 
   /*
-   * Starts serveral instances of the BlinkerThread class, each one operating
+   * Starts serveral instances of the SequencerThread class, each one operating
    * on a different LED.
    */
-  BlinkerThread blinker1(LED1_sequence);
-  BlinkerThread blinker2(LED2_sequence);
-  BlinkerThread blinker3(LED3_sequence);
+  SequencerThread blinker1(LED1_sequence);
+  SequencerThread blinker2(LED2_sequence);
+  SequencerThread blinker3(LED3_sequence);
 
   /*
    * Serves timer events.
diff --git a/ports/ARM7/chcore.h b/ports/ARM7/chcore.h
index 3604253b5..eed850266 100644
--- a/ports/ARM7/chcore.h
+++ b/ports/ARM7/chcore.h
@@ -74,8 +74,14 @@ typedef struct {
 }
 
 #ifdef THUMB
-extern void chSysLock(void);
-extern void chSysUnlock(void);
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void chSysLock(void);
+  void chSysUnlock(void);
+#ifdef __cplusplus
+}
+#endif
 #else /* !THUMB */
 #define chSysLock() asm("msr     CPSR_c, #0x9F")
 #define chSysUnlock() asm("msr     CPSR_c, #0x1F")
@@ -123,11 +129,17 @@ extern void chSysUnlock(void);
 
 /* It requires zero bytes, but better be safe.*/
 #define IDLE_THREAD_STACK_SIZE 8
-void _IdleThread(void *p) __attribute__((noreturn));
 
-void chSysHalt(void);
-void chSysSwitchI(Thread *otp, Thread *ntp);
-void chSysPuts(char *msg);
-void threadstart(void);
+#ifdef __cplusplus
+extern "C" {
+#endif
+  void _IdleThread(void *p) __attribute__((noreturn));
+  void chSysHalt(void);
+  void chSysSwitchI(Thread *otp, Thread *ntp);
+  void chSysPuts(char *msg);
+  void threadstart(void);
+#ifdef __cplusplus
+}
+#endif
 
 #endif /* _CHCORE_H_ */