From 6887e5cf8bcb6578309e09136172f445cf1f619b Mon Sep 17 00:00:00 2001
From: Mikko Rasa <tdb@tdb.fi>
Date: Thu, 26 Feb 2015 19:29:37 +0200
Subject: [PATCH] Read MFX locomotive name

---
 source/libr2c2/arducontrol.cpp | 121 +++++++++++++++++++++++++++++----
 source/libr2c2/arducontrol.h   |  11 ++-
 2 files changed, 119 insertions(+), 13 deletions(-)

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 0edb3dd..3d49fab 100644
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -958,22 +958,58 @@ ArduControl::MfxSearchTask::MfxSearchTask(ArduControl &c):
 	next_address(1),
 	size(0),
 	bits(0),
-	misses(0)
+	misses(0),
+	pending_info(0),
+	read_array(0),
+	read_offset(0),
+	read_length(0),
+	block_size(0)
 { }
 
 bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 {
+	if(read_length>0)
+	{
+		cmd.command[0] = MFX_READ;
+		cmd.command[1] = pending_info->address>>8;
+		cmd.command[2] = pending_info->address;
+		unsigned index = read_array*0x40+read_offset;
+		cmd.command[3] = index>>8;
+		cmd.command[4] = index;
+		unsigned length = (read_length>=4 ? 4 : read_length>=2 ? 2 : 1);
+		cmd.command[5] = length;
+		cmd.length = 6;
+
+		sleep(100*Time::msec);
+
+		return true;
+	}
+	else if(pending_info)
+	{
+		queue.push(*pending_info);
+		Tag tag;
+		tag.type = Tag::GENERAL;
+		tag.command = NEW_LOCO;
+		tag.id = pending_info->id;
+		control.completed_commands.push(tag);
+
+		if(control.debug>=1)
+			IO::print("Completed processing locomotive %s at address %d\n", pending_info->name, pending_info->address);
+
+		delete pending_info;
+		pending_info = 0;
+	}
+
 	if(size>32)
 	{
 		if(control.debug>=1)
 			IO::print("Assigning MFX address %d to decoder %08X\n", next_address, bits);
 
-		MfxInfo info;
-		info.protocol = "MFX";
-		info.address = next_address;
-		info.name = format("%08X", bits);
-		info.id = bits;
-		queue.push(info);
+		pending_info = new MfxInfo;
+		pending_info->protocol = "MFX";
+		pending_info->address = next_address;
+		pending_info->name = format("%08X", bits);
+		pending_info->id = bits;
 
 		cmd.command[0] = MFX_ASSIGN_ADDRESS;
 		cmd.command[1] = next_address>>8;
@@ -982,14 +1018,15 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 			cmd.command[3+i] = bits>>(24-i*8);
 		cmd.length = 7;
 
-		cmd.tag.type = Tag::GENERAL;
-		cmd.tag.command = NEW_LOCO;
-		cmd.tag.id = bits;
-
 		size = 0;
 		bits = 0;
+		misses = 0;
 		++next_address;
 
+		read_array = 0;
+		read_offset = 0;
+		read_length = 6;
+
 		return true;
 	}
 
@@ -999,7 +1036,7 @@ bool ArduControl::MfxSearchTask::get_work(PendingCommand &cmd)
 	cmd.command[5] = size;
 	cmd.length = 6;
 
-	sleep(200*Time::msec);
+	sleep(100*Time::msec);
 
 	if(control.debug>=1)
 		IO::print("Search %08X/%d\n", bits, size);
@@ -1030,6 +1067,66 @@ void ArduControl::MfxSearchTask::process_reply(const char *reply, unsigned lengt
 			misses = 0;
 		}
 	}
+	else if(type==MFX_READ_FEEDBACK && length>=3)
+	{
+		if(reply[1])
+		{
+			misses = 0;
+
+			for(unsigned i=2; i<length; ++i)
+				read_data[read_offset+i-2] = reply[i];
+			read_offset += length-2;
+			read_length -= length-2;
+
+			if(!read_length)
+			{
+				if(read_array==0)
+					block_size = static_cast<unsigned char>(read_data[4])*static_cast<unsigned char>(read_data[5]);
+
+				bool array_handled = false;
+				if(read_data[0]==0x18)
+				{
+					for(unsigned i=1; i<read_offset; ++i)
+						if(!read_data[i])
+						{
+							pending_info->name = string(read_data+1, i-1);
+							array_handled = true;
+							break;
+						}
+
+					if(!array_handled)
+						read_length = 4;
+				}
+				else
+					array_handled = true;
+
+				if(array_handled && control.debug>=1)
+				{
+					IO::print("MFX CA %03X:", read_array);
+					for(unsigned i=0; i<read_offset; ++i)
+						IO::print(" %02X", static_cast<unsigned char>(read_data[i]));
+					IO::print("\n");
+				}
+
+				if(array_handled && read_array<block_size)
+				{
+					++read_array;
+					read_offset = 0;
+					read_length = 1;
+				}
+			}
+		}
+		else
+		{
+			++misses;
+			if(misses>=10)
+			{
+				if(control.debug>=1)
+					IO::print("Failed to read MFX configuration from %d\n", pending_info->address);
+				read_length = 0;
+			}
+		}
+	}
 }
 
 void ArduControl::MfxSearchTask::set_next_address(unsigned a)
diff --git a/source/libr2c2/arducontrol.h b/source/libr2c2/arducontrol.h
index d68a149..b75d415 100644
--- a/source/libr2c2/arducontrol.h
+++ b/source/libr2c2/arducontrol.h
@@ -45,6 +45,7 @@ private:
 		MFX_SEARCH = 0x23,
 		MFX_ASSIGN_ADDRESS = 0x24,
 		MFX_PING = 0x25,
+		MFX_READ = 0x26,
 		MFX_SPEED = 0x28,
 		MFX_SPEED_FUNCS8 = 0x29,
 		MFX_SPEED_FUNCS16 = 0x2A,
@@ -63,7 +64,8 @@ private:
 		POWER_STATE = 0xC2,
 		S88_DATA = 0xD0,
 		MFX_SEARCH_FEEDBACK = 0xD1,
-		MFX_PING_FEEDBACK = 0xD2
+		MFX_PING_FEEDBACK = 0xD2,
+		MFX_READ_FEEDBACK = 0xD3
 	};
 
 	struct Tag
@@ -331,6 +333,13 @@ private:
 		unsigned misses;
 		Queue<MfxInfo> queue;
 
+		MfxInfo *pending_info;
+		unsigned read_array;
+		unsigned read_offset;
+		unsigned read_length;
+		char read_data[0x40];
+		unsigned block_size;
+
 	public:
 		MfxSearchTask(ArduControl &);
 
-- 
2.43.0