Read MFX locomotive name

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index 0edb3ddaf0c1a44e17164d23f8efcabd6ebb82f7..3d49fabf4784c6b02ac278d67d4086b5f6d0e918 100644 (file)
--- 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 d68a149a9275c957b5207e906d4f9586e3d63e9b..b75d415b5fb5a1e73b1b834a1d58989cb6cb426a 100644 (file)
--- 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 &);