X-Git-Url: http://git.tdb.fi/?p=r2c2.git;a=blobdiff_plain;f=source%2Flibr2c2%2Farducontrol.cpp;h=3d49fabf4784c6b02ac278d67d4086b5f6d0e918;hp=0edb3ddaf0c1a44e17164d23f8efcabd6ebb82f7;hb=6887e5cf8bcb6578309e09136172f445cf1f619b;hpb=8c7fc0b79ce88e0295e0e6ea52cb7eb753158d8a

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)