]> git.tdb.fi Git - r2c2.git/blob - source/libmarklin/intellibox.cpp
Fix a segfault in removing vehicles when creating a new train
[r2c2.git] / source / libmarklin / intellibox.cpp
1 /* $Id$
2
3 This file is part of the MSP Märklin suite
4 Copyright © 2010  Mikkosoft Productions, Mikko Rasa
5 Distributed under the GPL
6 */
7
8 #include <fcntl.h>
9 #include <termios.h>
10 #include <sys/poll.h>
11 #include <msp/io/print.h>
12 #include <msp/time/units.h>
13 #include <msp/time/utils.h>
14 #include "intellibox.h"
15
16 using namespace std;
17 using namespace Msp;
18
19 namespace Marklin {
20
21 Intellibox::Intellibox(const string &dev):
22         power(false),
23         halted(false),
24         update_sensors(false),
25         command_sent(false)
26 {
27         serial_fd = ::open(dev.c_str(), O_RDWR);
28         if(serial_fd<0)
29                 throw Exception("Couldn't open serial port\n");
30
31         static unsigned baud[]=
32         {
33                  2400, B2400,
34                  4800, B4800,
35                  9600, B9600,
36                 19200, B19200,
37                 0
38         };
39
40         termios attr;
41         tcgetattr(serial_fd, &attr);
42         cfmakeraw(&attr);
43         attr.c_cflag |= CSTOPB;
44
45         bool ok = false;
46         bool p50 = false;
47         for(unsigned i=0; baud[i]; i+=2)
48         {
49                 cfsetospeed(&attr, baud[i+1]);
50                 tcsetattr(serial_fd, TCSADRAIN, &attr);
51
52                 write(serial_fd, "\xC4", 1);
53
54                 pollfd pfd = { serial_fd, POLLIN, 0 };
55                 if(poll(&pfd, 1, 500)>0)
56                 {
57                         IO::print("IB detected at %d bits/s\n", baud[i]);
58                         char buf[2];
59                         p50 = (read(serial_fd, buf, 2)==2);
60                         ok = true;
61                         break;
62                 }
63         }
64
65         if(!ok)
66                 throw Exception("IB not detected");
67
68         if(p50)
69                 write(serial_fd, "xZzA1\r", 6);
70
71         command(CMD_STATUS);
72 }
73
74 void Intellibox::set_power(bool p)
75 {
76         power = p;
77         if(power)
78                 command(CMD_POWER_ON);
79         else
80                 command(CMD_POWER_OFF);
81         signal_power.emit(power);
82 }
83
84 void Intellibox::halt(bool h)
85 {
86         halted = h;
87         if(halted)
88         {
89                 for(map<unsigned, Locomotive>::iterator i=locos.begin(); i!=locos.end(); ++i)
90                         if(i->second.speed)
91                                 set_loco_speed(i->first, 0);
92         }
93
94         signal_halt.emit(halted);
95 }
96
97 const char *Intellibox::enumerate_protocols(unsigned i) const
98 {
99         if(i==0)
100                 return "MM";
101         return 0;
102 }
103
104 unsigned Intellibox::get_protocol_speed_steps(const string &proto) const
105 {
106         if(proto=="MM")
107                 return 14;
108         else
109                 throw InvalidParameterValue("Unknown protocol");
110 }
111
112 void Intellibox::add_loco(unsigned addr, const string &proto)
113 {
114         if(!locos.count(addr))
115         {
116                 locos[addr].protocol = proto;
117
118                 unsigned char data[2];
119                 data[0] = addr&0xFF;
120                 data[1] = (addr>>8)&0xFF;
121                 command(CMD_LOK_STATUS, addr, data, 2);
122         }
123 }
124
125 void Intellibox::set_loco_speed(unsigned addr, unsigned speed)
126 {
127         Locomotive &loco = locos[addr];
128         if(speed==loco.speed)
129                 return;
130         if(speed && halted)
131                 return;
132
133         loco.speed = speed;
134         loco_command(addr, speed, loco.reverse, loco.funcs|0x100);
135 }
136
137 void Intellibox::set_loco_reverse(unsigned addr, bool rev)
138 {
139         Locomotive &loco = locos[addr];
140         if(rev==loco.reverse)
141                 return;
142
143         loco.reverse = rev;
144         loco_command(addr, loco.speed, rev, loco.funcs|0x100);
145 }
146
147 void Intellibox::set_loco_function(unsigned addr, unsigned func, bool state)
148 {
149         Locomotive &loco = locos[addr];
150         if(state)
151                 loco.funcs |= 1<<func;
152         else
153                 loco.funcs &= ~(1<<func);
154         loco_command(addr, loco.speed, loco.reverse, loco.funcs);
155         signal_loco_function.emit(addr, func, state);
156 }
157
158 void Intellibox::add_turnout(unsigned addr)
159 {
160         if(!turnouts.count(addr))
161         {
162                 turnouts[addr];
163
164                 unsigned char data[2];
165                 data[0] = addr&0xFF;
166                 data[1] = (addr>>8)&0xFF;
167                 command(CMD_TURNOUT_STATUS, addr, data, 2);
168         }
169 }
170
171 void Intellibox::set_turnout(unsigned addr, bool state)
172 {
173         Turnout &turnout = turnouts[addr];
174         if(state==turnout.state || state==turnout.pending)
175                 return;
176
177         turnout.pending = state;
178         turnout.active = true;
179         turnout.off_timeout = Time::TimeStamp();
180
181         turnout_command(addr, state, true);
182 }
183
184 bool Intellibox::get_turnout(unsigned addr) const
185 {
186         map<unsigned, Turnout>::const_iterator i = turnouts.find(addr);
187         if(i!=turnouts.end())
188                 return i->second.state;
189         return false;
190 }
191
192 void Intellibox::add_sensor(unsigned addr)
193 {
194         if(!sensors.count(addr))
195         {
196                 sensors[addr];
197                 update_sensors = true;
198         }
199 }
200
201 bool Intellibox::get_sensor(unsigned addr) const
202 {
203         map<unsigned, Sensor>::const_iterator i = sensors.find(addr);
204         if(i!=sensors.end())
205                 return i->second.state;
206         return false;
207 }
208
209 void Intellibox::tick()
210 {
211         const Time::TimeStamp t = Time::now();
212
213         if(t>next_event_query)
214         {
215                 next_event_query = t+200*Time::msec;
216                 command(CMD_EVENT);
217         }
218
219         for(map<unsigned, Turnout>::iterator i=turnouts.begin(); i!=turnouts.end(); ++i)
220                 if(i->second.active && i->second.off_timeout && t>i->second.off_timeout)
221                 {
222                         i->second.active = false;
223                         i->second.off_timeout = Time::TimeStamp();
224                         turnout_command(i->first, i->second.state, false);
225                 }
226
227         for(map<unsigned, Sensor>::iterator i=sensors.begin(); i!=sensors.end(); ++i)
228                 if(i->second.off_timeout && t>i->second.off_timeout)
229                 {
230                         i->second.state = false;
231                         i->second.off_timeout = Time::TimeStamp();
232                         signal_sensor.emit(i->first, false);
233                 }
234
235         if(update_sensors)
236         {
237                 unsigned max_addr = (--sensors.end())->first;
238                 unsigned char data[2];
239                 data[0] = 0;
240                 data[1] = (max_addr+7)/8;
241                 command(CMD_SENSOR_PARAM_SET, data, 2);
242                 command(CMD_SENSOR_REPORT);
243                 update_sensors = false;
244         }
245
246         if(!queue.empty() && command_sent)
247         {
248                 pollfd pfd = { serial_fd, POLLIN, 0 };
249                 if(poll(&pfd, 1, 0)>0)
250                 {
251                         process_reply(t);
252                         queue.erase(queue.begin());
253                         command_sent = false;
254                 }
255                 else
256                         return;
257         }
258
259         if(!queue.empty())
260         {
261                 const CommandSlot &slot = queue.front();
262                 write(serial_fd, slot.data, slot.length);
263                 command_sent = true;
264         }
265 }
266
267 void Intellibox::flush()
268 {
269         Time::TimeStamp t = Time::now();
270         for(list<CommandSlot>::iterator i=queue.begin(); i!=queue.end(); ++i)
271         {
272                 write(serial_fd, i->data, i->length);
273                 pollfd pfd = { serial_fd, POLLIN, 0 };
274                 bool first = true;
275                 while(poll(&pfd, 1, (first ? -1 : 0))>0)
276                 {
277                         char data[16];
278                         read(serial_fd, data, 16);
279                         first = false;
280                 }
281         }
282
283         queue.clear();
284         command_sent = false;
285 }
286
287 void Intellibox::command(Command cmd)
288 {
289         command(cmd, 0, 0);
290 }
291
292 void Intellibox::command(Command cmd, const unsigned char *data, unsigned len)
293 {
294         command(cmd, 0, data, len);
295 }
296
297 void Intellibox::command(Command cmd, unsigned addr, const unsigned char *data, unsigned len)
298 {
299         CommandSlot slot;
300         slot.cmd = cmd;
301         slot.addr = addr;
302         slot.data[0] = cmd;
303         copy(data, data+len, slot.data+1);
304         slot.length = 1+len;
305         queue.push_back(slot);
306 }
307
308 void Intellibox::loco_command(unsigned addr, unsigned speed, bool rev, unsigned funcs)
309 {
310         unsigned char data[4];
311         data[0] = addr&0xFF;
312         data[1] = (addr>>8)&0xFF;
313
314         if(speed==0)
315                 data[2] = 0;
316         else if(speed==1)
317                 data[2] = 2;
318         else
319                 data[2] = (speed*19-18)/2;
320         
321         data[3] = (rev ? 0 : 0x20) | ((funcs&1) ? 0x10 : 0);
322
323         if(!(funcs&0x100))
324                 data[3] |= 0x80 | ((funcs>>1)&0xF);
325
326         command(CMD_LOK, addr, data, 4);
327 }
328
329 void Intellibox::turnout_command(unsigned addr, bool state, bool active)
330 {
331         unsigned char data[2];
332         data[0] = addr&0xFF;
333         data[1] = ((addr>>8)&0x7) | (active ? 0x40 : 0) | (state ? 0x80 : 0);
334         command(CMD_TURNOUT, addr, data, 2);
335 }
336
337 void Intellibox::process_reply(const Time::TimeStamp &t)
338 {
339         Command cmd = queue.front().cmd;
340
341         if(cmd==CMD_STATUS)
342         {
343                 unsigned char status;
344                 read_all(&status, 1);
345                 power = status&0x08;
346                 signal_power.emit(power);
347         }
348         else if(cmd==CMD_EVENT)
349         {
350                 for(unsigned i=0;; ++i)
351                 {
352                         unsigned char byte;
353                         read_all(&byte, 1);
354
355                         if(i==0)
356                         {
357                                 if(byte&0x01)
358                                         command(CMD_EVENT_LOK);
359                                 if(byte&0x20)
360                                         command(CMD_EVENT_TURNOUT);
361                                 if(byte&0x04)
362                                         command(CMD_EVENT_SENSOR);
363                         }
364                         else if(i==1)
365                         {
366                                 if(byte&0x40)
367                                         command(CMD_STATUS);
368                         }
369
370                         if(!(byte&0x80))
371                                 break;
372                 }
373         }
374         else if(cmd==CMD_EVENT_LOK)
375         {
376                 while(1)
377                 {
378                         unsigned char data[5];
379                         read_all(data, 1);
380                         if(data[0]==0x80)
381                                 break;
382                         read_all(data+1, 4);
383                 }
384         }
385         else if(cmd==CMD_EVENT_TURNOUT)
386         {
387                 unsigned char count;
388                 read_all(&count, 1);
389                 for(unsigned i=0; i<count; ++i)
390                 {
391                         unsigned char data[2];
392                         read_all(data, 2);
393
394                         unsigned addr = data[0]+((data[1]&7)<<8);
395                         Turnout &turnout = turnouts[addr];
396                         turnout.state = (data[1]&0x80)!=0;
397                         turnout.pending = turnout.state;
398                         signal_turnout.emit(addr, turnout.state);
399                 }
400         }
401         else if(cmd==CMD_EVENT_SENSOR)
402         {
403                 while(1)
404                 {
405                         unsigned char mod;
406                         read_all(&mod, 1);
407                         if(!mod)
408                                 break;
409
410                         unsigned char data[2];
411                         read_all(data, 2);
412                         for(unsigned i=0; i<16; ++i)
413                         {
414                                 unsigned addr = mod*16+i-15;
415                                 bool state = (data[i/8]>>(7-i%8))&1;
416
417                                 Sensor &sensor = sensors[addr];
418                                 if(state)
419                                 {
420                                         sensor.off_timeout = Time::TimeStamp();
421                                         if(!sensor.state)
422                                         {
423                                                 sensor.state = state;
424                                                 signal_sensor(addr, state);
425                                         }
426                                 }
427                                 else if(sensor.state)
428                                         sensor.off_timeout = t+700*Time::msec;
429                         }
430                 }
431         }
432         else if(cmd==CMD_LOK)
433         {
434                 Error err;
435                 read_status(&err);
436
437                 if(err==ERR_NO_ERROR)
438                 {
439                         unsigned addr = queue.front().addr;
440                         Locomotive &loco = locos[addr];
441                         signal_loco_speed.emit(addr, loco.speed, loco.reverse);
442                 }
443                 else
444                         error(cmd, err);
445         }
446         else if(cmd==CMD_TURNOUT)
447         {
448                 Error err;
449                 read_status(&err);
450
451                 unsigned addr = queue.front().addr;
452                 Turnout &turnout = turnouts[addr];
453
454                 if(err==ERR_NO_ERROR)
455                 {
456                         turnout.state = turnout.pending;
457                         if(turnout.active)
458                         {
459                                 signal_turnout.emit(addr, turnout.state);
460                                 turnout.off_timeout = t+500*Time::msec;
461                         }
462                 }
463                 else if(err==ERR_NO_I2C_SPACE)
464                         queue.push_back(queue.front());
465                 else
466                 {
467                         turnout.pending = turnout.state;
468                         error(cmd, err);
469                 }
470         }
471         else if(cmd==CMD_TURNOUT_STATUS)
472         {
473                 Error err;
474                 read_status(&err);
475
476                 if(err==ERR_NO_ERROR)
477                 {
478                         unsigned char data;
479                         read_all(&data, 1);
480
481                         unsigned addr = queue.front().addr;
482                         bool state = data&0x04;
483
484                         Turnout &turnout = turnouts[addr];
485                         if(state!=turnout.state)
486                         {
487                                 turnout.state = state;
488                                 turnout.pending = state;
489                                 signal_turnout.emit(addr, turnout.state);
490                         }
491                 }
492                 else
493                         error(cmd, err);
494         }
495         else if(cmd==CMD_LOK_STATUS)
496         {
497                 Error err;
498                 read_status(&err);
499
500                 if(err==ERR_NO_ERROR)
501                 {
502                         unsigned char data[3];
503                         read_all(data, 3);
504
505                         unsigned addr = queue.front().addr;
506                         Locomotive &loco = locos[addr];
507
508                         unsigned speed = (data[0]<=1 ? 0 : data[0]*2/19+1);
509                         bool reverse = !(data[1]&0x20);
510                         if(speed!=loco.speed || reverse!=loco.reverse)
511                         {
512                                 loco.speed = speed;
513                                 loco.reverse = reverse;
514                                 signal_loco_speed.emit(addr, loco.speed, loco.reverse);
515                         }
516
517                         unsigned funcs = (data[1]&0xF)<<1;
518                         if(data[1]&0x10)
519                                 funcs |= 1;
520                         if(funcs!=loco.funcs)
521                         {
522                                 unsigned changed = loco.funcs^funcs;
523                                 loco.funcs = funcs;
524                                 for(unsigned i=0; i<5; ++i)
525                                         if(changed&(1<<i))
526                                                 signal_loco_function.emit(addr, i, loco.funcs&(1<<i));
527                         }
528                 }
529                 else
530                         error(cmd, err);
531         }
532         else
533         {
534                 unsigned expected_bytes = 0;
535                 if(cmd==CMD_FUNC_STATUS)
536                         expected_bytes = 1;
537                 if(cmd==CMD_TURNOUT_GROUP_STATUS)
538                         expected_bytes = 2;
539                 if(cmd==CMD_LOK_CONFIG)
540                         expected_bytes = 4;
541
542                 Error err;
543                 read_status(&err);
544
545                 if(err==ERR_NO_ERROR)
546                 {
547                         unsigned char data[8];
548                         read_all(data, expected_bytes);
549                 }
550                 else
551                         error(cmd, err);
552         }
553 }
554
555 unsigned Intellibox::read_all(unsigned char *buf, unsigned len)
556 {
557         unsigned pos = 0;
558         while(pos<len)
559                 pos += read(serial_fd, buf+pos, len-pos);
560
561         return pos;
562 }
563
564 unsigned Intellibox::read_status(Error *err)
565 {
566         unsigned char c;
567         unsigned ret = read_all(&c, 1);
568         *err = static_cast<Error>(c);
569         return ret;
570 }
571
572 void Intellibox::error(Command cmd, Error err)
573 {
574         const char *cmd_str = 0;
575         switch(cmd)
576         {
577         case CMD_LOK: cmd_str = "CMD_LOK"; break;
578         case CMD_LOK_STATUS: cmd_str = "CMD_LOK_STATUS"; break;
579         case CMD_LOK_CONFIG: cmd_str = "CMD_LOK_CONFIG"; break;
580         case CMD_FUNC: cmd_str = "CMD_FUNC"; break;
581         case CMD_FUNC_STATUS: cmd_str = "CMD_FUNC_STATUS"; break;
582         case CMD_TURNOUT: cmd_str = "CMD_TURNOUT"; break;
583         case CMD_TURNOUT_FREE: cmd_str = "CMD_TURNOUT_FREE"; break;
584         case CMD_TURNOUT_STATUS: cmd_str = "CMD_TURNOUT_STATUS"; break;
585         case CMD_TURNOUT_GROUP_STATUS: cmd_str = "CMD_TURNOUT_GROUP_STATUS"; break;
586         case CMD_SENSOR_STATUS: cmd_str = "CMD_SENSOR_STATUS"; break;
587         case CMD_SENSOR_REPORT: cmd_str = "CMD_SENSOR_REPORT"; break;
588         case CMD_SENSOR_PARAM_SET: cmd_str = "CMD_SENSOR_PARAM_SET"; break;
589         case CMD_STATUS: cmd_str = "CMD_STATUS"; break;
590         case CMD_POWER_OFF: cmd_str = "CMD_POWER_OFF"; break;
591         case CMD_POWER_ON: cmd_str = "CMD_POWER_ON"; break;
592         case CMD_NOP: cmd_str = "CMD_NOP"; break;
593         case CMD_EVENT: cmd_str = "CMD_EVENT"; break;
594         case CMD_EVENT_LOK: cmd_str = "CMD_EVENT_LOK"; break;
595         case CMD_EVENT_TURNOUT: cmd_str = "CMD_EVENT_TURNOUT"; break;
596         case CMD_EVENT_SENSOR: cmd_str = "CMD_EVENT_SENSOR"; break;
597         default: cmd_str = "(unknown command)";
598         }
599
600         const char *err_str = 0;
601         switch(err)
602         {
603         case ERR_NO_ERROR: err_str = "ERR_NO_ERROR"; break;
604         case ERR_SYS_ERROR: err_str = "ERR_SYS_ERROR"; break;
605         case ERR_BAD_PARAM: err_str = "ERR_BAD_PARAM"; break;
606         case ERR_POWER_OFF: err_str = "ERR_POWER_OFF"; break;
607         case ERR_NO_LOK_SPACE: err_str = "ERR_NO_LOK_SPACE"; break;
608         case ERR_NO_TURNOUT_SPACE: err_str = "ERR_NO_TURNOUT_SPACE"; break;
609         case ERR_NO_DATA: err_str = "ERR_NO_DATA"; break;
610         case ERR_NO_SLOT: err_str = "ERR_NO_SLOT"; break;
611         case ERR_BAD_LOK_ADDR: err_str = "ERR_BAD_LOK_ADDR"; break;
612         case ERR_LOK_BUSY: err_str = "ERR_LOK_BUSY"; break;
613         case ERR_BAD_TURNOUT_ADDR: err_str = "ERR_BAD_TURNOUT_ADDR"; break;
614         case ERR_BAD_SO_VALUE: err_str = "ERR_BAD_SO_VALUE"; break;
615         case ERR_NO_I2C_SPACE: err_str = "ERR_NO_I2C_SPACE"; break;
616         case ERR_LOW_TURNOUT_SPACE: err_str = "ERR_LOW_TURNOUT_SPACE"; break;
617         case ERR_LOK_HALTED: err_str = "ERR_LOK_HALTED"; break;
618         case ERR_LOK_POWER_OFF: err_str = "ERR_LOK_POWER_OFF"; break;
619         default: cmd_str = "(unknown error)";
620         }
621
622         IO::print("Error: %s: %s\n", cmd_str, err_str);
623 }
624
625
626 Intellibox::Locomotive::Locomotive():
627         speed(0),
628         reverse(false),
629         funcs(0)
630 { }
631
632
633 Intellibox::Turnout::Turnout():
634         state(false),
635         active(false),
636         pending(false)
637 { }
638
639
640 Intellibox::Sensor::Sensor():
641         state(false)
642 { }
643
644 } // namespace Marklin