From 3a4d86cdac7ada70c31bf205e83a9d3a9521a93f Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Sat, 2 Oct 2010 11:48:08 +0000
Subject: [PATCH] Add error reporting to the Intellibox driver

---
 source/libmarklin/intellibox.cpp | 92 ++++++++++++++++++++++++++++----
 source/libmarklin/intellibox.h   |  2 +
 2 files changed, 84 insertions(+), 10 deletions(-)

diff --git a/source/libmarklin/intellibox.cpp b/source/libmarklin/intellibox.cpp
index 6efe875..416d021 100644
--- a/source/libmarklin/intellibox.cpp
+++ b/source/libmarklin/intellibox.cpp
@@ -416,8 +416,8 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 	}
 	else if(cmd==CMD_LOK)
 	{
-		unsigned char err;
-		read_all(&err, 1);
+		Error err;
+		read_status(&err);
 
 		if(err==ERR_NO_ERROR)
 		{
@@ -425,11 +425,13 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 			Locomotive &loco = locos[addr];
 			signal_loco_speed.emit(addr, loco.speed, loco.reverse);
 		}
+		else
+			error(cmd, err);
 	}
 	else if(cmd==CMD_TURNOUT)
 	{
-		unsigned char err;
-		read_all(&err, 1);
+		Error err;
+		read_status(&err);
 
 		unsigned addr = queue.front().addr;
 		Turnout &turnout = turnouts[addr];
@@ -446,12 +448,15 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 		else if(err==ERR_NO_I2C_SPACE)
 			queue.push_back(queue.front());
 		else
+		{
 			turnout.pending = turnout.state;
+			error(cmd, err);
+		}
 	}
 	else if(cmd==CMD_TURNOUT_STATUS)
 	{
-		unsigned char err;
-		read_all(&err, 1);
+		Error err;
+		read_status(&err);
 
 		if(err==ERR_NO_ERROR)
 		{
@@ -469,11 +474,13 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 				signal_turnout.emit(addr, turnout.state);
 			}
 		}
+		else
+			error(cmd, err);
 	}
 	else if(cmd==CMD_LOK_STATUS)
 	{
-		unsigned char err;
-		read_all(&err, 1);
+		Error err;
+		read_status(&err);
 
 		if(err==ERR_NO_ERROR)
 		{
@@ -504,6 +511,8 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 						signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
 			}
 		}
+		else
+			error(cmd, err);
 	}
 	else
 	{
@@ -515,14 +524,16 @@ void Intellibox::process_reply(const Time::TimeStamp &t)
 		if(cmd==CMD_LOK_CONFIG)
 			expected_bytes = 4;
 
-		unsigned char err;
-		read_all(&err, 1);
+		Error err;
+		read_status(&err);
 
 		if(err==ERR_NO_ERROR)
 		{
 			unsigned char data[8];
 			read_all(data, expected_bytes);
 		}
+		else
+			error(cmd, err);
 	}
 }
 
@@ -535,6 +546,67 @@ unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
 	return pos;
 }
 
+unsigned Intellibox::read_status(Error *err)
+{
+	unsigned char c;
+	unsigned ret = read_all(&c, 1);
+	*err = static_cast<Error>(c);
+	return ret;
+}
+
+void Intellibox::error(Command cmd, Error err)
+{
+	const char *cmd_str = 0;
+	switch(cmd)
+	{
+	case CMD_LOK: cmd_str = "CMD_LOK"; break;
+	case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
+	case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
+	case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
+	case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
+	case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
+	case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
+	case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
+	case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
+	case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
+	case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
+	case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
+	case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
+	case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
+	case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
+	case CMD_NOP: cmd_str = "CMD_NOP"; break;
+	case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
+	case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
+	case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
+	case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
+	default: cmd_str = "(unknown command)";
+	}
+
+	const char *err_str = 0;
+	switch(err)
+	{
+	case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
+	case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
+	case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
+	case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
+	case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
+	case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
+	case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
+	case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
+	case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
+	case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
+	case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
+	case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
+	case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
+	case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
+	case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
+	case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
+	default: cmd_str = "(unknown error)";
+	}
+
+	IO::print("Error: %s: %s\n", cmd_str, err_str);
+}
+
 
 Intellibox::Locomotive::Locomotive():
 	speed(0),
diff --git a/source/libmarklin/intellibox.h b/source/libmarklin/intellibox.h
index 483d0ef..c5c7e7a 100644
--- a/source/libmarklin/intellibox.h
+++ b/source/libmarklin/intellibox.h
@@ -139,6 +139,8 @@ private:
 	void turnout_command(unsigned, bool, bool);
 	void process_reply(const Msp::Time::TimeStamp &);
 	unsigned read_all(unsigned char *, unsigned);
+	unsigned read_status(Error *);
+	void error(Command, Error);
 };
 
 } // namespace Marklin
-- 
2.45.2