Add some more range checks in ArduControl and improve existing ones

[r2c2.git] / source / libr2c2 / arducontrol.cpp

diff --git a/source/libr2c2/arducontrol.cpp b/source/libr2c2/arducontrol.cpp
index a1d238103004d1e95cba46ffd65fb57ef7b6caf7..e940a2aed14527ed676a6cbb4af1786762411420 100644 (file)
--- a/source/libr2c2/arducontrol.cpp
+++ b/source/libr2c2/arducontrol.cpp
@@ -9,6 +9,11 @@ using namespace Msp;
 
 namespace R2C2 {
 
+ArduControl::ProtocolInfo ArduControl::protocol_info[2] =
+{
+       { 79, 14, 4 }  // MM
+};
+
 ArduControl::ArduControl(const string &dev):
        serial(dev),
        debug(true),
@@ -57,45 +62,45 @@ ArduControl::Protocol ArduControl::map_protocol(const string &proto_name)
 {
        if(proto_name=="MM")
                return MM;
+       else if(proto_name=="MFX")
+               return MFX;
        else
                throw invalid_argument("ArduControl::map_protocol");
 }
 
 unsigned ArduControl::get_protocol_speed_steps(const string &proto_name) const
 {
-       Protocol proto = map_protocol(proto_name);
-       if(proto==MM)
-               return 14;
-       else
-               return 0;
+       return protocol_info[map_protocol(proto_name)].max_speed;
 }
 
-void ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
+unsigned ArduControl::add_loco(unsigned addr, const string &proto_name, const VehicleType &)
 {
        if(!addr)
                throw invalid_argument("ArduControl::add_loco");
 
        Protocol proto = map_protocol(proto_name);
+       if(addr>protocol_info[proto].max_address)
+               throw invalid_argument("ArduControl::add_loco");
 
-       if(proto==MM)
-       {
-               if(addr>=80)
-                       throw invalid_argument("ArduControl::add_loco");
-       }
+       Locomotive loco(proto, addr);
+       insert_unique(locomotives, loco.id, loco);
 
-       insert_unique(locomotives, addr, Locomotive(proto, addr));
+       return loco.id;
 }
 
-void ArduControl::remove_loco(unsigned addr)
+void ArduControl::remove_loco(unsigned id)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
        remove_loco_from_refresh(loco);
-       locomotives.erase(addr);
+       locomotives.erase(id);
 }
 
-void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
+void ArduControl::set_loco_speed(unsigned id, unsigned speed)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
+       if(speed>protocol_info[loco.proto].max_speed)
+               throw invalid_argument("ArduControl::set_loco_speed");
+
        if(loco.speed.set(speed))
        {
                QueuedCommand cmd(loco, Locomotive::SPEED);
@@ -105,9 +110,9 @@ void ArduControl::set_loco_speed(unsigned addr, unsigned speed)
        }
 }
 
-void ArduControl::set_loco_reverse(unsigned addr, bool rev)
+void ArduControl::set_loco_reverse(unsigned id, bool rev)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
        if(loco.reverse.set(rev))
        {
                QueuedCommand cmd(loco, Locomotive::REVERSE);
@@ -117,9 +122,12 @@ void ArduControl::set_loco_reverse(unsigned addr, bool rev)
        }
 }
 
-void ArduControl::set_loco_function(unsigned addr, unsigned func, bool state)
+void ArduControl::set_loco_function(unsigned id, unsigned func, bool state)
 {
-       Locomotive &loco = get_item(locomotives, addr);
+       Locomotive &loco = get_item(locomotives, id);
+       if(func>protocol_info[loco.proto].max_func)
+               throw invalid_argument("ArduControl::set_loco_function");
+
        unsigned mask = 1<<func;
        if(loco.funcs.set((loco.funcs&~mask)|(mask*state)))
        {
@@ -190,12 +198,12 @@ void ArduControl::advance_next_refresh()
        }
 }
 
-void ArduControl::add_turnout(unsigned addr, const TrackType &type)
+unsigned ArduControl::add_turnout(unsigned addr, const TrackType &type)
 {
        if(!addr || !type.is_turnout())
                throw invalid_argument("ArduControl::add_turnout");
 
-       add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
+       return add_accessory(Accessory::TURNOUT, addr, type.get_state_bits());
 }
 
 void ArduControl::remove_turnout(unsigned addr)
@@ -213,9 +221,9 @@ unsigned ArduControl::get_turnout(unsigned addr) const
        return get_accessory(Accessory::TURNOUT, addr);
 }
 
-void ArduControl::add_signal(unsigned addr, const SignalType &)
+unsigned ArduControl::add_signal(unsigned addr, const SignalType &)
 {
-       add_accessory(Accessory::SIGNAL, addr, 1);
+       return add_accessory(Accessory::SIGNAL, addr, 1);
 }
 
 void ArduControl::remove_signal(unsigned addr)
@@ -233,7 +241,7 @@ unsigned ArduControl::get_signal(unsigned addr) const
        return get_accessory(Accessory::SIGNAL, addr);
 }
 
-void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
+unsigned ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bits)
 {
        AccessoryMap::iterator i = accessories.lower_bound(addr);
        AccessoryMap::iterator j = accessories.upper_bound(addr+bits-1);
@@ -247,6 +255,7 @@ void ArduControl::add_accessory(Accessory::Kind kind, unsigned addr, unsigned bi
        }
 
        insert_unique(accessories, addr, Accessory(kind, addr, bits));
+       return addr;
 }
 
 void ArduControl::remove_accessory(Accessory::Kind kind, unsigned addr)
@@ -278,7 +287,7 @@ unsigned ArduControl::get_accessory(Accessory::Kind kind, unsigned addr) const
        return acc.state;
 }
 
-void ArduControl::add_sensor(unsigned addr)
+unsigned ArduControl::add_sensor(unsigned addr)
 {
        if(!addr)
                throw invalid_argument("ArduControl::add_sensor");
@@ -287,6 +296,8 @@ void ArduControl::add_sensor(unsigned addr)
        unsigned octet_index = (addr-1)/8;
        if(octet_index>=n_s88_octets)
                n_s88_octets = octet_index+1;
+
+       return addr;
 }
 
 void ArduControl::remove_sensor(unsigned addr)
@@ -314,7 +325,7 @@ void ArduControl::tick()
                }
                else if(tag.type==Tag::LOCOMOTIVE)
                {
-                       LocomotiveMap::iterator i = locomotives.find(tag.address);
+                       LocomotiveMap::iterator i = locomotives.find(tag.id);
                        if(i==locomotives.end())
                                continue;
 
@@ -322,12 +333,12 @@ void ArduControl::tick()
                        if(tag.command==Locomotive::SPEED)
                        {
                                if(loco.speed.commit(tag.serial))
-                                       signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+                                       signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
                        }
                        else if(tag.command==Locomotive::REVERSE)
                        {
                                if(loco.reverse.commit(tag.serial))
-                                       signal_loco_speed.emit(loco.address, loco.speed, loco.reverse);
+                                       signal_loco_speed.emit(loco.id, loco.speed, loco.reverse);
                        }
                        else if(tag.command==Locomotive::FUNCTIONS)
                        {
@@ -337,13 +348,13 @@ void ArduControl::tick()
                                        unsigned changed = old^loco.funcs;
                                        for(unsigned j=0; changed>>j; ++j)
                                                if((changed>>j)&1)
-                                                       signal_loco_function.emit(loco.address, j, (loco.funcs>>j)&1);
+                                                       signal_loco_function.emit(loco.id, j, (loco.funcs>>j)&1);
                                }
                        }
                }
                else if(tag.type==Tag::ACCESSORY)
                {
-                       AccessoryMap::iterator i = accessories.find(tag.address);
+                       AccessoryMap::iterator i = accessories.find(tag.id);
                        if(i==accessories.end())
                                continue;
 
@@ -370,7 +381,7 @@ void ArduControl::tick()
                }
                else if(tag.type==Tag::SENSOR)
                {
-                       SensorMap::iterator i = sensors.find(tag.address);
+                       SensorMap::iterator i = sensors.find(tag.id);
                        if(i==sensors.end())
                                continue;
 
@@ -456,11 +467,12 @@ ArduControl::Tag::Tag():
        type(NONE),
        command(0),
        serial(0),
-       address(0)
+       id(0)
 { }
 
 
 ArduControl::Locomotive::Locomotive(Protocol p, unsigned a):
+       id((p<<16)|a),
        proto(p),
        address(a),
        speed(0),
@@ -515,7 +527,7 @@ unsigned ArduControl::Accessory::create_state_command(unsigned b, bool c, char *
                throw invalid_argument("Accessory::create_state_command");
 
        unsigned a = (address+b+3)*2;
-       if((state.pending>>b)&1)
+       if(!((state.pending>>b)&1))
                ++a;
        buffer[0] = MOTOROLA_SOLENOID;
        buffer[1] = a>>3;
@@ -667,7 +679,7 @@ void ArduControl::ControlThread::main()
                                                stag.type = Tag::SENSOR;
                                                stag.command = Sensor::STATE;
                                                stag.serial = i->second.state.serial;
-                                               stag.address = i->first;
+                                               stag.id = i->first;
                                                control.push_completed_tag(stag);
                                        }
 
@@ -697,7 +709,7 @@ ArduControl::QueuedCommand::QueuedCommand(Locomotive &loco, Locomotive::Command
 {
        tag.type = Tag::LOCOMOTIVE;
        tag.command = cmd;
-       tag.address = loco.address;
+       tag.id = loco.id;
        if(cmd==Locomotive::SPEED)
        {
                tag.serial = loco.speed.serial;
@@ -721,7 +733,7 @@ ArduControl::QueuedCommand::QueuedCommand(Accessory &acc, Accessory::Command cmd
 {
        tag.type = Tag::ACCESSORY;
        tag.command = cmd;
-       tag.address = acc.address;
+       tag.id = acc.address;
        if(cmd==Accessory::ACTIVATE || cmd==Accessory::DEACTIVATE)
        {
                tag.serial = acc.state.serial;