From: Mikko Rasa <tdb@tdb.fi>
Date: Mon, 9 Jun 2008 03:27:18 +0000 (+0000)
Subject: Rewrite command/reply system
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=540e931ec79129c8776f48434ff5b230b53337f1;p=r2c2.git

Rewrite command/reply system
Read lok events (but don't do anything with them yet)
Add some missing includes
---

diff --git a/source/3d/layout.cpp b/source/3d/layout.cpp
index 9c174fb..8093081 100644
--- a/source/3d/layout.cpp
+++ b/source/3d/layout.cpp
@@ -7,6 +7,7 @@ Distributed under the GPL
 
 #include <algorithm>
 #include <fstream>
+#include <limits>
 #include <msp/gl/rendermode.h>
 #include <msp/gl/select.h>
 #include <msp/gl/texture.h>
diff --git a/source/libmarklin/command.cpp b/source/libmarklin/command.cpp
index 9530cba..dd1cdbc 100644
--- a/source/libmarklin/command.cpp
+++ b/source/libmarklin/command.cpp
@@ -5,20 +5,41 @@ Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
+#include <cstring>
+#include <msp/strings/formatter.h>
 #include "command.h"
 
 using namespace std;
+using namespace Msp;
 
 namespace Marklin {
 
-Command::Command(const string &c):
+Command::Command(Cmd c, const unsigned char *d, unsigned l):
 	cmd(c),
+	len(1),
 	sent(false)
-{ }
+{
+	data[0]=cmd;
+	if(d)
+	{
+		memcpy(data+1, d, min(l, 127U));
+		len+=min(l, 127U);
+	}
+}
+
+void Command::send(int fd)
+{
+	write(fd, data, len);
+	sent=true;
+}
 
-void Command::set_sent(bool s)
+ostream &operator<<(ostream &out, const Command &cmd)
 {
-	sent=s;
+	out<<cmd.cmd;
+	for(unsigned i=1; i<cmd.len; ++i)
+		out<<format(" %02X", static_cast<int>(cmd.data[i]));
+
+	return out;
 }
 
 } // namespace Marklin
diff --git a/source/libmarklin/command.h b/source/libmarklin/command.h
index 6075ab2..a270be0 100644
--- a/source/libmarklin/command.h
+++ b/source/libmarklin/command.h
@@ -8,26 +8,33 @@ Distributed under the GPL
 #ifndef COMMAND_H_
 #define COMMAND_H_
 
+#include <ostream>
 #include <string>
 #include <sigc++/sigc++.h>
 #include "constants.h"
 
 namespace Marklin {
 
+class Reply;
+
 class Command
 {
 private:
-	std::string cmd;
-	bool        sent;
+	Cmd cmd;
+	unsigned char data[128];
+	unsigned len;
+	bool sent;
 
 public:
-	sigc::signal<void, Error, const std::string &> signal_done;
+	sigc::signal<void, const Reply &> signal_done;
+
+	Command(Cmd, const unsigned char *, unsigned);
 
-	Command(const std::string &);
+	void send(int);
+	bool get_sent() const   { return sent; }
+	Cmd get_command() const { return cmd; }
 
-	void              set_sent(bool);
-	const std::string &get_command() const { return cmd; }
-	bool              get_sent() const     { return sent; }
+	friend std::ostream &operator<<(std::ostream &, const Command &);
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/constants.h b/source/libmarklin/constants.h
index a61972d..f68737d 100644
--- a/source/libmarklin/constants.h
+++ b/source/libmarklin/constants.h
@@ -8,6 +8,8 @@ Distributed under the GPL
 #ifndef ERROR_H_
 #define ERROR_H_
 
+#include <ostream>
+
 namespace Marklin {
 
 enum Error
@@ -31,6 +33,8 @@ enum Error
 	ERR_UNKNOWN_ERROR=0xFF
 };
 
+std::ostream &operator<<(std::ostream &, const Error &);
+
 enum Cmd
 {
 	CMD_LOK=0x80,
@@ -55,6 +59,8 @@ enum Cmd
 	CMD_EVENT_SENSOR=0xCB
 };
 
+std::ostream &operator<<(std::ostream &, const Cmd &);
+
 } // namespace Marklin
 
 #endif
diff --git a/source/libmarklin/control.cpp b/source/libmarklin/control.cpp
index ecf1034..6c6ceb3 100644
--- a/source/libmarklin/control.cpp
+++ b/source/libmarklin/control.cpp
@@ -14,6 +14,7 @@ Distributed under the GPL
 #include <msp/time/utils.h>
 #include "command.h"
 #include "control.h"
+#include "reply.h"
 
 using namespace std;
 using namespace Msp;
@@ -22,7 +23,6 @@ namespace Marklin {
 
 Control::Control():
 	serial_fd(-1),
-	p50_enabled(false),
 	power(true),
 	poll_sensors(false),
 	debug(false)
@@ -43,9 +43,9 @@ void Control::set_power(bool p)
 {
 	power=p;
 	if(power)
-		command(string(1, CMD_POWER_ON));
+		command(CMD_POWER_ON);
 	else
-		command(string(1, CMD_POWER_OFF));
+		command(CMD_POWER_OFF);
 
 	signal_power_event.emit(power);
 }
@@ -76,6 +76,7 @@ void Control::open(const string &dev)
 	attr.c_cflag|=CSTOPB;
 
 	bool ok=false;
+	bool p50=false;
 	for(unsigned i=0; baud[i]; i+=2)
 	{
 		cfsetospeed(&attr, baud[i+1]);
@@ -86,19 +87,9 @@ void Control::open(const string &dev)
 		pollfd pfd={serial_fd, POLLIN, 0};
 		if(poll(&pfd, 1, 500)>0)
 		{
-			cout<<"IB detected at "<<baud[i]<<" bits/s, P50 is ";
+			cout<<"IB detected at "<<baud[i]<<" bits/s\n";
 			char buf[2];
-			if(read(serial_fd, buf, 2)==2)
-			{
-				p50_enabled=true;
-				cout<<"enabled";
-			}
-			else
-			{
-				p50_enabled=false;
-				cout<<"disabled";
-			}
-			cout<<'\n';
+			p50=(read(serial_fd, buf, 2)==2);
 			ok=true;
 			break;
 		}
@@ -106,13 +97,28 @@ void Control::open(const string &dev)
 
 	if(!ok)
 		throw Exception("IB not detected");
+	
+	if(p50)
+		write(serial_fd, "xZzA1\r", 6);
 
-	command(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
+	command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 }
 
-Command &Control::command(const string &cmd)
+Command &Control::command(Cmd cmd)
 {
-	queue.push_back(Command(cmd));
+	queue.push_back(Command(cmd, 0, 0));
+	return queue.back();
+}
+
+Command &Control::command(Cmd cmd, unsigned char data)
+{
+	queue.push_back(Command(cmd, &data, 1));
+	return queue.back();
+}
+
+Command &Control::command(Cmd cmd, const unsigned char *data, unsigned len)
+{
+	queue.push_back(Command(cmd, data, len));
 	return queue.back();
 }
 
@@ -171,18 +177,17 @@ void Control::tick()
 	if(t>next_event_query)
 	{
 		next_event_query=t+200*Time::msec;
-		command(string(1, CMD_EVENT)).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
+		command(CMD_EVENT).signal_done.connect(sigc::mem_fun(this, &Control::event_query_done));
 	}
 
 	if(poll_sensors)
 	{
-		unsigned max_addr=(--sensors.end())->second->get_address();
-		string cmd(3, 0);
-		cmd[0]=CMD_SENSOR_PARAM_SET;
-		cmd[1]=0;
-		cmd[2]=(max_addr+7)/8;
-		command(cmd);
-		command(string(1, CMD_SENSOR_REPORT));
+		unsigned max_addr=(--sensors.end())->first;
+		unsigned char data[2];
+		data[0]=0;
+		data[1]=(max_addr+7)/8;
+		command(CMD_SENSOR_PARAM_SET, data, 2);
+		command(CMD_SENSOR_REPORT);
 		poll_sensors=false;
 	}
 
@@ -191,16 +196,11 @@ void Control::tick()
 		pollfd pfd={serial_fd, POLLIN, 0};
 		if(poll(&pfd, 1, 0)>0)
 		{
-			string resp=read_reply(static_cast<Cmd>(static_cast<unsigned char>(queue.front().get_command()[0])));
+			Reply reply=Reply::read(serial_fd, queue.front().get_command());
 			if(debug)
-			{
-				printf("read:  ");
-				for(unsigned i=0; i<resp.size(); ++i)
-					printf("%02X ", static_cast<unsigned char>(resp[i]));
-				printf("(%d bytes)\n", resp.size());
-			}
-
-			queue.front().signal_done.emit(static_cast<Error>(resp[0]), resp.substr(1));
+				cout<<"R: "<<reply<<'\n';
+
+			queue.front().signal_done.emit(reply);
 			queue.erase(queue.begin());
 		}
 		else
@@ -209,26 +209,10 @@ void Control::tick()
 
 	if(!queue.empty())
 	{
-		string cmd=queue.front().get_command();
-
-		if(p50_enabled)
-		{
-			if(cmd[0]&0x80)
-				cmd="X"+cmd;
-			else
-				cmd="x"+cmd;
-		}
-
 		if(debug)
-		{
-			printf("write: ");
-			for(unsigned i=0; i<cmd.size(); ++i)
-				printf("%02X ", static_cast<unsigned char>(cmd[i]));
-			printf("(%d bytes)\n", cmd.size());
-		}
+			cout<<"W: "<<queue.front()<<'\n';
 
-		write(serial_fd, cmd.data(), cmd.size());
-		queue.front().set_sent(true);
+		queue.front().send(serial_fd);
 	}
 }
 
@@ -247,120 +231,52 @@ void Control::read_all(int fd, char *buf, int size)
 	}
 }
 
-string Control::read_reply(Cmd cmd)
-{
-	string result;
-	if(cmd==CMD_EVENT)
-	{
-		result+=ERR_NO_ERROR;
-		for(unsigned i=0; i<3; ++i)
-		{
-			char c;
-			read(serial_fd, &c, 1);
-			result+=c;
-			if(!(c&0x80)) break;
-		}
-	}
-	else if(cmd==CMD_EVENT_LOK)
-	{
-		result+=ERR_NO_ERROR;
-		char c[5];
-		read(serial_fd, c+4, 1);
-		result+=c[4];
-		while(c[4]&0x80)
-		{
-			read_all(serial_fd, c, 5);
-			result.append(c, 5);
-		}
-	}
-	else if(cmd==CMD_EVENT_TURNOUT)
-	{
-		result+=ERR_NO_ERROR;
-		char c[511];
-		read(serial_fd, c, 1);
-		read_all(serial_fd, c+1, c[0]*2);
-		result.append(c, c[0]*2+1);
-	}
-	else if(cmd==CMD_EVENT_SENSOR)
-	{
-		result+=ERR_NO_ERROR;
-		char c[3];
-		read(serial_fd, c+2, 1);
-		result+=c[2];
-		while(c[2])
-		{
-			read_all(serial_fd, c, 3);
-			result.append(c, 3);
-		}
-	}
-	else
-	{
-		if(cmd==CMD_STATUS)
-			result+=ERR_NO_ERROR;
-
-		unsigned expected_bytes=1;
-		if(cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
-			expected_bytes=2;
-		if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
-			expected_bytes=3;
-		if(cmd==CMD_LOK_STATUS)
-			expected_bytes=4;
-		if(cmd==CMD_LOK_CONFIG)
-			expected_bytes=5;
-		char c[5];
-		read_all(serial_fd, c, 1);
-		result+=c[0];
-		if(!c[0])
-		{
-			read_all(serial_fd, c+1, expected_bytes-1);
-			result.append(c+1, expected_bytes-1);
-		}
-	}
-
-	return result;
-}
-
-void Control::status_done(Error, const string &resp)
+void Control::status_done(const Reply &reply)
 {
-	power=((resp[0]&0x08)!=0);
+	power=((reply.get_data()[0]&0x08)!=0);
 	signal_power_event.emit(power);
 }
 
-void Control::event_query_done(Error, const string &resp)
+void Control::event_query_done(const Reply &reply)
 {
-	if(resp[0]&0x20)
-		command(string(1, CMD_EVENT_TURNOUT)).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
-	if(resp[0]&0x04)
-		command(string(1, CMD_EVENT_SENSOR)).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
-	if(resp.size()>1 && (resp[1]&0x40))
-		command(string(1, CMD_STATUS)).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
+	const unsigned char *data=reply.get_data();
+	if(data[0]&0x01)
+		command(CMD_EVENT_LOK);
+	if(data[0]&0x20)
+		command(CMD_EVENT_TURNOUT).signal_done.connect(sigc::mem_fun(this, &Control::turnout_event_done));
+	if(data[0]&0x04)
+		command(CMD_EVENT_SENSOR).signal_done.connect(sigc::mem_fun(this, &Control::sensor_event_done));
+	if(data[1]&0x40)
+		command(CMD_STATUS).signal_done.connect(sigc::mem_fun(this, &Control::status_done));
 }
 
-void Control::turnout_event_done(Error, const string &resp)
+void Control::turnout_event_done(const Reply &reply)
 {
-	unsigned count=resp[0];
+	const unsigned char *data=reply.get_data();
+	unsigned count=data[0];
 	for(unsigned i=0; i<count; ++i)
 	{
-		unsigned addr=static_cast<unsigned char>(resp[i*2+1])+((resp[i*2+2]&7)<<8);
-		bool status=!(resp[i*2+2]&0x80);
+		unsigned addr=(data[i*2+1])+((data[i*2+2]&7)<<8);
+		bool status=!(data[i*2+2]&0x80);
 		cout<<"Turnout "<<addr<<", status "<<status<<'\n';
 		signal_turnout_event.emit(addr, status);
 	}
 }
 
-void Control::sensor_event_done(Error, const string &resp)
+void Control::sensor_event_done(const Reply &reply)
 {
-	for(unsigned i=0; resp[i]; i+=3)
+	const unsigned char *data=reply.get_data();
+	for(unsigned i=0; data[i]; i+=3)
 	{
-		unsigned module=static_cast<unsigned char>(resp[i]);
+		unsigned module=data[i];
 
 		cout<<"S88 module "<<module<<", status ";
 		for(unsigned j=0; j<16; ++j)
-			cout<<((resp[i+1+j/8]>>(7-j%8))&1);
+			cout<<((data[i+1+j/8]>>(7-j%8))&1);
 		cout<<'\n';
 
 		for(unsigned j=0; j<16; ++j)
-			signal_sensor_event.emit(module*16+j-15, (resp[i+1+j/8]>>(7-j%8))&1);
+			signal_sensor_event.emit(module*16+j-15, (data[i+1+j/8]>>(7-j%8))&1);
 	}
 }
 
diff --git a/source/libmarklin/control.h b/source/libmarklin/control.h
index a911860..0ec3d5b 100644
--- a/source/libmarklin/control.h
+++ b/source/libmarklin/control.h
@@ -20,12 +20,12 @@ Distributed under the GPL
 namespace Marklin {
 
 class Command;
+class Reply;
 
 class Control
 {
 private:
 	int  serial_fd;
-	bool p50_enabled;
 	bool power;
 	std::list<Command> queue;
 	std::map<unsigned, Turnout *> turnouts;
@@ -50,7 +50,9 @@ public:
 	const std::map<unsigned, Sensor *> &get_sensors() const { return sensors; }
 	unsigned   get_queue_length() const { return queue.size(); }
 	void       open(const std::string &);
-	Command    &command(const std::string &);
+	Command    &command(Cmd);
+	Command    &command(Cmd, unsigned char);
+	Command    &command(Cmd, const unsigned char *, unsigned);
 
 	void       add_turnout(Turnout &);
 	Turnout    &get_turnout(unsigned) const;
@@ -64,10 +66,10 @@ public:
 private:
 	void read_all(int, char *, int);
 	std::string read_reply(Cmd);
-	void status_done(Error, const std::string &);
-	void event_query_done(Error, const std::string &);
-	void turnout_event_done(Error, const std::string &);
-	void sensor_event_done(Error, const std::string &);
+	void status_done(const Reply &);
+	void event_query_done(const Reply &);
+	void turnout_event_done(const Reply &);
+	void sensor_event_done(const Reply &);
 };
 
 } // namespace Marklin
diff --git a/source/libmarklin/locomotive.cpp b/source/libmarklin/locomotive.cpp
index 87effa0..a42d8d7 100644
--- a/source/libmarklin/locomotive.cpp
+++ b/source/libmarklin/locomotive.cpp
@@ -11,6 +11,7 @@ Distributed under the GPL
 #include "constants.h"
 #include "control.h"
 #include "locomotive.h"
+#include "reply.h"
 
 using namespace std;
 using namespace Msp;
@@ -70,50 +71,51 @@ void Locomotive::set_function(unsigned func, bool state)
 
 void Locomotive::refresh_status()
 {
-	char cmd[3];
-	cmd[0]=CMD_LOK_STATUS;
-	cmd[1]=addr&0xFF;
-	cmd[2]=(addr>>8)&0xFF;
-	control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
+	unsigned char data[2];
+	data[0]=addr&0xFF;
+	data[1]=(addr>>8)&0xFF;
+	control.command(CMD_LOK_STATUS, data, 2).signal_done.connect(sigc::mem_fun(this, &Locomotive::status_reply));
 }
 
 void Locomotive::send_command(bool setf)
 {
-	char cmd[16];
-	cmd[0]=CMD_LOK;
-	cmd[1]=addr&0xFF;
-	cmd[2]=(addr>>8)&0xFF;
+	unsigned char data[4];
+	data[0]=addr&0xFF;
+	data[1]=(addr>>8)&0xFF;
 
 	if(speed==0)
-		cmd[3]=0;
+		data[2]=0;
 	else if(speed==1)
-		cmd[3]=2;
+		data[2]=2;
 	else
-		cmd[3]=(speed*19-18)/2;
+		data[2]=(speed*19-18)/2;
 	
-	cmd[4]=(reverse?0:0x20) | ((funcs&1)?0x10:0);
+	data[3]=(reverse ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
 
 	if(setf)
 	{
-		cmd[4]|=0x80;
+		data[3]|=0x80;
 		for(unsigned i=0; i<4; ++i)
 			if((funcs>>i)&2)
-				cmd[4]|=(1<<i);
+				data[3]|=(1<<i);
 	}
-	control.command(string(cmd, 5));
+	control.command(CMD_LOK, data, 4);
 }
 
-void Locomotive::status_reply(Error err, const string &reply)
+void Locomotive::status_reply(const Reply &reply)
 {
-	if(err==ERR_NO_ERROR)
+	if(reply.get_error()==ERR_NO_ERROR)
 	{
-		if(static_cast<unsigned char>(reply[0])<=1)
+		const unsigned char *data=reply.get_data();
+
+		if(data[0]<=1)
 			speed=0;
 		else
-			speed=static_cast<unsigned char>(reply[0])*2/19+1;
-		reverse=(reply[1]&0x20)?false:true;
-		funcs=(reply[1]&0xF)<<1;
-		if(reply[1]&0x10)
+			speed=data[0]*2/19+1;
+
+		reverse=(data[1]&0x20) ? false : true;
+		funcs=(data[1]&0xF)<<1;
+		if(data[1]&0x10)
 			funcs|=1;
 
 		for(unsigned i=0; i<5; ++i)
diff --git a/source/libmarklin/locomotive.h b/source/libmarklin/locomotive.h
index b8d58c2..e3cbf7d 100644
--- a/source/libmarklin/locomotive.h
+++ b/source/libmarklin/locomotive.h
@@ -17,6 +17,7 @@ namespace Marklin {
 
 class Control;
 class LocoType;
+class Reply;
 
 class Locomotive
 {
@@ -45,7 +46,7 @@ public:
 	void     refresh_status();
 private:
 	void     send_command(bool);
-	void     status_reply(Error, const std::string &);
+	void     status_reply(const Reply &);
 	bool     reverse_timeout();
 };
 
diff --git a/source/libmarklin/reply.cpp b/source/libmarklin/reply.cpp
new file mode 100644
index 0000000..ede23cc
--- /dev/null
+++ b/source/libmarklin/reply.cpp
@@ -0,0 +1,120 @@
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#include <cstring>
+#include <unistd.h>
+#include <msp/strings/formatter.h>
+#include "constants.h"
+#include "reply.h"
+
+using namespace std;
+using namespace Msp;
+
+namespace {
+
+unsigned read_all(int fd, char *buf, unsigned size)
+{
+	unsigned pos=0;
+	while(pos<size)
+		pos+=read(fd, buf+pos, size-pos);
+
+	return pos;
+}
+
+}
+
+namespace Marklin {
+
+Reply::Reply():
+	err(ERR_NO_ERROR),
+	len(0)
+{
+	memset(data, 0, 128);
+}
+
+Reply Reply::read(int fd, Cmd cmd)
+{
+	Reply result;
+
+	char *data=reinterpret_cast<char *>(result.data);
+
+	if(cmd==CMD_EVENT)
+	{
+		for(unsigned i=0; i<3; ++i)
+		{
+			result.len+=read_all(fd, data+i, 1);
+			if(!(result.data[i]&0x80))
+				break;
+		}
+	}
+	else if(cmd==CMD_EVENT_LOK)
+	{
+		for(unsigned i=0;; i+=5)
+		{
+			result.len+=read_all(fd, data+i, 1);
+
+			if(result.data[i]&0x80)
+				break;
+
+			result.len+=read_all(fd, data+i+1, 4);
+		}
+	}
+	else if(cmd==CMD_EVENT_TURNOUT)
+	{
+		result.len+=read_all(fd, data, 1);
+		result.len+=read_all(fd, data+1, result.data[0]*2);
+	}
+	else if(cmd==CMD_EVENT_SENSOR)
+	{
+		for(unsigned i=0;; i+=3)
+		{
+			result.len+=read_all(fd, data+i, 1);
+
+			if(result.data[i]==0)
+				break;
+
+			result.len+=read_all(fd, data+i+1, 2);
+		}
+	}
+	else
+	{
+		bool expect_errcode=(cmd!=CMD_STATUS);
+
+		unsigned expected_bytes=0;
+		if(cmd==CMD_STATUS || cmd==CMD_FUNC_STATUS || cmd==CMD_TURNOUT_STATUS)
+			expected_bytes=1;
+		if(cmd==CMD_SENSOR_STATUS || cmd==CMD_TURNOUT_GROUP_STATUS)
+			expected_bytes=2;
+		if(cmd==CMD_LOK_STATUS)
+			expected_bytes=3;
+		if(cmd==CMD_LOK_CONFIG)
+			expected_bytes=4;
+
+		if(expect_errcode)
+		{
+			char c;
+			read_all(fd, &c, 1);
+			result.err=static_cast<Error>(c);
+		}
+
+		if(result.err==ERR_NO_ERROR)
+			result.len+=read_all(fd, data, expected_bytes);
+	}
+
+	return result;
+}
+
+ostream &operator<<(ostream &out, const Reply &reply)
+{
+	out<<reply.err;
+	for(unsigned i=0; i<reply.len; ++i)
+		out<<format(" %02X", static_cast<int>(reply.data[i]));
+
+	return out;
+}
+
+} // namespace Marklin
diff --git a/source/libmarklin/reply.h b/source/libmarklin/reply.h
new file mode 100644
index 0000000..d075b9b
--- /dev/null
+++ b/source/libmarklin/reply.h
@@ -0,0 +1,36 @@
+/* $Id$
+
+This file is part of the MSP Märklin suite
+Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
+Distributed under the GPL
+*/
+
+#ifndef LIBMARKLIN_REPLY_H_
+#define LIBMARKLIN_REPLY_H_
+
+#include <ostream>
+
+namespace Marklin {
+
+class Reply
+{
+private:
+	Error err;
+	unsigned char data[128];
+	unsigned len;
+
+	Reply();
+public:
+	Error get_error() const { return err; }
+	const unsigned char *get_data() const { return data; }
+
+	static Reply read(int, Cmd);
+
+	friend std::ostream &operator<<(std::ostream &, const Reply &);
+};
+
+
+
+} // namespace Marklin
+
+#endif
diff --git a/source/libmarklin/trafficmanager.cpp b/source/libmarklin/trafficmanager.cpp
index 29823ff..9666f12 100644
--- a/source/libmarklin/trafficmanager.cpp
+++ b/source/libmarklin/trafficmanager.cpp
@@ -5,6 +5,7 @@ Copyright © 2006-2008 Mikkosoft Productions, Mikko Rasa
 Distributed under the GPL
 */
 
+#include <algorithm>
 #include <msp/time/utils.h>
 #include "control.h"
 #include "layout.h"
@@ -39,12 +40,9 @@ TrafficManager::TrafficManager(Control &c, Layout &l):
 	}
 
 	for(list<Block *>::iterator i=blocks.begin(); i!=blocks.end(); ++i)
-	{
 		for(list<Block *>::iterator j=i; j!=blocks.end(); ++j)
 			if(j!=i)
 				(*i)->check_link(**j);
-		(*i)->print_debug();
-	}
 }
 
 TrafficManager::~TrafficManager()
diff --git a/source/libmarklin/turnout.cpp b/source/libmarklin/turnout.cpp
index ace7976..30793fc 100644
--- a/source/libmarklin/turnout.cpp
+++ b/source/libmarklin/turnout.cpp
@@ -10,6 +10,7 @@ Distributed under the GPL
 #include <msp/time/units.h>
 #include "command.h"
 #include "control.h"
+#include "reply.h"
 #include "turnout.h"
 
 using namespace std;
@@ -26,11 +27,10 @@ Turnout::Turnout(Control &c, unsigned a):
 
 	control.signal_turnout_event.connect(sigc::mem_fun(this, &Turnout::turnout_event));
 
-	char cmd[3];
-	cmd[0]=CMD_TURNOUT_STATUS;
-	cmd[1]=addr&0xFF;
-	cmd[2]=(addr>>8)&0xFF;
-	control.command(string(cmd, 3)).signal_done.connect(sigc::mem_fun(this, &Turnout::status_reply));
+	unsigned char data[2];
+	data[0]=addr&0xFF;
+	data[1]=(addr>>8)&0xFF;
+	control.command(CMD_TURNOUT_STATUS, data, 2).signal_done.connect(sigc::mem_fun(this, &Turnout::status_reply));
 }
 
 void Turnout::set_route(unsigned r)
@@ -45,22 +45,17 @@ void Turnout::set_route(unsigned r)
 
 void Turnout::command(bool on)
 {
-	char cmd[3];
-	cmd[0]=CMD_TURNOUT;
-	cmd[1]=addr&0xFF;
-	cmd[2]=((addr>>8)&0x7);
-	if(on)
-		cmd[2]|=0x40;
-	if(route==0)
-		cmd[2]|=0x80;
-	control.command(string(cmd, 3));
+	unsigned char data[2];
+	data[0]=addr&0xFF;
+	data[1]=((addr>>8)&0x7) | (on ? 0x40 : 0) | (route==0 ? 0x80 : 0);
+	control.command(CMD_TURNOUT, data, 2);
 }
 
-void Turnout::status_reply(Error err, const string &reply)
+void Turnout::status_reply(const Reply &reply)
 {
-	if(err==ERR_NO_ERROR)
+	if(reply.get_error()==ERR_NO_ERROR)
 	{
-		route=(reply[0]&4)?0:1;
+		route=(reply.get_data()[0]&0x04) ? 0 : 1;
 		signal_route_changed.emit(route);
 	}
 }
diff --git a/source/libmarklin/turnout.h b/source/libmarklin/turnout.h
index 0928ead..82b50d5 100644
--- a/source/libmarklin/turnout.h
+++ b/source/libmarklin/turnout.h
@@ -17,6 +17,7 @@ Distributed under the GPL
 namespace Marklin {
 
 class Control;
+class Reply;
 
 class Turnout
 {
@@ -35,7 +36,7 @@ public:
 	unsigned get_route() const   { return route; }
 private:
 	void command(bool);
-	void status_reply(Error, const std::string &);
+	void status_reply(const Reply &);
 	bool switch_timeout();
 	void turnout_event(unsigned, bool);
 };