diff --git a/runtime/org.eclipse.etrice.runtime.c/src/platforms/MT_POSIX_GENERIC_GCC/etThread.c b/runtime/org.eclipse.etrice.runtime.c/src/platforms/MT_POSIX_GENERIC_GCC/etThread.c
index 30d920a..cb4beb2 100644
--- a/runtime/org.eclipse.etrice.runtime.c/src/platforms/MT_POSIX_GENERIC_GCC/etThread.c
+++ b/runtime/org.eclipse.etrice.runtime.c/src/platforms/MT_POSIX_GENERIC_GCC/etThread.c
@@ -23,6 +23,7 @@
 
 #include <time.h>
 #include <sys/unistd.h>
+#include <errno.h>
 
 typedef void *(*threadFunc)(void *);
 void* etThread_execute(etThread* self);
@@ -91,13 +92,16 @@
 	ET_MSC_LOGGER_SYNC_ENTRY("etThread", "sleep")
 	{
 		/* TODO: nanosleep doesn't work at all */
-/*		struct timespec time; */
-/*		time.tv_nsec = 1000*1000*millis; */
-/*		time.tv_sec = 0; */
-/*		nanosleep(&time, NULL); */
-		if (millis<1000)
-			millis = 1000;
-		sleep(millis/1000);
+		struct timespec time;
+		time.tv_sec = millis / 1000;
+		time.tv_nsec = (millis - time.tv_sec * 1000) * 1000*1000;
+		while(nanosleep(&time, &time) != 0) {
+			if(errno != EINTR)
+				break;
+		}
+//		if (millis<1000)
+//			millis = 1000;
+//		sleep(millis/1000);
 	}
 	ET_MSC_LOGGER_SYNC_EXIT
 }
diff --git a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/AbstractMessageService.cpp b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/AbstractMessageService.cpp
index a52351d..e8b5877 100644
--- a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/AbstractMessageService.cpp
+++ b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/AbstractMessageService.cpp
@@ -18,8 +18,8 @@
 AbstractMessageService::AbstractMessageService(IRTObject* parent, const std::string& name, int node, int thread) :
 		RTObject(parent, name),
 		m_address(node, thread, 0),
-		m_messageQueue(this, "Queue"),
-		m_messageDispatcher(this, m_address.createInc(), "Dispatcher") {
+		m_messageQueue(NULL, "Queue"),
+		m_messageDispatcher(NULL, m_address.createInc(), "Dispatcher") {
 }
 
 Address AbstractMessageService::getFreeAddress() {
diff --git a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageService.cpp b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageService.cpp
index b6ef64a..e2d7493 100644
--- a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageService.cpp
+++ b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageService.cpp
@@ -67,7 +67,6 @@
 }
 
 MessageService::~MessageService() {
-	std::cout << "~MessageService" << std::endl;
 	etMutex_destruct(&m_mutex);
 	etSema_destruct(&m_executionSemaphore);
 	etThread_destruct(&m_thread);
@@ -77,6 +76,7 @@
 }
 
 void MessageService::start() {
+	m_running = true;
 	etThread_start(&m_thread);
 	if (m_execMode == IMessageService::POLLED || m_execMode == IMessageService::MIXED) {
 		etTimer_start(&m_timer);
@@ -84,7 +84,6 @@
 }
 
 void MessageService::run() {
-	m_running = true;
 	while (m_running) {
 		etMutex_enter(&m_mutex);
 		const Message* msg = getMessageQueue().pop(); // get next Message from Queue
diff --git a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageServiceController.cpp b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageServiceController.cpp
index f6af906..89ab179 100644
--- a/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageServiceController.cpp
+++ b/runtime/org.eclipse.etrice.runtime.cpp/src/common/messaging/MessageServiceController.cpp
@@ -112,13 +112,13 @@
 	// terminate all message services
 	etMutex_enter(&m_mutex);
 	m_terminateServices = m_messageServices;
-	etMutex_leave(&m_mutex);
 
 	std::map<int, IMessageService*>::iterator it = m_terminateServices.begin();
 	for (; it != m_terminateServices.end(); ++it) {
 		(it->second)->terminate();
 		//TODO TS: stop in order of priorities
 	}
+	etMutex_leave(&m_mutex);
 }
 
 void MessageServiceController::waitTerminate() {
diff --git a/tests/org.eclipse.etrice.runtime.cpp.tests/src/RunAllTestCases.cpp b/tests/org.eclipse.etrice.runtime.cpp.tests/src/RunAllTestCases.cpp
index b66828f..ac99ad9 100644
--- a/tests/org.eclipse.etrice.runtime.cpp.tests/src/RunAllTestCases.cpp
+++ b/tests/org.eclipse.etrice.runtime.cpp.tests/src/RunAllTestCases.cpp
@@ -20,7 +20,7 @@
 #include "messaging/RTObjectTest.h"
 #include "messaging/MessageSeQueueTest.h"
 #include "messaging/MessageDispatcherTest.h"
-//#include "messaging/MessageServiceTest.h"
+#include "messaging/MessageServiceTest.h"
 //#include "messaging/MessageServiceControllerTest.h"
 
 #include "etUnit/etUnit.h"
@@ -61,9 +61,9 @@
 
 	MessageDispatcherTest msgDispatcherTest;
 	msgDispatcherTest.run();
-//
-//	MessageServiceTest msgServiceTest;
-//	msgServiceTest.run();
+
+	MessageServiceTest msgServiceTest;
+	msgServiceTest.run();
 //
 //	MessageServiceControllerTest msgSvcCtrlTest;
 //	msgSvcCtrlTest.run();
diff --git a/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.cpp b/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.cpp
new file mode 100644
index 0000000..e777b40
--- /dev/null
+++ b/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.cpp
@@ -0,0 +1,170 @@
+/*******************************************************************************
+ * Copyright (c) 2016 protos software gmbh (http://www.protos.de).
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * CONTRIBUTORS:
+ * 		Jan Belle (initial contribution)
+ *
+ *******************************************************************************/
+
+#include "messaging/MessageServiceTest.h"
+#include "etUnit/etUnit.h"
+#include "common/messaging/MessageService.h"
+#include "common/messaging/MessageServiceController.h"
+#include "common/messaging/RTServices.h"
+
+using namespace etRuntime;
+
+MessageServiceTest::MessageServiceTest() :
+		etTestSuite("MessageServiceTest"), m_senderTerminated(0) {
+
+	RTServices::getInstance().getMsgSvcCtrl().resetAll();
+
+	etSema_construct(&m_sema);
+	etMutex_construct(&m_mutex);
+	etTime interval;
+	interval.sec = (etInt32) (timeoutTime / 1000);
+	interval.nSec = (timeoutTime - interval.sec * 1000) * 1000000;
+	etTimer_construct(&m_timer, &interval, MessageServiceTest::timeout,
+			static_cast<void*>(this));
+}
+
+MessageServiceTest::~MessageServiceTest() {
+	etMutex_destruct(&m_mutex);
+	etSema_destruct(&m_sema);
+	etTimer_destruct(&m_timer);
+}
+
+void MessageServiceTest::senderTerminated() {
+	etMutex_enter(&m_mutex);
+	m_senderTerminated++;
+	if (m_senderTerminated >= 2)
+		etSema_wakeup(&m_sema);
+	etMutex_leave(&m_mutex);
+}
+
+void MessageServiceTest::timeout() {
+	EXPECT_TRUE(m_caseId, "MessageService test failed (Timeout)", false);
+	senderTerminated();
+	senderTerminated();
+
+	MessageServiceController &msgSvcCtrl =
+			RTServices::getInstance().getMsgSvcCtrl();
+	IMessageService* msgSvc = msgSvcCtrl.getMsgSvc(0);
+	if (msgSvc) {
+		msgSvc->terminate();
+		msgSvcCtrl.setMsgSvcTerminated(*msgSvc);
+	}
+}
+
+void MessageServiceTest::testBlocked() {
+
+	Address addr(1, 2, 1);
+	MessageServiceController& msgSvcCtrl =
+			RTServices::getInstance().getMsgSvcCtrl();
+	MessageService msgService(NULL, IMessageService::BLOCKED, 1, 2,
+			"Test MessageService");
+	msgSvcCtrl.addMsgSvc(msgService);
+	MessageCounter msgCounter(NULL, "MessageCounter", addr);
+	msgService.addMessageReceiver(msgCounter);
+	Sender sender1(*this, msgService, addr);
+	Sender sender2(*this, msgService, addr);
+	msgSvcCtrl.start();
+	sender1.start();
+	sender2.start();
+
+	etThread_sleep(testingTime / 3);
+
+	// Construct timeout and terminate senders
+	etTimer_start(&m_timer);
+	sender1.terminate();
+	sender2.terminate();
+	// Wait for sender-threads to terminate
+	etSema_waitForWakeup(&m_sema);
+	etTimer_stop(&m_timer);
+
+	// Wait until all messages are delivered
+	etThread_sleep(2 * testingTime / 3);
+
+	// Construct timeout and stop MessageService
+	etTimer_start(&m_timer);
+	msgSvcCtrl.stop();
+	msgSvcCtrl.removeMsgSvc(msgService);
+
+	etTimer_stop(&m_timer);
+
+	EXPECT_EQUAL_INT32(m_caseId, "Blocked MessageService test failed",
+			sender1.getSentMessages() + sender2.getSentMessages(),
+			msgCounter.getMessageCount());
+
+}
+
+void MessageServiceTest::testPolled() {
+
+	etTime interval;
+	interval.sec = (etInt32) (interval_polled / 1000);
+	interval.nSec = (interval_polled - interval.sec * 1000) * 1000000;
+	MessageServiceController& msgSvcCtrl =
+			RTServices::getInstance().getMsgSvcCtrl();
+	MessageService msgService(NULL, IMessageService::POLLED, interval, 1, 2,
+			"Test MessageService");
+	msgSvcCtrl.addMsgSvc(msgService);
+	MessageCounter msgCounter(NULL, "Message Counter",
+			msgService.getFreeAddress());
+	msgService.addPollingMessageReceiver(msgCounter);
+	msgSvcCtrl.start();
+
+	etThread_sleep(testingTime);
+
+	etTimer_start(&m_timer);
+	msgSvcCtrl.stop();
+	etTimer_stop(&m_timer);
+	msgSvcCtrl.removeMsgSvc(msgService);
+	msgService.removePollingMessageReceiver(msgCounter);
+	etInt32 count = msgCounter.getMessageCount();
+	etInt32 expectedCount = testingTime / interval_polled;
+	EXPECT_TRUE(m_caseId, "Polled MessageService test failed",
+			0.9 * expectedCount <= count && 1.1 * expectedCount >= count);
+//	EXPECT_EQUAL_INT32(m_caseId, "Polled MessageService test failed", expectedCount, count);
+
+}
+
+void MessageServiceTest::runAllTestCases() {
+	ADD_TESTCASE_CPP(testBlocked)
+	ADD_TESTCASE_CPP(testPolled)
+}
+
+Sender::Sender(MessageServiceTest &msgServiceTest, MessageService &msgService,
+		Address &receiver, int priority) :
+		m_msgServiceTest(msgServiceTest), m_msgService(msgService), m_addr(
+				receiver), m_messagesSent(0), m_running(false) {
+
+	etThread_construct(&m_thread, static_cast<etStacksize>(1024), priority,
+			(etThreadname) "Thread Sender1", Sender::run,
+			static_cast<void*>(this));
+}
+
+Sender::~Sender() {
+	etThread_destruct(&m_thread);
+}
+
+void Sender::start() {
+	m_running = true;
+	etThread_start(&m_thread);
+}
+
+void Sender::terminate() {
+	m_running = false;
+}
+
+void Sender::run() {
+	while (m_running) {
+		m_msgService.receive(new Message(m_addr, 0));
+		m_messagesSent++;
+	}
+
+	m_msgServiceTest.senderTerminated();
+}
diff --git a/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.h b/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.h
new file mode 100644
index 0000000..cccf480
--- /dev/null
+++ b/tests/org.eclipse.etrice.runtime.cpp.tests/src/messaging/MessageServiceTest.h
@@ -0,0 +1,104 @@
+/*******************************************************************************
+ * Copyright (c) 2016 protos software gmbh (http://www.protos.de).
+ * All rights reserved. This program and the accompanying materials
+ * are made available under the terms of the Eclipse Public License v1.0
+ * which accompanies this distribution, and is available at
+ * http://www.eclipse.org/legal/epl-v10.html
+ *
+ * CONTRIBUTORS:
+ * 		Jan Belle (initial contribution)
+ *
+ *******************************************************************************/
+
+#ifndef SRC_MESSAGING_MESSAGESERVICETEST_H_
+#define SRC_MESSAGING_MESSAGESERVICETEST_H_
+
+#include "util/etTestSuite.h"
+#include "common/messaging/RTObject.h"
+#include "common/messaging/IMessageReceiver.h"
+#include "common/messaging/MessageService.h"
+#include "osal/etThread.h"
+#include "osal/etSema.h"
+#include "osal/etMutex.h"
+#include "osal/etTimer.h"
+
+class MessageServiceTest: public etTestSuite {
+public:
+	static const etInt32 testingTime = 1000;
+	static const etInt32 timeoutTime = 5000;
+	static const etInt32 interval_polled = 20;
+
+	MessageServiceTest(void);
+
+	~MessageServiceTest(void);
+
+	void senderTerminated(void);
+
+protected:
+	void testBlocked(void);
+	void testPolled(void);
+	void runAllTestCases(void);
+
+private:
+	int m_senderTerminated;
+	etSema m_sema;
+	etMutex m_mutex;
+	etTimer m_timer;
+
+	void timeout(void);
+	static void timeout(void* self) {
+		(static_cast<MessageServiceTest*>(self))->timeout();
+	}
+};
+
+class MessageCounter: public etRuntime::RTObject,
+		public etRuntime::IMessageReceiver {
+public:
+	MessageCounter(IRTObject *parent, const std::string &name,
+			const etRuntime::Address &address) :
+			RTObject(parent, name), m_counter(0), m_address(address) {
+	}
+
+	void receive(const etRuntime::Message *msg) {
+		m_counter++;
+	}
+
+	const etRuntime::Address& getAddress(void) const {
+		return m_address;
+	}
+
+	etInt32 getMessageCount(void) {
+		return m_counter;
+	}
+
+private:
+	etInt32 m_counter;
+	etRuntime::Address m_address;
+};
+
+class Sender {
+public:
+	Sender(MessageServiceTest &msgServiceTest,
+			etRuntime::MessageService &msgService, etRuntime::Address &receiver,
+			int priority = 0);
+	~Sender(void);
+	etInt32 getSentMessages(void) {
+		return m_messagesSent;
+	}
+	void start(void);
+	void terminate(void);
+	void run(void);
+
+private:
+	MessageServiceTest &m_msgServiceTest;
+	etRuntime::MessageService &m_msgService;
+	etThread m_thread;
+	etRuntime::Address m_addr;
+	etInt32 m_messagesSent;bool m_running;
+
+	static void run(void* self) {
+		(static_cast<Sender*>(self))->run();
+	}
+};
+
+#endif /* SRC_MESSAGING_MESSAGESERVICETEST_H_ */